JP5914831B2 - 多関節ロボットの機構誤差の補正方法 - Google Patents

Description

本発明は、多関節ロボットの機構誤差を補正する技術に関する。

多関節ロボットの機構誤差の補正方法として、穴と穴の距離が予め測定された複数の穴を有するプレートを治具として用いた方法がある（例えば、特許文献１参照）。

このような従来の多関節ロボットの機構誤差の補正方法では、多関節ロボットの手先をプレートの穴に嵌合させることで拘束し、拘束された状態の多関節ロボットの関節の角度に基づいてこの多関節ロボットの機構誤差を推定して補正する。

特許第３３８８０１６号公報

しかしながら、従来の機構誤差の補正方法では、プレートの状態や穴の精度によっては、機構誤差の補正精度が悪化する可能性があるという課題がある。

本発明は、このような課題に鑑みてなされたものであり、機構誤差の補正精度が悪化しない多関節ロボットの機構誤差の補正を提供することを目的とする。

上記目的を達成するために、本発明の一態様に係る多関節ロボットの機構誤差の補正方法は、６自由度を有する多関節ロボットの機構誤差を補正する多関節ロボットの機構誤差の補正方法であって、平面治具の表面の少なくとも４箇所の異なる位置それぞれにおいて、前記多関節ロボットの手先における平面接触部を拘束した状態で前記多関節ロボットの関節の角度を取得する取得工程と、前記取得工程において取得された前記多関節ロボットの関節の角度を用いて、前記多関節ロボットの機構誤差を算出して補正する補正工程と、を有し、前記取得工程は、前記平面治具の表面の少なくとも４箇所の異なる位置それぞれにおいて、前記手先における前記平面接触部の姿勢が前記平面治具の表面の法線と同じになるように前記手先を拘束した状態で、前記多関節ロボットの関節の角度を取得することを特徴とする。

本発明の多関節ロボットの機構誤差の補正によれば、機構誤差の補正精度の悪化を防ぐことができる。

図１は本発明の実施の形態１に係る多関節ロボットの姿勢を示す図であって、図１の（ａ）は平面治具の第一の位置における姿勢を示す図であり、図１の（ｂ）は平面治具の第二の位置における姿勢を示す図であり、図１の（ｃ）は平面治具の第三の位置における姿勢を示す図であり、図１の（ｄ）は平面治具の第四の位置における姿勢を示す図である。 図２は本発明の実施の形態１に係る多関節ロボットの制御部を示すブロック図である。 図３は本発明の実施の形態１に係る多関節ロボットの機構誤差の補正方法を示すフローチャートである。 図４は本発明の実施の形態１に係る多関節ロボットの機構誤差の補正方法の具体的な一例を示すフローチャートである。 図５は本発明の実施の形態２に係る多関節ロボットの姿勢を示す図であって、図５の（ａ）は球面治具の第一の位置における姿勢を示す図であり、図５の（ｂ）は球面治具の第二の位置における姿勢を示す図であり、図５の（ｃ）は球面治具の第三の位置における姿勢を示す図であり、図５の（ｄ）は球面治具の第四の位置における姿勢を示す図である。 図６は本発明の実施の形態３に係る多関節ロボットの姿勢を示す図であって、図６の（ａ）は平面治具の第一の位置における姿勢を示す図であり、図６の（ｂ）は平面治具の第二の位置における姿勢を示す図であり、図６の（ｃ）は平面治具の第三の位置における姿勢を示す図であり、図６の（ｄ）は平面治具の第四の位置における姿勢を示す図である。 図７は従来の多関節ロボットの姿勢を示す図である。

（本発明の基礎となった知見）
本発明者は、「背景技術」の欄において記載した、多関節ロボットの機構誤差の補正方法に関し、以下の問題が生じることを見出した。

図７に示すように、従来の多関節ロボット１００は、固定部１０２、複数の関節１０１、アーム１０４、１０５、手先１０３を備える。多関節ロボット１００の固定部１０２と手先１０３とは、複数の関節１０１同士をつなぐアーム１０４、１０５により接続されている。この多関節ロボット１００の手先１０３は、各関節１０１の角度が変わることで移動する。

そして、多関節ロボット１００の手先１０３に取り付けた部材１１０の先端１１１をプレート１１２の穴１１３に嵌合させることで拘束し、拘束された状態の多関節ロボット１００の関節１０１の角度に基づいてこの多関節ロボット１００の機構誤差を推定して補正する。

しかしながら、従来の機構誤差の補正方法では、相互間の距離が正確に計測された複数の穴１１３を有するプレート１１２を用いる必要がある。このため、例えば、室温変動などによりプレート１１２の温度が変動し、プレート１１２が膨張又は収縮して複数の穴１１３間の距離が変動した場合、機構誤差の補正精度が悪化するという問題がある。

このような問題を解決するために、本発明の一態様に係る多関節ロボットの機構誤差の補正方法は、６自由度を有する多関節ロボットの機構誤差を補正する多関節ロボットの機構誤差の補正方法であって、平面治具又は球面治具の表面の少なくとも４箇所の異なる位置それぞれにおいて前記多関節ロボットの手先を拘束した状態で、前記多関節ロボットの関節の角度を取得する取得工程と、前記取得工程において取得された前記多関節ロボットの関節の角度を用いて、前記多関節ロボットの機構誤差を算出して補正する補正工程と、を有することを特徴とする。

