WO2022024243A1

WO2022024243A1 - スカラロボットシステム

Info

Publication number: WO2022024243A1
Application number: PCT/JP2020/028987
Authority: WO
Inventors: 将吾東
Original assignee: 株式会社Fuji
Priority date: 2020-07-29
Filing date: 2020-07-29
Publication date: 2022-02-03
Also published as: JPWO2022024243A1; CN115803154A; JP7513721B2

Abstract

スカラロボットシステムは、基台と、記基台に設けられ第１軸周りに回動する第１アームと、第１アームに設けられ第１軸と平行な第２軸周りに回動する第２アームと、第２アームに対して第２軸と平行な第３軸の軸方向に移動するシャフトと、第１アームと第２アームとシャフトとの動作を制御する制御部と、第２アームの回動位置に対応する第１アームのアーム周りの回転角に基づく第１アームのねじれ補正係数を記憶する記憶部と、ねじれ補正係数を用いてシャフト先端の水平方向の位置を補正する補正部と、を備える。

Description

スカラロボットシステム

　本明細書は、スカラロボットシステムについて開示する。

　従来、ロボットのたわみを補正してアーム先端の位置や姿勢のずれを補正するたわみ補正装置が提案されている（例えば、特許文献１参照）。たわみ補正装置は、ロボットの動作領域内の複数の位置において、重量、重心位置の異なる複数の荷重条件下で、アーム先端の位置や姿勢のずれを表わすたわみ量を測定し、たわみ量データとして記憶する。続いて、たわみ補正装置は、使用ツールの重量、重心位置が近いたわみ量データを指定し、ロボットの動作プログラムの各教示点位置におけるたわみ量を、指定したたわみ量データを用いて算出する。そして、たわみ補正装置は、動作プログラムの各教示点位置を、算出したたわみ量分補正して変更する。

　また、ベースに固定されたＪ１軸に対して水平面内で旋回可能な第１アームと、第１アームに固定されたＪ２軸に対して水平面内で旋回可能な第２アームと、第２アームの先端部に有するＪ３軸の軸方向に移動可能なシャフトと、を備えるスカラロボットにおいて、第１アーム系の誤差として、アームのねじれによる角度誤差が含まれ、第２アーム系の誤差として、アームのねじれによる角度誤差が含まれるものも提案されている（例えば、特許文献２参照）。

特開２００４－２９９０１０号公報特開２０１２－６１２５号公報

　第１軸周りに回動する第１アームと、第１アームに設けられて第１軸と平行な第２軸周りに回動する第２アームと、第２アームに対して第２軸と平行な第３軸の軸方向に移動するシャフトとを備えるスカラロボットにおいては、第２アームの回動により第１アームと第２アームとを含むアーム部の重心位置が移動する。重心位置に第１アームの延在方向に直交する成分が含まれると、第１アームのアーム周りにモーメントが生じて第１アームがねじれ、シャフト先端（手先）の位置精度が悪化するおそれがある。

　本開示は、第２アームが回動位置に拘らず第１アームのアーム周りのねじれに基づく手先の位置ずれを良好に補正することができるスカラロボットシステムを提供することを主目的とする。

　本開示のスカラロボットシステムは、上述の主目的を達成するために以下の手段を採った。

　本開示のスカラロボットシステムは、
　基台と、
　前記基台に設けられ、第１軸周りに回動する第１アームと、
　前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
　前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
　前記第１アームと前記第２アームと前記シャフトとの動作を制御する制御部と、
　前記第２アームの回動位置に対応する前記第１アームのアーム周りの回転角に基づく、前記第１アームのねじれ補正係数を記憶する記憶部と、
　前記ねじれ補正係数を用いて前記シャフト先端の水平方向の位置を補正する補正部と、
　を備えることを要旨とする。

　この本開示のスカラロボットシステムは、基台に設けられて第１軸周りに回動する第１アームと、第１アームに設けられた第１軸と平行な第２軸周りに回動する第２アームと、第２アームに対して第２軸と平行な第３軸の軸方向に移動するシャフトと、これらの動作を制御する制御部と、記憶部と、補正部と、を備えるものである。記憶部は、第２アームの回動位置に対応する第１アームのアーム周りの回転角に基づく第１アームのねじれ補正係数を記憶する。補正部は、ねじれ補正係数を用いて前記シャフト先端の水平方向の位置を補正する。第１アームと第２アームとを含むアーム部の重心位置に、第１アームの延在方向に直交する成分が含まれると、第１アームのアーム周りにモーメントが生じて第１アームにねじれが生じる。スカラロボットシステムは、予め第２アームの回動位置に対応する第１アームのアーム周りの回転角に基づく第１アームのねじれ補正係数を記憶部に記憶しておき、これを補正値として反映させる。これにより、第２アームが回動位置に拘らず第１アームのアーム周りのねじれに基づく手先の位置ずれを良好に補正することができるスカラロボットシステムとすることができる。

