JP2016203282A

JP2016203282A - エンドエフェクタの姿勢変更機構を備えたロボット

Info

Publication number: JP2016203282A
Application number: JP2015085207A
Authority: JP
Inventors: 一樹藤本; Kazuki Fujimoto
Original assignee: Canon Electronics Inc
Current assignee: Canon Electronics Inc
Priority date: 2015-04-17
Filing date: 2015-04-17
Publication date: 2016-12-08

Abstract

【課題】高価なアブソリュートエンコーダを用いずに、ロボット又はエンドエフェクタの駆動停止時における、エンドエフェクタの姿勢情報の取得を可能とするエンドエフェクタ制御装置及びロボットを提供する。【解決手段】エンドエフェクタを三次元空間内で駆動する駆動手段と、前記エンドエフェクタの姿勢変更手段と、前記エンドエフェクタの駆動停止時において、前記エンドエフェクタの可動範囲を含む所定の範囲を撮影するために配置される少なくとも一つの撮像手段と、前記撮像手段により得た前記姿勢変更手段の一部の画像を含む二次元画像に基づいて、二次元平面上における前記姿勢変更手段の姿勢情報を取得する姿勢情報取得手段と、を備えたことを特徴とするエンドエフェクタ駆動装置にある。【選択図】図１

Description

本発明は、物品の加工や組立等の作業に用いられるロボットに関する。

物品の加工や組立等の作業に用いられるロボットは、複数のアクチュエータを駆動することで出力部の位置を制御するロボットである。実際に物品の加工や組立などの作業を行うツールであるエンドエフェクタは出力部に設置される。エンドエフェクタは例えば物品を把持するチャックや、溶接を行う溶接ガンなどが相当する。一般のロボットでは、エンドエフェクタの姿勢を制御するための補助的な駆動機構（以下エンドエフェクタ姿勢変更機構と呼ぶ）が、ロボット自体の駆動機構とは別に装備されている（特許文献１参照）。

またエンドエフェクタ姿勢変更機構の駆動源として、一般的にサーボモータが用いられる（特許文献１参照）。サーボモータにおける回転角検出手段としてアブソリュートエンコーダやインクリメンタルエンコーダなどが用いられる。

インクリメンタルエンコーダを用いた場合は、ロボットが作業を行っている際に電源停止、非常停止などの状態となった場合、エンドエフェクタ姿勢変更機構の回転角を失い、結果エンドエフェクタの姿勢情報を失う。その対応として、電源再投入直後の位置を精確に得ることが可能なアブソリュートエンコーダを用いる方法が一般的である。

特開２０１０−１７３０１９号公報

しかしながら、位置検出手段にアブソリュートエンコーダを採用するのはコストの増大を招く。またインクリメンタルエンコーダを用いた場合は、エンドエフェクタの姿勢情報を失った後に原点復帰動作を行わなければならず、姿勢が分からない状態から原点復帰動作を行うと、ロボット周辺に設置された治具等の障害物と干渉する恐れがある。

本発明は、高価なアブソリュートエンコーダを用いずに、ロボット又はエンドエフェクタの駆動停止時における、エンドエフェクタの姿勢情報の取得を可能とするエンドエフェクタ制御装置及びロボットを提供する。

本発明のエンドエフェクタ駆動装置は、エンドエフェクタを三次元空間内で駆動する駆動手段と、前記エンドエフェクタの姿勢変更手段と、前記エンドエフェクタの駆動停止時において、前記エンドエフェクタの可動範囲を含む所定の範囲を撮影するために配置される少なくとも一つの撮像手段と、前記撮像手段により得た前記姿勢変更手段の一部の画像を含む二次元画像に基づいて、二次元平面上における前記姿勢変更手段の姿勢情報を取得する姿勢情報取得手段と、を備えたことを特徴とする。

かかる本発明の態様によれば、エンドエフェクタの二次元画像に基づいて姿勢変更手段の姿勢情報を取得するため、駆動停止時におけるエンドエフェクタの姿勢情報の取得が可能である。

また、上記本発明では、前記駆動手段は、前記姿勢変更手段に接続された前記エンドエフェクタを、前記姿勢変更手段と共に駆動することを特徴とする。
かかる本発明の態様によれば、姿勢変更手段と共に、駆動停止時におけるエンドエフェクタの姿勢情報を取得できる。

また、上記本発明では、前記二次元平面は、ワークが載置されるステージの上面と平行な面であることを特徴とする。
かかる本発明の態様によれば、ステージ上面を基準とした二次元平面におけるエンドエフェクタの姿勢情報を取得できる。

また、上記本発明では、前記撮像手段は、ワークの位置や形状を認識するために用いる撮像手段を兼ねる、ことを特徴とする。
かかる本発明によれば、ワークの位置や形状認識のための撮像手段を用いて、エンドエフェクタの姿勢情報を取得できる。

なお、本発明は、上述したエンドエフェクタ駆動装置を備えたロボット（例えば、パラレルリンクボット等）にも広く適用可能である。
かかる本発明の態様によれば、エンドエフェクタの二次元平面上における姿勢情報を取得可能なロボットを実現できる。

本発明によれば、撮像により得た二次元画像から、エンドエフェクタの姿勢を求めることが可能である。この時必要な撮像手段は１つでよく、またアブソリュートエンコーダを用いないため、安価かつ簡易な構成で姿勢取得を実現することができる。

