特許 7524350 スカラロボットの制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-07-19

(45)【発行日】2024-07-29

(54)【発明の名称】スカラロボットの制御方法

(51)【国際特許分類】

   B25J 13/08 20060101AFI20240722BHJP

   B25J 9/06 20060101ALI20240722BHJP

【ＦＩ】

B25J13/08 A

B25J9/06 D

【請求項の数】 4

(21)【出願番号】P 2022566526

(86)(22)【出願日】2020-12-01

(86)【国際出願番号】 JP2020044678

(87)【国際公開番号】W WO2022118374

(87)【国際公開日】2022-06-09

【審査請求日】2023-10-05

(73)【特許権者】

【識別番号】000237271

【氏名又は名称】株式会社ＦＵＪＩ

(74)【代理人】

【識別番号】110000017

【氏名又は名称】弁理士法人アイテック国際特許事務所

(72)【発明者】

【氏名】藤城 武史

【審査官】臼井 卓巳

(56)【参考文献】

【文献】特開２０１８－０９４６５３（ＪＰ，Ａ）

【文献】特開２０１２－００６１２５（ＪＰ，Ａ）

【文献】特開平０５－２０４４２３（ＪＰ，Ａ）

【文献】特開昭６３－０４４２０６（ＪＰ，Ａ）

【文献】特開平０６－０５５４８５（ＪＰ，Ａ）

【文献】特開平０６－２０６１８６（ＪＰ，Ａ）

【文献】米国特許出願公開第２０１５／０１４２１７１（ＵＳ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ２５Ｊ ９／０６－１９／０４

Ｇ０５Ｂ １９／１８

(57)【特許請求の範囲】

【請求項1】

基台と、
前記基台に設けられ、第１軸周りに回動する第１アームと、
前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、
を備え、前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録し、
ワークの高さを取得し、前記カメラが前記ワークを撮像して撮像画像を取得したときに前記ワークの高さに基づく前記撮像画像の中心からのズレを修正する、
スカラロボットの制御方法。

【請求項2】

請求項１に記載のスカラロボットの制御方法であって、
前記第３軸と直交する方向における前記光軸の位置を前記カメラの取付位置として測定し、
前記測定したカメラの取付位置に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記位置パラメータに登録する、
スカラロボットの制御方法。

【請求項3】

基台と、
前記基台に設けられ、第１軸周りに回動する第１アームと、
前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、
を備え、前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第２アームの回動位置と前記第１軸から先のアーム部の質量と前記第１アームのアーム周りのねじれ量との関係を記憶し、
前記第２アームの回動位置と前記第１軸から先のアーム部の質量とに対応する前記第１アームのねじれ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録し、
前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録する、
スカラロボットの制御方法。

【請求項4】

基台と、
前記基台に設けられ、第１軸周りに回動する第１アームと、
前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、
を備え、前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第２アームの回動位置と前記第１軸から先のアーム部の質量と前記第１アームのたわみ量との関係を記憶すると共に、前記第２軸から先のアーム部の質量と前記第２アームのたわみ量との関係を記憶し、
前記第２アームの回動位置と前記第１軸から先のアーム部の質量とに対応する前記第１アームのたわみ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録すると共に、前記第２軸から先の第２アーム部の質量に対応する前記第２アームのたわみ量に基づく補正値を、前記第１リンクから前記第２リンクへの変換のための前記姿勢パラメータに登録し、
前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録する、
スカラロボットの制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本明細書は、スカラロボットの制御方法について開示する。

【背景技術】

【0002】

従来、作業座標に設けられた基準マークをアームに取り付けられたカメラにより計測し、ロボット座標による基準マークの計測位置と予め測定された作業座標における基準マークの位置との差から設置誤差を推定し、これを補正することによりロボットを制御するものが提案されている（例えば、特許文献１参照）。

【先行技術文献】

【特許文献】

【0003】

【文献】特開平４－２１１８０７号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

上述した特許文献１には、移動体に搭載されたロボットのように、ロボットに設置誤差がある場合の対応ついては記載されているものの、カメラに取付誤差がある場合の対応については何ら言及されていない。

【0005】

本開示は、アームにカメラが取り付けられたスカラロボットにおいて、カメラに取付誤差が生じていてもより高い精度でカメラの位置を制御することができるスカラロボットの制御方法を提供することを主目的とする。

【課題を解決するための手段】

【0006】

本開示は、上述の主目的を達成するために以下の手段を採った。

【0007】

本開示のスカラロボットの制御方法は、
基台と、
前記基台に設けられ、第１軸周りに回動する第１アームと、
前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、
を備え、
前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、
前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、
前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録する、
ことを要旨とする。

【0008】

この本開示のスカラロボットの制御方法は、第２アームにカメラが取り付けられたスカラロボットにおいて、第１アーム，第２アーム，シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いてカメラの位置を制御するものである。この方法は、第３軸に対する光軸の傾きをカメラの取付角度として測定し、測定したカメラの取付角度に基づく補正値を第２リンクから第３リンクへの変換のための姿勢パラメータに登録する。このように、カメラの取付角度を測定して、測定値に基づく補正値を対応するリンクへの変換のための姿勢パラメータに反映させることで、カメラに取付角度に誤差が生じていても、カメラの位置制御を精度よく行なうことが可能となる。

【図面の簡単な説明】

【0009】

【図1】スカラロボットの外観斜視図である。

【図2】ロボット本体の側面図である。

【図3】ロボット本体と制御装置との電気的な接続関係を示すブロック図である。

【図4】カメラ位置制御処理の一例を示すフローチャートである。

【図5】ロボットの座標系を説明する説明図である。