スカラロボットシステムの外観斜視図である。ロボット本体の側面図である。ロボット本体と制御装置との電気的な接続関係を示すブロック図である。ロボット制御処理の一例を示すフローチャートである。ロボットの座標系を説明する説明図である。ロボットの座標系を説明する説明図である。リンクパラメータ表の一例を示す説明図である。第１関節軸Ｊ１に設定されるリンク座標系Σ₁のＸ₁およびＹ₁軸を説明する説明図である。第２アームの水平旋回により第１アームに対して生じるねじれを説明する説明図である。手先質量により第１アームおよび第２アームに対して生じるたわみを説明する説明図である。第１アームねじれモデル式作成手順の一例を示す説明図である。第２アームを－９０度から＋９０度まで水平旋回する様子を示す説明図である。第１アームの先端部両端におけるＺ方向変位を測定する様子を示す説明図である。第１アームの先端部両端におけるＺ方向変位を測定する様子を示す説明図である。第１アームの先端部両端におけるＺ方向変位の測定値からたわみ成分を除去する様子を示す説明図である。アーム部の重心位置のＹ₁軸方向成分Ｌ_t’を説明する説明図である。第１アームに生じるねじれモーメントを説明する説明図である。第１アームたわみモデル式作成手順の一例を示す説明図である。多関節アームの手先に重りｍを付けた際の第１アームおよび第２アームの各先端部のＺ方向変位を測定する様子を示す説明図である。第１アームのたわみ量と重心位置×アーム質量との関係を示す説明図である。重り毎の第１アームのたわみ角度とＪ２角度との関係を示す説明図である。アーム部の重心位置のＸ₁軸方向成分Ｌ_d1’を説明する説明図である。第１アームに生じるたわみモーメントを説明する説明図である。第２アームたわみモデル式作成手順の一例を示す説明図である。第２アームのたわみ角度と手先質量との関係を示す説明図である。アーム部の重心位置Ｌ_d2を説明する説明図である。

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

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

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

　基台１１は、作業台２に固定されており、スカラアーム２０の基端側を支持する。スカラアーム２０は、第１アーム２１と第１アーム駆動部３０と第２アーム２２と第２アーム駆動部４０とシャフト２３とシャフト駆動部５０とカメラ６０とを備える。第１アーム２１は、基端部が第１関節軸Ｊ１を介して基台１１に連結され、第１関節軸Ｊ１の回動により基台１１に対して水平面内で回動（水平旋回）可能に構成される。第２アーム２２は、基端部が第２関節軸Ｊ２を介して第１アーム２１の先端部に連結され、第２関節軸Ｊ２の回動により第１アーム２１に対して水平面内で回動（水平旋回）可能に構成される。シャフト２３は、第２アーム２２の先端部に第３関節軸Ｊ３を介して連結され、第２アーム２２に対して第３関節軸Ｊ３の軸周りに回転可能かつ第３関節軸Ｊ３の軸方向に沿って昇降可能に構成される。本実施形態のスカラロボットシステムでは、シャフト２３の先端には、ワークＷをピックアップして保持するワーク保持部２４が設けられている。

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

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

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

　カメラ６０は、第２アーム２２の先端部側面に取り付けられている。カメラ６０は、作業対象のワークＷを撮像し、その撮像画像を制御装置７０へ出力する。制御装置７０は、撮像画像を処理することによりワークＷの位置を認識する。

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

　次に、こうして構成されたスカラロボットシステム１の動作について説明する。図４は、制御装置７０により実行されるロボット制御処理の一例を示すフローチャートである。この処理は、所定時間毎に繰り返し実行される。

　ロボット制御処理が実行されると、制御装置７０のＣＰＵ７１は、まず、ロボット本体１０の手先（ワーク保持部２４）の目標位置を取得する（ステップＳ１００）。続いて、ＣＰＵ７１は、手先の目標位置に対して逆運動学を解くことにより、手先を目標位置に移動させるための第１関節軸Ｊ１の角度θ_J1と第２関節軸Ｊ２の角度θ_J2とシャフト２３の先端の位置（シャフト位置）Ｚ_sとシャフト２３の回転角度（シャフト角度）θ_J4とを算出する（ステップＳ１１０）。

　次に、ＣＰＵ７１は、ワーク質量ｍを取得する（ステップＳ１２０）。ステップＳ１２０は、記憶装置７４に予め記憶（登録）された複数種のワークＷの質量のうち今回の作業対象物となるワークＷの質量を読み出すことにより行なわれる。そして、ＣＰＵ７１は、ワーク質量ｍとステップＳ１１０で算出した第２関節軸Ｊ２の角度θ_J2とに基づいて第１アーム２１のねじれ角度θ_tとたわみ角度θ_d1とを算出すると共に（ステップＳ１３０）、ワーク質量ｍに基づいて第２アーム２２のたわみ角度θ_d2を算出する（ステップＳ１４０）。第１アーム２１のねじれ角度θ_tの算出は、第１アーム２１のねじれモデル式（演算式）を用いて行なわれる。第１アーム２１のねじれモデル式は、第１アーム２１のねじれ角度θ_tと、第２関節軸Ｊ２の角度θ_J2と、ワーク質量ｍを含むアーム質量Ｍ（第１関節軸Ｊ１以降のワークＷを含むアーム部の質量）との関係を表すモデル式である。第１アーム２１のたわみ角度θ_d1の算出は、第１アーム２１のたわみモデル式（演算式）を用いて行なわれる。第１アーム２１のたわみモデル式は、第１アーム２１のたわみ角度θ_d1と、第２関節軸Ｊ２の角度θ_J2と、ワーク質量ｍを含むアーム質量Ｍ（第１関節軸Ｊ１以降のワークＷを含むアーム部の質量）との関係を表すモデル式である。第２アーム２２のたわみ角度θ_d2の算出は、第２アーム２２のたわみモデル式を用いて行なわれる。第２アーム２２のたわみモデル式は、第２アーム２２のたわみ角度θ_d2と、ワーク質量ｍを含むアーム質量Ｍ（第２関節軸Ｊ２以降のワークＷを含むアーム部の質量）との関係を表すモデル式である。これらのモデル式は、記憶装置７４に記憶されている。ねじれモデル式や各たわみモデル式の詳細については後述する。