これによれば、機構誤差の補正にジグの表面の形状を利用しているため、ジグの温度が変動しても、ジグの表面の形状は維持される。このため、機構誤差の補正精度の悪化を防ぐことができる。

以下、本発明の一態様に係る実施の形態について、図面を参照しながら説明する。なお、以下で説明する実施の形態は、いずれも本発明の一具体例を示すものである。以下の実施の形態で示される数値、形状、構成要素、構成要素の配置位置及び接続形態、ステップ、ステップの順序などは、本発明の一例である。また、以下の実施の形態における構成要素のうち、最上位概念を示す独立請求項に記載されていない構成要素については、任意の構成要素として説明される。また、以下の説明において、同じ構成には同じ符号を付して、適宜説明を省略している。

（実施の形態１）
図１は本発明の実施の形態１に係る多関節ロボットの姿勢を示す図である。図１の（ａ）は平面治具７の第一の位置５１における姿勢を示す図であり、図１の（ｂ）は平面治具７の第二の位置５２における姿勢を示す図であり、図１の（ｃ）は平面治具７の第三の位置５３における姿勢を示す図であり、図１の（ｄ）は平面治具７の第四の位置５４における姿勢を示す図である。なお、第一の位置５１、第二の位置５２、第三の位置５３及び第四の位置５４は、平面治具７の表面において、互いに異なる位置である。

本発明の実施の形態１における多関節ロボット３１は、図１に示すように、パラレルリンクロボットである。図１を用いて、本実施の形態１の多関節ロボット３１の基本構成を説明する。

図１の（ａ）に示すように、本実施の形態１の多関節ロボット３１は、基台２１と、手先２と、基台２１及び手先２を接続する６組のリンク３及びアーム４と、駆動源の一例としての６つのモータ５と、これらを制御する制御部４０と、を備える。

基台２１とアーム４とは、それぞれ関節１ａにより接続されている。リンク３とアーム４とは、それぞれ関節１ｂにより接続されている。リンク３と手先２とは、それぞれ関節１ｃにより接続されている。関節１ａは回り対偶で構成され、関節１ｂ、１ｃは球面対偶で構成されている。

本実施の形態１では、基台２１と手先２とは、６つの動力伝達部によって接続されている。ここで、動力伝達部とは、主にリンク３及びアーム４で構成されて、基台２１と手先２との間で動力を伝達するための部材である。また、本実施の形態１では、図１に示すように、基台２１及び手先２は、正六角形の板状部材である。アーム４は、基台２１に固定されたモータ５の回転軸を中心として回転可能である。各モータ５は、関節角度検出手段の一例としてのエンコーダ６をそれぞれ備えている。多関節ロボット３１は、エンコーダ６を用いることで、アーム４の角度を検出することができる。手先２には、平面接触部８が固定されている。平面接触部８は、平面治具７の表面に押し付けられたときに、手先２の姿勢を安定させるための部材である。具体的には、平面接触部８は、平面治具７の表面に押し付けられたときに、手先２の姿勢を平面治具７の表面に対して常に同じにするための部材である。本実施の形態１の平面接触部８は、底面が正方形の直方体である。なお、平面接触部８は、天面と底面とが平行であり、手先２が平面治具７の平面である表面に対して平行に面接触するものであれば、その形状は限定されない。平面接触部８は、接触部の一例である。また、平面治具７は、治具の一例であり、その表面が平面で構成されている。

本実施の形態１では、６つの動力伝達部は、平行に配置された２つを１組として、３組に分けられる。すなわち、本実施の形態１では、３組の動力伝達部により、基台２１と手先２とが接続されている。また、基台２１及び手先２それぞれにおいて、３組の動力伝達部は、１２０°の角度で均等な間隔で接続されている。

以上の構成により、本実施の形態１の多関節ロボット３１は、パラレルリンクロボットとして機能する。また、本実施の形態１の多関節ロボット３１の手先２は、６自由度の動作を行うことができる。

本実施の形態１は、このような多関節ロボット３１の機構誤差の補正を、平面治具７を用いて行うことを可能とする。具体的には、平面治具７の表面において異なる位置５１〜５４の４箇所に多関節ロボット３１の手先２に固定された平面接触部８を押し付けて接触させ、４箇所の異なる位置５１〜５４における法線方向に対する手先２の角度が同一となる姿勢に制御し、それぞれの姿勢での関節角度に基づいて多関節ロボット３１の機構誤差を測定して補正する。すなわち、本実施の形態１では、機構誤差を測定する際に、平面治具７の４箇所の異なる位置５１〜５４において、手先２の姿勢を平面治具７の表面に対して常に同じ姿勢にすると共に、手先２の位置及び姿勢を拘束している。なお、本実施の形態１に係る平面治具７は、その表面が平面な部材であり、多関節ロボット３１の基台２１との位置関係が変化しないように固定されている。本実施の形態１における機構誤差の補正方法の詳しい内容については、後述する。

図２は、本実施の形態１の多関節ロボットの機構誤差の補正方法の各処理を行う制御部４０を示すブロック図である。

図２に示すように、制御部４０は、取得部４１と、補正部４２とを有する。取得部４１は、平面治具７の表面の４箇所の位置５１〜５４のそれぞれにおいて、手先２が平面治具７により拘束された状態で、エンコーダ６の値を取得する。補正部４２は、取得部４１により取得されたエンコーダ６の値を用いて、多関節ロボット３１の機構誤差を算出し、補正する。

