JP2016057917A - 伝達角度算出方法、伝達角度補正装置、ロボットの制御方法およびロボットの制御装置 - Google Patents

Abstract

【課題】出力角度および入力角度を精度良く計算する。【解決手段】伝達角度算出装置（１０）は、ユニバーサルボールジョイントに接続された第一軸を回転させることにより第一角度、第二角度、および第一軸と第二軸との間の相対角度の複数組の姿勢情報として取得する姿勢情報取得部（１１）と、取得された複数組の姿勢情報とに基づいて、ユニバーサルボールジョイントの加工組立誤差を算出する加工組立誤差算出部（１２）と、算出された加工組立誤差が考慮された修正後関係式を作成する修正後関係式作成部（１３）と、修正後関係式に基づいて、前記第一角度に対応した修正後第二角度を算出する修正後第二角度算出部（１４）とを含む。【選択図】図１

Description

本発明は、ユニバーサルボールジョイントの伝達角度を算出する伝達角度算出方法およびそのような方法を実施する伝達角度算出装置に関する。

さらに、本発明は、少なくとも一つのユニバーサルボールジョイントを含むロボットの制御方法およびそのような方法を実施する、ロボットの制御装置に関する。

ユニバーサルボールジョイントは、同一直線上に無い二つの軸の間で動力を伝達する継手として知られている。ユニバーサルボールジョイントの構造および形状は、ＪＩＳ Ｂ１４５４に「こま型自在軸継手」として規定されている。このようなユニバーサルボールジョイントは、ロボットやパラレルメカニズムを駆動する要素として、使用されている。特許文献１および特許文献２にも、ユニバーサルボールジョイントを備えたパラレルメカニズムが開示されている。

図７は一般的なユニバーサルボールジョイントの略図である。図７に示されるユニバーサルボールジョイントの入力角度θと出力角度φとの関係は、以下の関係式（１）で表される。
cosαtanφ= tanθ （１）
ここで、文字αは入力角度θの第一軸と出力角度φの第二軸とがなす角度であり、角度θ、φ、αの単位はradianである。

ロボットやパラレルメカニズムにユニバーサルボールジョイントが使用されている場合には、前述した関係式（１）に基づいて、入力角度θおよび第一軸と第二軸とがなす角度αから出力角度φを計算できる（順運動）。同様に、関係式（１）に基づいて、出力角度φおよび第一軸と第二軸とがなす角度αから入力角度θを計算できる（逆運動）。

ところで、図７のＸＹＺ座標における空間上の単位方向ベクトルをＶ１、Ｖ２、Ｖ３、Ｖ４と定める。これにより、関係式（１）は以下のように導出される。なお、図７および他の図面においては、点Ａ〜点Ｊを以下のように定義している。
点Ａ：第一軸の基端
点Ｂ：第一軸の末端に取付けられた第一ヨークの一方の腕部下端
点Ｃ：第一軸の末端に取付けられた第一ヨークの他方の腕部下端
点Ｄ：第一軸の末端
点Ｅ：第二軸の基端
点Ｆ：第二軸の末端に取付けられた第二ヨークの一方の腕部下端
点Ｇ：第二軸の末端に取付けられた第二ヨークの他方の腕部下端
点Ｈ：第二軸の末端
点Ｊ：第一軸と第二軸との交点（十字軸の中心）
なお、点Ｂと点Ｃとは第一ヨークの互いに対向した位置にあり、点Ｆと点Ｇは第二ヨークの互いに対向した位置にあるものとする。

はじめに、図７の点Ａから点Ｊに向かう方向における単位方向ベクトルＶ１を（０、０、−１）とする。ベクトルＶ１と、点Ｂから点Ｃに向かう単位方向ベクトルＶ２とは常に直交している。従って、θ＝０のときのベクトルＶ２が（１、０、０）で表される場合には、ベクトルＶ２は（ｃｏｓθ、ｓｉｎθ、０）で表される。

次いで、図７の点Ｅから点Ｊに向かう単位方向ベクトルＶ３を（ｓｉｎα、０、ｃｏｓα）とする。ベクトルＶ３と、点Ｆから点Ｇに向かう単位方向ベクトルＶ４とは常に直交している。従って、φ＝０のときのベクトルＶ４が（０、１、０）で表される場合には、ベクトルＶ４は（−ｃｏｓαｓｉｎφ、ｃｏｓφ、ｓｉｎαｓｉｎφ）で表される。さらに、ベクトルＶ２とベクトルＶ４とは常に直交している。従って、Ｖ２とＶ４との内積はゼロである。このことを変形することにより、前述した関係式（１）が得られる。

特開２００８−１３４８３９号公報 特開２０１１−１０１９３８号公報