本発明を適用した実施形態１に係るパラレルリンクロボットにおける機構部の概略構成を示す図。図１に示すパラレルリンクロボットの出力部の部分拡大図。回転駆動機構１０６ａ回転時の出力部の部分拡大図。図３に示す出力部のｘｙ平面への投影図。回転軸がロボット基準座標に平行な軸である回転駆動機構１０６ｂにおける、回転駆動機構１０６ｂ回転時の出力部の部分拡大図。図５に示す出力部のｘｙ平面への投影図。図５に示す出力部のｘｚ平面への投影図。回転軸がロボット基準座標に平行な軸である回転駆動機構１０６ｂにおける、回転駆動機構１０６ａ及び回転駆動機構１０６ｂが共に回転した時の、出力部のｘｙ平面への投影図。回転軸が任意の軸である回転駆動機構１０６ｂにおける、回転駆動機構１０６ａ及び回転駆動機構１０６ｂが共に回転していない時の、出力部のｘｙ平面への投影図。回転軸が任意の軸である回転駆動機構１０６ｂにおける、回転駆動機構１０６ｂ回転時の出力部のｘｙ平面への投影図。回転駆動機構の回転角が不明な状態から回転角を取得するまでのフロー図。回転角取得時の信号処理を表すブロック図。フォトセンサにより回転検知する図。本発明を適用した実施形態２に係るパラレルリンクロボットにおける機構部の概略構成を示す図。図１４に示すパラレルリンクロボットの出力部の部分拡大図。本発明を適用したパラレルリンクロボットにおける機構部の計算モデル図。パラレルリンクロボットにおける機構部のｘｙ平面への投影図。ロボット位置が不明な状態から位置を取得するまでのフロー図。１つの先端位置に対して存在し得るθ_Ｂが計算上２つある例を示す、ｚｘ面投影図（アームＡＣ及びリンクＡＣは省略）。出力部位置の（ｘ、ｙ）、及びアクチュエータ位置θＡ、θＣが等しい２つのロボット姿勢の例を示す簡略図。（ａ）は、図１９におけるそれぞれのロボット姿勢を、ｘｙ平面に投影した図（計算モデル図）、（ｂ）は、図１９におけるそれぞれのロボット姿勢を、ｚｘ平面に投影した図（計算モデル図。アームＡＣ及びリンクＡＣは省略）。位置取得時の信号処理を示すブロック図。フォトセンサにより回転検知する図。

本実施形態では、パラレルリンクロボットを用いてエンドエフェクタ姿勢の取得方法（及びエンドエフェクタ駆動装置）を説明する。パラレルリンクロボットは複数のアクチュエータを駆動することで、リンク機構を介して、可動部である出力部の位置を制御するロボットである。

図１において、パラレルリンクロボット１００は、固定部材としてのベース部１００ａと、可動部である出力部１００ｂと、複数のモータ１０７、１０８、１１１と、複数のリンク部１０９、１１０、１１２と、を有する。ここで、１０７はモータＡ、１０８はモータＣ、１１１はモータＢに、それぞれ相当する。また、１０９はリンク部Ａ、１１０はリンク部Ｃ、１１２はリンク部Ｂに、それぞれ相当する。

ベース部１００ａは、略水平方向に配置された水平板１０１と、水平板１０１から略鉛直方向に設けられた鉛直板１０２とから構成され、上述の複数のモータ１０７、１０８、１１１を支持する。複数のモータ１０７、１０８、１１１は、それぞれ鉛直板１０２の所定位置に固定されている。本実施形態では、鉛直板１０２の幅方向中央の鉛直方向上側にモータＢ１１１を、鉛直板１０２の鉛直方向中間部で、幅方向中央を挟んだ両側に、それぞれモータＡ１０７、モータＣ１０８を配置している。水平板１０１上には、ワークや組立用の治具などが配置される。そして、この水平板１０１は、後述するｘｙ平面を規定する面となり、ワークが載置されるステージの上面となる。

モータＡ１０７には回転アームＡ１０７ａ、モータＣ１０８には回転アームＣ１０８ａがそれぞれ接続されており、モータの駆動によって各アームは水平板１０１に平行な面上を回動する。またモータＢ１１１には回転アームＢ１１２ａが接続され、鉛直板１０２に垂直な平面上を回動する。各アームの所定位置にはアーム側ジョイント部が、装着固定されている。

リンク部Ａ１０９は、平行リンク機構を形成する２本のリンクＡ１０９ａからなり、一端は出力軸１０５に、他端は回転アームＡ１０７ａに、それぞれジョイントを介して接続されている。また、リンク部Ｃ１１０は、平行リンク機構を形成する２本のリンクＣ１１０ａからなり、一端は出力軸１０５に、他端は回転アームＣ１０８ａに、それぞれジョイントを介して接続されている。

リンク部Ｂ１１２は、平行リンク機構を形成する２本のリンクＢ１１２ａからなり、一端は出力軸１０５に、他端は回転アームＢ１１１ａに、ジョイントを介して接続されている。