【図6】ロボットの座標系を説明する説明図である。

【図7】第１アームに対して生じるねじれを説明する説明図である。

【図8】第２アームが水平旋回した際のアーム部の重心位置のＹ₁軸方向成分Ｌ_t’を説明する説明図である。

【図9】第１アームに生じるねじれモーメントを説明する説明図である。

【図10】第１アームおよび第２アームに対して生じるたわみを説明する説明図である。

【図11】第２アームが水平旋回した際のアーム部の重心位置のＸ₁軸方向成分Ｌ_d1’を説明する説明図である。

【図12】第１アームに生じるたわみモーメントを説明する説明図である。

【図13】ワーク高さを説明する説明図である。

【図14】手先用リンクパラメータ表を示す説明図である。

【図15】カメラ用リンクパラメータ表を示す説明図である。

【図16】カメラの光軸がワークの置かれる基準面に対して傾いた状態で高さが異なるワークを撮像した場合に生じるワークに対する画像中心のズレを説明する説明図である。

【図17】カメラ用リンクパラメータ設定手順の一例を示す説明図である。

【図18】手先に測定用治具が取り付けられた状態を示す部分外観図である。

【図19】カメラ位置測定処理の一例を示すフローチャートである。

【図20】カメラ位置の測定の様子を示す説明図である。

【図21】カメラ傾き測定処理の一例を示すフローチャートである。

【図22】カメラ傾きの測定の様子を示す説明図である。

【発明を実施するための形態】

【0010】

次に、本開示を実施するための形態について図面を参照しながら説明する。

【0011】

図１は、スカラロボットの外観斜視図である。図２は、ロボット本体の側面図である。図３は、ロボット本体と制御装置との電気的な接続関係を示すブロック図である。

【0012】

スカラロボット１は、ワークＷに対して所定の作業（例えば、ワークＷをピックアップして搬送する搬送作業や、ワークＷを保持して組付対象物に組み付ける組付け作業など）を行なうものとして構成されている。スカラロボット１は、ロボット本体１０（図１および図２参照）と、ロボット本体１０を制御する制御装置７０（図３参照）と、を備える。

【0013】

ロボット本体１０は、基台１１とスカラアーム２０とカメラ６０とを備える。

【0014】

基台１１は、作業台２に固定されており、スカラアーム２０の基端側を支持する。スカラアーム２０は、第１アーム２１と第１アーム駆動部３０と第２アーム２２と第２アーム駆動部４０とシャフト２３とシャフト駆動部５０とを備える。第１アーム２１は、基端部が第１関節軸Ｊ１を介して基台１１に連結され、第１関節軸Ｊ１の回動により基台１１に対して水平面内で回動（水平旋回）可能に構成される。第２アーム２２は、基端部が第２関節軸Ｊ２を介して第１アーム２１の先端部に連結され、第２関節軸Ｊ２の回動により第１アーム２１に対して水平面内で回動（水平旋回）可能に構成される。シャフト２３は、第２アーム２２の先端部に第３関節軸Ｊ３を介して連結され、第２アーム２２に対して第３関節軸Ｊ３の軸周りに回転可能かつ第３関節軸Ｊ３の軸方向に沿って昇降可能に構成される。本実施形態のスカラロボットでは、シャフト２３の先端には、ワークＷをピックアップして保持するワーク保持部２４が設けられている。なお、ワーク保持部２４としては、例えば、負圧によりワークＷを吸着する吸着ノズルや、一対の爪によりワークＷを把持するメカニカルチャック、電磁石によりワークＷを吸着する電磁チャックなどを挙げることができる。

【0015】

第１アーム駆動部３０は、モータ３２とエンコーダ３４とを備える。モータ３２の回転軸は、図示しない減速機を介して第１関節軸Ｊ１に連結されている。第１アーム駆動部３０は、モータ３２を駆動することで減速機を介して第１関節軸Ｊ１に伝達されるトルクにより、第１関節軸Ｊ１を支点に第１アーム２１を回動させる。エンコーダ３４は、モータ３２の回転軸に取り付けられ、モータ３２の回転変位量を検出するロータリエンコーダとして構成される。

【0016】

第２アーム駆動部４０は、第１アーム駆動部３０と同様に、モータ４２とエンコーダ４４とを備える。モータ４２の回転軸は、図示しない減速機を介して第２関節軸Ｊ２に連結されている。第２アーム駆動部４０は、モータ４２を駆動することで減速機を介して第２関節軸Ｊ２に伝達されるトルクにより、第２関節軸Ｊ２を支点に第２アーム２２を回動させる。エンコーダ４４は、モータ４２の回転軸に取り付けられ、モータ４２の回転変位量を検出するロータリエンコーダとして構成される。

【0017】

シャフト駆動部５０は、図３に示すように、モータ５２ａ，５２ｂとエンコーダ５４ａ，５４ｂとを備える。モータ５２ａの回転軸は、図示しないベルトを介してシャフト２３に接続されている。シャフト駆動部５０は、モータ５２ａを駆動することによりシャフト２３を軸周りに回転させる。モータ５２ｂの回転軸は、図示しないボールねじ機構を介してシャフト２３に接続されている。シャフト駆動部５０は、モータ５２ｂを駆動することにより、ボールねじ機構によりモータ５２ｂの回転運動が直動運動に変換されることでシャフト２３を上下に昇降させる。エンコーダ５４ａは、シャフト２３の回転変位量を検出するロータリエンコーダとして構成される。エンコーダ５４ｂは、シャフト２３の昇降位置を検出するリニアエンコーダとして構成される。

【0018】