図１に示す多関節ロボット３１の動作について、図３を用いて説明する。図３は、本実施の形態１における多関節ロボット３１の機構誤差の補正方法を示すフローチャートである。

まず、取得部４１は、平面治具７の表面の４箇所の異なる位置５１〜５４のそれぞれにおいて、多関節ロボット３１の手先２を拘束した状態で、多関節ロボット３１の関節１ａの角度を取得する（ステップＳ０１：取得工程）。

次に、補正部４２は、ステップＳ０１の取得工程において取得された多関節ロボット３１の関節１ａの角度を用いて、多関節ロボット３１の機構誤差を算出して補正する（ステップＳ０２：補正工程）。

図４は、本実施の形態１における多関節ロボット３１の機構誤差の補正方法の具体的な一例を示すフローチャートである。

まず、取得部４１は、インデックスｉを設定し、このインデックスｉをｉ＝１に初期化する（ステップＳ１１）。インデックスｉは、本実施の形態１では、ｉ＝１が第一の位置５１に定義され、ｉ＝２が第二の位置５２に定義され、ｉ＝３が第三の位置５３に定義され、ｉ＝４が第四の位置５４に定義されている。また、本実施の形態１では、少なくとも４箇所の異なる位置５１〜５４で多関節ロボット３１の関節１ａの角度を取得すればよいため、ｉ＝４までを用いた場合について説明する。

続いて、取得部４１は、インデックスｉ＝１で定義された第一の位置５１（図１の（ａ）に示す平面治具７の表面の位置）において、手先２の平面接触部８を平面治具７の表面に押し付けて、手先２の角度が平面治具７の法線方向と同じになるように手先２の姿勢を拘束する。そして、その状態で、取得部４１は、エンコーダ６の値θｉを取得して、記憶する（ステップＳ１２）。

次に、取得部４１は、インデックスｉが４以上であるか否か、すなわちｉ≧４を判定する（ステップＳ１３）。

インデックスｉが４未満の場合（ステップＳ１３のＮｏ）、インデックスｉを１増やして、ｉ＝ｉ＋１とする（ステップＳ１４）。その後、ステップＳ１２、Ｓ１３を実行する。

つまり、ステップＳ１２〜ステップＳ１４を繰り返すことにより、取得部４１は、平面治具７の表面の４つの任意の異なる位置５１〜５４のそれぞれにおいて、手先２の角度が平面治具７の表面の法線方向と同じになるように手先２の姿勢が拘束された状態で、その状態におけるエンコーダ６の値θ１〜θ４を取得して、記憶する。このステップＳ１１〜ステップＳ１５が、ステップＳ０１の取得工程の一例である。

そして、インデックスｉが４以上の場合（ステップＳ１３のＹｅｓ）、４箇所の異なる位置５１〜５４のそれぞれで取得した４つのエンコーダ６の値θ１〜θ４を用いて、以下の計算を行って機構誤差を算出し、算出された機構誤差を補正する（ステップＳ１５）。このステップＳ１５が、ステップＳ０２の補正工程の一例である。

ステップＳ１５について、説明する。ステップＳ１５は、取得したエンコーダ６の値θｉから、本実施の形態１の機構誤差を算出する計算である。本実施の形態１の機構誤差の一例は、リンク３の長さＬの誤差である。本実施の形態１の多関節ロボット３１において、アーム４の角度と手先２の位置及び姿勢との関係を表す運動学方程式は、（式１）であり、Ｐ_ｉ、θ_ｉ、Ｌの変数で表される関数Ｆ１として導かれる。

（式１）は６次元の式である。ここで、Ｐ_ｉ＝（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ，θ_ｘｉ，θ_ｙｉ，θ_ｚｉ）は、手先２の位置及び姿勢を表す６次元ベクトルである。なお、Ｆ１は、パラレルリンクロボットの一般的な運動学方程式であるため、詳細な説明は省略する。また、Ｐ_ｉは未知の値であり、今後の計算により求める。

θ_ｉ＝（θ１_ｉ，θ２_ｉ，θ３_ｉ，θ４_ｉ，θ５_ｉ，θ６_ｉ）は、多関節ロボット３１のそれぞれのモータ５のエンコーダ６の値である６次元ベクトルである。つまり、θ_ｉは、平面治具７の４箇所の異なる位置５１〜５４において手先２を拘束することにより取得された値である。Ｌ＝（Ｌ１，Ｌ２，Ｌ３，Ｌ４，Ｌ５，Ｌ６）は、補正対象の６本のリンク３の長さを示しており、６次元ベクトルである。

本実施の形態１では、リンク３の長さＬは、未知の値である。本実施の形態１では、今後の計算により、このリンク３の長さＬの値を求めることで、リンク３の長さＬの誤差を算出して補正する。なお、本実施の形態１では、リンク３の長さＬ以外の多関節ロボット３１の機構寸法は、設計値と同じであると仮定している。以下の説明により求めるリンク３の実際の長さＬは、計測値の一例である。

ここで、４箇所の異なる位置５１〜５４での多関節ロボット３１の手先２の姿勢が、それぞれの位置５１〜５４において同一の方向（平面治具７の法線方向）に拘束された状態にあるため、次の（式２）に示すとおり、Ｐ_ｉ、α、βの変数で表される関数Ｎが導かれる。つまり、実施の形態１において手先２の姿勢が拘束状態であることを示す関係は、（式２）の関数Ｎとして導かれる。