出力部１００ｂの構成について、図２を用いて説明する。出力部１００ｂには、物品の加工や組立などの作業を行う部分であって、例えばワークを把持するチャック等のエンドエフェクタ２００や、エンドエフェクタを鉛直軸回りやその他の軸回りに回転させる回転駆動機構１０６が取り付けられ、また複数のアームを構成するリンク機構が結合された出力軸１０５が鉛直方向に配置されている。出力軸１０５の所定位置には各ジョイントが、装着固定される。

このように、本実施形態のパラレルリンクロボットでは、リンク部Ａ１０９及びリンク部Ｃ１１０の形成する平行リンク機構と、リンク部Ｂ１１２の形成する平行リンク機構とによって、出力軸１０５は一定の姿勢（傾きと向きとの両方）を保つように支持される。

撮像手段３００は、ベース部１００ａ及び出力部１００ｂに対し、撮像手段３００の光軸が垂直となるよう、鉛直上方に固定されている。これは把持するワークを検出するための撮像手段は、特にベース部に平行な平面であるｘｙ平面を撮像するよう設置されていることが多いためこのように設定した。このｘｙ平面（二次元平面）は、上述した水平板１０１と平行な面となる。すなわち、撮像手段３００は、水平板１０１の対向部に配置され、水平板１０１の全体をその上から撮影する手段となる。なお、本実施形態では、ｘｙの基準は、上記水平板１０１の上面で規定し、鉛直板１０２から水平板１０１が突出する方向をｘ方向、これに直交する方向をｙ方向とする。

また図２では出力軸１０５に回転駆動機構１０６ａが、回転駆動機構１０６ａに回転駆動機構１０６ｂがそれぞれ取り付けられている。回転駆動機構１０６ａと回転駆動機構１０６ｂはそれぞれ1つのユニットとして独立に駆動する。また独立であるがゆえに回転駆動機構を必ずしも取り付ける必要はなく、例えば回転駆動機構１０６ａにエンドエフェクタ２００を、回転駆動機構１０６ｂを介さずに取り付けても良い。以下では回転駆動機構１０６ａ及び１０６ｂが共に、回転駆動機構１０６ａの回転軸と同一直線上に取り付けられているものとして説明する。また回転駆動機構１０６ａ及び回転駆動機構１０６ｂはそれぞれ、内部に配設された不図示の駆動手段１０６ｄ、または外部に配設された不図示の駆動手段１０６ｄにより、不図示の駆動伝達手段を介して駆動される。
以降エンドエフェクタ姿勢を求めるために必要な情報である、回転駆動機構１０６の回転角を求める。
＜実施形態１＞

本実施形態では、エンドエフェクタ駆動装置を搭載し、回転駆動機構１０６ａによるエンドエフェクタ２００の鉛直軸回りの回転位置を、撮像手段３００により得た二次元画像により得る。撮像手段３００は、ロボットの鉛直上方からｚ軸の負の方向に、ｘｙ平面を撮像するよう設置される。

回転駆動機構１０６ａがθ°回転した時、回転駆動機構１０６ｂ及びエンドエフェクタ２００も回転駆動機構１０６ａの回転軸回りにθ°回転する（図３参照）。このときの出力部付近を撮像した二次元画像は図４のようになる。

このときの回転角θ°は、例えば回転駆動機構１０６ｂの稜線部１を見ればよい。回転前の稜線部１とｙ軸との成す角があらかじめ分かっていれば、稜線部１の検出により回転角を得ることができる。回転前の稜線部１がｙ軸と平行であったならば、稜線部１とｙ軸との成す角が回転角θ°となる。すなわち、稜線部１がｘｙ平面（二次元平面）においてどれだけ傾いているかによって、回転角θ°を求めることができる。この回転角θは、ｘｙ平面における仮想の座標点に基づいて算出してもよい。この座標点は、予めｘｙ平面と撮像手段３００との相対位置を決定しておくことで、撮像手段３００側に設定されていてもよいし、水平板１０１を座標板としてもよい。なお、このような回転前の稜線部１は、回転駆動機構１０６ｂ（エンドエフェクタ２００）が停止している場合において、その状況を撮像手段３００が撮影し、その画像に基づいて特定することができる。また、回転駆動機構１０６ｂの停止位置は、水平板１０１の上部内の範囲に収まるため、どこで回転駆動機構１０６ｂが停止したとしても、その回転角θを特定することができる。なお、撮像手段３００は、オートフォーカス機能を持ち、回転駆動機構１０６ｂのｚ方向の停止位置に関わらず、回転駆動機構１０６ａを撮影することが可能である。

なお検出する稜線は、回転駆動機構１０６ｂやエンドエフェクタ２００の稜線に関わらず、回転駆動機構１０６ａと一体となって回転する要素の稜線であってもよい。その際、検出する稜線が撮像時に隠れないために、例えば回転駆動機構１０６ｂやエンドエフェクタ２００が、出力軸１０５や回転駆動機構１０６ａよりも、撮像する平面から見て大きいことが望ましい。

また以上では回転駆動機構の回転角を求めるために稜線を検出しているが、他の方法を用いても良い。例えばエンドエフェクタ上にマーカーを二つ設置し、マーカー位置の座標をそれぞれ得ることで二次元画像上における回転角θ°を算出する。