ところで、前述した関係式（１）を導出する際には、以下の三つのことを前提にしている。
・図７の点Ａから点Ｊに向かう方向と点Ｂから点Ｃに向かう方向とは互いに直交している。
・図７の点Ｅから点Ｊに向かう方向と点Ｆから点Ｇに向かう方向とは互いに直交している。
・図７の点Ｂから点Ｃに向かう方向と点Ｆから点Ｇに向かう方向とは互いに直交している。

しかしながら、実際のユニバーサルボールジョイントは加工誤差および組立誤差（以下、これらを「加工組立誤差」と称す）を含んでいるので、実際には前述した方向が互いに完全に直交することはない。従って、関係式（１）を用いてユニバーサルジョイントの順運動および逆運動を計算した場合には、算出された出力角度および入力角度が位置決め誤差を含む可能性がある。従って、そのような位置決め誤差を含むことなしに、入力角度および出力角度を計算することが望まれている。

特許文献１においては、ユニバーサルボールジョイントの、パラレルメカニズム機械に対する回転中心位置の誤差を補正している。しかしながら、特許文献１では、ユニバーサルボールジョイントの個々の加工組立誤差を補正しているわけではない。従って、特許文献１では、前述した位置決め誤差を解消できない。

本発明はこのような事情に鑑みてなされたものであり、出力角度および入力角度を精度良く計算することのできる、ユニバーサルボールジョイントの伝達角度算出方法および伝達角度算出装置を提供することを目的とする。

前述した目的を達成するために１番目の発明によれば、ユニバーサルボールジョイントを介して連結される第一棒状部材および第二棒状部材のうち、前記第一棒状部材の中心線回りの第一角度を計測し、前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測し、前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得し、前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得し、前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出し、算出された前記加工組立誤差が考慮された修正後関係式を作成し、該修正後関係式に基づいて、前記第一角度に対応した修正後第二角度を算出する、ユニバーサルボールジョイントの伝達角度算出方法が提供される。
２番目の発明によれば、１番目の発明において、前記第一角度および前記第二角度のうちの少なくとも一方が、角度検出器によって計測される。
３番目の発明によれば、１番目の発明において、前記第一角度および前記第二角度のうちの少なくとも一方が、視覚センサによって計測される。
４番目の発明によれば、ユニバーサルボールジョイントを介して連結される第一棒状部材および第二棒状部材のうち、前記第一棒状部材の中心線回りの第一角度を計測する第一角度計測部と、前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測する第二角度計測部と、前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得する姿勢情報取得部と、を含み、該姿勢情報取得部は、前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得しており、さらに、前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出する加工組立誤差算出部と、算出された前記加工組立誤差が考慮された修正後関係式を作成する修正後関係式作成部と、該修正後関係式に基づいて、前記第一角度に対応した修正後第二角度を算出する修正後第二角度算出部と、を具備する、ユニバーサルボールジョイントの伝達角度算出装置が提供される。
５番目の発明によれば、４番目の発明において、前記第一角度計測部および前記第二角度計測部のうちの少なくとも一方は、角度検出器である。
６番目の発明によれば、４番目の発明において、前記第一角度計測部および前記第二角度計測部のうちの少なくとも一方は、視覚センサである。
７番目の発明によれば、少なくとも一つのユニバーサルボールジョイントを含むロボットの制御方法において、前記ユニバーサルボールジョイントを介して連結される第一棒状部材および第二棒状部材のうち、前記第一棒状部材の中心線回りの第一角度を計測し、前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測し、前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得し、前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得し、前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出し、算出された前記加工組立誤差が考慮された修正後関係式を作成し、要求される第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の入力情報として取得し、前記修正後関係式に基づいて、前記入力情報に対応する第一角度を算出し、算出された第一角度に基づいて前記第一棒状部材の中心線回りの角度を制御することにより、前記第二棒状部材の中心線回りに前記要求される第二角度を実現する、ロボットの制御方法が提供される。
８番目の発明によれば、７番目の発明において、前記第一角度は角度検出器によって計測され、前記第二角度は視覚センサによって計測される。
９番目の発明によれば、７番目または８番目の発明において、前記ロボットはパラレルリンク方式の機構構造を有する、請求項７または８に記載のロボットの制御方法。
１０番目の発明によれば、少なくとも一つのユニバーサルボールジョイントを含むロボットの制御装置において、ユニバーサルボールジョイントを介して連結される第一棒状部材および第二棒状部材のうち、前記第一棒状部材の中心線回りの第一角度を計測する第一角度計測部と、前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測する第二角度計測部と、前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得する姿勢情報取得部と、を具備し、該姿勢情報取得部は、前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得しており、さらに、前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出する加工組立誤差算出部と、算出された前記加工組立誤差が考慮された修正後関係式を作成する修正後関係式作成部と、要求される第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の入力情報として取得する入力情報取得部と、前記修正後関係式に基づいて、前記入力情報に対応する第一角度を算出する第一角度算出部と、を具備し、算出された第一角度に基づいて前記第一棒状部材の中心線回りの角度を制御することにより、前記第二棒状部材の中心線回りに前記要求される第二角度を実現する、ロボットの制御装置が提供される。
１１番目の発明によれば、１０番目の発明において、前記第一角度は角度検出器によって計測され、前記第二角度は視覚センサによって計測される。
１２番目の発明によれば、１０番目または１１番目の発明において、前記ロボットはパラレルリンク方式の機構構造を有する。