（式２）は２次元の式であり、Ｐ_ｉは（式１）と同じである。また、α及びβは、平面治具７の法線の方向を表す値である。

なお、ここで、多関節ロボット３１の手先２の座標系から固定座標系への変換行列は、（式３）のように表される。

（式３）において、Ｘ_ｈ、Ｙ_ｈ及びＺ_ｈは手先２の座標系における位置を表し、Ｘ_ｂ、Ｙ_ｂ及びＺ_ｂは固定座標系に変換した後の位置を表す。ここで、所定の条件で単位ベクトル変換を行うことを考える場合、次の（式４）を導くことができる。所定の条件とは、多関節ロボット３１の手先２の座標系におけるＺ方向の単位ベクトルが、平面治具７の表面の法線方向に拘束されており、手先座標系のＺ軸回りの回転が、拘束されていないという条件である。

ここで、α、β、γは、単位ベクトルであるため、α^２＋β^２＋γ^２＝１の関係が成り立つ。この関係を用いることで、（式４）においてγを消去することができ、次の（式５）の２式を導くことができる。

（式５）において、α、βは未知の値であり、今後の計算により値を求める。

さらに、４箇所の異なる位置５１〜５４での多関節ロボット３１の手先２は、平面治具７で同一平面上にあるため、次の（式６）が導かれる。

（式６）は１次元の式であり、Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉは、Ｐ_ｉの位置を示すＸ軸方向、Ｙ軸方向、及びＺ軸方向の３方向の成分である。ａ，ｂ，ｃ，ｄは、３次元空間上で平面を表す未知の値であり、今後の計算により求める。

なお、（式５）は、平面治具７の４箇所の異なる位置５１〜５４における法線方向の姿勢を示す関係式であり、（式６）は、平面治具７の表面である平面の４箇所の異なる位置５１〜５４を示す関係式である。

インデックスｉがｉ＝１〜４の４つの場合において各エンコーダ６の値が得られるため、（式１）、（式５）及び（式６）について４個ずつの式が得られる。つまり、（式１）の６次元式が４個であり、（式５）の２次元式が４個であり、（式６）の１次元式が４個であることから、４つのエンコーダθｉに基づいて３６個（６次元×４個＋２次元×４個＋１次元×４個）の連立方程式が得られる。未知数は、Ｐ_ｉを構成するＸ_ｉ、Ｙ_ｉ、Ｚ_ｉ、θ_ｘｉ、θ_ｙｉ及びθ_ｚｉ（ｉ＝１〜４）の２４個（６次元×４個）と、Ｌを構成するＬ１、Ｌ２、Ｌ３、Ｌ４、Ｌ５及びＬ６の６個と、α、β、ａ、ｂ、ｃ、及びｄの６個と、の計３６個である。上記の３６個の連立方程式を例えばニュートン法などによる数値解析により計算することで、３６個の未知のパラメータを求めることができる。これによりリンク３の長さＬの計測値が求められる。求めたリンク３の長さＬの計測値をリンク３の設計値と比較することで、リンク３の長さＬの誤差を算出することができる。そして、このリンク３の長さＬの誤差を補正することで、多関節ロボット３１の機構誤差の補正を行うことができる。

本実施の形態１によれば、多関節ロボット３１の機構誤差の補正に平面治具７の表面を用いているため、プレートの穴を用いる従来の機構誤差の補正の場合と比較して、温度変化に対する状態の変化が生じにくい。これは、平面形状を有する平面治具７が温度変化により膨張又は収縮した場合、従来の機構誤差の補正に用いた穴間の距離は変化するが、本実施の形態１の機構誤差の補正に用いた表面の形状は変化しにくいためである。そのため、本実施の形態１の多関節ロボットの機構誤差の補正方法を用いることで、温度変化に対し精度が低下する可能性が低い多関節ロボットの機構誤差の補正を、行うことができる。

なお、手先２の平面接触部８を押し付ける平面治具７の表面における４箇所の位置は、任意の異なる位置でよいが、それぞれの位置で多関節ロボット３１の姿勢が変化する４箇所である必要がある。なお、本実施の形態１の多関節ロボット３１はパラレルリンクロボットであるため、平面治具７の表面における４箇所の位置は、ロボットの中心からの距離が異なる位置であることが望ましい。また、この４箇所の異なる位置５１〜５４は、それぞれの間の距離がおよそ均等に離れた位置としてもよい。これらにより、機構誤差の補正精度をより高めることができる。

なお、「多関節ロボット３１の姿勢が変化する」とは、１つ以上の関節１ａ，１ｂ，１ｃの角度が変化することを指す。

なお、４箇所の異なる位置５１〜５４は、具体的には、多関節ロボット３１の可動範囲内で互いの距離が可能な限り長くなるようにした位置であって、４箇所を結んだ形状が正方形の形状としてもよい。

なお、本実施の形態１において、平面治具７の表面の形状を基台２１に対して平行な平面で説明したが、基台２１に対して、傾いた平面であっても上記の（式１）、（式５）及び（式６）を用いた機構誤差の補正方法を適用できる。

また、本実施の形態１に係る多関節ロボットの機構誤差の補正方法では、平面治具７の表面における４箇所の異なる位置５１〜５４において手先２を拘束した状態で測定することで解が求めることができるが、平面治具７に拘束される位置が４箇所より少ない場合は一意的に解を求めることができない。