カメラ６０は、第２アーム２２の先端部側面に取り付けられている。カメラ６０は、作業対象物であるワークＷを撮像し、その撮像画像を制御装置７０へ出力する。制御装置７０は、撮像画像を処理することによりワークＷの位置や姿勢を認識することができる。なお、カメラ６０は第２アーム２２に取り付けられているから、本実施形態では、スカラアーム２０は、カメラ６０を上下方向に移動させることはできない。

【0019】

制御装置７０は、図３に示すように、ＣＰＵ７１と、処理プログラムを記憶するＲＯＭ７２と、ワークメモリとしてのＲＡＭ７３と、ＨＤＤやＳＳＤ等の記憶装置７４と、入出力インタフェース（図示せず）とを備える。制御装置７０には、エンコーダ３４，４４，５４ａ，５４ｂからの位置信号やカメラ６０からの画像信号などが入出力インタフェースを介して入力される。制御装置７０からは、モータ３２，４２，５２ａ，５２ｂへの駆動信号などが入出力インタフェースを介して出力される。

【0020】

次に、こうして構成されたスカラロボット１の動作について説明する。特に、作業台２上の被写体であるワークＷを撮像するためのカメラ６０の位置制御について説明する。図４は、制御装置７０のＣＰＵ７１により実行されるカメラ位置制御処理の一例を示すフローチャートである。

【0021】

カメラ位置制御処理が実行されると、制御装置７０のＣＰＵ７１は、まず、図示しないフィーダから供給されるワークＷを撮像するための基台１１（後述するベース座標系Σ₀）から見たカメラ６０の画像中心の目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）を設定する（ステップＳ１００）。ステップＳ１００は、例えば、外部のコンピュータで作成されたプログラムのデータを読み込むことにより行なわれる。続いて、ＣＰＵ７１は、カメラ６０の画像中心の目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）に対して逆運動学を解くことにより、カメラ６０の画像中心を目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）に一致させるための第１関節軸Ｊ１の角度θ_J1と第２関節軸Ｊ２の角度θ_J2とを算出する（ステップＳ１１０）。

【0022】

次に、ＣＰＵ７１は、ステップＳ１１０で算出した第２関節軸Ｊ２の角度θ_J2に基づいて第１アーム２１のねじれ角度θ_tとたわみ角度θ_d1とを算出すると共に（ステップＳ１２０）、第２アーム２２のたわみ角度θ_d2を算出する（ステップＳ１３０）。いま、図５および図６に示すように、基台１１（第０リンク）にベース座標系Σ₀を設定し、第１アーム２１，第２アーム２２，シャフト２３，手先（ワーク保持部２４）を第１リンク，第２リンク，第３リンク，第４リンクとしてそれぞれリンク座標系を設定する場合を考える。ここで、リンク座標系Σ₁は、第１アーム２１の根元（第１関節軸Ｊ１）に設定される。リンク座標系Σ₂は、第２アーム２２の根元（第２関節軸Ｊ２）に設定される。リンク座標系Σ₃は、第２アーム２２とシャフト２３との連結部分に設定される。リンク座標系（ハンド座標系）Σ₄は、手先（ワーク保持部２４）に設定される。各座標系は、第１アーム２１および第２アーム２２が一直線状に伸びた状態をホームポジションとする。また、図中、Ｘ₀，Ｘ₁，Ｘ₂，Ｘ₃，Ｘ₄は、それぞれの座標系のＸ軸である。Ｙ₀，Ｙ₁，Ｙ₂，Ｙ₃，Ｙ₄は、それぞれの座標系のＹ軸である。Ｚ₀，Ｚ₁，Ｚ₂，Ｚ₃，Ｚ₄は、それぞれの座標系のＺ軸である。

【0023】

第１アーム２１のねじれ角度θ_tは、リンク座標系Σ₁において、第１関節軸Ｊ₁の軸方向をＺ₁軸方向とし、第１アーム２１の中心線に沿った方向をＸ₁軸方向とし、Ｘ₁軸およびＺ₁軸に直交する方向をＹ₁軸方向とした場合、図７に示すように、第１アーム２１のＸ₁軸周りの変位角度として定義される。そして、第１アーム２１のねじれ角度θ_tは、第１アーム２１のねじれ角度θ_tと第２関節軸Ｊ２の角度θ_J2とアーム質量Ｍ１との関係をあらわす次式（１）および（２）のねじれモデル式を用いて算出される。アーム質量Ｍ１は、スカラアーム２０の第１関節軸Ｊ１より先端側のアーム部の質量であり、ワーク保持部２４がワークＷを保持している場合には当該ワークＷの質量ｍも含み、ワークＷの質量ｍに応じた変数となる。「ｇ」は重力加速度である。「Ｌ_t・ｓｉｎθ_J2」は、図８に示すように、アーム部の重心位置Ｌ_tにおけるＹ₁軸方向成分である。「Ｉ_t」は、図９に示すように、アーム部に作用する重力による荷重によって第１アーム２１をＸ₁軸周りに回転させようとするモーメント（ねじれモーメント）であり、重心位置Ｌ_tがＸ₁軸上にあるときには値０であり、重心位置Ｌ_tがＸ₁軸からＹ₁軸方向に離れるにつれて大きくなる。すなわち、ねじれモーメントＩ_tは、第２関節軸Ｊ２の角度θ_J2が０度（基準角）のときに最小となり、第２関節軸Ｊ２の角度が－９０度のときと＋９０度のときに最大となる。「Ｋ_t」は、ねじれ剛性であり、予め実験などにより求められる。

【0024】

【数1】

【0025】