１番目および４番目の発明においては、第一角度（入力角度）を変化させることにより取得された複数組の姿勢情報を用いて、加工組立誤差を算出している。そして、加工組立誤差が反映された修正後関係式に基づいて、第二角度（出力角度）を算出している。それゆえ、順運動の場合に、ユニバーサルボールジョイントの第二角度（出力角度）を精度良く計算することができる。同様な手法により、逆運動の場合に、ユニバーサルボールジョイントの第一角度（入力角度）を精度良く計算することも可能である。
２番目および５番目の発明においては、角度検出器、例えばエンコーダは通常はモータに取付けられているので、第一角度または第二角度を安価に求めることができる。
３番目および６番目の発明においては、視覚センサにより取得された画像を処理することにより、第一角度または第二角度を正確に求めることができる。
７番目および１０番目の発明においては、第一角度（入力角度）を変化させることにより取得された複数組の姿勢情報を用いて、加工組立誤差を算出している。そして、加工組立誤差が反映された修正後関係式と所望の第二角度に基づいて、ユニバーサルボールジョイントの第一角度（入力角度）を精度良く計算している。そのような第一角度（入力角度）になるよう制御すれば、順運動の場合にユニバーサルボールジョイントにおいて第二角度（出力角度）を高精度で実現することができる。同様な手法により、逆運動の場合に、ユニバーサルボールジョイントにおいて第一角度（入力角度）を高精度で実現することも可能である。
８番目および１１番目の発明においては、比較的簡単な構成によって、出力角度および入力角度を精度良く計算できる。
９番目および１２番目の発明においては、７番目および１０番目の発明をパラレルリンクロボットに適用できる。

添付図面に示される本発明の典型的な実施形態の詳細な説明から、本発明のこれら目的、特徴および利点ならびに他の目的、特徴および利点がさらに明解になるであろう。

本発明に基づく制御装置を含むシステムを示す図である。 本発明の第一の実施形態におけるユニバーサルボールジョイントの略図である。 本発明の第一の実施形態に基づく制御装置の動作を示すフローチャートである。 第一角度θと関数ｆとの関係を示す第一の図である。 第一角度θと関数ｆとの関係を示す第二の図である。 第一角度θと関数ｆとの関係を示す第三の図である。 本発明の第二の実施形態に基づく制御装置を含むシステムを示す図である。 本発明の第二の実施形態に基づく制御装置の他の動作を示すフローチャートである。 一般的なユニバーサルボールジョイントの略図である。

以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
図１は本発明に基づく制御装置を含むシステム１を示す図である。図１に示されるように、システム１は、制御装置１０と、制御装置１０に接続されたロボット２０とで主に構成されている。図１に示されるロボット２０は少なくとも一つのユニバーサルボールジョイント３０を含んでいる。

図１に示される制御装置１０はデジタルコンピュータであり、ロボット２０を制御する。制御装置１０は、ユニバーサルボールジョイント３０を介して連結される第一棒状部材（第一軸）および第二棒状部材（第二軸）のうちの第一棒状部材の中心線回りの第一角度、第一角度に対応した第二棒状部材の中心線回りの第二角度、および第一棒状部材と第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得する姿勢情報取得部１１を含んでいる。

さらに、制御装置１０は、ユニバーサルボールジョイント３０の加工組立誤差をパラメータとして含んでいる、第一角度および第二角度についての関係式と、取得された複数組の姿勢情報とに基づいて、パラメータを同定することで加工組立誤差を算出する加工組立誤差算出部１２を含んでいる。さらに、制御装置１０は、算出された加工組立誤差が考慮された修正後関係式を取得する修正後関係式作成部１３と、修正後関係式に基づいて、前記第一角度に対応した修正後第二角度を算出する修正後第二角度算出部１４とを含んでいる。

さらに、図１に示されるように、制御装置１０は、要求される第二角度、および第一棒状部材と第二棒状部材との間の相対角度の情報を一組の入力情報として取得する入力情報取得部１５と、修正後関係式に基づいて、入力情報に対応する第一角度を算出する第一角度算出部１６とを含んでいる。

図２は本発明の第一の実施形態におけるユニバーサルボールジョイントの略図である。図２に示されるように、ユニバーサルボールジョイント３０は、第一軸３１ａと、その末端に取付けられた第一ヨーク３１ｂと、第二軸３２ａと、その末端に取付けられた第二ヨーク３２ｂと、第一ヨーク３１ｂおよび第二ヨーク３２ｂに係合した十字軸３３とを主に含んでいる。