本実施形態では回転駆動機構１０６ｃによるｘ軸もしくはｙ軸と平行な軸回りの回転角を、撮像手段３００により得た二次元画像により得る。撮像手段３００は上述した実施形態と同様に、ロボットの鉛直上方からｚ軸の負の方向に、ｘｙ平面を撮像するよう設置される。

図５は回転駆動機構１０６ｃがｙ軸回りにφ°回転した時を表しており、図６は出力部付近を撮像した二次元画像である。回転角φ°は、撮像したｘｙ平面の画像中における回転駆動機構１０６ｃの回転中心からエンドエフェクタ先端部までのｘ方向の距離L‘を得ることにより求まる。すなわち、ｘｚ平面に投影した出力部（図７参照）の幾何学的関係から、式（１）が成立する。ここでＬは回転駆動機構１０６ｃの回転中心からエンドエフェクタ２００の先端部までの距離であり、あらかじめ分かっている。ｔはエンドエフェクタ２００先端部の、ｘｚ平面に投影した時の厚みである。（２）式を解くことにより回転角φが求まり、式（２）で表される。

この回転駆動機構１０６ｃも回転駆動機構１０６ｂと同様に、回転駆動機構１０６ａとは独立に駆動する。よって回転駆動機構１０６ａと回転駆動機構１０６ｃが同時に駆動していても、それぞれの回転角を求めることが可能である。

図８は回転駆動機構１０６ａがθ°、回転駆動機構１０６ｂがφ°、それぞれ回転した時にｘｙ平面を撮像して得た二次元画像を表す。回転角θ°は第一の実施形態で述べたように、回転駆動機構１０６ｂ、エンドエフェクタ２００などの稜線を検出する方法やマーカーを検出する方法などにより得ることができる。回転角φ°を求める際は、ｘｙ座標をθ°分だけ回転させた座標である、ｘ’ｙ’座標上において、第二の実施形態で述べた方法を用いる。すなわち式（２）中のＬ’を、軸ｙ’からエンドエフェクタ２００の先端部までの距離としてφを求める。

別の実施形態では、回転駆動機構１０６ｂによるｘ軸もしくはｙ軸と平行な軸回りの回転について述べたが、本実施形態では任意軸回りの回転について説明する。具体的には回転駆動機構１０６ｃの回転軸Ａが４５°傾いている時についてであり、回転駆動機構１０６ａの回転角θ°が０°である時、回転軸は (ｘ、ｙ、ｚ)のベクトル形式で表すと（０、１、−１）の方向である（図９参照）。また回転駆動機構１０６ｃは回転駆動機構１０６ａとは別のユニットとしてそれぞれ独立に駆動する。

この４５度傾いた軸Ａ回りにω°回転した時を撮像して得た二次元画像を図１０に示す。この画像により得る情報から回転角ω°を求めるために、任意軸回りの回転による座標変換を行う。今、エンドエフェクタ２００の回転前の主軸方向が図９に示すように（ｕ、ｖ、ｗ）＝（０、０、−１）の方向で、回転後の主軸方向が（ｕ’、ｖ’、ｗ’）であるとき、回転行列Ｒを用いて式（３）が成立する。

となる。
ここでの定数ｍは回転前と後とにおいて、エンドエフェクタ２００の主軸方向のベクトルの大きさを一致させるための値である。

回転後のエンドエフェクタ２００の主軸方向は、図１０に示したｘｙ平面の撮像画像により得ることができる。この主軸方向は図１０中のエンドエフェクタ２００の稜線部２と平行である。よってこの稜線部２とｘ軸との成す角をα°とした時、回転後のエンドエフェクタ２００の主軸方向は、式（４）で表すことができる。

ただし、ここではベクトルの大きさは考慮しておらず、（３）式中の定数ｍにより式の妥当性を保証している。また図１０のみから得られる情報ではベクトル成分ｗ’は分からない。

稜線部２の選択においては、回転前においてエンドエフェクタ主軸方向を表すベクトルと同一方向であった稜線を選択しなければならない。図中の稜線部２は共に回転前は、回転前のエンドエフェクタ主軸方向（０、０、−１）に平行である。ここで稜線部２はエンドエフェクタの主軸に対して平行であればよく、任意の稜線を選択してもよい。またマーカーを用いた、エンドエフェクタ主軸ベクトルの取得であれば、稜線の選択で誤ることはない。

（３）式において、u’及びv’の成分について解くと式（５−１）及び（５−２）が導出できる。

式（５−１）及び（５−２）から定数ｍを消去し、ωについて解くと式（６）で表せる。

以上によりｘｙ平面を撮像した二次元画像により得た情報及び、回転軸のベクトル情報とエンドエフェクタ初期位置の主軸ベクトルの情報から、回転駆動機構１０６ｂの回転角ω°を得ることが可能となる。また本実施形態の説明においても回転駆動機構１０６ａを固定し、回転駆動機構１０６ｂの回転のみを考えた。しかしながら回転駆動機構１０６ａと回転駆動機構１０６ｂとの駆動の独立性により、上述した実施形態での説明と同様に、ｘｙ座標を回転駆動機構１０６ａの回転角θ°分だけ回転させた座標である、ｘ’ｙ’座標を基準として、本実施形態で述べた方法を用いればよい。