なお、４箇所よりも多い５箇所以上の位置において手先２を平面治具７の表面に拘束した状態で測定することにより得られた値に対して最小二乗法や平均などを用いることで、さらに機構誤差の補正精度を高めることができる。

（実施の形態２）
図５は、本発明の実施の形態２に係る多関節ロボット３２の姿勢を示す図である。図５の（ａ）は球面治具９の第一の位置６１における姿勢を示す図であり、図５の（ｂ）は球面治具９の第二の位置６２における姿勢を示す図であり、図５の（ｃ）は球面治具９の第三の位置６３における姿勢を示す図であり、図５の（ｄ）は球面治具９の第四の位置６４における姿勢を示す図である。第一の位置６１、第二の位置６２、第三の位置６３及び第四の位置６４は、球面治具９の表面のそれぞれ互いに異なる位置である。

図５に示すように、本実施の形態２に係る多関節ロボット３２の構成要素は、図１の平面接触部８を球面接触部１０に置き換えた以外は図１の多関節ロボット３１と同じであるため、適宜説明を省略する。

また、実施の形態２に係る多関節ロボット３２の機構誤差の補正は、表面が球面である球面治具９を用いて行う。球面治具９は、多関節ロボット３２の基台２１との位置関係が変化しないように固定されている。

手先２に固定された球面接触部１０は、球面治具９と同じ半径の凹面（凹型の曲面）を有する。ここで、球面接触部１０の凹面は球面治具９と隙間なく接触できる面である。球面接触部１０は、最低限、球面治具９に接触したときの姿勢及び位置が一意的に決まるような形状であればよい。球面接触部１０は、例えば中空の円筒形状であってもよいが、球面治具９と確実に接触させるためには、図５に図示したような凹型の曲面を有する球面接触部１０が望ましい。球面接触部１０は、接触部の一例である。球面治具９は、治具の一例であり、その表面が球面で構成されている。

このような多関節ロボット３２の機構誤差の補正を行う場合、球面治具９の表面の４箇所の異なる位置６１〜６４に手先２の球面接触部１０を接触させて、４箇所の異なる位置６１〜６４における法線方向に対する手先２の角度がそれぞれ同一となるように、球面治具９により手先２の姿勢が拘束された状態とする。具体的には、図５の（ａ）、（ｂ）、（ｃ）及び（ｄ）に示すように、４箇所の異なる位置６１〜６４において、手先２の球面接触部１０を球面治具９の表面である球面に押し付けることにより、手先２の手先座標系での球面接触部１０の球面の中心位置が４箇所の異なる位置６１〜６４のそれぞれで球面治具９の球面の中心と一致するように拘束する。つまり、手先２は、手先座標系における所定の方向に延びる軸（例えばＺ軸）が常に球面治具９の球面の中心と一致するように、球面接触部１０により固定されることになる。このようにして、手先２の球面接触部１０が球面治具９により拘束された後、４箇所の異なる位置６１〜６４のそれぞれにおいて、エンコーダ６の値を取得して、記憶する。なお、本実施の形態２に係る多関節ロボット３２の機構誤差の補正方法の処理は、計算式以外が実施の形態１に係る多関節ロボットの機構誤差の補正方法の処理と同様であるため、処理の説明は省略する。

次に、取得したエンコーダ６の値からの計算の内容を説明する。図５に示す本実施の形態２の多関節ロボット３２において、アーム４の角度と手先２の位置及び姿勢との関係を表す運動学方程式は、図１に示す実施の形態１の多関節ロボット３１の場合と同様に、前述の（式１）である。ここで、Ｐ_ｉ、θ_ｉ及びＬは実施の形態１の場合と同じであり、インデックスｉ＝１〜４である。

本実施の形態２では、４箇所の異なる位置６１〜６４のそれぞれにおいて、手先２の球面接触部１０の球面の中心が球面治具９の中心と一致することから、（式７）のように、Ｐ_ｉ、Ｔ及びＢの変数で表される関数Ｇが導かれる。つまり、実施の形態２において手先２の姿勢が拘束状態であることを示す関係は、（式７）の関数Ｇとして導かれる。

（式７）は３次元の式である。Ｔ＝（Ｔ_ｘ，Ｔ_ｙ，Ｔ_ｚ）は、手先２の球面接触部１０の球面の中心の位置を表す３次元ベクトルである。Ｔは未知の値であり、今後の計算により求められる。Ｂ＝（Ｂｘ，Ｂｙ，Ｂｚ）は、多関節ロボット３２の固定座標系における球面治具９の中心座標を表す３次元ベクトルである。Ｂは未知の値であり、今後の計算により求められる。

ここで、（式７）のＧを具体的に表すと、次の（式８）のように表される。（式８）は、手先座標系におけるＴ＝（Ｔ_ｘ，Ｔ_ｙ，Ｔ_ｚ）を固定座標系に変換したＢ＝（Ｂ_ｘ，Ｂ_ｙ，Ｂ_ｚ）が、多関節ロボット３２の手先２の位置及び姿勢によらず一定であるという関係に基づいて表される。