第１アーム２１のたわみ角度θ_d1は、図１０に示すように、第１アーム２１のＹ₁軸周りの変位角度として定義される。そして、第１アーム２１のたわみ角度θ_d1は、第１アーム２１のたわみ角度θ_d1と第２関節軸Ｊ２の角度θ_J2とアーム部の質量である上述したアーム質量Ｍ１との関係をあらわす次式（３）および（４）のたわみモデル式を用いて算出される。式中、「Ｌ_d1・ｃｏｓθ_J2」は、図１１に示すように、アーム部の重心位置Ｌ_tにおけるＸ₁軸方向成分である。「Ｉ_d1」は、図１２に示すように、アーム部に作用する重力による荷重によって第１アーム２１をＹ₁軸周りに回転させようとするモーメント（たわみモーメント）であり、重心位置Ｌ_d1が第１関節軸Ｊ１からＸ₁軸方向に離れるにつれて大きくなる。すなわち、たわみモーメントＩ_d1は、第２関節軸Ｊ２の角度が±１８０度のときに最小となり、第２関節軸Ｊ２の角度が０度（基準角）のときに最大となる。「Ｋ_d1」は、たわみ剛性であり、予め実験などにより求められる。

【0026】

【数2】

【0027】

第２アーム２２のたわみ角度θ_d2は、図１０に示すように、第２アーム２２のＹ₂軸周りの変位角度として定義される。そして、第２アーム２２のたわみ角度θ_d2は、第２アーム２２のたわみ角度θ_d2とアーム質量Ｍ２との関係をあらわす次式（５）および（６）のたわみモデル式を用いて算出される。ここで、アーム質量Ｍ２は、スカラアーム２０の第２関節軸Ｊ２より先端側のアーム部の質量であり、ワーク保持部２４がワークＷを保持している場合には当該ワークＷの質量ｍも含み、ワークＷの質量ｍに応じた変数となる。「Ｉ_d2」は、アーム部に作用する重力による荷重によって第２アーム２２をＹ₂軸周りに回転させようとするモーメント（たわみモーメント）であり、第２関節軸Ｊ２の角度θ_d2とは無関係に、重心位置Ｌ_d2（第２関節軸Ｊ２の中心から重心位置までの距離）とアーム質量Ｍ２とによって定まる。「Ｋ_d2」は、たわみ剛性であり、予め実験などにより求められる。

【0028】

【数3】

【0029】

これらのねじれモデル式およびたわみモデル式は、記憶装置７４に記憶されている。ステップＳ１１０，Ｓ１２０において、ＣＰＵ７１は、記憶装置７４から対応するモデル式を読み出すことにより、ねじれ角度θ_tおよびたわみ角度θ_d1，θ_d2を算出する。

【0030】

次に、ＣＰＵ７１は、第１関節軸Ｊ１の角度θ_J1と第２関節軸Ｊ２の角度θ_J2と第１アーム２１のねじれ角度θ_tおよびたわみ角度θ_d1と第２アーム２２のたわみ角度θ_d2とに基づいて順運動学を解くことにより、カメラ６０の画像中心の推定位置（Ｘ₀，Ｙ₀，Ｚ₀）を算出する（ステップＳ１４０）。すなわち、ＣＰＵ７１は、画像中心が目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）と一致するように逆運動学により算出される第１および第２関節軸Ｊ１，Ｊ２の角度θ_J1，θ_J2を角度指令値として制御したと仮定した場合に、第１アーム２１のねじれ及びたわみや第２アーム２２のたわみに起因して目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）からズレる画像中心の推定位置（Ｘ₀，Ｙ₀，Ｚ₀）を算出する。

【0031】

ＣＰＵ７１は、画像中心の推定位置（Ｘ₀，Ｙ₀，Ｚ₀）を算出すると、算出した画像中心の推定位置（Ｘ₀，Ｙ₀，Ｚ₀）とステップＳ１００で取得した画像中心の目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）との差分をとることにより、画像中心の位置ズレ量（ΔＸ₀，ΔＹ₀，ΔＺ₀）を算出する（ステップＳ１５０）。続いて、ＣＰＵ７１は、画像中心の目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）を位置ズレ量（ΔＸ₀，ΔＹ₀，ΔＺ₀）の分だけオフセットすることにより目標位置を補正し（ステップＳ１６０）、補正した目標位置に対して逆運動学を解くことによりカメラ６０の画像中心を目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）に一致させるための第１関節軸Ｊ１の角度指令値θ_J1＊と第２関節軸Ｊ２の角度指令値θ_J2＊とを算出する（ステップＳ１７０）。そして、ＣＰＵ７１は、エンコーダ３４により検出される第１関節軸Ｊ１の角度が算出した角度指令値θ_J1＊に一致すると共にエンコーダ４４により検出される第２関節軸Ｊ２の角度が算出した角度指令値θ_J2＊に一致するようフィードバック制御により対応するモータ３１，３２を制御して（ステップＳ１８０）、カメラ位置制御処理を終了する。

【0032】