以上の実施形態において、撮像した二次元画像上での稜線やマーカーの検出などにより、二次元平面上のエンドエフェクタ姿勢情報を得る工程を二次元姿勢取得工程と呼ぶ。また二次元姿勢取得工程により得た情報から、回転駆動機構１０６の回転角を算出する工程を三次元姿勢取得工程と呼ぶ。図１１に示したフローは、回転駆動機構１０６の回転角が不明な状態から、二次元平面を撮像し、二次元姿勢取得工程、三次元姿勢取得工程を経て、回転駆動機構１０６の回転角を得るという一連の流れを表したものである。

図１２は回転角取得時の信号処理を表すブロック図である。撮像手段３００によって得た二次元画像は、制御部４００に送られる。制御部４００では二次元姿勢取得工程、三次元姿勢取得工程を経て回転駆動機構１０６の回転角を取得し、回転角取得後に回転駆動機構を駆動する際は、取得した位置情報を基に回転駆動機構１０６を駆動する制御信号を生成する。

なお、撮像手段３００は、例えばバラ積みされたワークの位置や形状を認識するための撮像手段を兼ねても良い。

上述した実施形態ではロボット座標系におけるｘｙ平面に垂直な方向から撮像したが、ｙｚ平面に垂直な方向やｚｘ平面に垂直な方向、それ以外の傾きを持った平面から撮像しても良い。本実施形態では傾きを持った平面から撮像する。従って撮像により得る二次元画像では、例えば図１中に示すような斜視方向のロボット姿勢が得られる。ここで前述の傾きを持った撮像平面の位置情報、すなわち撮像点（カメラ）の位置及び撮像方向のベクトル情報があらかじめ分かっているものとする。

撮像平面の位置情報が分かっていれば、平行移動と回転移動との座標変換により、撮像平面の座標と任意の座標とを一致させることができる。よって、撮像平面及び座標を、例えばｘｙ平面の座標に一致するよう座標変換を行うことで、撮像平面で得たエンドエフェクタ姿勢情報を、ｘｙ平面におけるエンドエフェクタ姿勢情報に変換することができる。変換後は上述した実施形態で説明した内容と同様に、変換後の平面における姿勢情報から回転駆動機構の回転角を算出することが可能である。
なお、座標変換の工程は二次元姿勢取得工程に含まれる。

この考え方を基にすれば、適用可能なロボットはパラレルリンクロボットに限らない。例えばシリアルリンクロボットにおいては、エンドエフェクタ及び回転駆動機構を設置する出力部の姿勢は一定ではなく、出力部位置を決める複数のアクチュエータ及びアームによって様々な姿勢を取る。しかし、出力部の姿勢が分かっている時、撮像平面を、出力部姿勢を最もよく表す平面、例えば出力部材に平行な面と一致するよう座標変換を行えばよい。この変換によって、撮像平面でのエンドエフェクタ姿勢情報を、変換後の平面及び座標上におけるエンドエフェクタ姿勢情報に変換することができ、後は上述した実施形態で説明した方法により、回転駆動機構の回転角を算出することが可能である。

また上述した実施形態において、画像処理により得る回転駆動部の回転角の精度が十分でなければ、二次元画像を用いた回転角の取得後に各回転駆動部のより精確な位置出しを行っても良い。精確な位置出しは例えば、回転駆動機構を予め定められた基準位置まで駆動することにより行う。具体的には、回転駆動部の回転軸と一体に動作するように取り付けられた切欠き部のある薄板５０１の切欠き位置を、フォトセンサ５０２を用いることで検知する（図１３）。センサ出力が変化する位置を基準位置とし、この基準位置における回転駆動機構１０６の回転角を予め与えておけば、基準位置にて精確な回転駆動機構１０６の回転角を取得できる。

以上、本発明を実施の形態に基づいて詳細に説明したが、本発明は上述した実施形態に限定されるものではない。

例えば、本発明は、以下に述べるように、高価なアブソリュートエンコーダを使用せずに、緊急停止や異常停止等の停止時の各アクチュエータの駆動位置及びそれらによって決まる出力部位置が不明な状態からの位置取得が可能なパラレルリンクロボット等のロボット、あるいは、エンドエフェクタ駆動装置を提供することも可能である。

各アクチュエータ及び出力部の、撮像した方向（光軸方向）に対して垂直な二次元平面における位置情報を得る。これらの値を用いて未取得のアクチュエータ駆動位置と出力部の三次元位置を算出する。

このような本発明によれば、撮像により得た二次元画像から、各アクチュエータの駆動位置及び出力部の三次元位置を取得することが可能である。この時必要な撮像手段は１つでよく、安価かつ簡易な構成で位置取得を実現することができる。

＜実施形態２＞
図１４において、パラレルリンクロボット１００は、固定部材としてのベース部１００ａと、可動部である出力部１００ｂと、複数のモータ１０７、１０８、１１１と、複数のリンク部１０９、１１０、１１２と、を有する。ここで、１０７はモータＡ、１０８はモータＣ、１１１はモータＢに、それぞれ相当する。また、１０９はリンク部Ａ、１１０はリンク部Ｃ、１１２はリンク部Ｂに、それぞれ相当する。