　そして、ＣＰＵ７１は、第１関節軸Ｊ１の角度θ_J1と第２関節軸Ｊ２の角度θ_J2とシャフト位置Ｚ_sとシャフト角度θ_J4と第１アーム２１のねじれ角度θ_tおよびたわみ角度θ_d1と第２アーム２２のたわみ角度θ_d2とに基づいて順運動学を解くことにより基台１１（ベース座標系Σ₀）から見た手先の位置（Ｘ₀，Ｙ₀，Ｚ₀）を算出する（ステップＳ１５０）。手先の位置は、順運動学の同次変換行列に各変数θ_J1，θ_J2，θ_J4，Ｚ_s，θ_t，θ_d1，θ_d2を与えることにより算出することができる。順運動学の同次変換行列は、例えば、ロボット本体１０の各リンクに座標系を設定し、リンクパラメータ表を作成して各座標系間の関係^i-1Ｔ_i（ｉ＝１，２，３，４）を求め、各座標系間の関係^i-1Ｔ_iからアーム全体の変換行列⁰Ｔ₄を求めることにより導き出すことができる。

　各リンクの座標系は、図５および図６に示すように、ベース座標系Σ₀とリンク座標系Σ₁～Σ₃とハンド座標系Σ₄とを有する。ベース座標系Σ₀は、基台１１に設定される。リンク座標系Σ₁は、第１アーム２１の根元（第１関節軸Ｊ１）に設定される。リンク座標系Σ₂は、第２アーム２２の根元（第２関節軸Ｊ２）に設定される。リンク座標系Σ₃は、シャフト２３に設定される。ハンド座標系Σ₄は、手先（ワーク保持部２４）に設定される。各座標系は、第１アーム２１および第２アーム２２が一直線状に伸びた状態をホームポジションとする。また、図中、Ｘ₀，Ｘ₁，Ｘ₂，Ｘ₃，Ｘ₄は、それぞれの座標系のＸ軸である。Ｙ₀，Ｙ₁，Ｙ₂，Ｙ₃，Ｙ₄は、それぞれの座標系のＹ軸である。Ｚ₀，Ｚ₁，Ｚ₂，Ｚ₃，Ｚ₄は、それぞれの座標系のＺ軸である。ｔｈ１は、リンク座標系Σ₁のＺ₁軸周りの回転角を示す。Ｊ１ｒｘは、リンク座標系Σ₁のＸ₁軸周りの回転角を示し、Ｊ１ｒｙは、リンク座標系Σ₁のＹ₁軸周りの回転角を示す。さらに、Ｊ１ｒｘ２は、リンク座標系Σ₁がＺ₁軸周りにｔｈ１だけ回転した状態のＸ₁軸周りの回転角を示し、Ｊ１ｒｙ２は、リンク座標系Σ₁がＺ₁軸周りにｔｈ１だけ回転した状態のＹ₁軸周りの回転角を示す。ｔｈ２は、リンク座標系Σ₂のＺ₂軸周りの回転角を示す。Ｊ２ｒｘは、リンク座標系Σ₂のＸ₂軸周りの回転角を示し、Ｊ２ｒｙは、リンク座標系Σ₂のＹ２軸周りの回転角を示す。さらに、Ｊ２ｒｙ２は、リンク座標系Σ₂がＺ₂軸周りにｔｈ２回転した状態のＹ２軸周りの回転角を示す。Ｊ３ｒｘは、リンク座標系Σ₃のＸ３軸周りの回転角を示し、Ｊ３ｒｙは、リンク座標系Σ₃のＹ₃軸周りの回転角を示す。ｔｈ４は、ハンド座標系Σ₄のＺ₄軸周りの回転角を示す。また、Ｌ₁は、Ｚ₁軸とＺ₂軸とのＸ₁軸方向の距離（第１アーム２１のアーム長）を示す。Ｌ₂は、Ｚ₂軸とＺ₃軸とのＸ₂軸方向の距離（第２アーム２２のアーム長）を示す。Ｌ_z1は、ベース座標系Σ₀の原点からリンク座標系Σ₁の原点までのＺ₀軸方向の距離を示す。Ｌ_z2は、リンク座標系Σ₁の原点からリンク座標系Σ₂の原点までのＺ₁軸方向の距離を示す。ｄ３は、リンク座標系Σ₃の原点からハンド座標系Σ₄の原点までのＺ₃軸方向の距離を示す。