ＣＰＵ７１は、こうしてカメラ位置制御処理を実行すると、図示しないフィーダから供給されたワークＷをカメラ６０により撮像し、撮像画像に画像処理を施すことにより、ワークＷの位置および姿勢を認識する。そして、ＣＰＵ７１は、ワークＷをピックアップするための手先（ワーク保持部２４）の目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）を設定し、手先を目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）に移動させるための手先位置制御処理を実行する。目標位置（Ｘ₀＊，Ｙ₀＊）の設定は、認識したワークＷの位置および姿勢に基づいて行なわれる。目標位置Ｚ₀＊の設定は、ワークＷの高さ情報（ワーク高さＷＨ）に基づいて行なわれる。ワークＷの高さ情報は、上述した画像処理に用いられる画像処理データの一つとして予め登録されている。したがって、高さ情報の取得は、画像処理データの中からワーク高さＷＨを読み出すことにより行なうことができる。例えば、ワーク保持部２４として吸着ノズルや電磁チャックを用いてワークＷをピックアップする場合、ワークＷのピック位置はワークＷの上面（吸着面）となるため、目標位置Ｚ₀＊は、図１３に示すように、ワークＷが置かれる基準面Ｓ（固定値）に対してワーク高さＷＨだけ高い位置に設定される。なお、ワーク保持部２４としてワークＷの両側面を把持するメカニカルチャックを用いてワークＷをピックアップする場合、ワークＷのピック位置は、ワークＷの上面よりも低い位置となる。この場合、手先の目標位置Ｚ₀＊は、別途、入力されたピック位置を用いて設定される。

【0033】

手先位置制御処理は、シャフト駆動部５０によるシャフト２３の昇降が行なわれる点を除いて、基本的には、上述したカメラ位置制御処理と同様の処理により行なわれる。すなわち、手先位置制御処理では、ＣＰＵ７１は、まず、手先を設定した目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）に移動させるための第１関節軸Ｊ１の角度θ_J1と第２関節軸Ｊ２の角度θ_J2とシャフト２３の角度（シャフト角度）θ_J4とシャフト２３の昇降位置Ｚ_sとを逆運動学により算出する。続いて、ＣＰＵ７１は、算出したθ_J1，θ_J2，θ_J4，Ｚ_sと第１アーム２１のねじれ角度θ_tおよびたわみ角度θ_d1と第２アーム２２のねじれ角度θ_d2とに基づいてねじれやたわみを考慮した手先の推定位置（Ｘ₀，Ｙ₀，Ｚ₀）を順運動学により算出する。次に、ＣＰＵ７１は、手先の推定位置（Ｘ₀，Ｙ₀，Ｚ₀）と目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）との差分をとることにより位置ズレ量（ΔＸ₀，ΔＹ₀，ΔＺ₀）を算出する。次に、ＣＰＵ７１は、目標位置（Ｘ₀＊，Ｙ₀＊，Ｚ₀＊）を位置ズレ量（ΔＸ₀，ΔＹ₀，ΔＺ₀）の分だけオフセットすることにより目標位置を補正する。そして、ＣＰＵ７１は、補正した目標位置に手先を移動させるための第１関節軸Ｊ１の角度指令値θ_J1＊と第２関節軸Ｊ２の角度指令値θ_J2＊とシャフト２３の角度指令値θ_J4＊とシャフト２３の昇降位置指令値Ｚ_s＊とを逆運動学により算出し、各指令値に基づいて対応するモータ３１，３２，３３ａ，３３ｂをフィードバック制御により制御する。

【0034】

ここで、カメラ位置制御処理や手先位置制御処理において、第１アーム２１のねじれ及びたわみや第２アーム２２のたわみを考慮したカメラ６０の画像中心や手先（ワーク保持部２４）の推定位置を求めるための順運動学の計算は、変数θ_J1，θ_J2，θ_t，θ_d1，θ_d2を同次変換行列に与えることにより行なうことができる。同次変換行列は、ロボット本体１０に設定した各座標系のうち隣接する座標系間の関係^i-1Ｔ_i（ｉ＝１，２，３，４）であるリンクパラメータを求め、リンクパラメータに基づいてアーム全体の変換行列⁰Ｔ₄を求めることにより導き出すことができる。

【0035】

図１４は、手先用リンクパラメータ表の一例を示す説明図である。図１５は、カメラ用リンクパラメータ表の一例を示す説明図である。手先用リンクパラメータ表およびカメラ用リンクパラメータ表において、図１４および図１５中、第ｉリンク（ｉ＝１，２，３，４）の各リンクパラメータ（ｄｘ、ｄｙ、ｄｚ、ｒｘ、ｒｙ、ｒｚ、ｒｘ２、ｒｙ２）は、座標系Σ_i-1から座標系Σ_iへ変換するためのパラメータである。ｄｘ，ｄｙ，ｄｚは、Ｘ軸方向，Ｙ軸方向，Ｚ軸方向の並進量を示すパラメータ（位置パラメータ）である。ｒｘ（ｒｘ２），ｒｙ（ｒｙ２），ｒｚは、Ｘ軸周り，Ｙ軸周り，Ｚ軸周りの回転量を示すパラメータ（姿勢パラメータ）である。

【0036】