ベース部１００ａは、略水平方向に配置された水平板１０１と、水平板１０１から略鉛直方向に設けられた鉛直板１０２とから構成され、上述の複数のモータ１０７、１０８、１１１を支持する。複数のモータ１０７、１０８、１１１は、それぞれ鉛直板１０２の所定位置に固定されている。本実施形態では、鉛直板１０２の幅方向中央の鉛直方向上側にモータＢ１１１を、鉛直板１０２の鉛直方向中間部で、幅方向中央を挟んだ両側に、それぞれモータＡ１０７、モータＣ１０８を配置している。水平板１０１上には、ワークや組立用の治具などが配置される。

出力部１００ｂの構成について、図１５を用いて説明する。出力部１００ｂには、物品の加工や組立などの作業を行う部分であって、例えばワークを把持するチャック等のエンドエフェクタ２００が取り付けられ、複数のアームを構成するリンク機構が結合された出力軸１０５が鉛直方向に配置されている。出力軸１０５の所定位置には各ジョイントが、装着固定される。

このように構成されるロボットの場合、モータＡ１０７による回転アームＡ１０７ａの駆動と、モータＣ１０８による回転アームＣ１０８ａの駆動とを行うことで、出力部１００ｂは略水平方向に移動する。また、モータＢ１１１により回転アームＢ１１１ａを駆動することで、出力部１００ｂを上下方向に移動させると、リンク部Ａ１０９とリンク部Ｃ１１０は水平からずれる。このため、出力部１００ｂの三次元空間内（所定の空間内）の位置は、リンク部Ａ１０９とリンク部Ｃの傾斜を考慮して、回転アームＡ、Ｂ、Ｃの駆動量から幾何学的に算出することが可能である。また、逆運動の計算により、出力部１００ｂの空間位置座標からモータＡ、Ｂ、Ｃの駆動量を算出することが可能である。

図１６はパラレルリンクロボットのリンク機構部の計算モデル図である。リンクＡ１０９ａ、リンクＣ１１０ａ、リンクＢ１１２ａは本来、各二本ずつあるが、簡略化し各一本ずつであるとしてモデル化する。また本来、３本のリンク先端位置は出力部位置（基準点）に一致しないが、計算の簡易化のために出力部位置を仮想的に移動（オフセット）させることで、３本のリンク先端位置が一致するものとしている。各回転アームＡ、Ｂ、Ｃにおいて、アーム回転中心の位置をそれぞれＡ_０、Ｂ_０、Ｃ_０、アーム先端位置をそれぞれＡ_１、Ｂ_１、Ｃ_１とし、出力部１００ｂの代表位置（以下出力部位置）をＰとする。ｘ軸、ｙ軸、ｚ軸はそれぞれ図中の向きに取り、ｘｙｚ座標の原点ＯはＡ_０とＣ_０の中間位置にとる。また各アームＡ、Ｂ、Ｃがｘ軸と成す角（アーム回転角）をθ_Ａ、θ_Ｂ、θ_Ｃとし、図３中の矢印の向きを正とする。

上述したように、アーム回転中心位置Ａ_０、Ｂ_０、Ｃ_０の座標や各アームの長さ、各リンクの長さが予め与えられている時、θＡ、θＢ、θＣの値から出力部材位置Ｐの座標を求める計算を順運動学計算という。逆に位置Ｐの座標から駆動部のθＡ、θＢ、θＣの値を求める計算を逆運動学計算という。これらの計算方法として、受動関節の拘束条件から球面の方程式を連立し、関数を導く方法がある。例えばアームＡの先端部Ａ_１と、出力部ＰはリンクＡの長さで拘束され、Ａ_１の座標を（ｘ_Ａ１、ｙ_Ａ１、ｚ_Ａ１）、リンクＡの長さをＬ_Ａ、出力部Ｐの座標を（ｘ、ｙ、ｚ）とすると、以下の式を満たす。

ここでアームＡの長さをＲ_Ａ、Ａ_０の座標を（ｘ_Ａ０、ｙ_Ａ０、ｚ_Ａ０）とすると、

と表せる。他のリンクに関しても同様に式を立てることができ、それらの式を連立することにより、出力部Ｐの座標ｘ、ｙ、ｚは各アームの回転角θ_Ａ、θ_Ｂ、θ_Ｃの関数で表すことができる。逆にθ_Ａ、θ_Ｂ、θ_Ｃは出力部Ｐの座標ｘ、ｙ、ｚの関数で表すことができる。

以上では、３つのアーム回転角から出力部位置（ｘ、ｙ、ｚ）を、または出力部位置（ｘ、ｙ、ｚ）から３つのアーム回転角を求めているが、（θ_Ａ、θ_Ｂ、θ_Ｃ、ｘ、ｙ、ｚ）の６つのパラメータの内の何れか複数のパラメータを得て、アーム角の判別条件を用いた算出を行うことにより、残りのパラメータを取得することも可能である。以下詳しく説明する。

本実施形態では撮像手段３００を、ロボットの鉛直上方からｚ軸の負の方向に、ｘｙ平面を撮像するよう設置する。これは把持するワークを検出するための撮像手段は、特にｘｙ平面を撮像するよう設置されていることが多いためこのように設定した。この場合、撮像により得た二次元画像から取得できるアクチュエータ位置及び出力部位置は限られるが、二次元画像から取得できない位置情報は、ロボット構造上の拘束条件を用いて算出する。以下詳しく説明する。