前述したように、図２に示されるユニバーサルボールジョイント３０の入力角度θと出力角度φとは、前述した関係式（１）を満たす。
cosαtanφ= tanθ （１）

本発明においては、ユニバーサルボールジョイント３０の最上端である第一軸３１ａの基端ＡにモータＭ、例えばサーボモータが取付けられている。その結果、第一軸３１ａの基端Ａおよび末端Ｄの位置が固定されるようになり、ユニバーサルボールジョイント３０は第一角度θの方向に自由に回転できるようになる。

また、モータＭには角度検出器、例えばエンコーダ４１が取付けられている。エンコーダ４１は第一軸３１ａの中心線回りの第一角度θを計測する第一角度計測部としての役目を果たす。

また、ユニバーサルボールジョイント３０の第二軸３２ａには、治具４５が係合している。図示されるように、治具４５は、第二軸３２ａの直径に概ね等しい開口部を有するリング部分と、その両側から延びるロッドとを含んでいる。そして、治具４５のリング部分に、第二軸３２ａが挿入されている。また、少なくとも一方のロッドは図示しない固定部分に固定されている。

このような治具４５を第二軸３２ａに係合させているので、第二軸３２ａの基端Ｅおよび末端Ｈの位置が固定されうるようになる。従って、モータＭが回転した場合であっても、第一軸３１ａと第二軸３２ａとのなす角度αの値は一定になる。角度αは例えば３０度である。

また、ユニバーサルボールジョイント３０の最下部である第二軸３２ａの基端Ｅに薄板状のターゲットＴが固定されている。ターゲットＴは第二軸３２ａに対して垂直に配置されている。ターゲットＴは、第二軸３２ａの回転角度が把握できる目盛りを備えた円板であるのが好ましい。あるいは、ターゲットＴは正多角形の薄板であってもよい。

そして、図示されるように、視覚センサ、例えばカメラ４２でもってターゲットＴを撮像する。このため、カメラ４２は、例えば第二軸３２ａの延長線上に配置されるのが好ましい。そして、カメラ４２により撮像された画像は制御装置１０に送られて解析される。これにより、ターゲットＴの向きを把握し、その結果、図２に示される第二角度φを取得することができる。従って、カメラ４２は、第二軸３２ａの中心線回りの第二角度φを計測する第二角度計測部としての役目を果たす。

図３は本発明の第一の実施形態に基づく制御装置の動作を示すフローチャートである。以下、図３を参照しつつ、本発明の第一の実施形態における動作を説明する。はじめに、第一軸３１ａの基端Ａおよび末端Ｄならびに第二軸３２ａの基端Ｅおよび末端Ｈの位置を前述したように固定する。

そして、ステップＳ１１において、モータＭを駆動して、第一軸３１ａを初期位置から所定角度、例えば３０度だけ回転させる。次いで、ステップＳ１２において、回転後の第一軸３１ａの第一角度θをエンコーダ４１により取得する。さらに、ステップＳ１３においては、この状態において、カメラ４２を用いてターゲットＴを撮像し、それにより、回転後の第二軸３２ａの第二角度φを計測する。そして、ステップＳ１４において姿勢情報取得部１１は、第一角度θ、第二角度φおよび第一軸３１ａと第二軸３２ａとの相対角度αを一組の姿勢情報として取得し、記憶する。なお、前述したように第一の実施形態においては、角度αは一定である。

その後、ステップＳ１５において、姿勢情報取得部１１が所定数の姿勢情報を取得したか否かを判定する。所定数の姿勢情報が取得されていない場合には、ステップＳ１１に戻る。そして、所定数の姿勢情報が取得されるまで、ステップＳ１１〜ステップＳ１４の処理を繰返すものとする。

所定数の姿勢情報、例えば十二組の姿勢情報が取得されると、ステップＳ１６に進む。なお、第一軸３１ａを３０度ずつ回転させる場合には、一周分だけ第一軸３１ａが回転すると、十二組の姿勢情報が得られる。当然のことながら、第一軸３１ａの回転角度を３０度よりも小さくし、十二よりも多数の姿勢情報を取得してもよい。この場合には、後述する計算をより精密に行えるのが分かるであろう。

ステップＳ１６においては、加工組立誤差算出部１２は、ユニバーサルボールジョイント３０が以下のような加工組立誤差ε１、ε２、ε３を含むものとして、角度θ、角度φ、角度αの関係式を導出する。
・図２の点Ａから点Ｊに向かう方向と点Ｂから点Ｃに向かう方向のなす角度は（９０＋ε１）度である。
・図２の点Ｅから点Ｊに向かう方向と点Ｆから点Ｇに向かう方向のなす角度は（９０＋ε２）度である。
・図２の点Ｂから点Ｃに向かう方向と点Ｆから点Ｇに向かう方向のなす角度は（９０＋ε３）度である。