　図７は、リンクパラメータ表の一例を示す説明図である。図中、リンク番号ｉ（ｉ＝１，２，３，４）の各パラメータ（ｄｘ、ｄｙ、ｄｚ、ｒｘ、ｒｙ、ｒｚ、ｒｘ２、ｒｙ２）は、座標系Σ_i-1から座標系Σ_iへ変換するためのパラメータである。ｄｘ，ｄｙ，ｄｚは、座標系Σ_i-1のＸ_i-1軸方向，Ｙ_i-1軸方向，Ｚ_i-1軸方向の移動量を示す。ｒｘ（ｒｘ２），ｒｙ（ｒｙ２），ｒｚは、Ｘ_i-1軸周り，Ｙ_i-1軸周り，Ｚ_i-1軸周りの回転量を示す。表中、ｔｈ１は、第１関節軸Ｊ１の回転角θ_J1に相当し、ｔｈ２は、第２関節軸Ｊ２の回転角θ_J2に相当し、ｔｈ４は、シャフト２３の回転角θ_J4に相当する。ｄ３は、リンク座標系Σ₃の原点から手先（シャフト位置Ｚ_s）までの距離に相当する。また、Ｊ１ｒｘ２は、第１アーム２１のねじれ角度θ_tに相当し、Ｊ１ｒｙ２は、第１アーム２１のたわみ角度θ_d1に相当する。Ｊ２ｒｙ２は、第２アーム２２のたわみ角度θ_d2に相当する。

　アーム全体の変換行列⁰Ｔ₄は、リンクパラメータ表による各座標系間の関係^i-1Ｔ_i（ｉ＝１，２，３，４）を用いて次式（１）により求めることができる。各座標系間の関係（行列）^i-1Ｔ_iは、次式（２）により求めることができる。なお、ｄｘ，ｄｙ，ｄｚ，ｒｘ，ｒｙ，ｒｚ，ｒｘ２，ｒｙ２の各行列Ｔ_dx，Ｔ_dy，Ｔ_dz，Ｔ_rx，Ｔ_ry，Ｔ_rz，Ｔ_rx2，Ｔ_ry2を次式（３）～（１０）に示す。この変換行列⁰Ｔ₄（同次変換行列）に各変数θ_J1，θ_J2，θ_J4，Ｚ_s，θ_t，θ_d1，θ_d2を与えることにより、第１アーム２１のねじれ及びたわみと第２アーム２２のたわみとに起因した誤差を含んだ手先の位置を計算することができる。

　なお、手先の誤差要因には、ねじれ角度θ_tやたわみ角度θ_d1，θ_d2の他に、リンク長誤差や関節取付誤差、関節回転オフセット、関節傾き等も含まれる。したがって、これらの誤差についても、変換行列⁰Ｔ₄に与えることで、当該誤差を含んだ手先の位置を計算することができる。

　ＣＰＵ７１は、こうして誤差を含む手先の位置を算出すると、算出した誤差を含む手先の位置とステップＳ１００で取得した手先の目標位置との差分をとることにより、各軸（ベース座標系Σ₀におけるＸ₀軸、Ｙ₀軸およびＺ₀軸）における手先の位置ずれ量を算出する（ステップＳ１６０）。続いて、ＣＰＵ７１は、目標位置を手先の位置ずれ量の分だけオフセットすることにより目標位置を補正した補正後位置を算出する（ステップＳ１７０）。次に、ＣＰＵ７１は、補正後位置に対して逆運動学を解くことにより第１関節軸Ｊ１の角度θ_J1の目標値と第２関節軸Ｊ２の角度θ_J2の目標値とシャフト位置Ｚ_sの目標値とシャフト角度θ_J4の目標値とを算出する（ステップＳ１８０）。そして、ＣＰＵ７１は、各目標値が補正後目標位置に一致するよう対応するモータを制御して（ステップＳ１９０）、ロボット制御処理を終了する。

　次に、第１アーム２１のねじれ角度θ_tの算出に用いられるねじれモデル式を作成する手順と、第１アーム２１のたわみ角度θ_d1の算出に用いられるたわみモデル式を作成する手順について説明する。ここで、図８および図９に示すように、第１関節軸Ｊ１の延在方向をＺ₁軸方向とし、第１アーム２１の中心線に沿った方向をＸ₁軸方向とし、Ｘ₁軸およびＺ₁軸に直交する方向をＹ₁軸方向とした上述したリンク座標系Σ₁を考えた場合、第１アーム２１のねじれ角度θ_tは、第１アーム２１のＸ₁軸周りの変位角度として定義される。また、第１アーム２１のたわみ角度θ_d1は、図１０に示すように、第１アーム２１のＹ₁軸周りの変位角度として定義される。さらに、第２アーム２２のたわみ角度θ_d2は、第２アーム２２のＹ２軸周りの変位角度として定義される。