また、図５または図６に示すように、Ｌ₁は、Ｚ₁軸とＺ₂軸とのＸ₁軸方向の距離（第１アーム２１のアーム長）を示す。Ｌ₂は、Ｚ₂軸とＺ₃軸とのＸ₂軸方向の距離（第２アーム２２のアーム長）を示す。Ｌ_z1は、ベース座標系Σ₀の原点からリンク座標系Σ₁の原点までのＺ₀軸方向の距離を示す。Ｌ_z2は、リンク座標系Σ₁の原点からリンク座標系Σ₂の原点までのＺ₁軸方向の距離を示す。ｔｈ１は、リンク座標系Σ₁のＺ₁軸周りの回転角であり、第１関節軸Ｊ１の角度θ_J1に相当する。ｔｈ２は、リンク座標系Σ₂のＺ₂軸周りの回転角であり、第２関節軸Ｊ２の角度θ_J2に相当する。ｔｈ４は、リンク座標系Σ₄のＺ₄軸周りの回転角であり、シャフト２３の角度（シャフト角度）θ_J4に相当する。ｄ３は、リンク座標系Σ₃の原点から手先までの距離であり、昇降位置Ｚ_sに相当する。Ｊ１ｒｘは、リンク座標系Σ₁のＸ₁軸周りの回転角を示す。Ｊ１ｒｙは、リンク座標系Σ₁のＹ₁軸周りの回転角を示す。Ｊ１ｒｘ２は、リンク座標系Σ₁がＺ₁軸周りにｔｈ１だけ回転した後のＸ₁軸周りの回転角を示し、第１アーム２１のねじれ角度θ_tに相当する。Ｊ１ｒｙ２は、リンク座標系Σ₁がＺ₁軸周りにｔｈ１だけ回転した後のＹ₁軸周りの回転角を示し、第１アーム２１のたわみ角度θ_d1に相当する。Ｊ２ｒｘは、リンク座標系Σ₂のＸ₂軸周りの回転角を示す。Ｊ２ｒｙは、リンク座標系Σ₂のＹ₂軸周りの回転角を示す。Ｊ２ｒｙ２は、リンク座標系Σ₂がＺ₂軸周りにｔｈ２回転した後のＹ₂軸周りの回転角を示し、第２アーム２２のたわみ角度θ_d2に相当する。Ｊ３ｒｘは、リンク座標系Σ₃のＸ₃軸周りの回転角を示す。Ｊ３ｒｙは、リンク座標系Σ₃のＹ₃軸周りの回転角を示す。

【0037】

アーム全体の変換行列⁰Ｔ₄は、リンクパラメータ表による各座標系間の関係^i-1Ｔ_i（ｉ＝１，２，３，４）を用いて次式（７）により求めることができる。各座標系間の関係（行列）^i-1Ｔ_iは、次式（８）により求めることができる。なお、ｄｘ，ｄｙ，ｄｚ，ｒｘ，ｒｙ，ｒｚ，ｒｘ２，ｒｙ２の各行列Ｔ_dx，Ｔ_dy，Ｔ_dz，Ｔ_rx，Ｔ_ry，Ｔ_rz，Ｔ_rx2，Ｔ_ry2を次式（９）～（１６）に示す。

【0038】

【数4】

【0039】

手先位置制御処理において、手先用リンクパラメータ表のＪ１ｒｘ２に第１アーム２１のねじれ角度θ_tを与え、Ｊ１ｒｙ２に第１アーム２１のたわみ角度θ_d1を与え、Ｊ２ｒｙ２に第２アーム２２のたわみ角度θ_d2を与えることで、順運動学（変換行列⁰Ｔ₄）により第１アーム２１のねじれ及びたわみと第２アーム２２のたわみとに起因した誤差を含む手先の推定位置を計算することができる。

【0040】

カメラ用リンクパラメータは、図１５に示すように、リンク座標系Σ₂からリンク座標系Σ₃への変換用パラメータｄｘ，ｄｙ，ｄｚ，ｒｘ，ｒｙに、Ｌ₂＋ＨＣＸ，ＨＣＹ，０，ＨＣＲＡ，ＨＣＲＢが設定され、リンク座標系Σ₃からハンド座標系Σ₄への変換用パラメータのｄｚ，ｒｚにそれぞれＨＣＺ，０が設定されている点で、手先用リンクパラメータとは異なる。ＨＣＸは、Ｘ₂軸方向におけるカメラ６０の位置を示し、ＨＣＹは、Ｙ₂軸方向におけるカメラ６０の位置を示す。また、ＨＣＲＡは、Ｘ₃軸周りのカメラ６０の傾きを示し、ＨＣＲＢは、Ｙ₃軸回りのカメラ６０の傾きを示す。ＨＣＺは、カメラ６０（レンズ先端部）から被写体であるワークＷの上面までの距離（ワーキングディスタンスＷＤ）を示す。

【0041】

カメラ位置制御処理において、カメラ用リンクパラメータ表のＪ１ｒｘ２に第１アーム２１のねじれ角度θ_tを与え、Ｊ１ｒｙ２に第１アーム２１のたわみ角度θ_d1を与え、Ｊ２ｒｙ２に第２アーム２２のたわみ角度θ_d2を与えることで、順運動学により第１アーム２１のねじれ及びたわみと第２アーム２２のたわみとに起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。また、カメラ６０のＸ₂軸方向における位置ＸおよびＹ₂軸方向における位置Ｙを予め測定しておき、カメラ用リンクパラメータ表のＨＣＸに位置Ｘを与え、ＨＣＹに位置Ｙを与えることで、順運動学によりカメラ６０の取付位置に起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。さらに、カメラ６０のＸ₃軸周りにおける傾きＲＡおよびＹ₃軸周りにおける傾きＲＢを予め測定しておき、カメラ用リンクパラメータ表のＨＣＲＡに傾きＲＡを与え、ＨＣＲＢに傾きＲＢを与えることで、順運動学によりカメラ６０の取付角度に起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。

【0042】

さらに、本実施形態では、カメラ用リンクパラメータ表のＨＣＺに与えられるワーキングディスタンスＷＤは、カメラ６０（レンズ先端部）とワークＷが置かれる基準面Ｓとの距離（固定値）から上述したワーク高さＷＨを減じることにより算出される。図１６に示すように、ワークＷが置かれる基準面Ｓに対してカメラ６０の光軸（図中、破線参照）が傾いている状態で高さの異なるワークＷを撮像すると、高さの違いΔＷＨに応じてそれぞれのワークＷの撮像画像の画像中心がズレる現象が生じる。本実施形態では、画像処理データからワーク高さＷＨを取得し、ＨＣＺに反映させることで、ワークＷの高さの違いによる画像中心のズレを修正することが可能となる。