ここで、関係式の導出を簡潔にする目的で、以下のように三次元単位ベクトルＥｘ、Ｅｙ、Ｅｚと三次元回転行列Ｒｘ（Ｗ）、Ｒｙ（Ｐ）、Ｒｚ（Ｒ）とを定義する。

これらの定義に従えば、図２の点Ａから点Ｊに向かう方向Ｖ１は「−Ｅｚ」で表される。方向Ｖ１と点Ｂから点Ｃに向かう方向Ｖ２とのなす角度は常に（９０＋ε１）度である。従って、θ＝０のときの方向Ｖ２がＲｙ（ε１）Ｅｘで表される場合には、方向Ｖ２はＲｚ（θ）Ｒｙ（ε１）Ｅｘで表すことができる。

また、図２の点Ｅから点Ｊに向かう方向Ｖ３はＲｙ（α）Ｅｚで表される。方向Ｖ３と点Ｆから点Ｇに向かう方向Ｖ４とのなす角度は常に（９０＋ε２）度である。従って、φ＝０のときの方向Ｖ４がＲｙ（α）Ｒｘ（ε２）Ｅｙで表される場合には、方向Ｖ４はＲｙ（α）Ｒｚ（φ）Ｒｘ（ε２）Ｅｙで表すことができる。

そして、方向Ｖ２と方向Ｖ４とのなす角度は常に（９０＋ε３）度であるため、以下の式（２）が成立する。
（Ｖ２とＶ４の内積）＝ｓｉｎε３ （２）

ここで、式（２）は、角度θ、φ、αおよび加工組立誤差ε１、ε２、ε３をパラメータとして含む等式である。そして、加工組立誤差ε１＝ε２＝ε３＝０の場合には、関係式（１）と式（２）とは互いに一致する。

そして、式（２）を変形すると、第二角度φは角度θ、αおよび加工組立誤差ε１、ε２、ε３を変数とした、以下の関数ｆで表される。
φ＝ｆ（θ、α、ε１、ε２、ε３） （３）

ここで、姿勢情報取得部１１が取得した十二組の姿勢情報を用いて、加工組立誤差算出部１２は、「φｉ−ｆ（θｉ、α、ε１、ε２、ε３）」の二乗和ができる限り小さくなるように、ニュートン法等により、角度αおよび加工組立誤差ε１、ε２、ε３を決定する。なお、この場合には、文字ｉは１〜１２の整数である。また、ニュートン法を用いる場合には、角度αおよび加工組立誤差ε１、ε２、ε３の初期値をそれぞれ３０度、０度、０度、０度とするとよい。

そして、ステップＳ１７においては、修正後関係式作成部１３が、算出された角度αおよび加工組立誤差ε１、ε２、ε３を前述した式（３）に入力して、修正後関係式ｆ’を作成する。次いで、ステップＳ１８においては、修正後第二角度算出部１４は、修正後関係式に第一角度θを入力して、第二角度φを算出する。これにより、入力角度である第一角度θに基づいて、正確な修正後第二角度φを算出することができる。

このようにして算出された第二角度φは、カメラ４２を用いて取得された第二角度φとは異なり、加工組立誤差ε１、ε２、ε３を含んでいない正確な出力角度である。従って、本発明においては、順運動の出力角度を精度良く計算することができる。なお、同様な手法により、逆運動の入力角度を精度良く計算できることは明らかであろう。

ところで、図４Ａ〜図４Ｃは第一角度θと関数ｆとの関係を示す図である。図４Ａ〜図４Ｃの横軸は第一角度θを示しており、その単位はｄｅｇである。図４Ａ〜図４Ｃの縦軸は、それぞれ以下の通りであり、その単位はｄｅｇである。
図４Ａ ｆ（θ、３０、０．５、０、０）−ｆ（θ、３０、０、０、０）
図４Ｂ ｆ（θ、３０、０、０．５、０）−ｆ（θ、３０、０、０、０）
図４Ｃ ｆ（θ、３０、０、０、０．５）−ｆ（θ、３０、０、０、０）

言い換えれば、図４Ａにおいては、加工組立誤差ε１、ε２、ε３のうちのε１のみを０．５ｄｅｇに設定した関数ｆと、全ての加工組立誤差ε１、ε２、ε３をゼロに設定した関数との間の偏差を縦軸にしている。図４Ｂにおいては、加工組立誤差ε１、ε２、ε３のうちのε２のみを０．５ｄｅｇに設定した関数ｆと、全ての加工組立誤差ε１、ε２、ε３をゼロに設定した関数との間の偏差を縦軸にしている。さらに、図４Ｃにおいては、加工組立誤差ε１、ε２、ε３のうちのε３のみを０．５ｄｅｇに設定した関数ｆと、全ての加工組立誤差ε１、ε２、ε３をゼロに設定した関数との間の偏差を縦軸にしている。図４Ａ〜図４Ｃに示される実線Ａ１〜Ａ３はそのような偏差を示している。