　図１１は、第１アームのねじれモデル式を作成する第１アームねじれモデル式作成手順の一例を示す説明図である。第１アームねじれモデル式作成手順では、ステップＳ２００～Ｓ２９０により実行される。ステップＳ２００は、第１関節軸Ｊ１の角度θ_J1が０度（基準角）となるようにモータ３２を制御する。ステップＳ２１０は、手先（ワーク保持部２４）にワークＷを想定した重りｍを取り付け、図１２に示すように第２関節軸Ｊ２が基準角（Ｘ₁軸方向）に対して９０度小さい－９０度から９０度大きい＋９０度まで所定角度Δθ_J2ずつ（例えば１０度ずつ）回転するようにモータ４２を制御し、基準角（０度）における第１アーム２１の先端部両端のＺ₁軸方向における位置を基準として、第２関節軸Ｊ２が所定角度Δθ_J2回転する毎に第１アーム２１の先端部両端のＺ₁軸方向における変位（Ｚ方向変位Ｚ_tr，Ｚ_tl）を測定する。なお、ステップＳ２１０の測定は、図１３および図１４に示すように、第１アーム２１の先端部の両側面にそれぞれ測定対象物Ｐｒ，Ｐｌを設置し、レーザ変位計を用いて測定対象物Ｐｒ，Ｐｌにそれぞれ下方からレーザを当てて反射光を受光することにより行なうことができる。ステップＳ２２０は、手先（ワーク保持部２４）に取り付ける重りｍを順次変えて、重りｍ毎に、ステップＳ２１０のＺ方向変位Ｚ_tr，Ｚ_tlを測定する動作を繰り返す。次のステップＳ２３０は、重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎のＺ方向変位Ｚ_tr，Ｚ_tlの平均値Ｚ_rlaveを次式（１１）により算出する。続くステップＳ２４０は、重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎にＺ方向変位Ｚ_tr，Ｚ_tlから平均値Ｚ_rlaveを減じることにより重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎の第１アーム２１のねじれ角度θ_tの実測値を導出する。ここで、ステップＳ２１０，Ｓ２２０で測定される第１アーム２１の先端部両端のＺ方向変位Ｚ_tr，Ｚ_tlには、第１アーム２１のたわみによる成分が含まれる。一方、平均値Ｚ_rlaveは、第１アーム２１の中心線上のＺ方向変位を示し、ねじれ成分は含まれない。したがって、図１５に示すように、第２関節軸Ｊ２の角度θ_J2毎にＺ方向変位Ｚ_tr，Ｚ_tlから平均値Ｚ_rlaveをそれぞれ減算することにより、Ｚ方向変位Ｚ_tr，Ｚ_tlからたわみ成分を除去することができる。

　ステップＳ２５０は、第１アーム２１、第２アーム２２およびシャフト２３の質量と手先（重りｍ）の質量とから第１関節軸Ｊ１以降（スカラアーム２０の第１関節軸Ｊ１から先端側）のアーム部（重りｍを含む）の重心位置Ｌ_tを算出する。ステップＳ２６０は、アーム部の重心位置Ｌ_tと第２関節軸Ｊ２の角度θ_J2とから角度θ_J2における重心位置Ｌ_tのＹ₁軸方向成分Ｌ_t’（θ_J2）を次式（１２）により算出する。重心位置Ｌ_tのＹ₁軸方向成分Ｌ_t’（θ_J2）は、図１６に示すように、重心位置Ｌ_tのＸ₁軸に対するＹ₁軸方向における距離を示す。ステップＳ２７０は、第１アーム２１のねじれモーメントＩ_tを算出する。ねじれモーメントＩ_tは、図１７に示すように、アーム部の重力（図中、下向き矢印）により第１アーム２１をＸ₁軸周りに回転させようとするものであり、重心位置Ｌ_tがＸ₁軸上にあるときには値０であり、重心位置Ｌ_tがＸ₁軸からＹ₁軸方向に離れるにつれて大きくなる。すなわち、ねじれモーメントＩ_tは、第２関節軸Ｊ２の角度θ_J2が０度（基準角）のときに最小となり、第２関節軸Ｊ２の角度が－９０度のときと＋９０度のときに最大となる。ねじれモーメントＩ_tは、重りｍを含むアーム部の質量をＭとし、重力加速度をｇとしたとき、次式（１３）により求めることができる。ステップＳ２８０は、第１アーム２１のねじれ角度θ_tと第２関節軸Ｊ２の角度θ_J2とアーム部の質量Ｍとの関係を表わす第１アーム２１のねじれモデル式を作成する。具体的には、ステップＳ２８０は、ねじれモーメントＩ_tをねじれ剛性Ｋ_tで除した次式（１４）により定まるねじれモデル式を作成する。ステップＳ２９０は、ステップＳ２４０で導出した重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎の第１アーム２１のねじれ角度θ_tの実測値からねじれ剛性Ｋ_tを最適化する。このステップＳ２９０は、最急降下法やニュートンラフソン法などの最適化問題を解くための種々のアルゴリズムを採用することができる。