インデックスｉがｉ＝１〜４の４つの場合、（式１）及び（式８）について４個ずつの式が得られる。本実施の形態２では、（式１）の６次元式が４個であり、（式８）の３次元式が４個であるため、３６個（６次元×４個＋３次元×４個）の連立方程式が得られる。未知数は、Ｐ_ｉを構成するＸ_ｉ、Ｙ_ｉ、Ｚ_ｉ、θ_ｘｉ、θ_ｙｉ及びθ_ｚｉ（ｉ＝１〜４）の２４個（６次元×４個）と、Ｌを構成するＬ１、Ｌ２、Ｌ３、Ｌ４、Ｌ５及びＬ６の６個と、Ｔを構成するＴ_ｘ、Ｔ_ｙ及びＴ_ｚの３個と、Ｂを構成するＢ_ｘ、Ｂ_ｙ及びＢ_ｚの３個と、の計３６個である。上記の３６個の連立方程式を例えばニュートン法などによる数値解析により計算することで、３６個の未知のパラメータを求めることができる。これによりリンク３の長さＬの計測値を求めることができる。求めたリンク３の長さＬをリンク３の設計値と比較することで、リンク３の長さＬの誤差を算出することができる。そして、このリンク３の長さＬの誤差を補正することで、多関節ロボット３２の機構誤差の補正を行うことができる。

本実施の形態２に係る多関節ロボットの機構誤差の補正方法によれば、多関節ロボット３２の手先２を球面治具９の表面の４箇所の異なる位置６１〜６４に押し付けることにより、手先２が球面治具９の表面に拘束された状態とし、その状態におけるエンコーダ６の値を用いることで、多関節ロボット３２の機構誤差（リンク３の長さＬについて、設計値と求められた値との差）を計算して補正することができる。本実施の形態２では、多関節ロボット３２の機構誤差の補正を行うために、温度が変化しても変形しにくい球面形状を有する球面治具９を用いている。そのため、温度変化に対し精度が低下する可能性が低い多関節ロボットの機構誤差の補正を行うことができる。

なお、手先２を押し付ける球面治具９の表面における４箇所の位置は、任意の異なる位置であればよいが、それぞれの位置で多関節ロボット３２の姿勢が互いに変化する４箇所である必要がある。当該４箇所の異なる位置６１〜６４はそれぞれの間の距離がおよそ均等に離れており、かつ、当該４箇所の異なる位置６１〜６４における手先２の姿勢の角度がそれぞれ可能な限り異なることが、機構誤差の補正精度を高める上で望ましい。

また、本実施の形態２に係る多関節ロボットの機構誤差の補正方法では、球面治具９の表面における４箇所の異なる位置６１〜６４において手先２を拘束した状態で測定することで解が求めることができるが、球面治具９に拘束される位置が４箇所より少ない場合は一意的に解を求めることができない。なお、球面治具９の５箇所以上の位置において、手先２を球面治具９の表面に拘束した状態で測定することにより得られた値を最小二乗法や平均などを用いることで、さらに構成精度を高めることができる。

実施の形態２に係る多関節ロボットの機構誤差の補正方法は、実施の形態１のような大きい平面の平面治具７を設置することが困難な装置に組み込まれた多関節ロボット３２の補正を行う場合に有効である。

（実施の形態３）
図６は、本発明の実施の形態３に係る多関節ロボット３３の姿勢を示す図であって、図６の（ａ）は平面治具７の第一の位置７１における姿勢を示す図であり、図６の（ｂ）は平面治具７の第二の位置７２における姿勢を示す図であり、図６の（ｃ）は平面治具７の第三の位置７３における姿勢を示す図であり、図６の（ｄ）は平面治具７の第四の位置７４における姿勢を示す図である。なお、第一の位置７１、第二の位置７２、第三の位置７３及び第四の位置７４は、平面治具７の表面上のそれぞれ互いに異なる位置である。

図６に示すように、実施の形態３に係る多関節ロボット３３は、実施の形態１に係る多関節ロボット３１及び実施の形態２に係る多関節ロボット３２のパラレルリンクロボットとは異なり、シリアルリンクロボットである。

多関節ロボット３３は、手先１２と、ロボット固定部１８と、手先１２及びロボット固定部１８を接続する６本のリンク１３とを有する。手先１２及びロボット固定部１８は、６本のリンク１３により直列に接続されている。手先１２はリンク１３の一つに固定され、６本のリンク１３及びロボット固定部１８はそれぞれ６個の関節１１により接続されている。この６個の関節１１にそれぞれ設けられた６個のモータ１４により、手先１２は６自由度の動作を行うことができる。また、モータ１４は関節角度検出手段としてのエンコーダ１５を備えており、このエンコーダ１５を用いて各関節１１の角度を検出することができる。また、手先１２には、平面治具７の表面に押し付けられたときに、手先１２の姿勢を平面治具７の表面に対して常に同じ姿勢となるように安定させるための平面接触部１６が固定されている。平面接触部１６は、接触部の一例である。

つまり、平面接触部１６が平面治具７に押し付けられることで、手先１２の位置及び姿勢は、実施の形態１に係る多関節ロボット３１の手先２と同様に平面治具７により拘束される。平面接触部１６は、底面が正方形の直方体であるが、天面と底面とが平行であると共に手先１２が平面治具７の平面である表面に対して平行に面接触するものであれば、その形状は限定されない。なお、平面治具７は、多関節ロボット３３のロボット固定部１８との位置関係が変化しないように固定されている。