そして、本発明に基づく前述した算出方法によって加工組立誤差ε１、ε２、ε３を同定して作成された修正後関係式ｆ’を使用して、図４Ａ〜図４Ｃのそれぞれにおいて、以下の偏差を同様に取得する。
図４Ａ ｆ’（θ、３０、０．５、０、０）−ｆ’（θ、３０、０、０、０）
図４Ｂ ｆ’（θ、３０、０、０．５、０）−ｆ’（θ、３０、０、０、０）
図４Ｃ ｆ’（θ、３０、０、０、０．５）−ｆ’（θ、３０、０、０、０）

このようにして算出された偏差は、図４Ａから図４Ｃに破線Ｂ１〜Ｂ３で示されるように、ほぼゼロになる。従って、本発明によって加工組立誤差の影響を効果的に排除できるのが分かるであろう。

図５は本発明の第二の実施形態に基づく制御装置を含むシステムを示す図である。図５においては、ロボット２０としてパラレルリンクロボット２０が示されている。パラレルリンクロボット２０は、基礎部２１と、可動プレート２２と、基礎部２１と可動プレート２２とを連結する三つのリンク部５ａ〜５ｃとを有する。基礎部２１は、各リンク部５ａ〜５ｃをそれぞれ駆動する三つのサーボモータ（図示しない）を備えている。

各リンク部５ａ〜５ｃは、それぞれ駆動リンク５１ａ〜５１ｃと、受動リンク５２ａ〜５２ｃとを含む。駆動リンク５１ａ〜５１ｃは基礎部２１から径方向外側に延在している。駆動リンク５１ａ〜５１ｃの一端部はサーボモータの出力軸に連結されている。駆動リンク５１ａ〜５１ｃの他端部には、受動リンク５２ａ〜５２ｃの一端部が球面軸受けを介してそれぞれ連結されている。受動リンク５２ａ〜５２ｃの他端部は、球面軸受けを介して可動プレート２２にそれぞれ連結されている。サーボモータが駆動されると、駆動リンク５１ａ〜５１ｃおよび受動リンク５２ａ〜５２ｃが鉛直面内で回転し、基礎部２１の下方の三次元点空間で可動プレート２２を任意の位置に変位させることができる。

図５に示されるように、受動リンク５２ａには、エンコーダ４１を備えたモータＭが取付けられている。モータＭの出力軸は、第一軸３１ａとしての駆動軸に連結されている。駆動軸３１ａは、可動プレート２２から下方に延びる第二軸３２ａとしての取付部材用軸３２ａを回転させるのに使用される。取付部材用軸３２ａの先端には、エンドエフェクタを取付けるための取付部材（図示しない）が配置されているものとする。

図５に示されるように、駆動軸３１ａの回転方向と取付部材用軸３２ａの回転方向とは互いに異なる。このため、可動プレート２２に固定されたユニバーサルボールジョイント３０が駆動軸３１ａと取付部材用軸３２ａとの間に設けられている。モータＭを回転させることによって、ユニバーサルボールジョイント３０の回転中心位置を変えることなしに、取付部材用軸３２ａを回転させられる。

図６は本発明の第二の実施形態に基づく制御装置の他の動作を示すフローチャートである。図６のステップＳ１１からステップＳ１７は前述したのと同様であるので、一部の説明を省略する。第二の実施形態においては、エンコーダ４１がモータＭの回転角度を入力角度θとして計測する（ステップＳ１２）。これと同時に、取付部材用軸３２ａの先端の真下に配置されたカメラ４２が取付部材用軸３２ａの画像を取得する。そして、制御装置１０が画像処理を行って、取付部材用軸３２ａの回転角度を出力角度φとして取得する（ステップＳ１３）。

また、駆動軸３１ａと取付部材用軸３２ａとの間の角度αは、受動リンク５２ａの位置姿勢から推測できる。受動リンク５２ａの位置姿勢は、各リンク部５ａ〜５ｂのそれぞれのジョイント部の回転角度の値から計算することができる。

このように、ロボットとしてパラレルリンクロボット２０を用いた場合であっても、入力角度θ、出力角度αおよび角度αを取得できる。そして、前述したのと同様な手法により、加工組立誤差ε１、ε２、ε３を算出する（ステップＳ１４〜ステップＳ１６）。それゆえ、これらが考慮された修正後関係式を用いて順運動の出力角度および逆運動の入力角度を精度良く計算することができる（ステップＳ１７）。つまり、第二の実施形態においても前述したのと同様な効果が得られるのが分かるであろう。

さらに、第二の実施形態においては、ステップＳ２１において、操作者が、キーボードなどの入力装置を用いて所望の第二角度φを制御装置１０に入力する。そして、入力情報取得部１５が、角度αと所望の第二角度φとを一組の入力情報として取得する。次いで、ステップＳ２２においては、第一角度算出部１６がステップＳ１７で作成された修正後関係式に入力情報を適用し、入力情報に対応する第一角度を算出する。