　図１８は、第１アームたわみモデル式作成手順の一例を示す説明図である。第１アームたわみモデル式作成手順は、ステップＳ３００～Ｓ３９０により実行される。ステップＳ３００は、第１関節軸Ｊ１および第２関節軸Ｊ２の各角度θ_J1，θ_J2がそれぞれ０度（基準角）となるようにモータ３２，４２を制御する。これにより、第１アーム２１および第２アーム２２は、Ｘ₁軸方向に沿って伸びた状態となる。ステップＳ３１０は、手先（ワーク保持部２４）にワークＷを想定した質量の異なる重りｍを順次取り付け、手先に重りを付けない状態での第１アーム２１の先端部のＺ₁軸方向における位置を基準として、重りｍ毎に、第１アーム２１の先端部のＺ₁軸方向における変位（Ｚ方向変位Ｚ_d1）を測定する。なお、ステップＳ３１０の測定は、図１９に示すように、第１アーム２１の先端部の底面を測定対象面とし、レーザ変位計を用いて測定対象面にレーザを当てて反射光を受光することにより行なうことができる。続くステップＳ３２０は、手先に重りｍが取り付けられておらず且つ第２関節軸Ｊ２の角度θ_J2が０度（基準角）であるときの第１アーム２１のたわみ角度θ_d1である初期たわみ角度を設定する。第１関節軸Ｊ１以降（第１関節軸Ｊ１からアーム先端まで）のアーム部の重心位置（Ｘ₁軸上の重心位置）とアーム部の質量とを乗じたもの（重心位置×アーム質量）が値０（無重力状態）であるとき、第１アーム２１は、全くたわんでいない状態となる。このため、第１アーム２１の初期たわみ量は、図２０に示すように、重心位置×アーム質量をｘ軸とし、Ｚ方向変位Ｚ_d1（第１アーム２１のたわみ量）をｙ軸として重りｍ毎の両者の組み合わせデータをｘｙグラフ上にプロットし、プロットした点を結んだ直線の延長線（図中、破線参照）におけるｙ切片を求めることにより導出することができる。第１アーム２１の初期たわみ角度は、導出した初期たわみ量に対して逆正接関数を適用することにより求めることができる。

　ステップＳ３３０は、第２関節軸Ｊ２が－９０度から＋９０度まで所定角度Δθ_J2ずつ（例えば１０度ずつ）回転するようにモータ４２を制御し、基準角（０度）における第１アーム２１の先端部のＺ₁軸方向における位置を基準として、第２関節軸Ｊ２が所定角度Δθ_J2回転する毎に第１アーム２１の先端部のＺ₁軸方向における変位（Ｚ方向変位Ｚ_d1）を測定する。更に、ステップＳ３３０は、手先にワークＷを想定した質量の異なる重りｍを順次取り付け、重りｍ毎に、第２関節軸Ｊ２が所定角度Δθ_J2回転する毎にＺ方向変位Ｚ_d1を測定する動作を繰り返す。続くステップＳ３４０は、重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎に測定したＺ方向変位Ｚ_d1（たわみ量）に対してそれぞれ逆正接関数を適用して各角度変位を求め、求めた各角度変位をステップＳ３２０で設定した初期たわみ角度に加えることにより、図２１に示すように重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎のたわみ角度θ_d1の実測値を導出する。

　ステップＳ３５０は、第１アーム２１、第２アーム２２およびシャフト２３の質量と手先（重りｍ）の質量とから第１関節軸Ｊ１以降（スカラアーム２０の第１関節軸Ｊ１から先端側）のアーム部（重りｍを含む）の重心位置Ｌ_d1を算出する。ステップＳ３６０は、アーム部の重心位置Ｌ_d1と第２関節軸Ｊ２の角度θ_J2とから角度θ_J2における重心位置Ｌ_d1のＸ₁軸方向成分Ｌ_d1’（θ_J2）を次式（１５）により算出する。重心位置Ｌ_d1のＸ₁軸方向成分Ｌ_d1’（θ_J2）は、図２２に示すように、第２関節軸Ｊ２の中心を通りＹ₁軸に平行なＹ₁’軸に対する重心位置Ｌ_d1のＸ₁軸方向における距離を示す。ステップＳ３７０は、第１アーム２１のたわみモーメントＩ_d1を算出する。たわみモーメントＩ_d1は、本実施形態では、図２３に示すように、アーム部の重力（図中、下向き矢印）により第１アーム２１をＹ₁軸周りに回転させようとするものであり、重心位置Ｌ_d1が第１関節軸Ｊ１からＸ₁軸方向に離れるにつれて大きくなる。すなわち、たわみモーメントＩ_d1は、第２関節軸Ｊ２の角度が±１８０度のときに最小となり、第２関節軸Ｊ２の角度が０度（基準角）のときに最大となる。たわみモーメントＩ_d1は、第１アーム２１のアーム長をＬ₁とし、アーム部の質量をＭとし、重力加速度をｇとしたとき、次式（１６）により求めることができる。ステップＳ３８０は、第１アーム２１のたわみ角度θ_d1と第２関節軸Ｊ２の角度θ_J2とアーム部の質量Ｍとの関係を表わす第１アーム２１のたわみモデル式を作成する。具体的には、ステップＳ３８０は、たわみモーメントＩ_d1をたわみ剛性Ｋ_d1で除した次式（１７）により定まるモデル式を作成する。ステップＳ３９０は、ステップＳ３４０で導出した重りｍ毎および第２関節軸Ｊ２の角度θ_J2毎の第１アーム２１のたわみ角度θ_d1の実測値からたわみ剛性Ｋ_d1を最適化する。このステップＳ３９０は、ねじれ剛性Ｋ_tと同様に、最急降下法やニュートンラフソン法などの最適化問題を解くための種々のアルゴリズムを採用することができる。