以上のような構成により、実施の形態１と同様の方法で、図６に示すように、平面治具７の表面の４箇所の異なる位置７１〜７４において、手先１２の姿勢及び位置を平面治具７に拘束させることにより取得されたエンコーダ１５の値を用いて、多関節ロボット３３の機構誤差の補正を行うことができる。なお、本実施の形態３に係る多関節ロボット３３の機構誤差の補正方法の処理は、計算式以外が実施の形態１に係る多関節ロボット３１の機構誤差の補正方法の処理と同様であるため、処理の流れの説明は省略する。

次に、取得したエンコーダ１５の値からの計算の内容を説明する。図６に示す本実施の形態３の多関節ロボット３３における、モータ１４の角度と手先１２の位置及び姿勢との関係を表す運動学方程式は、（式９）に示すとおり、Ｐ_ｉ、θ_ｉ、Ｋの変数で表される関数Ｆ２として導かれる。

（式９）は６次元の式である。Ｐ_ｉ＝（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ，θ_ｘｉ，θ_ｙｉ，θ_ｚｉ）は手先１２の位置及び姿勢を表す６次元ベクトルであり、ｉ（ｉ＝１〜４の整数）は４箇所での手先を示すインデックスである。Ｐ_ｉは未知の値であり、今後の計算により求められる。

θ_ｉ＝（θ１_ｉ，θ２_ｉ，θ３_ｉ，θ４_ｉ，θ５_ｉ，θ６_ｉ）は、多関節ロボット３３のそれぞれのモータ１４のエンコーダ１５の値である６次元ベクトルである。つまり、θ_ｉは、平面治具７の４箇所の異なる位置７１〜７４において手先２を拘束することにより取得された値であり、既知である。Ｋ＝（Ｋ１，Ｋ２，Ｋ３，Ｋ４，Ｋ５，Ｋ６）は、補正対象である６本のリンク１３の長さ（図６の寸法１７）であり、６次元ベクトルである。Ｋは未知の値であり、今後の計算により求められる。なお、本実施の形態３では、リンク１３の長さＫ以外の多関節ロボット３３の機構寸法は、設計値の理想どおりであると仮定している。

また、各４箇所の異なる位置７１〜７４での多関節ロボット３３の手先１２の姿勢は、それぞれの位置７１〜７４において同一の方向（平面治具７の法線方向）に拘束されている。また、このとき、多関節ロボット３３の手先１２の位置は平面治具７で同一平面上にある。これらの拘束条件から（式５）及び（式６）が実施の形態１と同様に得られる。

（式５）、（式６）及び（式９）の連立方程式を数値解析で解くことで、実施の形態１と同様にリンク１３の長さＫを求めることができる。

本実施の形態３に係る多関節ロボット３３の機構誤差の補正方法によれば、多関節ロボット３３の手先１２を平面治具７の表面の４箇所の異なる位置７１〜７４に押し付けることにより手先１２を平面治具７の表面に拘束された状態とし、その状態におけるエンコーダ１５の値を用いて多関節ロボット３３の機構誤差（リンク１３の長さＫについて、設計値と計測値との差）を計算する。

本実施の形態３でも、多関節ロボット３３の機構誤差の補正を行うために、温度が変化しても変形しにくい平面形状を有する平面治具７を用いている。そのため、温度変化に対して精度が低下する可能性が低くなり、多関節ロボット３３の機構誤差の補正を精度よく行うことができる。

なお、手先１２を押し付ける平面治具７の表面における４箇所の位置７１〜７４は、任意の異なる位置であるが、それぞれの位置で多関節ロボット３３の姿勢が変化する４箇所である必要がある。当該４箇所の異なる位置７１〜７４は、それぞれの間の距離がおよそ均等に離れていることが機構誤差の補正精度を高める上で望ましい。

なお、本実施の形態３において、平面治具７の表面の形状をロボット固定部１８が取り付けられている面に対して平行な平面で説明したが、ロボット固定部１８が取り付けられている面に対し、傾いた平面であっても上記の（式５）、（式６）及び（式９）を用いた機構誤差の補正方法を適用できる。

また、本実施の形態３に係る多関節ロボット３３の機構誤差の補正方法では、平面治具７の表面における４箇所の異なる位置７１〜７４において手先１２を拘束した状態で測定することで解を求めることができるが、平面治具７に拘束される位置が４箇所より少ない場合は一意的に解を求めることができない。なお、４箇所よりも多い５箇所以上の位置において手先１２を平面治具７の表面に拘束した状態で測定することにより得られた値を最小二乗法や平均などを用いることで、さらに構成精度を高めることができる。

（他の実施の形態１）
上記実施の形態１〜３に係る多関節ロボットの機構誤差の補正方法では、多関節ロボット３１〜３３の手先２、１２を、平面治具７又は球面治具９の表面における４箇所の異なる位置５１〜５４、６１〜６４、７１〜７４で拘束した状態で、エンコーダ６、１５の値を取得した。だが、このときに、多関節ロボット３１〜３３の手先２、１２を平面治具７又は球面治具９の表面に押し付けて姿勢を拘束した状態で、手先２、１２を移動させながら、エンコーダ６、１５の値を予め定められたタイミング（例えば０．１ｓ間隔のタイミング）で連続的かつ自動的に複数取得するようにしてもよい。このように、多関節ロボット３１〜３３の関節の角度であるエンコーダ６、１５の値を取得し記憶する取得工程を、手先２、１２を移動させながら連続的かつ自動的に複数回行うことにより、多関節ロボット３１〜３３の機構誤差の補正を容易かつ短時間に実施することができる。ここで、取得工程において記憶される多関節ロボット３１〜３３の角度を、多数個（例えば１０００個）取得されたエンコーダ６、１５の値のうちで、手先２、１２が拘束される平面治具７、球面治具９の表面の複数の位置の間の距離がおよそ均等に離れる４箇所における値を用いて計算することにより、より高精度に機構誤差の補正を行うことができる。なお、この場合の手先２、１２が拘束される平面治具７、球面治具９の表面の複数の位置の間の距離がおよそ均等に離れる４箇所は、設計値として予め記憶されている長さＬ、Ｋの値に基づいて導出される手先２の位置に基づいて導出される。