【0043】

次に、カメラ６０の位置Ｘ，Ｙや傾きＲＡ，ＲＢを測定してカメラ用リンクパラメータに反映させるための手順について説明する。図１７は、カメラ用リンクパラメータ設定手順の一例を示す説明図である。カメラ用リンクパラメータ設定手順では、ステップＳ２００～Ｓ２４０により実行される。ステップＳ２００は、手先（ワーク保持部２４）に測定用治具１００を取り付ける。図１８は、手先（ワーク保持部２４）に測定用治具が取り付けられた状態を示す部分外観図である。図示するように、測定用治具１００は、装着箇所を中心としてシャフト２３に直交する方向に所定距離だけ離間すると共に周方向に所定間隔（９０度の間隔）をおいて配置された複数（４つ）のマークＭＡを有する。測定用治具１００は、シャフト２３に対して位相合わせされた状態で取り付けられ、各マークＭＡは、カメラ６０が設計値通りに取り付けられている場合に、カメラ６０の画像中心がマークＭＡの中心と一致するように配置されている。

【0044】

ステップＳ２１０は、測定用治具１００を用いてカメラ６０の位置Ｘ，Ｙを測定する。このステップＳ２１０は、制御装置７０のＣＰＵ７１により図１９に例示するカメラ位置測定処理を実行することにより行なわれる。カメラ位置測定処理では、ＣＰＵ７１は、まず、手先（ワーク保持部２４）を測定位置に移動させる（ステップＳ３００）。この処理は、マークＭＡがカメラ６０の被写界深度内となる昇降位置指令値Ｚ_s＊を設定してモータ５２ｂを制御することにより行なわれる。続いて、ＣＰＵ７１は、シャフト２３が所定ピッチずつ回転するようにモータ５２ａを制御し、シャフト２３が所定ピッチずつ回転する毎に、カメラ６０によりマークＭＡを撮像する処理を繰り返す（ステップＳ３１０）。次に、ＣＰＵ７１は、撮像画像を処理することによりカメラ６０により測定されたマークＭＡの実測位置と設計上のマークＭＡの位置（設計位置）との位置ズレ量ΔＸ，ΔＹを算出する（ステップＳ３２０）。図２０は、カメラ位置の測定の様子を示す説明図である。図中、白丸は、カメラ６０の取付位置が設計値通りであった場合に撮像画像に写るマーク画像を示し、黒丸は、カメラ６０でマークＭＡを実際に撮像することにより撮像画像に写ったマーク画像を示す。位置ズレ量ΔＸは、設計位置の円弧と実測位置の円弧とのＸ軸方向の位置の差である。位置ズレ量ΔＹは、設計位置の円弧と実測位置の円弧とのＹ軸方向の位置の差である。そして、ＣＰＵ７１は、カメラ６０の設計位置Ｘｄｖ，ＹｄｖにステップＳ１３０で算出した位置ズレ量ΔＸ，ΔＹを加算することにより、実際のカメラ６０の位置Ｘ，Ｙを算出する（ステップＳ３３０）。

【0045】

ステップＳ２２０は、測定用治具１００を用いてカメラ６０の傾きＲＡ，ＲＢを測定する。このステップＳ２２０は、制御装置７０のＣＰＵ７１により図２１に例示するカメラ傾き測定処理を実行することにより行なわれる。カメラ傾き測定処理では、ＣＰＵ７１は、まず、マークＭＡをカメラ６０の正面（真下）に移動させる（ステップＳ４００）。この処理は、シャフト角度θ_J4が予め定められた所定角度となるようにモータ５２ａを制御することにより行なわれる。続いて、ＣＰＵ７１は、カメラ６０の被写界深度内の所定量ｂだけマークＭＡを上下させ、下位置にあるマークＭＡと上位置にあるマークＭＡとをカメラ６０によりそれぞれ撮像する（ステップＳ４１０）。次に、ＣＰＵ７１は、撮像画像を処理することにより、下位置で撮像したマークＭＡの実測位置と設計位置とのＸＹ軸方向の位置ズレ量ΔＸａ，ΔＹａと、上位置で撮像したマークＭＡの実測位置と設計位置とのＸＹ軸方向の位置ズレ量ΔＸｃ，ΔＹｃとを算出する（ステップＳ４２０）。図２２は、カメラ傾きの測定の様子を示す説明図である。カメラ６０が取付角度が設計値通りであった場合に撮像画像に写るマーク画像の中心は、カメラ６０の画像中心と一致する。図示するように、位置ズレ量ΔＹａは、下位置でマークＭＡを撮像したときの撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＹ軸方向における差である。位置ズレ量ΔＹｃは、上位置でマークＭＡを撮像したときに撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＹ軸方向における差である。図示しないが、位置ズレ量ΔＸａは、下位置でマークＭＡを撮像したときの撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＸ軸方向における差である。位置ズレ量ΔＸｃは、上位置でマークＭＡを撮像したときに撮像画像に写ったマーク画像の中心位置（実測位置）と設計位置とのＹ軸方向における差である。そして、ＣＰＵ７１は、算出した位置ズレ量ΔＹａ，ΔＹｂと上述した所定量ｂとに基づいて次式（１７）によりＸ軸回りのカメラ６０の傾きＲＡを算出すると共に、算出した位置ズレ量ΔＸａ，ΔＸｂと所定量ｂとに基づいて次式（１８）によりＹ軸回りのカメラ６０の傾きＲＢを算出する（ステップＳ４３０）。なお、式（１７）は、図２２から容易に導き出すことができる。式（１８）についても同様である。