　次に、第２アーム２２のたわみモデル式を作成する手順について説明する。図２４は、第２アームたわみモデル式作成手順の一例を示す説明図である。第２アームたわみモデル式作成手順は、ステップＳ４００～Ｓ４６０により実行される。ステップＳ４００は、手先にワークＷを想定した質量の異なる重りｍを順次取り付け、手先に重りを付けない状態での第２アーム２２の先端部のＺ₁軸方向における位置を基準として、重りｍ毎に、第２アーム２２の先端部のＺ₁軸方向における変位（Ｚ方向変位Ｚ_d2）を測定する。なお、ステップＳ４００の測定は、図１９に示すように第２アーム２２の先端部に装着されたシャフト２３の先端面を測定対象面とし、レーザ変位計を用いて測定対象面にレーザを当てて反射光を受光することにより行なうことができる。続くステップＳ４１０は、手先に重りｍが取り付けられていない状態での第２アーム２２のたわみ角度である初期たわみ角度を設定する。第２アーム２２の初期たわみ量は、上述したステップＳ３２０と同様に、重心位置×アーム質量をｘ軸とし、第２アーム２２のたわみ量（Ｚ方向変位Ｚ_d2）をｙ軸として重りｍ毎の両者の組み合わせデータをｘｙグラフ上にプロットし、プロットした点を結んだ直線の延長線におけるｙ切片を求めることにより導出することができる。第２アーム２２の初期たわみ角度は、導出した初期たわみ量に対して逆正接関数を適用することにより求めることができる。ステップＳ４２０は、重りｍ毎に測定したＺ方向変位Ｚ_d2（たわみ量）に対してそれぞれ逆正接関数を適用して各角度変位を求め、求めた各角度変位をステップＳ４１０で設定した初期たわみ角度に加えることにより、図２５に示すように重りｍ（手先質量）毎の第２アーム２２のたわみ角度θ_d2の実測値を導出する。この第２アーム２２のたわみ角度θ_d2は、Ｘ₁軸を基準とした角度であり、第１アーム２１のたわみ角度θ_d1が重畳しているため、ステップＳ３４０で求めた第１アーム２１のたわみ角度ｄ１の実測値を減算することで、図１０に示すように、Ｘ₂軸を基準とした第２アーム２２のたわみ角度θ_d2を得ることができる。

　ステップＳ４３０は、第２アーム２２およびシャフト２３の質量と手先（重りｍ）の質量とから第２関節軸Ｊ２以降（スカラアーム２０の第２関節軸Ｊ２から先端側）のアーム部（重りｍを含む）の重心位置Ｌ_d2を算出する。続くステップＳ４４０は、第２アーム２２のたわみモーメントＩｄ２を算出する。たわみモーメントＩｄ２は、図２６に示すように、第２関節軸Ｊ２の角度θ_d2とは無関係に、重心位置Ｌ_d2（第２関節軸Ｊ２の中心から重心までの距離）とアーム部の質量とによって定まる。すなわち、たわみモーメントＩｄ２は、アーム部の質量をＭとし、重力加速度をｇとしたとき、次式（１８）により求めることができる。ステップＳ４５０は、たわみモーメントＩｄ２をたわみ剛性Ｋ_d2で除した次式（１９）により第２アーム２２のたわみ角度θ_d2とアーム部の質量Ｍとの関係を表わす第２アーム２２のたわみモデル式を作成する。ステップＳ４６０は、ステップＳ４２０で求めた重りｍ毎の第２アーム２２のたわみ角度θ_d2の実測値からたわみ剛性Ｋ_d2を最適化する。このステップＳ４６０は、ねじれ剛性Ｋ_tやたわみ剛性Ｋ_d1と同様に、最急降下法やニュートンラフソン法などの最適化問題を解くための種々のアルゴリズムを採用することができる。

　ここで、実施形態の主要な要素と請求の範囲に記載した本開示の主要な要素との対応関係について説明する。即ち、本実施形態では、第１アーム２１が第１アームに相当し、第１関節軸Ｊ１が第１軸に相当し、第２アーム２２が第２アームに相当し、第２関節軸Ｊ２が第２軸に相当し、シャフト２３がシャフトに相当し、第３関節軸Ｊ３が第３軸に相当し、図４のロボット制御処理のＳ１８０，Ｓ１９０の処理を実行する制御装置７０のＣＰＵ７１が制御部に相当し、記憶装置７４が記憶部に相当し、図４のロボット制御処理のＳ１００～Ｓ１７０の処理を実行するＣＰＵ７１が補正部に相当する。また、ワーク保持部２４がワーク保持部に相当する。

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

　例えば、上述した実施形態では、制御装置７０は、記憶装置７４に予め記憶されたねじれモデル式を用いて第２関節軸Ｊ２の角度θ_J2に基づく第１アーム２１のねじれ角度θ_tを算出し、算出したねじれ角度θ_tに基づいて手先の位置ずれを補正するものとした。しかし、制御装置７０は、第１アーム２１のねじれ角度θ_tと第２関節軸Ｊ２の角度θ_J2との関係あるいは第１アーム２１のねじれ角度θ_tに基づくねじれ補正値と第２関節軸Ｊ２の角度θ_J2との関係を表わすマップを予め記憶しておき、第２関節軸Ｊ２の角度θ_J2に基づいて当該マップからねじれ補正値を導出するようにしてもよい。