次いで、ステップＳ２３においては、算出された第一角度だけ駆動軸３１ａをその中心線回りに回転させる。これにより、取付部材用軸３２ａの中心線回りに所望の第二角度φを実現することができる。すなわち、第二の実施形態においては、所望の第二角度φを容易且つ高精度で実現できるのが分かるであろう。なお、同様な手法により、逆運動の場合に、ユニバーサルボールジョイントにおける第一角度θ（入力角度）を高精度で実現することも可能である。

ところで、前述した第一および第二の実施形態においては、加工組立誤差ε１、ε２、ε３を算出する際に、以下の事項を前提にしている。
図２に示される三つの点Ａ、点Ｄ、点Ｊは一直線上にある。
図２に示される三つの点Ｂ、点Ｃ、点Ｊは一直線上にある。
図２に示される三つの点Ｅ、点Ｈ、点Ｊは一直線上にある。
図２に示される三つの点Ｆ、点Ｇ、点Ｊは一直線上にある。
しかしながら、現実のユニバーサルボールジョイント３０は加工組立誤差を含んでいるので、前述した三つの点が一直線上に位置することはない。

本発明の第三の実施形態（図示しない）においては、入力角度θがゼロであるときに点Ａと点Ｄとが図２に示される座標系のＺ軸上にあるものと仮定する。このとき、点Ｂと点Ｃとが図２に示される座標系においてそれぞれ（Ｘ、Ｙ、Ｚ）＝（Ｌ＋λ、ζ、０）、（Ｘ、Ｙ、Ｚ）＝（−Ｌ＋λ、ζ、０）に在ると考えられる。

ここで、文字Ｌは点Ｂと点Ｊとの間の設計上の距離であると共に、点Ｃと点Ｊとの間の設計上の距離である。文字λ、ζは誤差パラメータであり、設計上はその値はゼロである。ただし、現実には誤差パラメータλ、ζはゼロではない。反対に、例えば誤差パラメータζがゼロでない場合には、ユニバーサルボールジョイント３０の第一軸３１ａおよび第一ヨーク３１ｂに含まれる点Ａ、点Ｂ、点Ｃ、点Ｄは設計上は一つの平面上に含まれるものの、実際には一つの平面上にはない。

第三の実施形態においては、これら誤差パラメータλ、ζを考慮した入力角度θと出力角度φとの間の関係式を考える。これら誤差パラメータλ、ζが０でないと仮定した状況において、点Ａ、点Ｂ、点Ｅの位置を固定したまま、入力角度θを変化させて点Ａと点Ｂとを回転させる。

この場合には、角度αが一定にならず、角度αは入力角度θについて３６０度の周期で振動するようになる。その理由は、点Ｅと点Ｈとを含むユニバーサルボールジョイント３０の第二軸３２ａに着目すると、入力角度θを第一軸３１ａ回りに変化させるにつれて、点Ｊは第一軸３１ａ回りに回転するためである。

但し、点Ｅと点Ｊとの間の距離が大きいほど、角度αの振動は小さくなる。従って、誤差パラメータλ、ζの入力角度θや出力角度φに対する影響は小さくなる。それゆえ、図２において、治具４５によって点Ｅを固定すれば、第一の実施形態の場合と同様に、φ＝ｆ（θ、α、λ、ζ）の式（変形した式（３））から誤差パラメータλ、ζを同定することができる。また、φ＝ｆ（θ、α、λ、ζ、ε１、ε２、ε３）の関係式を用いて、誤差パラメータλ、ζおよび加工組立誤差ε１、ε２、ε３を同時に同定することも可能である。従って、第三の実施形態の場合にも前述したのと同様な効果が得られるのは明らかであろう。

典型的な実施形態を用いて本発明を説明したが、当業者であれば、本発明の範囲から逸脱することなしに、前述した変更および種々の他の変更、省略、追加を行うことができるのを理解できるであろう。

１ システム
５ａ〜５ｃ リンク部
１０ 制御装置、伝達角度算出装置
１１ 姿勢情報取得部
１２ 加工組立誤差算出部
１３ 修正後関係式作成部
１４ 修正後第二角度算出部
１５ 入力情報取得部
１６ 第一角度算出部
２０ ロボット、パラレルリンクロボット
２１ 基礎部
２２ 可動プレート
３０ ユニバーサルボールジョイント
３１ａ 第一軸、駆動軸（第一棒状部材）
３１ｂ 第一ヨーク
３２ａ 第二軸、取付部材用軸（第二棒状部材）
３２ｂ 第二ヨーク
３３ 十字軸
４１ エンコーダ（角度検出器）
４２ カメラ（視覚センサ）
５１ａ〜５１ｃ 駆動リンク
５２ａ〜５２ｃ 受動リンク
Ｍ モータ
θ 入力角度（第一角度）
φ 出力角度（第二角度）
ε１、ε２、ε３ 加工組立誤差

Claims (12)