本実施形態ではアームＡ、Ｃはｘｙ平面上を駆動する。よってｘｙ平面を撮像して得た二次元画像において基準軸やアームの回転中心などを認識する処理を行うことで、アーム回転角θ_Ａ，θ_Ｃ及び出力部Ｐのｘ、ｙを得ることができる（図１７を参照）。撮像して得た画像から位置を得るまでの工程を画像処理工程と呼び、これは図１８中のＳ６２に相当する。なお各パラメータは画像から直接その値を得る、もしくは算出することにより得る。例えば（ｘ、ｙ）を画像から直接求める場合は、出力部中心にマーカーを予め設置し、その位置を読み取る方法がある。間接的に算出する場合は、例えばリンクＡ、Ｃをそれぞれｘｙ平面上に投影したときのリンクとｘ軸とが成す角を得る。さらにｘｙ平面上におけるＡ_１とＰの座標及びＣ_１とＰの座標に関して、それぞれ正接の式を立て、それらを連立させることにより（ｘ、ｙ）が求まる。

画像処理工程にて得た値（ｘ、ｙ、θ_Ａ、θ_Ｃ）を（２−１）式に代入しｚについて解くとｚは２つの解を得る。ｚの２つの解をそれぞれｚ_１、ｚ_２とする。
ここでリンクＢによる拘束を考慮する。アームＢの先端部Ｂ_１の座標を（ｘ_Ｂ１、ｙ_Ｂ１、ｚ_Ｂ１）、リンクＢの長さをＬ_Ｂ、出力部Ｐの座標を（ｘ、ｙ、ｚ）とすると、以下の式を満たす。

またアームＢの長さをＲ_Ｂ、Ｂ_０の座標を（ｘ_Ｂ０、ｙ_Ｂ０、ｚ_Ｂ０）とすると、以下の式を満たす。

（２−３）式に求めたｚと（２−４）式を代入しθ_Ｂについて解くと、１つのｚの値に対しθ_Ｂは２つの解を得る。これは具体的に示すと、図１９のような場合である。図１９は、ある１つの出力部材位置Ｐに対し、アームＢが取り得るθ_Ｂが２つある例を示す図である。しかしこれは、あくまで計算上θＢが２つ得られることを示しており、実際にはどちらか１つの値のみを取るよう予め条件が設定されている。本実施例では、１つの出力部位置に対して取り得るアーム回転角が２つある場合、アームが取り得る２つの角度のうち、値の大きな角度（アームＣに関しては値の小さな角度）を採用することとする。なおアーム回転角の正負は図１６中に示された各アーム回転座標系に従う。従って図４で言えば、出力部位置（ｘ、ｚ）に対するアームＢ回転角はθ_Ｂ１に決まる。

前述のように得られるｚの解は２つあり、ｚ＝ｚ_１の時はθ_Ｂ＝θ_Ｂ１が、またｚ＝ｚ_２の時はθ_Ｂ＝θ_Ｂ２が得られたとする。よってここまででｚとθ_Ｂの組合せが２つ存在し、これは出力部のｘｙ座標とアームＡ、Ｃの回転位置が同じである姿勢が２つあることを表す。以下現在のロボットの姿勢を表す正しいｚとθ_Ｂの組合せの判別を行う。

図２０はｚとθ_Ｂに関する２つの組合せに相当する２つのロボット姿勢を示す図である。また図２１（ａ）は、図７中の２つの姿勢をｘｙ平面上に、（ｂ）はｚｘ面上に投影した時のモデル図である（（ｂ）に関してはアームＡＣ及びリンクＡＣは省略している）。

ｚとθ_Ｂの組合せについて、どちらが現在のパラレルリンクの姿勢を表す正しい解かを判別するには、例えばｘｙ平面上に投影されたアームＢの長さを見ればよい。ｘｙ平面上に投影されたアームの長さをＲ_Ｂ’とすると、以下の式を満たす。

実際にｘｙ平面を撮像した時のアームＢ先端位置が、図２１におけるＢ_１１位置であった場合、Ｒ_Ｂ’は線分Ｂ_０Ｂ_１１の大きさである。このＲ_Ｂ’を得る（これは画像処理工程内で行う）ことで、（２−５）式よりθ_Ｂを算出できる。なお計算上θ_Ｂは２つの解を持つが、これらの２つの解と、θ_Ｂ１及びθ_Ｂ２とを比較することで１つのθ_Ｂが決まる。画像処理工程で得た位置情報を用いて未取得位置を算出する（判別も含む）工程を計算工程と呼び、図１８中のＳ６３に相当する。

図２２は位置取得時の信号処理を表すブロック図である。撮像手段３００によって得た二次元画像は、制御部４００に送られる。制御部４００では画像処理工程、計算工程を経て各アクチュエータの駆動位置及び出力部位置を取得し、位置取得後にロボットを駆動する際は、取得した位置情報を基に各アクチュエータを駆動する制御信号を生成する。

以上のように図１８に示したフローに従って、各アクチュエータ位置及び出力部位置不明な状態から、撮像手段によって撮像し、撮像により得た二次元画像から位置情報を取得（画像処理工程）し、未取得の位置情報を算出（計算工程）することで、パラレルリンクロボットの各アクチュエータ及び出力部の位置が取得可能となる。