　上述した実施形態では、制御装置７０は、記憶装置７４に予め記憶された第１アーム２１のたわみモデル式を用いて第２関節軸Ｊ２の角度θ_J2に基づく第１アーム２１のたわみ角度θ_d1を算出し、算出したたわみ角度θ_d1に基づいて手先の位置ずれを補正するものとした。しかし、制御装置７０は、第１アーム２１のたわみ角度θ_d1と第２関節軸Ｊ２の角度θ_J2との関係あるいは第１アーム２１のたわみ角度θ_d1に基づくたわみ補正値と第２関節軸Ｊ２の角度θ_J2との関係を表わすマップを予め記憶しておき、第２関節軸Ｊ２の角度θ_J2に基づいて当該マップからたわみ補正値を導出するようにしてもよい。更に、制御装置７０は、記憶装置７４に予め記憶されたたわみモデル式を用いて手先質量ｍ（ワークＷの重さ）に基づく第１アーム２１，第２アーム２２のたわみ角度θ_d1，θ_d2を算出し、算出したたわみ角度θ_d1，θ_d2に基づくたわみ補正値により手先の位置ずれを補正するものとした。しかし、制御装置７０は、第１アーム２１，第２アーム２２のたわみ角度θ_d1，θ_d2と手先質量ｍ（ワークＷの重さ）との関係あるいは第１アーム２１，第２アーム２２のたわみ角度θ_d1，θ_d2に基づくたわみ補正値と手先質量ｍ（ワークＷの重さ）との関係を表わすマップを予め記憶しておき、手先質量ｍに基づいて当該マップからたわみ補正値を導出するようにしてもよい。

　以上説明したように、本開示のスカラロボットシステムは、基台と、前記基台に設けられ、第１軸周りに回動する第１アームと、前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、前記第１アームと前記第２アームと前記シャフトとの動作を制御する制御部と、前記第２アームの回動位置に対応する前記第１アームのアーム周りの回転角に基づく、前記第１アームのねじれ補正係数を記憶する記憶部と、前記ねじれ補正係数を用いて前記シャフト先端の水平方向の位置を補正する補正部と、を備えることを要旨とする。

　こうした本開示のスカラロボットシステムにおいて、前記ねじれ補正係数を用いて前記シャフトの移動量を補正する前記補正部を備えるものとしてもよい。

　また、本開示のスカラロボットシステムにおいて、前記第２アームの回動位置に対応する前記第１アームの第１たわみ量と前記第２アームの第２たわみ量とを記憶する前記記憶部と、前記第１たわみ量と前記第２たわみ量とを用いて前記補正を行う前記補正部と、を備えるものとしてもよい。こうすれば、第２アームの回動位置に拘わらず第１アームのたわみに基づくシャフト先端の位置ずれを良好に補正することができる。この態様の本開示のスカラロボットシステムにおいて、前記シャフトはワークを保持するワーク保持部を有し、前記ワークの重さに対応する前記ねじれ補正係数と前記ワークの重さに対応する前記第１たわみ量および前記第２たわみ量とを記憶する前記記憶部を備えるものとしてもよい。こうすれば、重さの異なる多様なワークをワーク保持部で保持する場合であっても、手先の位置ずれを良好に補正することができる。

更に、本開示のスカラロボットシステムにおいて、前記ねじれ補正係数のモデル式と前記第１たわみ量のモデル式と前記第２たわみ量のモデル式とを記憶する前記記憶部と、前記ねじれ補正係数のモデル式と前記第１たわみ量のモデル式と前記第２たわみ量のモデル式とを用いて前記補正を行う前記補正部と、を備えるものとしてもよい。こうすれば、マップを用いて補正を行なうものに比して、必要な記憶部の記憶容量を低減することができる。

　なお、本開示は、スカラロボットシステムの形態としたが、アームの位置ずれ補正方法の形態としてもよい。

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

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

Claims

　基台と、
　前記基台に設けられ、第１軸周りに回動する第１アームと、
　前記第１アームに設けられ、前記第１軸と平行な第２軸周りに回動する第２アームと、
　前記第２アームに対して前記第２軸と平行な第３軸の軸方向に移動するシャフトと、
　前記第１アームと前記第２アームと前記シャフトとの動作を制御する制御部と、
　前記第２アームの回動位置に対応する前記第１アームのアーム周りの回転角に基づく、前記第１アームのねじれ補正係数を記憶する記憶部と、
　前記ねじれ補正係数を用いて前記シャフト先端の水平方向の位置を補正する補正部と、
　を備えるスカラロボットシステム。
　請求項１に記載のスカラロボットシステムであって、
　前記ねじれ補正係数を用いて前記シャフトの移動量を補正する前記補正部
　を備えるスカラロボットシステム。
　請求項１または２に記載のスカラロボットシステムであって、
　前記第２アームの回動位置に対応する前記第１アームの第１たわみ量と前記第２アームの第２たわみ量とを記憶する前記記憶部と、
　前記第１たわみ量と前記第２たわみ量とを用いて前記補正を行う前記補正部と、
　を備えるスカラロボットシステム。
　請求項３に記載のスカラロボットシステムであって、
　前記シャフトはワークを保持するワーク保持部を有し、
　前記ワークの重さに対応する前記ねじれ補正係数と前記ワークの重さに対応する前記第１たわみ量および前記第２たわみ量とを記憶する前記記憶部
　を備えるスカラロボットシステム。
　請求項１または２に記載のスカラロボットシステムであって、
　前記ねじれ補正係数のモデル式と前記第１たわみ量のモデル式と前記第２たわみ量のモデル式とを記憶する前記記憶部と、
　前記ねじれ補正係数のモデル式と前記第１たわみ量のモデル式と前記第２たわみ量のモデル式とを用いて前記補正を行う前記補正部と、
　を備えるスカラロボットシステム。