ロボットの制御装置及びロボットの姿勢決定方法
【課題】ティーチング作業が、一定の条件下でより簡単になるロボットの制御装置を提供する。
【解決手段】ロボットアームの手先座標におけるカメラ２１の視線に一致するアプローチベクトルａが、常に目標点である撮像対象物２２の１点を向くようにティーチングを行う場合、手先の位置が教示されると、ベース座標原点から目標点を示すベクトルBＰWと、手先を示すベクトルBＰTとからアプローチベクトルａを求める。オリエントベクトルｏをノルムを「１．０」にして決定すると、ノーマルベクトルｏをアプローチベクトルａとオリエントベクトルｏとの外積で決定する。教示点での手先の姿勢を示す回転行列BＲTを、既知である手先座標系の姿勢を示す回転行列BＲWと、ノーマル，オリエント，アプローチの各ベクトルで記述される回転行列TＲWの逆行列とから求め、回転行列BＲTで決まる手先の姿勢を教示点での姿勢として設定する。
【解決手段】ロボットアームの手先座標におけるカメラ２１の視線に一致するアプローチベクトルａが、常に目標点である撮像対象物２２の１点を向くようにティーチングを行う場合、手先の位置が教示されると、ベース座標原点から目標点を示すベクトルBＰWと、手先を示すベクトルBＰTとからアプローチベクトルａを求める。オリエントベクトルｏをノルムを「１．０」にして決定すると、ノーマルベクトルｏをアプローチベクトルａとオリエントベクトルｏとの外積で決定する。教示点での手先の姿勢を示す回転行列BＲTを、既知である手先座標系の姿勢を示す回転行列BＲWと、ノーマル，オリエント，アプローチの各ベクトルで記述される回転行列TＲWの逆行列とから求め、回転行列BＲTで決まる手先の姿勢を教示点での姿勢として設定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットについてティーチングを行うためのロボットの制御装置及びロボットの姿勢決定方法に関する。
【背景技術】
【０００２】
ロボットをティーチングする場合には、一般に、ロボットの手先を移動させる軌跡に沿った位置を教示すると共に、その教示点においてロボットが取るべき姿勢も教示するようになっている。例えば、特許文献１には、ティーチング及びその後に行われる補間に関連する技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−２７５４８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、ロボットの手先にカメラを取り付け、そのカメラを移動させてワークの画像を撮像する、という作業を行う場合については、ティーチングを行う際のロボットの姿勢は、当然にカメラの視線が固定されているワークを捉える方向となるように決定される。そのためには、例えば、各教示点をできるだけ至近距離で設定して各教示点における手先の座標と姿勢とを人力で設定し、教示点間を従来と同様の方式で補間する必要がある。或いは、各教示点をある程度距離を離すように設定し、同様に各教示点における手先の座標と姿勢とを人力で設定した後、教示点間の補間を、ツールを固定してロボットの手先がワークを把持して作業する形態の外部ＴＣＰ（Tool Center Point；先端点）動作による補間とすることも考えられる。
【０００５】
上記の何れのやり方であっても、各教示点について座標及び姿勢を決定する作業は必要であるが、座標を決定する作業については、ロボット本体の座標であるベース座標（基底座標）系によって行うため、教示点が増えるだけ手間はかかるが作業自体がやり難いということはない。しかしながら、姿勢を決定する作業については、ロボットの手先の座標系で行うことになり、手先の向きによりｘｙｚ方向が変化するため姿勢が把握し難く、１つ１つの作業が非常にやり難いという問題がある。
【０００６】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先に取り付けたカメラによりワークを撮像する作業について、手先の姿勢を教示する作業をより容易にするロボットの制御装置及びロボットの姿勢決定方法を提供することにある。
【課題を解決するための手段】
【０００７】
請求項１記載のロボットの制御装置によれば、ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットについてティーチングを行うことを前提とする。この場合、第１ベクトル決定手段は、手先の位置が教示されると、ロボットの基底座標原点から目標点を示すベクトルと、基底座標原点から手先（教示点）を示すベクトルとからアプローチベクトルを求める。
【０００８】
第２ベクトル決定手段は、手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、第２ベクトルのノルムを「１．０」に設定し、第２ベクトルの座標値を正規直交条件に基づいて決定する。そして、第３ベクトル決定手段は、残った第３ベクトルを、アプローチベクトルと第２ベクトルとの外積で決定するので、この段階で、手先座標のノーマル，オリエント，アプローチの各ベクトルが求められる。
【０００９】
すると、手先姿勢決定手段は、教示点における手先の姿勢を、既知である基底座標系に基づく手先座標系の姿勢を示す回転行列と、ノーマル，オリエント，アプローチの各ベクトルで記述される回転行列の逆行列とから求め、教示姿勢設定手段は、手先姿勢決定手段により決定された手先の姿勢を、教示点における姿勢として自動的に設定する。すなわち、ロボットの手先に取り付けられたカメラの視線が常にワーク方向を向くように教示点を設定するためユーザが行う作業は、基底座標系に従い位置を与える座標を入力するだけで済むことになり、ユーザの作業負担を大きく軽減できる。そして、教示点におけるロボットの手先の姿勢は計算により求められ自動的に設定されるので、作業者は、各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。
【００１０】
請求項２記載のロボットの制御装置によれば、請求項１と同様の条件でティーチングを行うことを前提とする。第１ベクトル決定手段は、手先の位置が教示されると、その教示点におけるカメラの撮像面と平行な平面内に存在する任意２つのベクトルの外積で求められる、教示点に立つ法線ベクトルを手先座標のアプローチベクトルとして決定する。アプローチベクトルが決定されれば、第２，第３ベクトル決定手段と、手先姿勢決定手段並びに教示姿勢設定手段が請求項１と同様に作用することで、教示点におけるロボットの手先の姿勢が自動的に設定される。したがって、作業者は、請求項１と同様に各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。
【００１１】
請求項３記載のロボットの制御装置によれば、補間手段が２つの教示点の間を補間して１つ以上の補間点を決定すると、第１〜第３ベクトル決定手段は、補間点についてもアプローチベクトル，第２ベクトル，第３ベクトルをそれぞれ決定し、手先姿勢決定手段は補間点における手先の姿勢も求める。したがって、ロボットの手先の移動軌跡を補間する場合についても、各補間点における手先の姿勢を自動的に設定することが可能となる。
【図面の簡単な説明】
【００１２】
【図１】第１実施例であり、ロボット本体のベース座標，手先の位置・姿勢，ワーク座標の位置・姿勢の関係を示す図
【図２】手先の姿勢を決定する処理を示すフローチャート
【図３】姿勢補間処理を示すフローチャート
【図４】各補間点の姿勢についても補間を行うイメージを示す図
【図５】ロボットを含む制御システムの構成を示す図
【図６】制御装置を中心とする機能ブロック図
【図７】第２実施例を示す図１相当図
【図８】図２相当図
【発明を実施するための形態】
【００１３】
（第１実施例）
以下、本発明の第１実施例について図１ないし図６を参照して説明する。図５は、垂直多関節型（６軸）ロボットを含む制御システムの構成を示す。このロボット本体１は、ベース（回転軸）２上に、この場合６軸のアームを有し、そのアームの先端には、図示しないハンド等のツールや、後述するカメラなどが取り付けられる。前記ベース２上には、第１関節Ｊ１を介して第１のアーム３が回転可能に連結されている。この第１のアーム３には、第２関節Ｊ２を介して上方に延びる第２のアーム４の下端部が回転可能に連結され、さらに、この第２のアーム４の先端部には、第３関節Ｊ３を介して第３のアーム５が回転可能に連結されている。
【００１４】
この第３のアーム５の先端には第４関節Ｊ４を介して第４のアーム６が回転可能に連結され、この第４のアーム６の先端には第５関節Ｊ５を介して第５のアーム７が回転可能に連結され、この第５のアーム７には第６関節Ｊ６を介して第６のアーム８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６においては、図示しないサーボモータにより各アーム３〜８を回転駆動するようになっている。
【００１５】
ロボット本体１と制御装置（コントローラ，第１〜第３ベクトル決定手段，手先姿勢決定手段，教示姿勢設定手段，補間手段）１１との間は、接続ケーブル１２によって接続されている。これにより、ロボット本体１の各軸を駆動するサーボモータは、制御装置１１により制御される。パッドコントローラ（入力手段）１３は、例えば横長の本体１４の両端側に、ユーザが左右両手でそれぞれ把持するための把持部１５Ｌ，１５Ｒが設けられており、ユーザが把持部１５Ｌ，１５Ｒを両手で把持した状態で、親指により押圧操作可能なジョイパッド１６Ｌ，１６Ｒが配置されている。また、把持部１５Ｌ，１５Ｒの図中上方には、人差し指により押圧操作可能なボタン１７Ｌ，１７Ｒが配置されている。
【００１６】
ロボット本体１のアーム８には、ＣＣＤ（Charge Coupled Device）又はＣＭＯＳイメージセンサを用いたカメラ（撮像手段）２１が取り付けられており、そのカメラ２１により撮像対象物（ワーク）２２を画像で捉える。制御装置１１には、パーソナルコンピュータ（パソコン）２３がケーブル２４を介して接続されている。
【００１７】
パソコン２３には、メモリやハードディスクなどの記憶装置（記憶手段）が内蔵されている。パッドコントローラ１３は、接続ケーブル１８を経由してパソコン２３に接続され、通信インターフェイスを介してパソコン２３との間で高速のデータ転送を実行するようになっている。ジョイパッド１６Ｌ，１６Ｒ等が操作されて入力された操作信号等の情報は、パッドコントローラ１３からパソコン２３を介して制御装置１１へ送信される。カメラ２１は、ケーブル２５を介してパソコン２３に接続されており、カメラ２１が捉えた画像のデータはパソコン２３に送信されて、ディスプレイ２３Ｄに表示される。
【００１８】
図６に示すように、制御装置１１は、制御部としてのＣＰＵ３１、各関節のモータ３０を駆動する駆動手段としての駆動回路３２、検出回路３３などを備えている。そして、ＣＰＵ３１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ３４、ロボット本体１の動作プログラム等を記憶したＲＡＭ３５および前記ティーチングペンダント３を接続するためのインターフェース３６が接続されている。なお、図６では、ショルダ部５、下アーム６、上アーム７、手首８を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ３０も一台だけ示した。
【００１９】
上記検出回路３３は、各関節の現在位置（回転角度）および現在速度（回転速度）を検出するためのもので、この検出回路３３には、各関節を駆動するモータ３０に設けられたロータリエンコーダ３７が接続されている。ロータリエンコーダ３７は位置センサおよび速度センサを兼用するもので、各モータ３０の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路３３に与えられる。検出回路３３は、各ロータリエンコーダ３７からのパルス信号に基づいて各モータ３０ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ３７から入力されるパルス信号数に基づいて各モータ３０ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ３０の駆動回路３２およびＣＰＵ３１に与えられるようになっている。
【００２０】
そして、各駆動回路３２は、ＣＰＵ３１から与えられる位置指令値および速度指令値と検出回路３３から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ３０に供給してそれらを駆動する。これにより、ロボットアーム先端（手先）であるフランジ（図示せず）の中心部が指令位置を順に通る経路を辿って動作することで、種々の作業を行う。
【００２１】
ロボットアーム先端の移動経路は、パッドコントローラ１３（若しくは、図示しないティーチングペンダント）を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示される。そして、従来の教示作業であれば、各指令位置でのロボットアーム先端の姿勢も教示するが、本実施例では、姿勢は後述のように決定される。そして教示された指令位置と決定された姿勢とは、ＲＡＭ３５に記憶される。制御装置１１は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。
【００２２】
なお、ロボットアーム先端の位置は、フランジに固定された３次元座標（ツール座標）の原点がロボット座標（基底座標，ベース座標）上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、フランジに固定のツール座標の３軸の単位ベクトル（ノーマル，オリエント，アプローチの各ベクトル）がロボット座標上で示す向きによって定義される。
【００２３】
図５に示すように、ロボットアームの先端である手先に取り付けたカメラ２１により撮像対象物２２を捉える場合、手先の姿勢については、基本的にカメラ２１の視線が常に撮像対象物（被写体）２２を向くように教示するのが通常である。したがって、このような状況下では、作業者が教示せずとも、手先の姿勢は幾何学的に決定することができる。
【００２４】
図１は、ロボット本体１の座標であるベース座標の原点を基準とする、手先；カメラ２１（ツール座標）の位置・姿勢と、撮像対象物２２（ワーク座標）の位置・姿勢との関係を示す図である。また、図２は、手先の姿勢を決定する処理を示すフローチャートである。ベース座標から見たワーク座標の位置はベクトルBＰW，姿勢は回転行列BＲWで示されるが（書式の制約上，ベクトル及び回転行列の表記は図中の表記と異なる）、撮像対象物２２が固定されているのでこれらは既知である。また、ベース座標から見たツール座標の位置はベクトルBＰT，姿勢は回転行列BＲTで示され、ベクトルBＰTは手先の位置を教示することで決定される（ステップＳ１）。
【００２５】
そして、カメラ２１の視線が撮像対象物２２を向く方向は、ツール座標のアプローチベクトルａ（第１ベクトル）が示す方向に一致するので、当該ベクトルａ＝（ａx，ａy，ａz）は図１に示す各座標間の関係から（１）式で計算される（ステップＳ２，第１ベクトル決定手段）。
【００２６】
【数１】
また、手先の姿勢を示す回転行列BＲTは、（２）式となる。
【００２７】
【数２】
ワーク座標の姿勢である回転行列BＲWは既知であるから、ツール座標から見たワーク座標の姿勢である回転行列TＲWが決まれば、手先の姿勢が決まる。
【００２８】
ここで、回転行列TＲWは、ツール座標の姿勢そのものであるから、ツール座標のノーマルベクトルｎ，オリエントベクトルｏ，アプローチベクトルａによって記述される。
【００２９】
【数３】
アプローチベクトルａは（１）式で決まるが、手先の姿勢に関してはそれが必要な条件であり、ノーマルベクトルｎ，オリエントベクトルｏについては任意で良い。
【００３０】
そして、ノーマル，オリエント，アプローチの各ベクトルｎ，ｏ，ａについては正規直交条件がある。したがって、アプローチベクトルａとオリエントベクトルｏとの内積はゼロであり、
ａxｏx＋ａyｏy＋ａzｏz＝０ …（４）
ｏx2＋ｏy2＋ｏz2＝０ …（５）
であるから、例えば、オリエントベクトルｏのノルムを「１．０」（単位長）に決定すれば、（４），（５）式からオリエントベクトルｏ（第２ベクトル）が決まる（ステップＳ３，第２ベクトル決定手段）。
【００３１】
すると、ノーマルベクトルｎ（第３ベクトル）については、アプローチベクトルａとオリエントベクトルｏとの外積で求められるので、
【００３２】
【数４】
となる（ステップＳ４，第３ベクトル決定手段）。
【００３３】
ステップＳ２〜Ｓ４により、ノーマル，オリエント，アプローチの各ベクトルｎ，ｏ，ａが決まるので、回転行列TＲWが決定され（ステップＳ５）、（２）式により、手先の姿勢である回転行列BＲTが計算される（ステップＳ６，手先姿勢決定手段）。以上の処理により、手先の位置を教示により決定したことに基づいて、手先の姿勢が計算で自動的に決定される。
【００３４】
手先の位置を教示して手先の姿勢を計算により決定すると、次に、複数の教示位置（指令位置）の間をスプライン曲線により補間する（補間手段）。スプライン補間方式の詳細については、例えば特開２００７−４２０２１号公報等に開示されているので、ここでは説明を省略する。
【００３５】
そして更に、指令位置について行った補間を手先の姿勢についても同様に適用して補間を行う。図３は、姿勢補間処理を示すフローチャートである。位置及び姿勢について教示点が設定されると（ステップＳ１１）、ロボット本体１の手先を撮像対象物２２周りで動作させるため、教示点（ベース座標から見たツール座標）が、ツール（手先）から見た撮像対象物２２への位置，姿勢（ツール座標から見たワーク座標）となるように座標変換する（ステップＳ１２）。
【００３６】
次に、各教示位置におけるそれぞれの教示姿勢を座標値として特定する。教示された姿勢は回転角度値（Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分）で与えられているので、回転行列を用いて空間座標値（Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値）に変換する（回転行列変換処理，姿勢座標変換処理手段）。各位置における手先の姿勢に対応する回転行列が、変換された座標によって表現される。すなわち、この回転行列が、手先の姿勢を特定するノーマルベクトルｎ（Ｘ軸），オリエントベクトルｏ（Ｙ軸），アプローチベクトルａ（Ｚ軸）を、それぞれベース座標上で表現した３次元座標値となる。また、各教示位置の各方向座標値と、当該教示位置における各方向姿勢座標値とからなるものが移動目標位置に対応する。
【００３７】
そして、教示位置を原点とするＸ，Ｙ，Ｚの各軸方向に、任意の長さを伸ばして仮想した点を姿勢点（増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値）とする（ステップＳ１３，姿勢ベクトル増幅処理手段）。すなわち、図４に示すように、教示姿勢を、教示位置（ＳＣ）を原点にして立つ方向ベクトルと考えれば、その方向ベクトルの３次元座標値（ＳＮ，ＳＯ，ＳＡ）を決定することで教示姿勢が特定される（ただし、図４では、ＳＯ，ＳＡのみ図示）。尚、「任意の長さ」は教示姿勢を特定可能な長さであれば良いので、例えば上記座標系の単位ベクトルのｎ（＞２）倍等とする。
【００３８】
ここで、回転行列を用いて表わされた姿勢を示す座標値は、ゼロに近い値となる。補間を行うための演算は、制御装置１１に内蔵されるＣＰＵ３１で行われるが、ＣＰＵ３１には計算限界桁があるので、最初からゼロに近い値を用いるとその計算限界桁を超えた部分に各教示点での姿勢に影響を与える値が存在することになる。しかし、ＣＰＵ３１の計算上ではその微妙な差が計算誤差として飲み込まれてしまうので、姿勢が滑らかに変化するように補間値を演算できなくなる。そこで、そのような事態を回避するためベクトルの長さをｎ倍して、ＣＰＵ３１の計算限界桁内で姿勢を滑らかに変化させるように演算するための値を取り込むようにする。
【００３９】
ステップＳ１３の処理により、１つの教示姿勢について３つの姿勢点がそれぞれ３次元座標値として決定されるが、これらを「位置」と同様に扱うことで教示姿勢を補間することができる。すなわち、Ｘ，Ｙ，Ｚそれぞれの姿勢点を，例えば上述した教示位置のようにスプライン曲線によって、滑らかな軌跡としてそれぞれ連結するように軌跡処理を行い補間する（ステップＳ１４，増幅姿勢座標値連結処理手段）。
【００４０】
ここで、図４は、教示位置並びに教示姿勢を補間する処理をイメージ的に示している。尚、図示が煩雑になるのを避けるため、上述のように姿勢点についてはＹ，Ｚの２軸分だけ示している。図４（ａ），（ｂ）は、各教示位置を曲線補間した場合を示し、図４（ｃ）は、各教示位置における教示姿勢についてＹ方向姿勢点及びＺ方向姿勢点を決定した場合であり、各矢印がオリエントベクトル，アプローチベクトルに対応する。そして、図４（ｄ）はＹ方向姿勢点を曲線補間した場合、図４（ｅ）はＺ方向姿勢点を曲線補間した場合である。また、図示しないＸ方向（ノーマルベクトルｏ）姿勢点についても同様に曲線補間する。
【００４１】
ステップＳ１４では、Ｘ，Ｙ，Ｚの各方向について姿勢点を独立に補間したが、これらの各方向の補間点については３次元座標の各軸に対応することから、互いの関係が正規直交条件を満たす必要がある。したがって、その条件を満たすように修正を行う（ステップＳ１５）。以上で教示姿勢の補間処理が完了する。
それから、ステップＳ１５で得た補間点を、ベースから見たツールの位置・姿勢となるように座標変換すると（ステップＳ１６）、補間点を加えて得られた手先の軌道（Ｘ，Ｙ，Ｚ各方向姿勢点の軌道）を、各軸Ｊ１〜Ｊ６の角度に変換する（ステップＳ１７，教示姿勢決定手段）。以上で姿勢の補間処理が終了する。
【００４２】
以上のように本実施例によれば、制御装置１１は、ロボットアームの手先座標におけるカメラ２１の視線に一致するアプローチベクトルａが、常に目標点である撮像対象物２２の１点を向くようにティーチングを行う場合、手先の位置が教示されると、ロボット本体１のベース座標原点から目標点を示すベクトルBＰWと、ベース座標原点から手先を示すベクトルBＰTとからアプローチベクトルａを求める。また、オリエントベクトルｏを決定するためそのノルムを「１．０」に決定し、その他の２つの座標値を正規直交条件に基づき決定すると、ノーマルベクトルｏを、アプローチベクトルａとオリエントベクトルｏとの外積で決定する。
【００４３】
そして、教示点における手先の姿勢を示す回転行列BＲTを、既知であるベース座標系に基づく手先座標系の姿勢を示す回転行列BＲWと、ノーマル，オリエント，アプローチの各ベクトルで記述される回転行列TＲWの逆行列とから求め、回転行列BＲTで決まる手先の姿勢を、教示点における姿勢として自動的に設定する。
【００４４】
すなわち、手先座標におけるアプローチベクトルａを常に目標点の方向に向かせるという状況下では、教示点におけるロボット本体１の手先の姿勢は計算により求められるので自動的に設定できる。したがって、作業者は、ベース座標系において各教示点の座標値を入力するだけで良く、各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。また、制御装置１１は、２つの教示点の間を補間して１つ以上の補間点を決定すると、補間点についてもアプローチベクトルａ，オリエントベクトルｏ，ノーマルベクトルｎをそれぞれ決定して補間点における手先の姿勢を求めて手先の姿勢も自動的に設定するので、各補間点における手先の姿勢についても、カメラ２１の視線が撮像対象物２２を捉えるように設定できる。
【００４５】
従来、手先の教示点毎に、位置を示す座標値を付与する作業と姿勢を付与する作業とが必要であったが、本実施例の方法，方式によれば「姿勢を付与する作業」が不要となるので、教示点毎に座標値を付与する作業と、撮像対象物２２（ワーク）の座標を付与する作業だけとなる。「姿勢を付与する作業」が非常に煩雑であることを鑑みれば、実質的な作業量は１／２以下になると言って良い。
【００４６】
（第２実施例）
図７及び図８は第２実施例であり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。第２実施例では、第１実施例とは異なる方法でアプローチベクトルａを決定する。すなわち、カメラ２１の形状は既知であり、その形状より、カメラ２１の外形において撮像面と平行になる平面は予め特定される。この平面に含まれる任意の異なる２つのベクトルを面ベクトルｓ１，ｓ２とすると、法線ベクトルａは（７）式となる。
【００４７】
【数５】
そして、前記法線ベクトルａの方向は、手先の姿勢を決定するアプローチベクトルａの方向、すなわちカメラ２１の視線方向に一致する（ここでは、両者を同一視している）。したがって、上記法線ベクトルａが、例えば撮像対象物２２の先端方向を向くようにロボットアームの手先の姿勢を決定すれば良い。この場合も、ノーマルベクトルｎ，オリエントベクトルｏが任意であることは第１実施例と同様である。
【００４８】
図８に示すフローチャートでは、ステップＳ２に替わるステップＳ２’（第１ベクトル決定手段）において、（７）式によりアプローチベクトルａを決定すれば、後は第１実施例と全く同様にして手先の姿勢である回転行列BＲTを計算できる。
【００４９】
以上のように第２実施例によれば、手先の位置が教示されると、その教示点におけるカメラ２１の撮像面と平行な平面内に存在する任意２つのベクトルｓ１，ｓ２の外積で求められる、教示点に立つ法線ベクトルを手先座標のアプローチベクトルａとして決定する。そして、オリエントベクトルｏ，ノーマルベクトルｎについては第１実施例と同様にして決定し、教示点における手先の姿勢を自動的に設定するようにした。すなわち、教示点におけるロボット本体１の手先の姿勢は、やはり計算により求めることが可能であるから自動的に設定できる。したがって、作業者は、第１実施例と同様に各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。
【００５０】
本発明は上記し、又は図面に記載した実施例にのみ限定されるものではなく、以下のような変型又は拡張が可能である。
第２ベクトルとしてノーマルベクトルｎを先に計算し、第３ベクトルとしてオリエントベクトルｏを、アプローチベクトルａとノーマルベクトルｎとの外積により計算しても良い。
６軸ロボットに限ることなく、その他の多軸ロボットについても適用可能である。
【符号の説明】
【００５１】
図面中、１はロボット本体、１１は制御装置（第１〜第３ベクトル決定手段，手先姿勢決定手段，教示姿勢設定手段，補間手段）、２１はカメラを示す。
【技術分野】
【０００１】
本発明は、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットについてティーチングを行うためのロボットの制御装置及びロボットの姿勢決定方法に関する。
【背景技術】
【０００２】
ロボットをティーチングする場合には、一般に、ロボットの手先を移動させる軌跡に沿った位置を教示すると共に、その教示点においてロボットが取るべき姿勢も教示するようになっている。例えば、特許文献１には、ティーチング及びその後に行われる補間に関連する技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００５−２７５４８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、ロボットの手先にカメラを取り付け、そのカメラを移動させてワークの画像を撮像する、という作業を行う場合については、ティーチングを行う際のロボットの姿勢は、当然にカメラの視線が固定されているワークを捉える方向となるように決定される。そのためには、例えば、各教示点をできるだけ至近距離で設定して各教示点における手先の座標と姿勢とを人力で設定し、教示点間を従来と同様の方式で補間する必要がある。或いは、各教示点をある程度距離を離すように設定し、同様に各教示点における手先の座標と姿勢とを人力で設定した後、教示点間の補間を、ツールを固定してロボットの手先がワークを把持して作業する形態の外部ＴＣＰ（Tool Center Point；先端点）動作による補間とすることも考えられる。
【０００５】
上記の何れのやり方であっても、各教示点について座標及び姿勢を決定する作業は必要であるが、座標を決定する作業については、ロボット本体の座標であるベース座標（基底座標）系によって行うため、教示点が増えるだけ手間はかかるが作業自体がやり難いということはない。しかしながら、姿勢を決定する作業については、ロボットの手先の座標系で行うことになり、手先の向きによりｘｙｚ方向が変化するため姿勢が把握し難く、１つ１つの作業が非常にやり難いという問題がある。
【０００６】
本発明は上記事情に鑑みてなされたものであり、その目的は、ロボットの手先に取り付けたカメラによりワークを撮像する作業について、手先の姿勢を教示する作業をより容易にするロボットの制御装置及びロボットの姿勢決定方法を提供することにある。
【課題を解決するための手段】
【０００７】
請求項１記載のロボットの制御装置によれば、ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットについてティーチングを行うことを前提とする。この場合、第１ベクトル決定手段は、手先の位置が教示されると、ロボットの基底座標原点から目標点を示すベクトルと、基底座標原点から手先（教示点）を示すベクトルとからアプローチベクトルを求める。
【０００８】
第２ベクトル決定手段は、手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、第２ベクトルのノルムを「１．０」に設定し、第２ベクトルの座標値を正規直交条件に基づいて決定する。そして、第３ベクトル決定手段は、残った第３ベクトルを、アプローチベクトルと第２ベクトルとの外積で決定するので、この段階で、手先座標のノーマル，オリエント，アプローチの各ベクトルが求められる。
【０００９】
すると、手先姿勢決定手段は、教示点における手先の姿勢を、既知である基底座標系に基づく手先座標系の姿勢を示す回転行列と、ノーマル，オリエント，アプローチの各ベクトルで記述される回転行列の逆行列とから求め、教示姿勢設定手段は、手先姿勢決定手段により決定された手先の姿勢を、教示点における姿勢として自動的に設定する。すなわち、ロボットの手先に取り付けられたカメラの視線が常にワーク方向を向くように教示点を設定するためユーザが行う作業は、基底座標系に従い位置を与える座標を入力するだけで済むことになり、ユーザの作業負担を大きく軽減できる。そして、教示点におけるロボットの手先の姿勢は計算により求められ自動的に設定されるので、作業者は、各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。
【００１０】
請求項２記載のロボットの制御装置によれば、請求項１と同様の条件でティーチングを行うことを前提とする。第１ベクトル決定手段は、手先の位置が教示されると、その教示点におけるカメラの撮像面と平行な平面内に存在する任意２つのベクトルの外積で求められる、教示点に立つ法線ベクトルを手先座標のアプローチベクトルとして決定する。アプローチベクトルが決定されれば、第２，第３ベクトル決定手段と、手先姿勢決定手段並びに教示姿勢設定手段が請求項１と同様に作用することで、教示点におけるロボットの手先の姿勢が自動的に設定される。したがって、作業者は、請求項１と同様に各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。
【００１１】
請求項３記載のロボットの制御装置によれば、補間手段が２つの教示点の間を補間して１つ以上の補間点を決定すると、第１〜第３ベクトル決定手段は、補間点についてもアプローチベクトル，第２ベクトル，第３ベクトルをそれぞれ決定し、手先姿勢決定手段は補間点における手先の姿勢も求める。したがって、ロボットの手先の移動軌跡を補間する場合についても、各補間点における手先の姿勢を自動的に設定することが可能となる。
【図面の簡単な説明】
【００１２】
【図１】第１実施例であり、ロボット本体のベース座標，手先の位置・姿勢，ワーク座標の位置・姿勢の関係を示す図
【図２】手先の姿勢を決定する処理を示すフローチャート
【図３】姿勢補間処理を示すフローチャート
【図４】各補間点の姿勢についても補間を行うイメージを示す図
【図５】ロボットを含む制御システムの構成を示す図
【図６】制御装置を中心とする機能ブロック図
【図７】第２実施例を示す図１相当図
【図８】図２相当図
【発明を実施するための形態】
【００１３】
（第１実施例）
以下、本発明の第１実施例について図１ないし図６を参照して説明する。図５は、垂直多関節型（６軸）ロボットを含む制御システムの構成を示す。このロボット本体１は、ベース（回転軸）２上に、この場合６軸のアームを有し、そのアームの先端には、図示しないハンド等のツールや、後述するカメラなどが取り付けられる。前記ベース２上には、第１関節Ｊ１を介して第１のアーム３が回転可能に連結されている。この第１のアーム３には、第２関節Ｊ２を介して上方に延びる第２のアーム４の下端部が回転可能に連結され、さらに、この第２のアーム４の先端部には、第３関節Ｊ３を介して第３のアーム５が回転可能に連結されている。
【００１４】
この第３のアーム５の先端には第４関節Ｊ４を介して第４のアーム６が回転可能に連結され、この第４のアーム６の先端には第５関節Ｊ５を介して第５のアーム７が回転可能に連結され、この第５のアーム７には第６関節Ｊ６を介して第６のアーム８が回転可能に連結されている。なお、各関節Ｊ１〜Ｊ６においては、図示しないサーボモータにより各アーム３〜８を回転駆動するようになっている。
【００１５】
ロボット本体１と制御装置（コントローラ，第１〜第３ベクトル決定手段，手先姿勢決定手段，教示姿勢設定手段，補間手段）１１との間は、接続ケーブル１２によって接続されている。これにより、ロボット本体１の各軸を駆動するサーボモータは、制御装置１１により制御される。パッドコントローラ（入力手段）１３は、例えば横長の本体１４の両端側に、ユーザが左右両手でそれぞれ把持するための把持部１５Ｌ，１５Ｒが設けられており、ユーザが把持部１５Ｌ，１５Ｒを両手で把持した状態で、親指により押圧操作可能なジョイパッド１６Ｌ，１６Ｒが配置されている。また、把持部１５Ｌ，１５Ｒの図中上方には、人差し指により押圧操作可能なボタン１７Ｌ，１７Ｒが配置されている。
【００１６】
ロボット本体１のアーム８には、ＣＣＤ（Charge Coupled Device）又はＣＭＯＳイメージセンサを用いたカメラ（撮像手段）２１が取り付けられており、そのカメラ２１により撮像対象物（ワーク）２２を画像で捉える。制御装置１１には、パーソナルコンピュータ（パソコン）２３がケーブル２４を介して接続されている。
【００１７】
パソコン２３には、メモリやハードディスクなどの記憶装置（記憶手段）が内蔵されている。パッドコントローラ１３は、接続ケーブル１８を経由してパソコン２３に接続され、通信インターフェイスを介してパソコン２３との間で高速のデータ転送を実行するようになっている。ジョイパッド１６Ｌ，１６Ｒ等が操作されて入力された操作信号等の情報は、パッドコントローラ１３からパソコン２３を介して制御装置１１へ送信される。カメラ２１は、ケーブル２５を介してパソコン２３に接続されており、カメラ２１が捉えた画像のデータはパソコン２３に送信されて、ディスプレイ２３Ｄに表示される。
【００１８】
図６に示すように、制御装置１１は、制御部としてのＣＰＵ３１、各関節のモータ３０を駆動する駆動手段としての駆動回路３２、検出回路３３などを備えている。そして、ＣＰＵ３１には、ロボット全体のシステムプログラム等を記憶したＲＯＭ３４、ロボット本体１の動作プログラム等を記憶したＲＡＭ３５および前記ティーチングペンダント３を接続するためのインターフェース３６が接続されている。なお、図６では、ショルダ部５、下アーム６、上アーム７、手首８を可動部として一つのブロックで示し、これに応じてそれらの関節の駆動源であるモータ３０も一台だけ示した。
【００１９】
上記検出回路３３は、各関節の現在位置（回転角度）および現在速度（回転速度）を検出するためのもので、この検出回路３３には、各関節を駆動するモータ３０に設けられたロータリエンコーダ３７が接続されている。ロータリエンコーダ３７は位置センサおよび速度センサを兼用するもので、各モータ３０の回転角度に応じたパルス信号を出力し、そのパルス信号は検出回路３３に与えられる。検出回路３３は、各ロータリエンコーダ３７からのパルス信号に基づいて各モータ３０ひいては各関節の現在位置を検出すると共に、単位時間当たり各ロータリエンコーダ３７から入力されるパルス信号数に基づいて各モータ３０ひいては各関節の現在速度を検出し、その位置および速度の情報は、各モータ３０の駆動回路３２およびＣＰＵ３１に与えられるようになっている。
【００２０】
そして、各駆動回路３２は、ＣＰＵ３１から与えられる位置指令値および速度指令値と検出回路３３から与えられる現在位置および現在速度とを比較し、その偏差に応じた電流を各モータ３０に供給してそれらを駆動する。これにより、ロボットアーム先端（手先）であるフランジ（図示せず）の中心部が指令位置を順に通る経路を辿って動作することで、種々の作業を行う。
【００２１】
ロボットアーム先端の移動経路は、パッドコントローラ１３（若しくは、図示しないティーチングペンダント）を用いて行う教示作業によって与えられる。この教示作業では、ロボットアーム先端が辿るべき軌跡上の複数の位置が指令位置として順に教示される。そして、従来の教示作業であれば、各指令位置でのロボットアーム先端の姿勢も教示するが、本実施例では、姿勢は後述のように決定される。そして教示された指令位置と決定された姿勢とは、ＲＡＭ３５に記憶される。制御装置１１は、実際のロボット作業に際して、与えられた複数の指令位置間を曲線補間してそれら指令位置を順に滑らかに辿る曲線を設定し、ロボットアーム先端がその曲線上を移動するように制御する。
【００２２】
なお、ロボットアーム先端の位置は、フランジに固定された３次元座標（ツール座標）の原点がロボット座標（基底座標，ベース座標）上でどの位置にあるかによって示される。また、ロボットアーム先端の姿勢は、フランジに固定のツール座標の３軸の単位ベクトル（ノーマル，オリエント，アプローチの各ベクトル）がロボット座標上で示す向きによって定義される。
【００２３】
図５に示すように、ロボットアームの先端である手先に取り付けたカメラ２１により撮像対象物２２を捉える場合、手先の姿勢については、基本的にカメラ２１の視線が常に撮像対象物（被写体）２２を向くように教示するのが通常である。したがって、このような状況下では、作業者が教示せずとも、手先の姿勢は幾何学的に決定することができる。
【００２４】
図１は、ロボット本体１の座標であるベース座標の原点を基準とする、手先；カメラ２１（ツール座標）の位置・姿勢と、撮像対象物２２（ワーク座標）の位置・姿勢との関係を示す図である。また、図２は、手先の姿勢を決定する処理を示すフローチャートである。ベース座標から見たワーク座標の位置はベクトルBＰW，姿勢は回転行列BＲWで示されるが（書式の制約上，ベクトル及び回転行列の表記は図中の表記と異なる）、撮像対象物２２が固定されているのでこれらは既知である。また、ベース座標から見たツール座標の位置はベクトルBＰT，姿勢は回転行列BＲTで示され、ベクトルBＰTは手先の位置を教示することで決定される（ステップＳ１）。
【００２５】
そして、カメラ２１の視線が撮像対象物２２を向く方向は、ツール座標のアプローチベクトルａ（第１ベクトル）が示す方向に一致するので、当該ベクトルａ＝（ａx，ａy，ａz）は図１に示す各座標間の関係から（１）式で計算される（ステップＳ２，第１ベクトル決定手段）。
【００２６】
【数１】
また、手先の姿勢を示す回転行列BＲTは、（２）式となる。
【００２７】
【数２】
ワーク座標の姿勢である回転行列BＲWは既知であるから、ツール座標から見たワーク座標の姿勢である回転行列TＲWが決まれば、手先の姿勢が決まる。
【００２８】
ここで、回転行列TＲWは、ツール座標の姿勢そのものであるから、ツール座標のノーマルベクトルｎ，オリエントベクトルｏ，アプローチベクトルａによって記述される。
【００２９】
【数３】
アプローチベクトルａは（１）式で決まるが、手先の姿勢に関してはそれが必要な条件であり、ノーマルベクトルｎ，オリエントベクトルｏについては任意で良い。
【００３０】
そして、ノーマル，オリエント，アプローチの各ベクトルｎ，ｏ，ａについては正規直交条件がある。したがって、アプローチベクトルａとオリエントベクトルｏとの内積はゼロであり、
ａxｏx＋ａyｏy＋ａzｏz＝０ …（４）
ｏx2＋ｏy2＋ｏz2＝０ …（５）
であるから、例えば、オリエントベクトルｏのノルムを「１．０」（単位長）に決定すれば、（４），（５）式からオリエントベクトルｏ（第２ベクトル）が決まる（ステップＳ３，第２ベクトル決定手段）。
【００３１】
すると、ノーマルベクトルｎ（第３ベクトル）については、アプローチベクトルａとオリエントベクトルｏとの外積で求められるので、
【００３２】
【数４】
となる（ステップＳ４，第３ベクトル決定手段）。
【００３３】
ステップＳ２〜Ｓ４により、ノーマル，オリエント，アプローチの各ベクトルｎ，ｏ，ａが決まるので、回転行列TＲWが決定され（ステップＳ５）、（２）式により、手先の姿勢である回転行列BＲTが計算される（ステップＳ６，手先姿勢決定手段）。以上の処理により、手先の位置を教示により決定したことに基づいて、手先の姿勢が計算で自動的に決定される。
【００３４】
手先の位置を教示して手先の姿勢を計算により決定すると、次に、複数の教示位置（指令位置）の間をスプライン曲線により補間する（補間手段）。スプライン補間方式の詳細については、例えば特開２００７−４２０２１号公報等に開示されているので、ここでは説明を省略する。
【００３５】
そして更に、指令位置について行った補間を手先の姿勢についても同様に適用して補間を行う。図３は、姿勢補間処理を示すフローチャートである。位置及び姿勢について教示点が設定されると（ステップＳ１１）、ロボット本体１の手先を撮像対象物２２周りで動作させるため、教示点（ベース座標から見たツール座標）が、ツール（手先）から見た撮像対象物２２への位置，姿勢（ツール座標から見たワーク座標）となるように座標変換する（ステップＳ１２）。
【００３６】
次に、各教示位置におけるそれぞれの教示姿勢を座標値として特定する。教示された姿勢は回転角度値（Ｘ方向回転成分，Ｙ方向回転成分，Ｚ方向回転成分）で与えられているので、回転行列を用いて空間座標値（Ｘ方向姿勢座標値，Ｙ方向姿勢座標値，Ｚ方向姿勢座標値）に変換する（回転行列変換処理，姿勢座標変換処理手段）。各位置における手先の姿勢に対応する回転行列が、変換された座標によって表現される。すなわち、この回転行列が、手先の姿勢を特定するノーマルベクトルｎ（Ｘ軸），オリエントベクトルｏ（Ｙ軸），アプローチベクトルａ（Ｚ軸）を、それぞれベース座標上で表現した３次元座標値となる。また、各教示位置の各方向座標値と、当該教示位置における各方向姿勢座標値とからなるものが移動目標位置に対応する。
【００３７】
そして、教示位置を原点とするＸ，Ｙ，Ｚの各軸方向に、任意の長さを伸ばして仮想した点を姿勢点（増幅Ｘ方向姿勢座標値，増幅Ｙ方向姿勢座標値，増幅Ｚ方向姿勢座標値）とする（ステップＳ１３，姿勢ベクトル増幅処理手段）。すなわち、図４に示すように、教示姿勢を、教示位置（ＳＣ）を原点にして立つ方向ベクトルと考えれば、その方向ベクトルの３次元座標値（ＳＮ，ＳＯ，ＳＡ）を決定することで教示姿勢が特定される（ただし、図４では、ＳＯ，ＳＡのみ図示）。尚、「任意の長さ」は教示姿勢を特定可能な長さであれば良いので、例えば上記座標系の単位ベクトルのｎ（＞２）倍等とする。
【００３８】
ここで、回転行列を用いて表わされた姿勢を示す座標値は、ゼロに近い値となる。補間を行うための演算は、制御装置１１に内蔵されるＣＰＵ３１で行われるが、ＣＰＵ３１には計算限界桁があるので、最初からゼロに近い値を用いるとその計算限界桁を超えた部分に各教示点での姿勢に影響を与える値が存在することになる。しかし、ＣＰＵ３１の計算上ではその微妙な差が計算誤差として飲み込まれてしまうので、姿勢が滑らかに変化するように補間値を演算できなくなる。そこで、そのような事態を回避するためベクトルの長さをｎ倍して、ＣＰＵ３１の計算限界桁内で姿勢を滑らかに変化させるように演算するための値を取り込むようにする。
【００３９】
ステップＳ１３の処理により、１つの教示姿勢について３つの姿勢点がそれぞれ３次元座標値として決定されるが、これらを「位置」と同様に扱うことで教示姿勢を補間することができる。すなわち、Ｘ，Ｙ，Ｚそれぞれの姿勢点を，例えば上述した教示位置のようにスプライン曲線によって、滑らかな軌跡としてそれぞれ連結するように軌跡処理を行い補間する（ステップＳ１４，増幅姿勢座標値連結処理手段）。
【００４０】
ここで、図４は、教示位置並びに教示姿勢を補間する処理をイメージ的に示している。尚、図示が煩雑になるのを避けるため、上述のように姿勢点についてはＹ，Ｚの２軸分だけ示している。図４（ａ），（ｂ）は、各教示位置を曲線補間した場合を示し、図４（ｃ）は、各教示位置における教示姿勢についてＹ方向姿勢点及びＺ方向姿勢点を決定した場合であり、各矢印がオリエントベクトル，アプローチベクトルに対応する。そして、図４（ｄ）はＹ方向姿勢点を曲線補間した場合、図４（ｅ）はＺ方向姿勢点を曲線補間した場合である。また、図示しないＸ方向（ノーマルベクトルｏ）姿勢点についても同様に曲線補間する。
【００４１】
ステップＳ１４では、Ｘ，Ｙ，Ｚの各方向について姿勢点を独立に補間したが、これらの各方向の補間点については３次元座標の各軸に対応することから、互いの関係が正規直交条件を満たす必要がある。したがって、その条件を満たすように修正を行う（ステップＳ１５）。以上で教示姿勢の補間処理が完了する。
それから、ステップＳ１５で得た補間点を、ベースから見たツールの位置・姿勢となるように座標変換すると（ステップＳ１６）、補間点を加えて得られた手先の軌道（Ｘ，Ｙ，Ｚ各方向姿勢点の軌道）を、各軸Ｊ１〜Ｊ６の角度に変換する（ステップＳ１７，教示姿勢決定手段）。以上で姿勢の補間処理が終了する。
【００４２】
以上のように本実施例によれば、制御装置１１は、ロボットアームの手先座標におけるカメラ２１の視線に一致するアプローチベクトルａが、常に目標点である撮像対象物２２の１点を向くようにティーチングを行う場合、手先の位置が教示されると、ロボット本体１のベース座標原点から目標点を示すベクトルBＰWと、ベース座標原点から手先を示すベクトルBＰTとからアプローチベクトルａを求める。また、オリエントベクトルｏを決定するためそのノルムを「１．０」に決定し、その他の２つの座標値を正規直交条件に基づき決定すると、ノーマルベクトルｏを、アプローチベクトルａとオリエントベクトルｏとの外積で決定する。
【００４３】
そして、教示点における手先の姿勢を示す回転行列BＲTを、既知であるベース座標系に基づく手先座標系の姿勢を示す回転行列BＲWと、ノーマル，オリエント，アプローチの各ベクトルで記述される回転行列TＲWの逆行列とから求め、回転行列BＲTで決まる手先の姿勢を、教示点における姿勢として自動的に設定する。
【００４４】
すなわち、手先座標におけるアプローチベクトルａを常に目標点の方向に向かせるという状況下では、教示点におけるロボット本体１の手先の姿勢は計算により求められるので自動的に設定できる。したがって、作業者は、ベース座標系において各教示点の座標値を入力するだけで良く、各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。また、制御装置１１は、２つの教示点の間を補間して１つ以上の補間点を決定すると、補間点についてもアプローチベクトルａ，オリエントベクトルｏ，ノーマルベクトルｎをそれぞれ決定して補間点における手先の姿勢を求めて手先の姿勢も自動的に設定するので、各補間点における手先の姿勢についても、カメラ２１の視線が撮像対象物２２を捉えるように設定できる。
【００４５】
従来、手先の教示点毎に、位置を示す座標値を付与する作業と姿勢を付与する作業とが必要であったが、本実施例の方法，方式によれば「姿勢を付与する作業」が不要となるので、教示点毎に座標値を付与する作業と、撮像対象物２２（ワーク）の座標を付与する作業だけとなる。「姿勢を付与する作業」が非常に煩雑であることを鑑みれば、実質的な作業量は１／２以下になると言って良い。
【００４６】
（第２実施例）
図７及び図８は第２実施例であり、第１実施例と同一部分には同一符号を付して説明を省略し、以下異なる部分について説明する。第２実施例では、第１実施例とは異なる方法でアプローチベクトルａを決定する。すなわち、カメラ２１の形状は既知であり、その形状より、カメラ２１の外形において撮像面と平行になる平面は予め特定される。この平面に含まれる任意の異なる２つのベクトルを面ベクトルｓ１，ｓ２とすると、法線ベクトルａは（７）式となる。
【００４７】
【数５】
そして、前記法線ベクトルａの方向は、手先の姿勢を決定するアプローチベクトルａの方向、すなわちカメラ２１の視線方向に一致する（ここでは、両者を同一視している）。したがって、上記法線ベクトルａが、例えば撮像対象物２２の先端方向を向くようにロボットアームの手先の姿勢を決定すれば良い。この場合も、ノーマルベクトルｎ，オリエントベクトルｏが任意であることは第１実施例と同様である。
【００４８】
図８に示すフローチャートでは、ステップＳ２に替わるステップＳ２’（第１ベクトル決定手段）において、（７）式によりアプローチベクトルａを決定すれば、後は第１実施例と全く同様にして手先の姿勢である回転行列BＲTを計算できる。
【００４９】
以上のように第２実施例によれば、手先の位置が教示されると、その教示点におけるカメラ２１の撮像面と平行な平面内に存在する任意２つのベクトルｓ１，ｓ２の外積で求められる、教示点に立つ法線ベクトルを手先座標のアプローチベクトルａとして決定する。そして、オリエントベクトルｏ，ノーマルベクトルｎについては第１実施例と同様にして決定し、教示点における手先の姿勢を自動的に設定するようにした。すなわち、教示点におけるロボット本体１の手先の姿勢は、やはり計算により求めることが可能であるから自動的に設定できる。したがって、作業者は、第１実施例と同様に各教示点について手先の姿勢を教示する必要が無くなり、作業時間を大きく短縮することができる。
【００５０】
本発明は上記し、又は図面に記載した実施例にのみ限定されるものではなく、以下のような変型又は拡張が可能である。
第２ベクトルとしてノーマルベクトルｎを先に計算し、第３ベクトルとしてオリエントベクトルｏを、アプローチベクトルａとノーマルベクトルｎとの外積により計算しても良い。
６軸ロボットに限ることなく、その他の多軸ロボットについても適用可能である。
【符号の説明】
【００５１】
図面中、１はロボット本体、１１は制御装置（第１〜第３ベクトル決定手段，手先姿勢決定手段，教示姿勢設定手段，補間手段）、２１はカメラを示す。
【特許請求の範囲】
【請求項１】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、前記ロボットの基底座標原点から前記目標点を示すベクトルと、前記基底座標原点から前記手先（教示点）を示すベクトルとから前記アプローチベクトルを求める第１ベクトル決定手段と、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定する第２ベクトル決定手段と、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定する第３ベクトル決定手段と、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求める手先姿勢決定手段と、
前記手先姿勢決定手段により決定された手先の姿勢を、前記教示点における姿勢として自動的に設定する教示姿勢設定手段とを備えたことを特徴とするロボットの制御装置。
【請求項２】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、その教示点における前記カメラの撮像面と平行な平面内に存在する任意の２つのベクトルの外積で求められる、前記教示点に立つ法線ベクトルを前記手先座標のアプローチベクトルとして決定する第１ベクトル決定手段と、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定する第２ベクトル決定手段と、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定する第３ベクトル決定手段と、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求める手先姿勢決定手段と、
前記手先姿勢決定手段により決定された手先の姿勢を、前記教示点における姿勢として自動的に設定する教示姿勢設定手段とを備えたことを特徴とするロボットの制御装置。
【請求項３】
２つの教示点の間を補間して１つ以上の補間点を決定する補間手段を備え、
前記第１〜第３ベクトル決定手段は、前記補間点についても前記アプローチベクトル，前記第２ベクトル，前記第３ベクトルをそれぞれ決定し、
前記手先姿勢決定手段は、前記補間点における手先の姿勢も求めることを特徴とする請求項１又は２記載のロボットの制御装置。
【請求項４】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、前記ロボットの基底座標原点から前記目標点を示すベクトルと、前記基底座標原点から前記手先（教示点）を示すベクトルとから前記アプローチベクトルを求め、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定し、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定し、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求め、
前記決定された手先の姿勢を、前記教示点における姿勢として自動的に設定することを特徴とするロボットの姿勢決定方法。
【請求項５】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、その教示点における前記カメラのの撮像面と平行な平面内に存在する任意の２つのベクトルの外積で求められる、前記教示点に立つ法線ベクトルを前記手先座標のアプローチベクトルとして決定し、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定し、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定し、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求め、
前記決定された手先の姿勢を、前記教示点における姿勢として自動的に設定することを特徴とするロボットの姿勢決定方法。
【請求項６】
２つの教示点の間を補間して１つ以上の補間点を決定すると、
前記補間点についても前記アプローチベクトル，前記第２ベクトル，前記第３ベクトルをそれぞれ決定し、
前記補間点における手先の姿勢も求めることを特徴とする請求項４又は５記載のロボットの姿勢決定方法。
【請求項１】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、前記ロボットの基底座標原点から前記目標点を示すベクトルと、前記基底座標原点から前記手先（教示点）を示すベクトルとから前記アプローチベクトルを求める第１ベクトル決定手段と、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定する第２ベクトル決定手段と、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定する第３ベクトル決定手段と、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求める手先姿勢決定手段と、
前記手先姿勢決定手段により決定された手先の姿勢を、前記教示点における姿勢として自動的に設定する教示姿勢設定手段とを備えたことを特徴とするロボットの制御装置。
【請求項２】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、その教示点における前記カメラの撮像面と平行な平面内に存在する任意の２つのベクトルの外積で求められる、前記教示点に立つ法線ベクトルを前記手先座標のアプローチベクトルとして決定する第１ベクトル決定手段と、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定する第２ベクトル決定手段と、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定する第３ベクトル決定手段と、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求める手先姿勢決定手段と、
前記手先姿勢決定手段により決定された手先の姿勢を、前記教示点における姿勢として自動的に設定する教示姿勢設定手段とを備えたことを特徴とするロボットの制御装置。
【請求項３】
２つの教示点の間を補間して１つ以上の補間点を決定する補間手段を備え、
前記第１〜第３ベクトル決定手段は、前記補間点についても前記アプローチベクトル，前記第２ベクトル，前記第３ベクトルをそれぞれ決定し、
前記手先姿勢決定手段は、前記補間点における手先の姿勢も求めることを特徴とする請求項１又は２記載のロボットの制御装置。
【請求項４】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、前記ロボットの基底座標原点から前記目標点を示すベクトルと、前記基底座標原点から前記手先（教示点）を示すベクトルとから前記アプローチベクトルを求め、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定し、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定し、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求め、
前記決定された手先の姿勢を、前記教示点における姿勢として自動的に設定することを特徴とするロボットの姿勢決定方法。
【請求項５】
ロボットの手先に取り付けたカメラによりワークを撮像するため、手先座標におけるアプローチベクトルが常に目標点を向くように、垂直多関節型のロボットの動作軌跡をティーチングする際に、
前記手先の位置が教示されると、その教示点における前記カメラのの撮像面と平行な平面内に存在する任意の２つのベクトルの外積で求められる、前記教示点に立つ法線ベクトルを前記手先座標のアプローチベクトルとして決定し、
前記手先座標のオリエントベクトル，ノーマルベクトルの一方を第２ベクトルとして決定するため、前記第２ベクトルのノルムを「１．０」に設定し、前記第２ベクトルの座標値を正規直交条件に基づいて決定し、
前記オリエントベクトル，前記ノーマルベクトルの他方である第３ベクトルを、前記アプローチベクトルと前記第２ベクトルとの外積で決定し、
前記教示点における手先の姿勢を、前記基底座標系に基づく前記手先座標系の姿勢を示す回転行列と、前記ノーマル，オリエント，アプローチの各ベクトルによって記述される回転行列の逆行列とから求め、
前記決定された手先の姿勢を、前記教示点における姿勢として自動的に設定することを特徴とするロボットの姿勢決定方法。
【請求項６】
２つの教示点の間を補間して１つ以上の補間点を決定すると、
前記補間点についても前記アプローチベクトル，前記第２ベクトル，前記第３ベクトルをそれぞれ決定し、
前記補間点における手先の姿勢も求めることを特徴とする請求項４又は５記載のロボットの姿勢決定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１３−４９１０２（Ｐ２０１３−４９１０２Ａ）
【公開日】平成２５年３月１４日（２０１３．３．１４）
【国際特許分類】
【出願番号】特願２０１１−１８７３４３（Ｐ２０１１−１８７３４３）
【出願日】平成２３年８月３０日（２０１１．８．３０）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
【公開日】平成２５年３月１４日（２０１３．３．１４）
【国際特許分類】
【出願日】平成２３年８月３０日（２０１１．８．３０）
【出願人】（５０１４２８５４５）株式会社デンソーウェーブ (1,155)
【Ｆターム（参考）】
[ Back to top ]