なお、ロボットの各アクチュエータ及び出力部の位置を取得するための撮像手段は、例えばバラ積みされたワークの位置や形状を認識するための撮像手段を兼ねても良い。

この実施形態ではロボット座標系におけるｘｙ平面に垂直な方向から撮像したが、ｙｚ平面に垂直な方向やｚｘ平面に垂直な方向、それ以外の傾きを持った平面から撮像しても良い。傾きを持った平面から撮像する場合は、３つのアーム位置を含んだ画像であれば、全てのアーム角度を、アーム角度の判別条件を考慮することなく取得できる。本実施形態では傾きを持った平面から撮像する。従って撮像により得る二次元画像は、例えば図１中に示すような斜視方向のロボット姿勢が得られる。

各アーム角度を得るには、まず基準となる軸と、アーム位置を表す軸の検出が必要である。これらの軸が検出できれば、軸と軸とが交わる角度がアーム回転角である。基準となる軸は予め設定されたｘｙｚ座標の軸であり、本実施例では例えばベース部１００ａの鉛直板１０２の外形状を表す各々の稜線がｘ、ｙ、ｚ軸のそれぞれに平行であるので、基準軸の検出は容易である。アーム位置の検出は、例えばアーム回転軸とアーム先端位置とを結ぶ直線が、アーム稜線と平行であるならば、アーム稜線を検出すればよい。

このように各アーム回転角を取得することは画像処理により可能である。全アーム角度が取得できれば、出力部位置は、ロボットの構成上の拘束条件を用いて算出できる。

また、上述した実施形態において、画像処理により得るアクチュータ位置及び出力部位置の精度が十分でなければ、二次元画像を用いた位置の取得後に各アクチュエータのより精確な位置出しを行っても良い。精確な位置出しは例えば、各アクチュエータを予め定められた基準位置まで駆動することにより行う。具体的には、アクチュエータと共に動作するようにアームに取り付けられた切欠きの入った薄板５０１の切欠き位置を、フォトセンサ５０２を用いることで検知する（図２３）。センサ出力が変化する位置を基準位置とし、この基準位置におけるアクチュエータ回転角を予め与えておけば、基準位置にてアクチュエータ回転角を取得できる。

なお、上述した実施形態２については、次のような特徴を有する。
（１）複数のアクチュエータと、各アクチュエータに連結される駆動アーム及び平行リンクと、複数の平行リンクにより支持される出力部と、によって構成されるパラレルリンクロボットにおいて、少なくとも前記駆動アームを認識できる撮像手段を有し、前記撮像手段により得た二次元画像から、各駆動アーム及び前記出力部の二次元位置を取得する画像処理工程と、前記画像処理工程にて取得した位置を用いた計算処理によって、未取得の各駆動アームの駆動位置及び前記出力部の三次元位置を算出する計算工程と、を備えることを特徴とするパラレルリンクロボット。
（２）前記撮像手段は、ワークの位置や形状を認識するために用いる撮像手段を兼ねる、ことを特徴とする請求項１に記載のパラレルリンクロボット。
（３）各駆動アーム及び前記出力軸の位置を、前記画像処理工程と前記計算工程により取得後、各駆動アームを予め定められた基準位置まで駆動することを特徴とする請求項１又は２に記載のパラレルリンクロボット。

１００パラレルリンクロボット
１０５出力軸
１０６回転駆動機構
１０６ａ回転駆動機構４軸目
１０６ｂ回転駆動機構５軸目（任意軸回り）
１０６ｃ回転駆動機構５軸目
１０６ｄ駆動手段
１０７モータＡ（アクチュエータＡ）
１０７ａ回転アームＡ（駆動部）
１０８モータＣ（アクチュエータＣ）
１０８ａ回転アームＣ（駆動部）
１０９リンク部Ａ
１０９ａリンクＡ
１１０リンク部Ｃ
１１０ａリンクＣ
１１１モータＢ（アクチュエータＢ）
１１１ａ回転アームＢ（駆動部）
１１２リンク部Ｂ
１１２ａリンクＢ
２００エンドエフェクタ
３００撮像手段
４００制御部

Claims

エンドエフェクタを三次元空間内で駆動する駆動手段と、
前記エンドエフェクタの姿勢変更手段と、
前記エンドエフェクタの駆動停止時において、前記エンドエフェクタの可動範囲を含む所定の範囲を撮影するために配置される少なくとも一つの撮像手段と、
前記撮像手段により得た前記姿勢変更手段の一部の画像を含む二次元画像に基づいて、二次元平面上における前記姿勢変更手段の姿勢情報を取得する姿勢情報取得手段と、
を備えたことを特徴とするエンドエフェクタ駆動装置。
前記駆動手段は、前記姿勢変更手段に接続された前記エンドエフェクタを、前記姿勢変更手段と共に駆動することを特徴とする請求項１に記載のエンドエフェクタ駆動装置。
前記二次元平面は、ワークが載置されるステージの上面と平行な面であることを特徴とする請求項１又は２に記載のエンドエフェクタ駆動装置。
前記撮像手段は、
ワークの位置や形状を認識するために用いる撮像手段を兼ねる、
ことを特徴とする請求項１に記載の、エンドエフェクタ駆動装置。
請求項１乃至４のいずれか１項に記載のエンドエフェクタ駆動装置を備えたことを特徴とするロボット。