【0046】

【数5】

【0047】

ステップ２３０は、算出した位置ＸをパラメータＨＣＸに設定し、位置ＹをパラメータＨＣＹに設定し、傾きＲＡをパラメータＨＣＲＡに設定し、傾きＲＢをパラメータＨＣＲＢに設定する。ステップＳ２４０は、各々設定したリンクパラメータＨＣＸ，ＨＣＹ，ＨＣＲＡ，ＨＣＲＢをカメラ用リンクパラメータとして登録する。これにより、カメラ６０が設計値通りに取り付けられていなかった場合に、カメラ位置制御処理において順運動学によりカメラ６０の取付位置や取付角度に起因した誤差を含むカメラ６０の画像中心の推定位置を計算することができる。この推定位置に基づいて目標位置を補正して制御することで、カメラ６０の画像中心を精度よく目標位置に合わせることが可能となる。

【0048】

ここで、実施形態の主要な要素と請求の範囲に記載した本開示の主要な要素との対応関係について説明する。即ち、本実施形態では、第１アーム２１が第１アームに相当し、第１関節軸Ｊ１が第１軸に相当し、第２アーム２２が第２アームに相当し、第２関節軸Ｊ２が第２軸に相当し、シャフト２３がシャフトに相当し、第３関節軸Ｊ３が第３軸に相当し、カメラ６０がカメラに相当する。

【0049】

なお、本開示は上述した実施形態に何ら限定されることはなく、本開示の技術的範囲に属する限り種々の態様で実施し得ることはいうまでもない。

【0050】

例えば、上述した実施形態では、カメラ位置Ｘ，Ｙとカメラ傾きＲＡ，ＲＢとを測定し、それぞれカメラ用リンクパラメータＨＣＸ，ＨＣＹ，ＨＣＲＡ，ＨＣＲＢに反映させるものとした。しかし、カメラ位置Ｘ，Ｙとカメラ傾きＲＡ，ＲＢとのうちいずれか一方の測定を省略してもよい。

【0051】

以上説明したように、本開示のスカラロボットの制御方法は、基台と、前記基台に設けられ、第１軸周りに回動する第１アームと、前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、光軸が前記第３軸と平行となるように前記第２アームに取り付けられたカメラと、を備え、前記第１アーム，前記第２アーム，前記シャフトを第１リンク，第２リンク，第３リンクとして各リンク間の位置の関係をあらわした位置パラメータと各リンク間の姿勢の関係をあらわした姿勢パラメータとを用いて前記カメラの位置を制御するスカラロボットの制御方法であって、前記第３軸に対する前記光軸の傾きを前記カメラの取付角度として測定し、前記測定したカメラの取付角度に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記姿勢パラメータに登録することを要旨とする。

【0052】

この本開示のスカラロボットの制御方法によれば、カメラの取付角度を測定して、測定値に基づく補正値を対応するリンクへの変換のための姿勢パラメータに反映させることで、カメラに取付角度に誤差が生じていても、カメラの位置制御を精度よく行なうことが可能となる。

【0053】

こうした本開示のスカラロボットの制御方法において、前記第３軸と直交する方向における前記光軸の位置を前記カメラの取付位置として測定し、前記測定したカメラの取付位置に基づく補正値を前記第２リンクから前記第３リンクへの変換のための前記位置パラメータに登録するものとしてもよい。こうすれば、カメラの取付角度に加えて取付位置に誤差が生じていても、カメラの位置制御を精度よく行なうことができる。

【0054】

さらに、本開示のスカラロボットの制御方法において、前記第２アームの回動位置と前記第１アームのアーム周りのねじれ量との関係を記憶し、前記第２アームの回動位置に対応する前記第１アームのねじれ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録するものとしてもよい。こうすれば、第１アームのねじれが生じていても、カメラの位置制御を精度よく行なうことができる。

【0055】

また、本開示のスカラロボットの制御方法において、前記第２アームの回動位置に対応する前記第１アームのたわみ量と前記第２アームのたわみ量とを記憶し、前記第２アームの回動位置に対応する前記第１アームのたわみ量に基づく補正値を、前記基台を第０リンクとして該第０リンクから前記第１リンクへの変換のための前記姿勢パラメータに登録すると共に、前記第２アームの回動位置に対応する前記第２アームのたわみ量に基づく補正値を、前記第１リンクから前記第２リンクへの変換のための前記姿勢パラメータに登録するものとしてもよい。こうすれば、第１アームや第２アームにたわみが生じていても、カメラの位置制御を精度よく行なうことができる。

【産業上の利用可能性】

【0056】

本開示は、スカラロボットの製造産業などに利用可能である。

【符号の説明】

【0057】

１ スカラロボットシステム、２ 作業台、１０ ロボット本体、１１ 基台、２０ スカラアーム、２１ 第１アーム、２２ 第２アーム、２３ シャフト、２４ ワーク保持部、３０ 第１アーム駆動部、３２ モータ、３４ エンコーダ、４０ 第２アーム駆動部、４２ モータ、４４ エンコーダ、５０ シャフト駆動部、５２ａ，５２ｂ モータ、５４ａ，５４ｂ エンコーダ、６０ カメラ、７０ 制御装置、７１ ＣＰＵ、７２ ＲＯＭ、７３ ＲＡＭ、７４ 記憶装置、１００ 測定用治具、Ｊ１ 第１関節軸、Ｊ２ 第２関節軸、Ｊ３ 第３関節軸、 ＭＡ マーク、Ｗ ワーク。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】