（他の実施の形態２）
上記実施の形態１、２に係る多関節ロボット３１、３２の機構誤差の補正方法では、リンク３の長さＬの誤差を補正しているが、リンク３の長さだけでなく、アーム４の長さ、手先２の位置を把握するための基準となる手先２上の位置である原点位置（例えば、多関節ロボット３１、３２の電源をＯＦＦにした場合に手先２が収束する位置）の設計値と計測値との誤差の補正についても、同様にして算出することができる。なお、原点位置の誤差の補正については、上記実施の形態３に係る多関節ロボットの機構誤差の補正方法でも適用できる。

（他の実施の形態３）
上記実施の形態１〜３に係る多関節ロボットの機構誤差の補正方法では、多関節ロボット３１〜３３の駆動機構が電源ＯＦＦの状態で、ユーザが手先２、１２を４箇所の異なる位置５１〜５４、６１〜６４、７１〜７４に移動させてエンコーダ６、１５の値を取得した場合について説明したが、例えば、多関節ロボット３１〜３３が自動的に手先２、１２を４箇所の異なる位置５１〜５４、６１〜６４、７１〜７４に移動させてエンコーダ６、１５の値を取得するようにしてもよい。なお、この場合に、手先２、１２が平面治具７、球面治具９に拘束されている状態を多関節ロボット３１〜３３が判定する方法としては、押圧センサなどの手先２、１２に対してかかる力の大きさを検出できるセンサを利用することが考えられる。上記の構成により、平面治具７、球面治具９に手先２、１２を押し当てる工程を自動的に行なうことができる。

以上、本発明の一つまたは複数の態様に係る多関節ロボットの機構誤差の補正方法について、実施の形態に基づいて説明したが、本発明は、この実施の形態に限定されるものではない。本発明の趣旨を逸脱しない限り、当業者が思いつく各種変形を本実施の形態に施したものや、異なる実施の形態における構成要素を組み合わせて構築される形態も、本発明の一つまたは複数の態様の範囲内に含まれてもよい。

本発明の多関節ロボットの機構誤差の補正方法は、産業用の多関節ロボットの機構誤差の補正方法として有用である。

１ａ，１ｂ，１ｃ，１１，１０１ 関節
２，１２ 手先
３，１３ リンク
４，１０４，１０５ アーム
５，１４ モータ
６，１５ エンコーダ
７ 平面治具
８，１６ 平面接触部
９ 球面治具
１０ 球面接触部
１７ 寸法
２１ 基台
３１，３２，３３，１００ 多関節ロボット
４０ 制御部
４１ 取得部
４２ 補正部
５１，６１，７１ 第一の位置
５２，６２，７２ 第二の位置
５３，６３，７３ 第三の位置
５４，６４，７４ 第四の位置
１０２ 固定部
１０３ 手先
１１０ 部材
１１１ 先端
１１２ プレート
１１３ 穴
１１４ 距離

Claims (5)

1. ６自由度を有する多関節ロボットの機構誤差を補正する多関節ロボットの機構誤差の補正方法であって、
   平面治具の表面の少なくとも４箇所の異なる位置それぞれにおいて、前記多関節ロボットの手先における平面接触部を拘束した状態で前記多関節ロボットの関節の角度を取得する取得工程と、
   前記取得工程において取得された前記多関節ロボットの関節の角度を用いて、前記多関節ロボットの機構誤差を算出して補正する補正工程と、を有し、
   前記取得工程は、前記平面治具の表面の少なくとも４箇所の異なる位置それぞれにおいて、前記手先における前記平面接触部の姿勢が前記平面治具の表面の法線と同じになるように前記手先を拘束した状態で、前記多関節ロボットの関節の角度を取得する、
   多関節ロボットの機構誤差の補正方法。
2. 前記多関節ロボットが、パラレルリンクロボットである、
   請求項１に記載の多関節ロボットの機構誤差の補正方法。
3. 前記取得工程は、前記平面治具の表面の少なくとも４箇所の異なる位置それぞれを通るように、前記平面治具の表面に接触させた状態の前記手先を移動させながら前記関節の角度を取得する、
   請求項１又は２に記載の多関節ロボットの機構誤差の補正方法。
4. 前記機構誤差は、前記多関節ロボットの手先と基台とを接続するリンクの長さの設計値と計測値との差である、
   請求項１から３のいずれか１項に記載の多関節ロボットの機構誤差の補正方法。
5. 前記機構誤差は、前記手先の原点位置の設計値と計測値との差である、
   請求項１から３のいずれか１項に記載の多関節ロボットの機構誤差の補正方法。

Images