1. ユニバーサルボールジョイント（３０）を介して連結される第一棒状部材（３１ａ）および第二棒状部材（３２ａ）のうち、前記第一棒状部材の中心線回りの第一角度を計測し、
   前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測し、
   前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得し、
   前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得し、
   前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出し、
   算出された前記加工組立誤差が考慮された修正後関係式を作成し、
   該修正後関係式に基づいて、前記第一角度に対応した修正後第二角度を算出する、ユニバーサルボールジョイントの伝達角度算出方法。
2. 前記第一角度および前記第二角度のうちの少なくとも一方が、角度検出器（４１）によって計測される請求項１に記載の伝達角度算出方法。
3. 前記第一角度および前記第二角度のうちの少なくとも一方が、視覚センサ（４２）によって計測される請求項１に記載の伝達角度算出方法。
4. ユニバーサルボールジョイント（３０）を介して連結される第一棒状部材（３１ａ）および第二棒状部材（３２ａ）のうち、前記第一棒状部材の中心線回りの第一角度を計測する第一角度計測部と、
   前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測する第二角度計測部と、
   前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得する姿勢情報取得部（１１）と、を含み、
   該姿勢情報取得部は、前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得しており、
   さらに、
   前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出する加工組立誤差算出部（１２）と、
   算出された前記加工組立誤差が考慮された修正後関係式を作成する修正後関係式作成部（１３）と、
   該修正後関係式に基づいて、前記第一角度に対応した修正後第二角度を算出する修正後第二角度算出部（１４）と、を具備する、ユニバーサルボールジョイントの伝達角度算出装置（１０）。
5. 前記第一角度計測部および前記第二角度計測部のうちの少なくとも一方は、角度検出器（４１）である請求項４に記載の伝達角度算出装置。
6. 前記第一角度計測部および前記第二角度計測部のうちの少なくとも一方は、視覚センサ（４２）である請求項４に記載の伝達角度算出装置。
7. 少なくとも一つのユニバーサルボールジョイント（３０）を含むロボット（２０）の制御方法において、
   前記ユニバーサルボールジョイントを介して連結される第一棒状部材（３１ａ）および第二棒状部材（３２ａ）のうち、前記第一棒状部材の中心線回りの第一角度を計測し、
   前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測し、
   前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得し、
   前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得し、
   前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出し、
   算出された前記加工組立誤差が考慮された修正後関係式を作成し、
   要求される第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の入力情報として取得し、
   前記修正後関係式に基づいて、前記入力情報に対応する第一角度を算出し、
   算出された第一角度に基づいて前記第一棒状部材の中心線回りの角度を制御することにより、前記第二棒状部材の中心線回りに前記要求される第二角度を実現する、ロボットの制御方法。
8. 前記第一角度は角度検出器（４１）によって計測され、前記第二角度は視覚センサ（４２）によって計測される、請求項７に記載のロボットの制御方法。
9. 前記ロボットはパラレルリンク方式の機構構造を有する、請求項７または８に記載のロボットの制御方法。
10. 少なくとも一つのユニバーサルボールジョイント（３０）を含むロボット（２０）の制御装置（１０）において、
    ユニバーサルボールジョイントを介して連結される第一棒状部材（３１ａ）および第二棒状部材（３２ａ）のうち、前記第一棒状部材の中心線回りの第一角度を計測する第一角度計測部と、
    前記第二棒状部材の中心線回りの、前記第一角度に対応した第二角度を計測する第二角度計測部と、
    前記第一角度、前記第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の姿勢情報として取得する姿勢情報取得部（１１）と、を具備し、
    該姿勢情報取得部は、前記第一棒状部材の前記中心線回りの前記第一角度を変化させることにより複数組の姿勢情報を取得しており、
    さらに、
    前記ユニバーサルボールジョイントの加工組立誤差を含んでいる、前記第一角度および前記第二角度についての関係式と、取得された前記複数組の姿勢情報とに基づいて、前記加工組立誤差を算出する加工組立誤差算出部（１２）と、
    算出された前記加工組立誤差が考慮された修正後関係式を作成する修正後関係式作成部（１３）と、
    要求される第二角度、および前記第一棒状部材と前記第二棒状部材との間の相対角度の情報を一組の入力情報として取得する入力情報取得部（１５）と、
    前記修正後関係式に基づいて、前記入力情報に対応する第一角度を算出する第一角度算出部（１６）と、を具備し、
    算出された第一角度に基づいて前記第一棒状部材の中心線回りの角度を制御することにより、前記第二棒状部材の中心線回りに前記要求される第二角度を実現する、ロボットの制御装置。
11. 前記第一角度は角度検出器（４１）によって計測され、前記第二角度は視覚センサ（４２）によって計測される、請求項１０に記載のロボットの制御装置。
12. 前記ロボットはパラレルリンク方式の機構構造を有する、請求項１０または１１に記載のロボットの制御装置。

Images