ロボットの位置姿勢補間方法及びロボットの制御装置
【課題】ロボットの手先の姿勢が一定の条件を満たすように補間できるロボットの位置姿勢補間方法を提供する。
【解決手段】各教示点を通過する移動軌跡がスプライン曲線となるように補間点を生成し(S3)、各教示点毎に、教示点と同じ座標系における撮像対象物の座標を求め、各教示点に対応する撮像対象物の座標間を、スプライン曲線により補間して補間点を生成する(S4)。教示点側の補間点を始点,撮像対象物側の補間点を終点とするアプローチベクトルを全ての補間点に対して求め(S5)、2つの教示点間の各補間点におけるノーマルベクトルの回転角を補間すると(S6)各補間点に立つアプローチ,ノーマルベクトルの外積よりオリエントベクトルを求め(S7)各補間点と3つの姿勢ベクトルの座標とから各補間点における手先の姿勢を決定する。
【解決手段】各教示点を通過する移動軌跡がスプライン曲線となるように補間点を生成し(S3)、各教示点毎に、教示点と同じ座標系における撮像対象物の座標を求め、各教示点に対応する撮像対象物の座標間を、スプライン曲線により補間して補間点を生成する(S4)。教示点側の補間点を始点,撮像対象物側の補間点を終点とするアプローチベクトルを全ての補間点に対して求め(S5)、2つの教示点間の各補間点におけるノーマルベクトルの回転角を補間すると(S6)各補間点に立つアプローチ,ノーマルベクトルの外積よりオリエントベクトルを求め(S7)各補間点と3つの姿勢ベクトルの座標とから各補間点における手先の姿勢を決定する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後、前記位置及び前記姿勢を補間する方法及びロボットの制御装置に関する。
【背景技術】
【0002】
ロボットをティーチングする場合には、一般に、ロボットの手先を移動させる軌跡に沿った位置を教示すると共に、その教示点においてロボットの手先が取るべき姿勢も教示するようになっている。例えば、特許文献1には、ティーチング及びその後に行われる補間に関連する技術が開示されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2005−275484号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
ところで、 ロボットの手先にカメラを取り付け、そのカメラを移動させて固定されているワークの画像を撮像する、という作業を行う場合、ティーチングを行う際のロボットの姿勢は、当然にカメラの視線が固定されているワークを捉える方向となるように決定されることが望まれる。
【0005】
カメラを移動させると共に、カメラがワークを捉える焦点距離を変化させながら撮像を行う場合、ロボットの手先の位置をティーチングした後は、移動軌跡が滑らかに繋がるようにティーチングした位置の間を例えばスプライン曲線等の自由曲線を用いて補間する。各教示点については、カメラの視線はワークの方向を向くようにティーチングされているが、補間した位置においても、カメラの視線はワークの方向を向くことが望ましい。しかしながら、従来、手先の姿勢について一定の条件を満たすように補間する技術は存在しなかった。
【0006】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先の姿勢が一定の条件を満たすように補間できるロボットの位置姿勢補間方法及びロボットの制御装置を提供することにある。
【課題を解決するための手段】
【0007】
請求項1記載の位置姿勢補間方法によれば、各教示点を通過する移動軌跡が自由曲線となるように教示点間の位置を移動順番を付けて補間して補間点を生成し(第1ステップ)、各教示点毎に、教示点と同じ座標系における前記目標点の座標を求め(第2ステップ)、各教示点に対応する各目標点の座標間を、第1ステップと同じアルゴリズムの自由曲線により補間して第1ステップと同じ移動順番を付した補間点を生成する(第3ステップ)。尚、第1〜第3ステップについては、実行順序を適宜入れ替えても問題は無い。例えば、先に各教示点に対応する各目標点の座標間を補間した場合は、その後、教示点間を補間した点に同じ移動順番を付せば良い。
【0008】
ここで、教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢とする。第1姿勢正規化ベクトルはアプローチベクトルに対応し、第2,第3正規化姿勢ベクトルは、ノーマル,オリエントベクトルの何れかに対応する。
そして、第1ステップで生成した補間点を始点とし、第3ステップにおいて生成した補間点を終点とする第1姿勢ベクトルを、第1ステップで生成した全ての補間点に対して求める(第4ステップ)。すなわち、第4ステップでは、第1姿勢ベクトルについてはその長さ(ノルム)を正規化された一定の長さ(単位長)にせず、対象物との距離に応じて変化させることになる。
【0009】
それから、2つの教示点間の各補間点における第2姿勢正規化ベクトルの回転角を補間すると(第5ステップ)、各補間点に立つ前記第1姿勢ベクトル及び前記第2正規化姿勢ベクトルとの外積より第3姿勢ベクトルを求め(第6ステップ)、第1ステップで生成した補間点と、第4〜第6ステップで得られた各姿勢ベクトルの座標とから、各補間点における手先の姿勢を決定する(第7ステップ)。尚、手先の姿勢を決定することが目的であるから、第3姿勢ベクトルを求めるに際してその長さが正規化されているか否かは問題とならない。
すなわち、第4ステップでは、第1姿勢ベクトルの長さを対象物の距離に応じて変化させるので、第2,第3姿勢ベクトルも併せて決定される手先の姿勢は、第1姿勢ベクトルが常に対象物の方を向くように補間処理される。したがって、手先の位置を補間する際に、各補間点における手先の姿勢が、少なくとも対象物の方向を向くように補間することができる。
【0010】
請求項2記載の位置姿勢補間方法によれば、第1〜第4ステップを、請求項1と同様に実行する。それから、第2,第3姿勢ベクトルのそれぞれについて、隣り合う2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルがなす角θiを求め(第5ステップ)、第2,第3姿勢ベクトルのそれぞれについて、2つの教示点i,(i+1)の間で補間された補間点jにつき、教示点i,補間点jにそれぞれ立つ2つのベクトルがなす角θjを求める(第6ステップ)。
次に、第2,第3姿勢ベクトルの何れか一方について、2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルの外積Piを求め、求めた外積Piを正規化して正規化ベクトルnPiを求め(第7ステップ)、正規化ベクトルnPiを回転軸とし、当該正規化ベクトルnPiについて、角度θjを回転角とする正規化クオータニオンQjを求める(第8ステップ)。
【0011】
それから、正規化ベクトルnPiとして選択した姿勢ベクトルを、正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求め(第9ステップ)、補間点jに立つ第1姿勢ベクトルRaj,及び回転させた姿勢ベクトルRbjの外積を求め、補間点jに立つ、正規化ベクトルとして選択しなかった姿勢ベクトルRcjを求める(第10ステップ)。そして、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求め直し(第11ステップ)、これらの3つの姿勢ベクトルRaj,Rbj,Rcjより、補間点jにおける手先の姿勢角を求める(第12ステップ)。
【0012】
すなわち、請求項1の方法による場合、補間点を基準として姿勢を補間する結果、手先の位置の変化に応じて手先の姿勢が変化する率が線形にならないことが想定される。これに対して請求項2の方法によれば、正規化ベクトルnPiについて、角度θjを回転角とする正規化クオータニオンQjを求め、その正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求める。そして、残りの姿勢ベクトルRcjを、補間点jに立つ第1姿勢ベクトルRajと姿勢ベクトルRbjとから求め、再度、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求めなおし、それら3つの姿勢ベクトルRaj,Rbj,Rcjより補間点jにおける手先の姿勢角を求めるので、手先位置の変化に応じた手先姿勢の変化率を線形にすることができ、姿勢をより滑らかに変化させることができる。
【0013】
尚、請求項1における第5〜第7ステップと、請求項2における第5〜第12ステップとは、第2姿勢ベクトルと第3姿勢ベクトルとを決定する手順が、請求項1では第2姿勢ベクトル,第3姿勢ベクトルの順で決定するのに対し、請求項2では第2姿勢ベクトル,第3姿勢ベクトルの何れか一方,他方の順で決定する点が相違している。この相違点を除けば、請求項2における第5〜第12ステップは、請求項1における第5〜第7ステップの手順をより詳細且つ具体的に述べたものに等しい(例えば上記第5ステップにおける回転角の補間処理を、正規化クオータニオンQjを用いて行うことを限定している)。
【図面の簡単な説明】
【0014】
【図1】第1実施例であり、ロボット本体の手先の位置及び姿勢について、教示点間の位置及び姿勢を補間する処理を示すフローチャート
【図2】図1の処理に対応するイメージをベクトルモデルで示す図(その1)
【図3】図1の処理に対応するイメージをベクトルモデルで示す図(その2)
【図4】アプローチベクトルの長さを変化させることの意義を説明する図
【図5】垂直多関節型ロボットを含む制御システムの構成を示す図
【図6】制御系の機能ブロック図
【図7】第2実施例を示す図1相当図(その1)
【図8】図1相当図(その2)
【図9】姿勢ベクトルAiの方向が撮像対象物の方向に対してずれを生じている状態を説明する図
【図10】正規化クオータニオンQnjを用いる作用効果を説明する図(その1)
【図11】正規化クオータニオンQnjを用いる作用効果を説明する図(その2)
【発明を実施するための形態】
【0015】
(第1実施例)
以下、本発明の第1実施例について図1ないし図6を参照して説明する。図5は、垂直多関節型(6軸)ロボットを含む制御システムの構成を示す。このロボット本体1は、ベース(回転軸)2上に、この場合6軸のアームを有し、そのアームの先端には、図示しないハンド等のツールや、後述するカメラなどが取り付けられる。前記ベース2上には、第1関節J1を介して第1のアーム3が回転可能に連結されている。この第1のアーム3には、第2関節J2を介して上方に延びる第2のアーム4の下端部が回転可能に連結され、さらに、この第2のアーム4の先端部には、第3関節J3を介して第3のアーム5が回転可能に連結されている。
【0016】
この第3のアーム5の先端には第4関節J4を介して第4のアーム6が回転可能に連結され、この第4のアーム6の先端には第5関節J5を介して第5のアーム7が回転可能に連結され、この第5のアーム7には第6関節J6を介して第6のアーム8が回転可能に連結されている。なお、各関節J1〜J6においては、図示しないサーボモータにより各アーム3〜8を回転駆動するようになっている。
【0017】
ロボット本体1と制御装置11との間は、接続ケーブル12によって接続されている。これにより、ロボット本体1の各軸を駆動するサーボモータは、制御装置11により制御される。操作パッド(入力手段)13は、例えば横長の本体14の両端側に、ユーザが左右両手でそれぞれ把持するための把持部15L,15Rが設けられており、ユーザが把持部15L,15Rを両手で把持した状態で、親指により押圧操作可能な操作スティック16L,16Rが配置されている。また、把持部15L,15Rの図中上方には、人差し指により押圧操作可能なボタン17L,17Rが配置されている。
【0018】
ロボット本体1のアーム8には、CCD(Charge Coupled Device)又はCMOSイメージセンサを用いたカメラ(撮像手段)21が取り付けられており、そのカメラ21により撮像対象物(ワーク)22を画像で捉える。制御装置11には、パーソナルコンピュータ(パソコン)23がケーブル24を介して接続されている。
【0019】
パソコン23には、メモリやハードディスクなどの記憶装置(記憶手段)が内蔵されている。操作パッド13は、接続ケーブル18を経由してパソコン23に接続され、通信インターフェイスを介してパソコン23との間で高速のデータ転送を実行するようになっている。操作スティック16L,16R等が操作されて入力された操作信号等の情報は、操作パッド13からパソコン23を介して制御装置11へ送信される。カメラ21は、ケーブル25を介してパソコン23に接続されており、カメラ21が捉えた画像のデータはパソコン23に送信されて、ディスプレイ23Dに表示される。
【0020】
図6に示すように、制御装置11は、制御部としてのCPU31、各関節のモータ30を駆動する駆動手段としての駆動回路32、検出回路33などを備えている。そして、CPU31には、ロボット全体のシステムプログラム等を記憶したROM34、ロボット本体1の動作プログラム等を記憶したRAM35および前記ティーチングペンダント3を接続するためのインターフェイス36が接続されている。なお、図6では、ショルダ部5、下アーム6、上アーム7、手首8を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ30も一台だけ示した。
【0021】
上記検出回路33は、各関節の現在位置(回転角度)および現在速度(回転速度)を検出するためのもので、この検出回路33には、各関節を駆動するモータ30に設けられたロータリエンコーダ37が接続されている。ロータリエンコーダ37は位置センサおよび速度センサを兼用するもので、各モータ30の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路33に与えられる。検出回路33は、各ロータリエンコーダ37からのパルス信号に基づいて各モータ30ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ37から入力されるパルス信号数に基づいて各モータ30ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ30の駆動回路32およびCPU31に与えられるようになっている。
【0022】
そして、各駆動回路32は、CPU31から与えられる位置指令値および速度指令値と検出回路33から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ30に供給してそれらを駆動する。これにより、ロボットアーム先端(手先)であるフランジ(図示せず)の中心部が指令位置を順に通る経路を辿って動作することで、種々の作業を行う。
【0023】
ロボットアーム先端の移動経路は、操作パッド13(若しくは、図示しないティーチングペンダント)を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示される。そして、従来の教示作業であれば、各指令位置でのロボットアーム先端の姿勢も教示するが、本実施例では、姿勢は後述のように自動的に決定される。そして教示された指令位置と決定された姿勢とは、RAM35に記憶される。制御装置11は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。
【0024】
なお、ロボットアーム先端の位置は、ツール(図5Σc)に固定された3次元座標(ツール座標)の原点がロボット座標(基底座標,ベース座標)上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、ツールに固定のツール座標の3軸の単位ベクトル(ノーマル,オリエント,アプローチの各正規化姿勢ベクトル)がロボット座標上で示す向きによって定義される。
【0025】
次に、本実施例の作用について図1ないし図4を参照して説明する。図1は、ロボット本体1の手先(TCP)の位置及び姿勢の教示を行った後、教示点間における手先の位置及び姿勢を補間する処理を示すフローチャートであり、図2及び図3は、図1の処理に対応するイメージをベクトルモデルで示している。尚、図1に示すフローチャートは、理解を容易にするため処理の概略を原理的に示しており、より詳細な処理手順については第2実施例で説明する。
【0026】
先ず、各教示点について、手先の座標;TCP座標Pi(Xi,Yi,Zi)と、各軸の回転角である姿勢角(Rxi,Ryi,Rzi)とを記録する(S1)。この時、手先の姿勢は、カメラ21の視線(Z軸)が撮像対象物22の方向を向くように教示されている。続いて、各教示点毎に、撮像対象物22までの距離を求めて設定する(S2)。カメラ21を使用する場合、上記の距離は焦点距離となる(図2(a)参照)。
【0027】
それから、教示したTCP座標Piの間を、例えばスプライン曲線等の自由曲線を用いて補間し(S3,図2(b)参照,第1ステップ,位置補間手段)、各教示点から撮像対象物22までの距離を頂点とする座標(Ti,Ti+1,Ti+2)について、それらの間をステップS3と同様に例えばスプライン曲線等の自由曲線を用いて補間する(S4,図2(c)参照,第2及び第3ステップ,目標点座標決定手段,目標点位置補間手段)。尚、上記座標(Ti,Ti+1,Ti+2)については、教示点と同じ座標系で表す。また、「i」,「i+1」等は手先位置の移動順番を示す。
【0028】
次に、ステップS3で求めた各教示点の補間点と、この補間点に対応してステップS4で求めた頂点座標の補間点の2点から、アプローチベクトル(第1姿勢ベクトル)を生成する(S5,図3(a)参照),第4ステップ,第1姿勢ベクトル決定手段)。このアプローチベクトルは、カメラ21の視線方向であるZ軸に一致している。尚、S4をS2もしくは、S3の前に実行順序を移動させても全く問題ないことは明らかである。
【0029】
ここで、図4は、アプローチベクトルを上記のように決定することの意義を説明する図である。図4(a)は従来技術による補間処理を示しており、従来は教示点P1,P2間の位置P12を補間するだけであるため、各教示点P1,P2のアプローチベクトルがターゲット(撮像対象物22)の方向を向いていても、補間点P12でのアプローチベクトルがターゲットの方向を向くとは限らない。この場合、各補間点におけるアプローチベクトルの長さは正規化された単位長で常に一定である。
図4(b)は本実施例による補間処理を示しており、補間点P12でのアプローチベクトルを、各教示点の補間点と、この補間点に対応する撮像対象物22側の補間点の2点から生成することで、各補間点におけるアプローチベクトルの長さは一定ではなく変化することになるから、アプローチベクトルがターゲットの方向を向くことが保証される。
【0030】
再び図1を参照する。手先の姿勢を決定する他の2軸ベクトルはオリエントベクトルとノーマルベクトルであるが、これらの何れか一方について、教示点間の回転軸を中心として回転角の補間を行い、補間軌道に対応したオリエント若しくはノーマルベクトルを生成する(S6,第5ステップ,回転角補間手段)。
ここで、図3(b)〜(d)は、ステップS6においてノーマルベクトル(第2正規化姿勢ベクトル)を生成する際に回転角を補間する処理のイメージであり、図3(b)は、図3(a)に示すA側,教示点側よりアプローチベクトルに沿って撮像対象物22の方向を見た場合のノーマル,オリエントベクトルを示している。図3(c)は、各教示点におけるノーマルベクトルの回転角θを、スプライン補間した場合の角度変化の曲線を示しており、図3(d)は、図3(c)のように補間した結果、補間点においてノーマルベクトルが示す回転角θである。
【0031】
そして、図1に示すS7では、S6でノーマル,オリエントベクトルの一方が決まれば、他方のベクトル(第3姿勢ベクトル)は正規直交条件によりアプローチベクトルとの外積により求めることができる(第7ステップ,第3姿勢ベクトル決定手段)。これにより、各補間点についての3つの姿勢ベクトル;ノーマル,オリエント,アプローチの各ベクトルが決まるので、これらにより姿勢が決定される(第8ステップ,姿勢決定手段)。したがって、各補間点における手先のアプローチベクトルが撮像対象物22の方向を向くと共に、ある教示点から補間点を経て次の教示点に至るまでのノーマル,オリエントベクトルの回転角が滑らかに変化するように手先の姿勢が補間されたことになる。
【0032】
以上のように本実施例によれば、各教示点を通過する移動軌跡がスプライン曲線となるように教示点間の位置を移動順番を付けて補間して補間点を生成し、各教示点毎に、教示点と同じ座標系における撮像対象物22の座標を求め、各教示点に対応する撮像対象物22の座標間を、スプライン曲線により補間して同じ移動順番を付した補間点を生成する。そして、教示点側の補間点を始点,撮像対象物22側の補間点を終点とするアプローチベクトルを全ての補間点に対して求め、2つの教示点間の各補間点における例えばノーマルベクトルの回転角を補間すると、各補間点に立つアプローチベクトルとノーマルベクトルとの外積よりオリエントベクトルを求め、各補間点と3つの姿勢ベクトルの座標とから各補間点における手先の姿勢を決定するようにした。
【0033】
すなわち、アプローチベクトルの長さを撮像対象物22までの距離に応じて変化させるので、ノーマル,オリエントベクトルも併せて決定される手先の姿勢は、アプローチベクトルが常に対象物の方を向くように補間処理される。したがって、手先の位置を補間する際に、各補間点における手先の姿勢が、少なくとも撮像対象物22の方向を向くように補間することができる。加えて、ある教示点から補間点を経て次の教示点に至るまでのノーマル,オリエントベクトルの回転角が滑らかに変化するように手先の姿勢を補間することができる。
【0034】
(第2実施例)
図7ないし図11は第2実施例であり、第1実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。前述したように、第2実施例は第1実施例で示した処理手順をより詳細且つ具体的に示すものである。図7(a)に示すフローチャートにおいて、先ず、教示点毎にTCP座標Xciと、姿勢角(Rx,Ry,Rz)とを教示すると(S11,第1実施例では前提)、各TCP座標間(例えば、Xci, Xci+1との間)について例えばスプライン曲線等の自由曲線を用いて補間を行い、補間点を含む軌道を生成する(S12,第1ステップ,位置補間手段)。尚、添え字「i」,「i+1」は教示点について順次付す番号であり、以下では「キーフレーム番号」と称す。
【0035】
次に、姿勢角(Rxi,Ryi,Rzi)より、各教示点に立つ正規化姿勢ベクトル(Ni,Oi,Ai)を求める(S13)。そして、座標(Xt,Yt,Zt)を姿勢ベクトル(Ni,Oi,Ai)を用いて記述した場合の、TCP座標を始点とする姿勢ベクトルの長さ(ni,oi,ai)を求め(S14,第2ステップ)、姿勢ベクトル(Ni,Oi,Ai)の長さを(ni,oi,ai)とした時の頂点の座標Xtiを求める(S15,目標点座標決定手段,図7(b)参照)。すなわち、座標Xtiは、TCP座標を基準として撮像対象物22の位置を示す座標である。
【0036】
ここで、姿勢ベクトルAiの方向が撮像対象物22の方向に完全に一致している場合、座標Xtiは姿勢ベクトルAiの長さを示し、他の姿勢ベクトルの長さni,oiはゼロとなるが、姿勢ベクトルAiの方向が撮像対象物22の方向とずれを生じていれば他の姿勢ベクトルの長さni,oiも所定の値を示すことになる(図9参照)。
【0037】
次に、アプローチベクトルAiの長さをaiとして、当該ベクトルAiの先端座標の間を、S12と同様にスプライン曲線により補間して補間点を含む軌道を生成する(S16,第3及び第4ステップ,目標点位置補間手段)。それから、教示点TCPの座標と、長さaiのアプローチベクトルAi先端のスプライン補間軌道について、各フィールド座標(補間した座標)Xcj,Xtjとを結ぶベクトル(フィールド毎の正規化アプローチベクトルAj)を求める(S17,第1姿勢ベクトル決定手段,図7(c)参照)。尚、添え字「j」は補間点について順次付す番号であり、以下では「フィールド番号」と称す。
【0038】
そして、教示点Xi,Xi+1におけるノーマルベクトルNi,Ni+1の内積を演算し、両者がなす角度Θni(i=0,…,N)を求め(S18,教示点ベクトル角度算出手段)、その角度Θniについてもスプライン補間することで各補間点におけるノーマルベクトルが示す角度Θnjを求める(S19,補間点ベクトル角度算出手段)。また、教示点Xi,Xi+1におけるオリエントベクトルOi,Oi+1についても同様に内積を演算し、両者がなす角度Θoi(i=0,…,N)を求め(S20)、その角度Θoiについてもスプライン補間することで各補間点におけるオリエントベクトルが示す角度Θojを求める(S21)。尚、S18〜S21が第5ステップに対応する。
【0039】
引き続き図8(a)に示す処理において、教示点のノーマルベクトルNi,Ni+1の外積Npiを演算し、その結果をノーマルベクトルの仮の回転中心として用いることにする(S22,図8(b)参照)。外積Npiの値がゼロでなければ(S23:YES)、外積Npiを正規化し(S24,第7ステップ,正規化ベクトル決定手段)、正規化ベクトルNpiを回転軸として回転角θnjによる正規化クオータニオンQnjを求める(S25,第8ステップ,正規化クオータニオン算出手段)。尚、回転角θnjは、
θnj=Θnj−Θni …(1)
で表され、フィールド番号jがキーフレーム番号i,i+1間に有る場合に、ノーマルベクトルの回転角が教示点Xiから補間点Xjに至る間に変化する角度の差分である(第6ステップ)。この回転角θnjについては、S25で正規化クオータニオンQnjを求めるまでに求めておけば良い。また、正規化クオータニオンQnjは、(2)式で求められる。
【0040】
【数1】
尚、Npjは、教示点Xi,補間点Xcjにおけるノーマルベクトルの外積である。
【0041】
次に、正規化したノーマルベクトルNiを、正規化クオータニオンQnjにより回転させたノーマルベクトルNjを(3)式により求める(S26,図8(c)参照,第9ステップ,回転姿勢ベクトル算出手段)。
【0042】
【数2】
尚、Qnj-1は、正規化クオータニオンQnjの逆行列である。すると、補間点XcjにおけるアプローチベクトルAjと、ノーマルベクトルNjとが求められたので、これらの外積よりオリエントベクトルOjを求めることができる(S27,図8(d)参照,第10ステップ,最終姿勢ベクトル算出手段)。またここで、アプローチベクトルAjとオリエントベクトルOjの外積からノーマルベクトルNjを求め直す(S27a,第11ステップ)。
【0043】
以上より、フィールド番号jについて、アプローチベクトルAj,ノーマルベクトルNj,オリエントベクトルOjが求められたので、補間点Xcjにおける姿勢角(Rxj,Ryj,Rzj)が得られる(S28,図8(e)参照,第12ステップ)。尚、図8(e)に示すNj(old)は、第11ステップにおいて、アプローチベクトルAjとオリエントベクトルOjの外積からノーマルベクトルNjを求め直す前に求められているノーマルベクトルNjを示す。
【0044】
一方、S23において外積Npiの値がゼロである場合は(NO)、S22で用いた教示点のノーマルベクトルNi,Ni+1に替えてオリエントベクトルOi,Oi+1を用いてそれらの外積Opiを演算する(S29)。そして、外積Opiの値がゼロでなければ(S30:YES),S24〜S27におけるノーマルベクトルNをオリエントベクトルOに置き換えて同様の処理を行う(S31)。また、S30において外積Opiの値がゼロである場合は(NO)手先の姿勢が全く変化していないことを意味するので(S32)、そのまま処理を終了する。
【0045】
尚、図7,図8に示すフローチャートは、1つのキーフレーム番号iについて、次のキーフレーム番号(i+1)との間を補間した点を示すフィールド番号の全てについて処理を実行し、次に、キーフレーム番号(i+1)と次のキーフレーム番号(i+2)との間を補間した点を示すフィールド番号の全てについて処理を実行する、というように順次フィールド番号とキーフレーム番号とをインクリメントして行う。
【0046】
次に、上述したように正規化クオータニオンQnjを用いることによる作用効果について図10及び図11を参照して説明する。図10は、位置Aから位置Bまで円弧に沿って回転させる間の回転角を補間する場合をモデル的に示している。補間のやり方には色々あるが、例えば図10(b)に示すように位置A,B間に直線を引いて、その直線を4等分するように3点p1,p2,p3を決定する(図10(c)参照)。これらの3点を用いて回転角を補間するのであれば、円の中心から各点p1,p2,p3を通り、円周上に至る位置AB1,AB2,AB3が各補間点における回転角を与える。
【0047】
位置AB2は、位置A,B間の回転角の丁度1/2となるが、位置AB1については、位置A,AB2間の1/2よりも小さくなり、位置AB3については、位置AB2,B間の1/2よりも大きくなる。その結果、図11に破線で示すように回転角の変化率が線形とならず、滑らかに変化しない。第1実施例では、回転角の補間方式を任意としているため、回転角の変化がこの破線のようになる場合も想定される。これに対して正規化クオータニオンQnjを用いて回転させることで各補間点のノーマル若しくはオリエントベクトルを決定すると、図11に実線で示すように回転角の変化率が線形となるので滑らかに変化するようになる。
【0048】
以上のように第2実施例によれば、教示点である各TCP座標間についてスプライン曲線を用いて補間を行い、補間点を含む軌道を生成すると、各教示点に立つ正規化姿勢ベクトルを求め、撮像対象物22の座標(Xt,Yt,Zt)を姿勢ベクトル(Ni,Oi,Ai)を用いて記述した場合の、TCP座標を始点とする姿勢ベクトルの長さ(ni,oi,ai)を求め、姿勢ベクトル(Ni,Oi,Ai)の長さを(ni,oi,ai)とした時の頂点の座標Xtiを求める。次に、アプローチベクトルAiの長さをaiとして、当該ベクトルAiの先端座標の間をスプライン曲線により補間して補間点を含む軌道を生成すると、教示点TCPの座標と、長さaiのアプローチベクトルAi先端のスプライン補間軌道において各フィールド座標とを結ぶ正規化アプローチベクトルAjを求める。
【0049】
そして、教示点Xi,Xi+1におけるノーマルベクトルNi,Ni+1の内積を演算し、得られた角度Θniをスプライン補間して各補間点におけるノーマルベクトルが示す角度Θnjを求め、オリエントベクトルOi,Oi+1についても同様に内積を演算し、得られた角度Θoiをスプライン補間して各補間点におけるオリエントベクトルが示す角度Θojを求める。
【0050】
更に、教示点のノーマルベクトルNi,Ni+1の外積Npiを演算して正規化し、正規化ベクトルNpiを回転軸として回転角θnjによる正規化クオータニオンQnjを求めると、正規化したノーマルベクトルNiを正規化クオータニオンQnjにより回転させたノーマルベクトルNjを求める。そして、補間点XcjにおけるアプローチベクトルAjと、ノーマルベクトルNjとの外積よりオリエントベクトルOjを求め、再度、アプローチベクトルAjとオリエントベクトルOjとの外積よりノーマルベクトルNjを求め直し、補間点Xcjにおける姿勢角(Rxj,Ryj,Rzj)を得るようにした。すなわち、補間点におけるノーマル,オリエントベクトルの何れか一方を求めるに当たり、正規化クオータニオンQnjを用いて回転角をえることにより、手先位置の変化に応じた手先姿勢の変化率を線形にすることができ、姿勢をより滑らかに、自然に変化させることができる。
【0051】
本発明は上記し又は図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
補間に用いる曲線はスプライン曲線に限ることなく、その他ベジェ曲線等の自由曲線を用いても良い。
【符号の説明】
【0052】
図面中、1はロボット本体、11は制御装置、21はカメラ(撮像手段)、22は撮像対象物(ワーク)を示す。
【技術分野】
【0001】
本発明は、垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後、前記位置及び前記姿勢を補間する方法及びロボットの制御装置に関する。
【背景技術】
【0002】
ロボットをティーチングする場合には、一般に、ロボットの手先を移動させる軌跡に沿った位置を教示すると共に、その教示点においてロボットの手先が取るべき姿勢も教示するようになっている。例えば、特許文献1には、ティーチング及びその後に行われる補間に関連する技術が開示されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開2005−275484号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
ところで、 ロボットの手先にカメラを取り付け、そのカメラを移動させて固定されているワークの画像を撮像する、という作業を行う場合、ティーチングを行う際のロボットの姿勢は、当然にカメラの視線が固定されているワークを捉える方向となるように決定されることが望まれる。
【0005】
カメラを移動させると共に、カメラがワークを捉える焦点距離を変化させながら撮像を行う場合、ロボットの手先の位置をティーチングした後は、移動軌跡が滑らかに繋がるようにティーチングした位置の間を例えばスプライン曲線等の自由曲線を用いて補間する。各教示点については、カメラの視線はワークの方向を向くようにティーチングされているが、補間した位置においても、カメラの視線はワークの方向を向くことが望ましい。しかしながら、従来、手先の姿勢について一定の条件を満たすように補間する技術は存在しなかった。
【0006】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先の姿勢が一定の条件を満たすように補間できるロボットの位置姿勢補間方法及びロボットの制御装置を提供することにある。
【課題を解決するための手段】
【0007】
請求項1記載の位置姿勢補間方法によれば、各教示点を通過する移動軌跡が自由曲線となるように教示点間の位置を移動順番を付けて補間して補間点を生成し(第1ステップ)、各教示点毎に、教示点と同じ座標系における前記目標点の座標を求め(第2ステップ)、各教示点に対応する各目標点の座標間を、第1ステップと同じアルゴリズムの自由曲線により補間して第1ステップと同じ移動順番を付した補間点を生成する(第3ステップ)。尚、第1〜第3ステップについては、実行順序を適宜入れ替えても問題は無い。例えば、先に各教示点に対応する各目標点の座標間を補間した場合は、その後、教示点間を補間した点に同じ移動順番を付せば良い。
【0008】
ここで、教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢とする。第1姿勢正規化ベクトルはアプローチベクトルに対応し、第2,第3正規化姿勢ベクトルは、ノーマル,オリエントベクトルの何れかに対応する。
そして、第1ステップで生成した補間点を始点とし、第3ステップにおいて生成した補間点を終点とする第1姿勢ベクトルを、第1ステップで生成した全ての補間点に対して求める(第4ステップ)。すなわち、第4ステップでは、第1姿勢ベクトルについてはその長さ(ノルム)を正規化された一定の長さ(単位長)にせず、対象物との距離に応じて変化させることになる。
【0009】
それから、2つの教示点間の各補間点における第2姿勢正規化ベクトルの回転角を補間すると(第5ステップ)、各補間点に立つ前記第1姿勢ベクトル及び前記第2正規化姿勢ベクトルとの外積より第3姿勢ベクトルを求め(第6ステップ)、第1ステップで生成した補間点と、第4〜第6ステップで得られた各姿勢ベクトルの座標とから、各補間点における手先の姿勢を決定する(第7ステップ)。尚、手先の姿勢を決定することが目的であるから、第3姿勢ベクトルを求めるに際してその長さが正規化されているか否かは問題とならない。
すなわち、第4ステップでは、第1姿勢ベクトルの長さを対象物の距離に応じて変化させるので、第2,第3姿勢ベクトルも併せて決定される手先の姿勢は、第1姿勢ベクトルが常に対象物の方を向くように補間処理される。したがって、手先の位置を補間する際に、各補間点における手先の姿勢が、少なくとも対象物の方向を向くように補間することができる。
【0010】
請求項2記載の位置姿勢補間方法によれば、第1〜第4ステップを、請求項1と同様に実行する。それから、第2,第3姿勢ベクトルのそれぞれについて、隣り合う2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルがなす角θiを求め(第5ステップ)、第2,第3姿勢ベクトルのそれぞれについて、2つの教示点i,(i+1)の間で補間された補間点jにつき、教示点i,補間点jにそれぞれ立つ2つのベクトルがなす角θjを求める(第6ステップ)。
次に、第2,第3姿勢ベクトルの何れか一方について、2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルの外積Piを求め、求めた外積Piを正規化して正規化ベクトルnPiを求め(第7ステップ)、正規化ベクトルnPiを回転軸とし、当該正規化ベクトルnPiについて、角度θjを回転角とする正規化クオータニオンQjを求める(第8ステップ)。
【0011】
それから、正規化ベクトルnPiとして選択した姿勢ベクトルを、正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求め(第9ステップ)、補間点jに立つ第1姿勢ベクトルRaj,及び回転させた姿勢ベクトルRbjの外積を求め、補間点jに立つ、正規化ベクトルとして選択しなかった姿勢ベクトルRcjを求める(第10ステップ)。そして、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求め直し(第11ステップ)、これらの3つの姿勢ベクトルRaj,Rbj,Rcjより、補間点jにおける手先の姿勢角を求める(第12ステップ)。
【0012】
すなわち、請求項1の方法による場合、補間点を基準として姿勢を補間する結果、手先の位置の変化に応じて手先の姿勢が変化する率が線形にならないことが想定される。これに対して請求項2の方法によれば、正規化ベクトルnPiについて、角度θjを回転角とする正規化クオータニオンQjを求め、その正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求める。そして、残りの姿勢ベクトルRcjを、補間点jに立つ第1姿勢ベクトルRajと姿勢ベクトルRbjとから求め、再度、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求めなおし、それら3つの姿勢ベクトルRaj,Rbj,Rcjより補間点jにおける手先の姿勢角を求めるので、手先位置の変化に応じた手先姿勢の変化率を線形にすることができ、姿勢をより滑らかに変化させることができる。
【0013】
尚、請求項1における第5〜第7ステップと、請求項2における第5〜第12ステップとは、第2姿勢ベクトルと第3姿勢ベクトルとを決定する手順が、請求項1では第2姿勢ベクトル,第3姿勢ベクトルの順で決定するのに対し、請求項2では第2姿勢ベクトル,第3姿勢ベクトルの何れか一方,他方の順で決定する点が相違している。この相違点を除けば、請求項2における第5〜第12ステップは、請求項1における第5〜第7ステップの手順をより詳細且つ具体的に述べたものに等しい(例えば上記第5ステップにおける回転角の補間処理を、正規化クオータニオンQjを用いて行うことを限定している)。
【図面の簡単な説明】
【0014】
【図1】第1実施例であり、ロボット本体の手先の位置及び姿勢について、教示点間の位置及び姿勢を補間する処理を示すフローチャート
【図2】図1の処理に対応するイメージをベクトルモデルで示す図(その1)
【図3】図1の処理に対応するイメージをベクトルモデルで示す図(その2)
【図4】アプローチベクトルの長さを変化させることの意義を説明する図
【図5】垂直多関節型ロボットを含む制御システムの構成を示す図
【図6】制御系の機能ブロック図
【図7】第2実施例を示す図1相当図(その1)
【図8】図1相当図(その2)
【図9】姿勢ベクトルAiの方向が撮像対象物の方向に対してずれを生じている状態を説明する図
【図10】正規化クオータニオンQnjを用いる作用効果を説明する図(その1)
【図11】正規化クオータニオンQnjを用いる作用効果を説明する図(その2)
【発明を実施するための形態】
【0015】
(第1実施例)
以下、本発明の第1実施例について図1ないし図6を参照して説明する。図5は、垂直多関節型(6軸)ロボットを含む制御システムの構成を示す。このロボット本体1は、ベース(回転軸)2上に、この場合6軸のアームを有し、そのアームの先端には、図示しないハンド等のツールや、後述するカメラなどが取り付けられる。前記ベース2上には、第1関節J1を介して第1のアーム3が回転可能に連結されている。この第1のアーム3には、第2関節J2を介して上方に延びる第2のアーム4の下端部が回転可能に連結され、さらに、この第2のアーム4の先端部には、第3関節J3を介して第3のアーム5が回転可能に連結されている。
【0016】
この第3のアーム5の先端には第4関節J4を介して第4のアーム6が回転可能に連結され、この第4のアーム6の先端には第5関節J5を介して第5のアーム7が回転可能に連結され、この第5のアーム7には第6関節J6を介して第6のアーム8が回転可能に連結されている。なお、各関節J1〜J6においては、図示しないサーボモータにより各アーム3〜8を回転駆動するようになっている。
【0017】
ロボット本体1と制御装置11との間は、接続ケーブル12によって接続されている。これにより、ロボット本体1の各軸を駆動するサーボモータは、制御装置11により制御される。操作パッド(入力手段)13は、例えば横長の本体14の両端側に、ユーザが左右両手でそれぞれ把持するための把持部15L,15Rが設けられており、ユーザが把持部15L,15Rを両手で把持した状態で、親指により押圧操作可能な操作スティック16L,16Rが配置されている。また、把持部15L,15Rの図中上方には、人差し指により押圧操作可能なボタン17L,17Rが配置されている。
【0018】
ロボット本体1のアーム8には、CCD(Charge Coupled Device)又はCMOSイメージセンサを用いたカメラ(撮像手段)21が取り付けられており、そのカメラ21により撮像対象物(ワーク)22を画像で捉える。制御装置11には、パーソナルコンピュータ(パソコン)23がケーブル24を介して接続されている。
【0019】
パソコン23には、メモリやハードディスクなどの記憶装置(記憶手段)が内蔵されている。操作パッド13は、接続ケーブル18を経由してパソコン23に接続され、通信インターフェイスを介してパソコン23との間で高速のデータ転送を実行するようになっている。操作スティック16L,16R等が操作されて入力された操作信号等の情報は、操作パッド13からパソコン23を介して制御装置11へ送信される。カメラ21は、ケーブル25を介してパソコン23に接続されており、カメラ21が捉えた画像のデータはパソコン23に送信されて、ディスプレイ23Dに表示される。
【0020】
図6に示すように、制御装置11は、制御部としてのCPU31、各関節のモータ30を駆動する駆動手段としての駆動回路32、検出回路33などを備えている。そして、CPU31には、ロボット全体のシステムプログラム等を記憶したROM34、ロボット本体1の動作プログラム等を記憶したRAM35および前記ティーチングペンダント3を接続するためのインターフェイス36が接続されている。なお、図6では、ショルダ部5、下アーム6、上アーム7、手首8を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ30も一台だけ示した。
【0021】
上記検出回路33は、各関節の現在位置(回転角度)および現在速度(回転速度)を検出するためのもので、この検出回路33には、各関節を駆動するモータ30に設けられたロータリエンコーダ37が接続されている。ロータリエンコーダ37は位置センサおよび速度センサを兼用するもので、各モータ30の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路33に与えられる。検出回路33は、各ロータリエンコーダ37からのパルス信号に基づいて各モータ30ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ37から入力されるパルス信号数に基づいて各モータ30ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ30の駆動回路32およびCPU31に与えられるようになっている。
【0022】
そして、各駆動回路32は、CPU31から与えられる位置指令値および速度指令値と検出回路33から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ30に供給してそれらを駆動する。これにより、ロボットアーム先端(手先)であるフランジ(図示せず)の中心部が指令位置を順に通る経路を辿って動作することで、種々の作業を行う。
【0023】
ロボットアーム先端の移動経路は、操作パッド13(若しくは、図示しないティーチングペンダント)を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示される。そして、従来の教示作業であれば、各指令位置でのロボットアーム先端の姿勢も教示するが、本実施例では、姿勢は後述のように自動的に決定される。そして教示された指令位置と決定された姿勢とは、RAM35に記憶される。制御装置11は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。
【0024】
なお、ロボットアーム先端の位置は、ツール(図5Σc)に固定された3次元座標(ツール座標)の原点がロボット座標(基底座標,ベース座標)上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、ツールに固定のツール座標の3軸の単位ベクトル(ノーマル,オリエント,アプローチの各正規化姿勢ベクトル)がロボット座標上で示す向きによって定義される。
【0025】
次に、本実施例の作用について図1ないし図4を参照して説明する。図1は、ロボット本体1の手先(TCP)の位置及び姿勢の教示を行った後、教示点間における手先の位置及び姿勢を補間する処理を示すフローチャートであり、図2及び図3は、図1の処理に対応するイメージをベクトルモデルで示している。尚、図1に示すフローチャートは、理解を容易にするため処理の概略を原理的に示しており、より詳細な処理手順については第2実施例で説明する。
【0026】
先ず、各教示点について、手先の座標;TCP座標Pi(Xi,Yi,Zi)と、各軸の回転角である姿勢角(Rxi,Ryi,Rzi)とを記録する(S1)。この時、手先の姿勢は、カメラ21の視線(Z軸)が撮像対象物22の方向を向くように教示されている。続いて、各教示点毎に、撮像対象物22までの距離を求めて設定する(S2)。カメラ21を使用する場合、上記の距離は焦点距離となる(図2(a)参照)。
【0027】
それから、教示したTCP座標Piの間を、例えばスプライン曲線等の自由曲線を用いて補間し(S3,図2(b)参照,第1ステップ,位置補間手段)、各教示点から撮像対象物22までの距離を頂点とする座標(Ti,Ti+1,Ti+2)について、それらの間をステップS3と同様に例えばスプライン曲線等の自由曲線を用いて補間する(S4,図2(c)参照,第2及び第3ステップ,目標点座標決定手段,目標点位置補間手段)。尚、上記座標(Ti,Ti+1,Ti+2)については、教示点と同じ座標系で表す。また、「i」,「i+1」等は手先位置の移動順番を示す。
【0028】
次に、ステップS3で求めた各教示点の補間点と、この補間点に対応してステップS4で求めた頂点座標の補間点の2点から、アプローチベクトル(第1姿勢ベクトル)を生成する(S5,図3(a)参照),第4ステップ,第1姿勢ベクトル決定手段)。このアプローチベクトルは、カメラ21の視線方向であるZ軸に一致している。尚、S4をS2もしくは、S3の前に実行順序を移動させても全く問題ないことは明らかである。
【0029】
ここで、図4は、アプローチベクトルを上記のように決定することの意義を説明する図である。図4(a)は従来技術による補間処理を示しており、従来は教示点P1,P2間の位置P12を補間するだけであるため、各教示点P1,P2のアプローチベクトルがターゲット(撮像対象物22)の方向を向いていても、補間点P12でのアプローチベクトルがターゲットの方向を向くとは限らない。この場合、各補間点におけるアプローチベクトルの長さは正規化された単位長で常に一定である。
図4(b)は本実施例による補間処理を示しており、補間点P12でのアプローチベクトルを、各教示点の補間点と、この補間点に対応する撮像対象物22側の補間点の2点から生成することで、各補間点におけるアプローチベクトルの長さは一定ではなく変化することになるから、アプローチベクトルがターゲットの方向を向くことが保証される。
【0030】
再び図1を参照する。手先の姿勢を決定する他の2軸ベクトルはオリエントベクトルとノーマルベクトルであるが、これらの何れか一方について、教示点間の回転軸を中心として回転角の補間を行い、補間軌道に対応したオリエント若しくはノーマルベクトルを生成する(S6,第5ステップ,回転角補間手段)。
ここで、図3(b)〜(d)は、ステップS6においてノーマルベクトル(第2正規化姿勢ベクトル)を生成する際に回転角を補間する処理のイメージであり、図3(b)は、図3(a)に示すA側,教示点側よりアプローチベクトルに沿って撮像対象物22の方向を見た場合のノーマル,オリエントベクトルを示している。図3(c)は、各教示点におけるノーマルベクトルの回転角θを、スプライン補間した場合の角度変化の曲線を示しており、図3(d)は、図3(c)のように補間した結果、補間点においてノーマルベクトルが示す回転角θである。
【0031】
そして、図1に示すS7では、S6でノーマル,オリエントベクトルの一方が決まれば、他方のベクトル(第3姿勢ベクトル)は正規直交条件によりアプローチベクトルとの外積により求めることができる(第7ステップ,第3姿勢ベクトル決定手段)。これにより、各補間点についての3つの姿勢ベクトル;ノーマル,オリエント,アプローチの各ベクトルが決まるので、これらにより姿勢が決定される(第8ステップ,姿勢決定手段)。したがって、各補間点における手先のアプローチベクトルが撮像対象物22の方向を向くと共に、ある教示点から補間点を経て次の教示点に至るまでのノーマル,オリエントベクトルの回転角が滑らかに変化するように手先の姿勢が補間されたことになる。
【0032】
以上のように本実施例によれば、各教示点を通過する移動軌跡がスプライン曲線となるように教示点間の位置を移動順番を付けて補間して補間点を生成し、各教示点毎に、教示点と同じ座標系における撮像対象物22の座標を求め、各教示点に対応する撮像対象物22の座標間を、スプライン曲線により補間して同じ移動順番を付した補間点を生成する。そして、教示点側の補間点を始点,撮像対象物22側の補間点を終点とするアプローチベクトルを全ての補間点に対して求め、2つの教示点間の各補間点における例えばノーマルベクトルの回転角を補間すると、各補間点に立つアプローチベクトルとノーマルベクトルとの外積よりオリエントベクトルを求め、各補間点と3つの姿勢ベクトルの座標とから各補間点における手先の姿勢を決定するようにした。
【0033】
すなわち、アプローチベクトルの長さを撮像対象物22までの距離に応じて変化させるので、ノーマル,オリエントベクトルも併せて決定される手先の姿勢は、アプローチベクトルが常に対象物の方を向くように補間処理される。したがって、手先の位置を補間する際に、各補間点における手先の姿勢が、少なくとも撮像対象物22の方向を向くように補間することができる。加えて、ある教示点から補間点を経て次の教示点に至るまでのノーマル,オリエントベクトルの回転角が滑らかに変化するように手先の姿勢を補間することができる。
【0034】
(第2実施例)
図7ないし図11は第2実施例であり、第1実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。前述したように、第2実施例は第1実施例で示した処理手順をより詳細且つ具体的に示すものである。図7(a)に示すフローチャートにおいて、先ず、教示点毎にTCP座標Xciと、姿勢角(Rx,Ry,Rz)とを教示すると(S11,第1実施例では前提)、各TCP座標間(例えば、Xci, Xci+1との間)について例えばスプライン曲線等の自由曲線を用いて補間を行い、補間点を含む軌道を生成する(S12,第1ステップ,位置補間手段)。尚、添え字「i」,「i+1」は教示点について順次付す番号であり、以下では「キーフレーム番号」と称す。
【0035】
次に、姿勢角(Rxi,Ryi,Rzi)より、各教示点に立つ正規化姿勢ベクトル(Ni,Oi,Ai)を求める(S13)。そして、座標(Xt,Yt,Zt)を姿勢ベクトル(Ni,Oi,Ai)を用いて記述した場合の、TCP座標を始点とする姿勢ベクトルの長さ(ni,oi,ai)を求め(S14,第2ステップ)、姿勢ベクトル(Ni,Oi,Ai)の長さを(ni,oi,ai)とした時の頂点の座標Xtiを求める(S15,目標点座標決定手段,図7(b)参照)。すなわち、座標Xtiは、TCP座標を基準として撮像対象物22の位置を示す座標である。
【0036】
ここで、姿勢ベクトルAiの方向が撮像対象物22の方向に完全に一致している場合、座標Xtiは姿勢ベクトルAiの長さを示し、他の姿勢ベクトルの長さni,oiはゼロとなるが、姿勢ベクトルAiの方向が撮像対象物22の方向とずれを生じていれば他の姿勢ベクトルの長さni,oiも所定の値を示すことになる(図9参照)。
【0037】
次に、アプローチベクトルAiの長さをaiとして、当該ベクトルAiの先端座標の間を、S12と同様にスプライン曲線により補間して補間点を含む軌道を生成する(S16,第3及び第4ステップ,目標点位置補間手段)。それから、教示点TCPの座標と、長さaiのアプローチベクトルAi先端のスプライン補間軌道について、各フィールド座標(補間した座標)Xcj,Xtjとを結ぶベクトル(フィールド毎の正規化アプローチベクトルAj)を求める(S17,第1姿勢ベクトル決定手段,図7(c)参照)。尚、添え字「j」は補間点について順次付す番号であり、以下では「フィールド番号」と称す。
【0038】
そして、教示点Xi,Xi+1におけるノーマルベクトルNi,Ni+1の内積を演算し、両者がなす角度Θni(i=0,…,N)を求め(S18,教示点ベクトル角度算出手段)、その角度Θniについてもスプライン補間することで各補間点におけるノーマルベクトルが示す角度Θnjを求める(S19,補間点ベクトル角度算出手段)。また、教示点Xi,Xi+1におけるオリエントベクトルOi,Oi+1についても同様に内積を演算し、両者がなす角度Θoi(i=0,…,N)を求め(S20)、その角度Θoiについてもスプライン補間することで各補間点におけるオリエントベクトルが示す角度Θojを求める(S21)。尚、S18〜S21が第5ステップに対応する。
【0039】
引き続き図8(a)に示す処理において、教示点のノーマルベクトルNi,Ni+1の外積Npiを演算し、その結果をノーマルベクトルの仮の回転中心として用いることにする(S22,図8(b)参照)。外積Npiの値がゼロでなければ(S23:YES)、外積Npiを正規化し(S24,第7ステップ,正規化ベクトル決定手段)、正規化ベクトルNpiを回転軸として回転角θnjによる正規化クオータニオンQnjを求める(S25,第8ステップ,正規化クオータニオン算出手段)。尚、回転角θnjは、
θnj=Θnj−Θni …(1)
で表され、フィールド番号jがキーフレーム番号i,i+1間に有る場合に、ノーマルベクトルの回転角が教示点Xiから補間点Xjに至る間に変化する角度の差分である(第6ステップ)。この回転角θnjについては、S25で正規化クオータニオンQnjを求めるまでに求めておけば良い。また、正規化クオータニオンQnjは、(2)式で求められる。
【0040】
【数1】
尚、Npjは、教示点Xi,補間点Xcjにおけるノーマルベクトルの外積である。
【0041】
次に、正規化したノーマルベクトルNiを、正規化クオータニオンQnjにより回転させたノーマルベクトルNjを(3)式により求める(S26,図8(c)参照,第9ステップ,回転姿勢ベクトル算出手段)。
【0042】
【数2】
尚、Qnj-1は、正規化クオータニオンQnjの逆行列である。すると、補間点XcjにおけるアプローチベクトルAjと、ノーマルベクトルNjとが求められたので、これらの外積よりオリエントベクトルOjを求めることができる(S27,図8(d)参照,第10ステップ,最終姿勢ベクトル算出手段)。またここで、アプローチベクトルAjとオリエントベクトルOjの外積からノーマルベクトルNjを求め直す(S27a,第11ステップ)。
【0043】
以上より、フィールド番号jについて、アプローチベクトルAj,ノーマルベクトルNj,オリエントベクトルOjが求められたので、補間点Xcjにおける姿勢角(Rxj,Ryj,Rzj)が得られる(S28,図8(e)参照,第12ステップ)。尚、図8(e)に示すNj(old)は、第11ステップにおいて、アプローチベクトルAjとオリエントベクトルOjの外積からノーマルベクトルNjを求め直す前に求められているノーマルベクトルNjを示す。
【0044】
一方、S23において外積Npiの値がゼロである場合は(NO)、S22で用いた教示点のノーマルベクトルNi,Ni+1に替えてオリエントベクトルOi,Oi+1を用いてそれらの外積Opiを演算する(S29)。そして、外積Opiの値がゼロでなければ(S30:YES),S24〜S27におけるノーマルベクトルNをオリエントベクトルOに置き換えて同様の処理を行う(S31)。また、S30において外積Opiの値がゼロである場合は(NO)手先の姿勢が全く変化していないことを意味するので(S32)、そのまま処理を終了する。
【0045】
尚、図7,図8に示すフローチャートは、1つのキーフレーム番号iについて、次のキーフレーム番号(i+1)との間を補間した点を示すフィールド番号の全てについて処理を実行し、次に、キーフレーム番号(i+1)と次のキーフレーム番号(i+2)との間を補間した点を示すフィールド番号の全てについて処理を実行する、というように順次フィールド番号とキーフレーム番号とをインクリメントして行う。
【0046】
次に、上述したように正規化クオータニオンQnjを用いることによる作用効果について図10及び図11を参照して説明する。図10は、位置Aから位置Bまで円弧に沿って回転させる間の回転角を補間する場合をモデル的に示している。補間のやり方には色々あるが、例えば図10(b)に示すように位置A,B間に直線を引いて、その直線を4等分するように3点p1,p2,p3を決定する(図10(c)参照)。これらの3点を用いて回転角を補間するのであれば、円の中心から各点p1,p2,p3を通り、円周上に至る位置AB1,AB2,AB3が各補間点における回転角を与える。
【0047】
位置AB2は、位置A,B間の回転角の丁度1/2となるが、位置AB1については、位置A,AB2間の1/2よりも小さくなり、位置AB3については、位置AB2,B間の1/2よりも大きくなる。その結果、図11に破線で示すように回転角の変化率が線形とならず、滑らかに変化しない。第1実施例では、回転角の補間方式を任意としているため、回転角の変化がこの破線のようになる場合も想定される。これに対して正規化クオータニオンQnjを用いて回転させることで各補間点のノーマル若しくはオリエントベクトルを決定すると、図11に実線で示すように回転角の変化率が線形となるので滑らかに変化するようになる。
【0048】
以上のように第2実施例によれば、教示点である各TCP座標間についてスプライン曲線を用いて補間を行い、補間点を含む軌道を生成すると、各教示点に立つ正規化姿勢ベクトルを求め、撮像対象物22の座標(Xt,Yt,Zt)を姿勢ベクトル(Ni,Oi,Ai)を用いて記述した場合の、TCP座標を始点とする姿勢ベクトルの長さ(ni,oi,ai)を求め、姿勢ベクトル(Ni,Oi,Ai)の長さを(ni,oi,ai)とした時の頂点の座標Xtiを求める。次に、アプローチベクトルAiの長さをaiとして、当該ベクトルAiの先端座標の間をスプライン曲線により補間して補間点を含む軌道を生成すると、教示点TCPの座標と、長さaiのアプローチベクトルAi先端のスプライン補間軌道において各フィールド座標とを結ぶ正規化アプローチベクトルAjを求める。
【0049】
そして、教示点Xi,Xi+1におけるノーマルベクトルNi,Ni+1の内積を演算し、得られた角度Θniをスプライン補間して各補間点におけるノーマルベクトルが示す角度Θnjを求め、オリエントベクトルOi,Oi+1についても同様に内積を演算し、得られた角度Θoiをスプライン補間して各補間点におけるオリエントベクトルが示す角度Θojを求める。
【0050】
更に、教示点のノーマルベクトルNi,Ni+1の外積Npiを演算して正規化し、正規化ベクトルNpiを回転軸として回転角θnjによる正規化クオータニオンQnjを求めると、正規化したノーマルベクトルNiを正規化クオータニオンQnjにより回転させたノーマルベクトルNjを求める。そして、補間点XcjにおけるアプローチベクトルAjと、ノーマルベクトルNjとの外積よりオリエントベクトルOjを求め、再度、アプローチベクトルAjとオリエントベクトルOjとの外積よりノーマルベクトルNjを求め直し、補間点Xcjにおける姿勢角(Rxj,Ryj,Rzj)を得るようにした。すなわち、補間点におけるノーマル,オリエントベクトルの何れか一方を求めるに当たり、正規化クオータニオンQnjを用いて回転角をえることにより、手先位置の変化に応じた手先姿勢の変化率を線形にすることができ、姿勢をより滑らかに、自然に変化させることができる。
【0051】
本発明は上記し又は図面に記載した実施例にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
補間に用いる曲線はスプライン曲線に限ることなく、その他ベジェ曲線等の自由曲線を用いても良い。
【符号の説明】
【0052】
図面中、1はロボット本体、11は制御装置、21はカメラ(撮像手段)、22は撮像対象物(ワーク)を示す。
【特許請求の範囲】
【請求項1】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間する方法において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する第1ステップと、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める第2ステップと、
前記各教示点に対応する各目標点の座標間を、前記第1ステップと同じアルゴリズムの自由曲線により補間して前記第1ステップと同じ移動順番を付した補間点を生成する第3ステップと、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記第1ステップで生成した補間点を始点とし、前記第3ステップにおいて生成した補間点を終点とする第1姿勢ベクトルを、前記第1ステップで生成した全ての補間点に対して求める第4ステップと、
2つの教示点間の各補間点について、前記第2正規化姿勢ベクトルの回転角を補間する第5ステップと、
各補間点に立つ前記第1姿勢ベクトル及び前記第2正規化姿勢ベクトルとの外積より、第3姿勢ベクトルを求める第6ステップと、
前記第1ステップで生成した補間点と、前記第4ステップから第6ステップで得られた各姿勢ベクトルの座標とから、各補間点における前記手先の姿勢を決定する第7ステップとからなることを特徴とするロボットの位置姿勢補間方法。
【請求項2】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間する方法において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する第1ステップと、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める第2ステップと、
前記各教示点に対応する各目標点の座標間を、前記第1ステップと同じアルゴリズムの自由曲線により補間して前記第1ステップと同じ移動順番を付した補間点を生成する第3ステップと、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記第1ステップで生成した補間点を始点とし、前記第3ステップにおいて生成した補間点を終点とする第1姿勢ベクトルを、前記第1ステップで生成した全ての補間点に対して求める第4ステップと、
前記第2,第3姿勢ベクトルのそれぞれについて、隣り合う2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルがなす角θiを求める第5ステップと、
前記第2,第3姿勢ベクトルのそれぞれについて、前記2つの教示点i,(i+1)の間で補間された補間点jについて、前記教示点i,前記補間点jにそれぞれ立つ2つのベクトルがなす角θjを求める第6ステップと、
前記第2,第3姿勢ベクトルの何れか一方について、前記2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルの外積Piを求め、求めた外積Piを正規化して正規化ベクトルnPiを求める第7ステップと、
前記正規化ベクトルnPiを回転軸とし、当該正規化ベクトルnPiについて、前記角度θjを回転角として、正規化クオータニオンQjを求める第8ステップと、
前記正規化ベクトルnPiとして選択した姿勢ベクトルを、前記正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求める第9ステップと、
前記補間点jに立つ前記第1姿勢ベクトルRaj,及び前記回転させた姿勢ベクトルRbjの外積を求め、前記補間点jに立つ、前記正規化ベクトルとして選択しなかった姿勢ベクトルRcjを求める第10ステップと、
再度、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求め直す第11ステップと、
前記3つの姿勢ベクトルRaj,Rbj,Rcjより、前記補間点jにおける前記手先の姿勢角を求める第12ステップとからなることを特徴とするロボットの位置姿勢補間方法。
【請求項3】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間するロボットの制御装置において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する位置補間手段と、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める目標点座標決定手段と、
前記各教示点に対応する各目標点の座標間を、前記位置補間手段と同じアルゴリズムの自由曲線により補間して前記位置補間手段により付された移動順番と同じ移動順番を付した補間点を生成する目標点位置補間手段と、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記位置補間手段により生成された補間点を始点とし、前記目標点位置補間手段により生成された補間点を終点とする第1姿勢ベクトルを、前記位置補間手段により生成された全ての補間点に対して求める第1姿勢ベクトル決定手段と、
2つの教示点間の各補間点について、前記第2正規化姿勢ベクトルの回転角を補間する回転角補間手段と、
各補間点に立つ前記第1姿勢ベクトル及び前記第2正規化姿勢ベクトルとの外積より、第3姿勢ベクトルを求める第3姿勢ベクトル決定手段と、
前記位置補間手段により生成された補間点と、前記各姿勢ベクトルの座標とから、各補間点における前記手先の姿勢を決定する姿勢決定手段とを備えることを特徴とするロボットの制御装置。
【請求項4】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間するロボットの制御装置において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する位置補間手段と、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める目標点座標決定手段と、
前記各教示点に対応する各目標点の座標間を、前記位置補間手段と同じアルゴリズムの自由曲線により補間して前記位置補間手段により付された移動順番と同じ移動順番を付した補間点を生成する目標点位置補間手段と、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記位置補間手段により生成された補間点を始点とし、前記目標点位置補間手段により生成された補間点を終点とする第1姿勢ベクトルを、前記位置補間手段により生成された全ての補間点に対して求める第1姿勢ベクトル決定手段と、
前記第2,第3姿勢ベクトルのそれぞれについて、隣り合う2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルがなす角θiを求める教示点ベクトル角度算出手段と、
前記第2,第3姿勢ベクトルのそれぞれについて、前記2つの教示点i,(i+1)の間で補間された補間点jについて、前記教示点i,前記補間点jにそれぞれ立つ2つのベクトルがなす角θjを求める補間点ベクトル角度算出手段と、
前記第2,第3姿勢ベクトルの何れか一方について、前記2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルの外積Piを求め、求めた外積Piを正規化して正規化ベクトルnPiを求める正規化ベクトル決定手段と、
前記正規化ベクトルnPiを回転軸とし、当該正規化ベクトルnPiについて、前記角度θjを回転角として、正規化クオータニオンQjを求める正規化クオータニオン算出手段と、
前記正規化ベクトルnPiとして選択した姿勢ベクトルを、前記正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求める回転姿勢ベクトル算出手段と、
前記補間点jに立つ前記第1姿勢ベクトルRaj,及び前記回転させた姿勢ベクトルRbjの外積を求め、前記補間点jに立つ、前記正規化ベクトルとして選択しなかった姿勢ベクトルRcjを求める最終姿勢ベクトル算出手段と、
再度、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求め直し、前記3つの姿勢ベクトルRaj,Rbj,Rcjより、前記補間点jにおける前記手先の姿勢角を求める姿勢角決定手段とを備えることを特徴とするロボットの制御装置。
【請求項1】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間する方法において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する第1ステップと、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める第2ステップと、
前記各教示点に対応する各目標点の座標間を、前記第1ステップと同じアルゴリズムの自由曲線により補間して前記第1ステップと同じ移動順番を付した補間点を生成する第3ステップと、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記第1ステップで生成した補間点を始点とし、前記第3ステップにおいて生成した補間点を終点とする第1姿勢ベクトルを、前記第1ステップで生成した全ての補間点に対して求める第4ステップと、
2つの教示点間の各補間点について、前記第2正規化姿勢ベクトルの回転角を補間する第5ステップと、
各補間点に立つ前記第1姿勢ベクトル及び前記第2正規化姿勢ベクトルとの外積より、第3姿勢ベクトルを求める第6ステップと、
前記第1ステップで生成した補間点と、前記第4ステップから第6ステップで得られた各姿勢ベクトルの座標とから、各補間点における前記手先の姿勢を決定する第7ステップとからなることを特徴とするロボットの位置姿勢補間方法。
【請求項2】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間する方法において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する第1ステップと、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める第2ステップと、
前記各教示点に対応する各目標点の座標間を、前記第1ステップと同じアルゴリズムの自由曲線により補間して前記第1ステップと同じ移動順番を付した補間点を生成する第3ステップと、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記第1ステップで生成した補間点を始点とし、前記第3ステップにおいて生成した補間点を終点とする第1姿勢ベクトルを、前記第1ステップで生成した全ての補間点に対して求める第4ステップと、
前記第2,第3姿勢ベクトルのそれぞれについて、隣り合う2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルがなす角θiを求める第5ステップと、
前記第2,第3姿勢ベクトルのそれぞれについて、前記2つの教示点i,(i+1)の間で補間された補間点jについて、前記教示点i,前記補間点jにそれぞれ立つ2つのベクトルがなす角θjを求める第6ステップと、
前記第2,第3姿勢ベクトルの何れか一方について、前記2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルの外積Piを求め、求めた外積Piを正規化して正規化ベクトルnPiを求める第7ステップと、
前記正規化ベクトルnPiを回転軸とし、当該正規化ベクトルnPiについて、前記角度θjを回転角として、正規化クオータニオンQjを求める第8ステップと、
前記正規化ベクトルnPiとして選択した姿勢ベクトルを、前記正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求める第9ステップと、
前記補間点jに立つ前記第1姿勢ベクトルRaj,及び前記回転させた姿勢ベクトルRbjの外積を求め、前記補間点jに立つ、前記正規化ベクトルとして選択しなかった姿勢ベクトルRcjを求める第10ステップと、
再度、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求め直す第11ステップと、
前記3つの姿勢ベクトルRaj,Rbj,Rcjより、前記補間点jにおける前記手先の姿勢角を求める第12ステップとからなることを特徴とするロボットの位置姿勢補間方法。
【請求項3】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間するロボットの制御装置において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する位置補間手段と、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める目標点座標決定手段と、
前記各教示点に対応する各目標点の座標間を、前記位置補間手段と同じアルゴリズムの自由曲線により補間して前記位置補間手段により付された移動順番と同じ移動順番を付した補間点を生成する目標点位置補間手段と、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記位置補間手段により生成された補間点を始点とし、前記目標点位置補間手段により生成された補間点を終点とする第1姿勢ベクトルを、前記位置補間手段により生成された全ての補間点に対して求める第1姿勢ベクトル決定手段と、
2つの教示点間の各補間点について、前記第2正規化姿勢ベクトルの回転角を補間する回転角補間手段と、
各補間点に立つ前記第1姿勢ベクトル及び前記第2正規化姿勢ベクトルとの外積より、第3姿勢ベクトルを求める第3姿勢ベクトル決定手段と、
前記位置補間手段により生成された補間点と、前記各姿勢ベクトルの座標とから、各補間点における前記手先の姿勢を決定する姿勢決定手段とを備えることを特徴とするロボットの制御装置。
【請求項4】
垂直多関節型のロボットの手先から対象物までの距離を変化させるように、前記手先を移動させる位置及び前記手先の姿勢を教示した後に、前記位置及び前記姿勢を補間するロボットの制御装置において、
教示点を通過する移動軌跡が自由曲線となるように教示点の間の位置を移動順番を付けて補間し、補間点を生成する位置補間手段と、
各教示点毎に、教示点と同じ座標系における前記目標点の座標を求める目標点座標決定手段と、
前記各教示点に対応する各目標点の座標間を、前記位置補間手段と同じアルゴリズムの自由曲線により補間して前記位置補間手段により付された移動順番と同じ移動順番を付した補間点を生成する目標点位置補間手段と、
教示点における手先の姿勢角を3方向に分解した正規化姿勢ベクトルについて、手先から対象物の目標点に向かう方向を第1姿勢とし、その他を第2,第3姿勢として、前記位置補間手段により生成された補間点を始点とし、前記目標点位置補間手段により生成された補間点を終点とする第1姿勢ベクトルを、前記位置補間手段により生成された全ての補間点に対して求める第1姿勢ベクトル決定手段と、
前記第2,第3姿勢ベクトルのそれぞれについて、隣り合う2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルがなす角θiを求める教示点ベクトル角度算出手段と、
前記第2,第3姿勢ベクトルのそれぞれについて、前記2つの教示点i,(i+1)の間で補間された補間点jについて、前記教示点i,前記補間点jにそれぞれ立つ2つのベクトルがなす角θjを求める補間点ベクトル角度算出手段と、
前記第2,第3姿勢ベクトルの何れか一方について、前記2つの教示点i,(i+1)にそれぞれ立つ2つのベクトルの外積Piを求め、求めた外積Piを正規化して正規化ベクトルnPiを求める正規化ベクトル決定手段と、
前記正規化ベクトルnPiを回転軸とし、当該正規化ベクトルnPiについて、前記角度θjを回転角として、正規化クオータニオンQjを求める正規化クオータニオン算出手段と、
前記正規化ベクトルnPiとして選択した姿勢ベクトルを、前記正規化クオータニオンQjにより回転させた姿勢ベクトルRbjを求める回転姿勢ベクトル算出手段と、
前記補間点jに立つ前記第1姿勢ベクトルRaj,及び前記回転させた姿勢ベクトルRbjの外積を求め、前記補間点jに立つ、前記正規化ベクトルとして選択しなかった姿勢ベクトルRcjを求める最終姿勢ベクトル算出手段と、
再度、姿勢ベクトルRajとRcjの外積より姿勢ベクトルRbjを求め直し、前記3つの姿勢ベクトルRaj,Rbj,Rcjより、前記補間点jにおける前記手先の姿勢角を求める姿勢角決定手段とを備えることを特徴とするロボットの制御装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【公開番号】特開2013−59815(P2013−59815A)
【公開日】平成25年4月4日(2013.4.4)
【国際特許分類】
【出願番号】特願2011−198222(P2011−198222)
【出願日】平成23年9月12日(2011.9.12)
【出願人】(501428545)株式会社デンソーウェーブ (1,155)
【Fターム(参考)】
【公開日】平成25年4月4日(2013.4.4)
【国際特許分類】
【出願日】平成23年9月12日(2011.9.12)
【出願人】(501428545)株式会社デンソーウェーブ (1,155)
【Fターム(参考)】
[ Back to top ]