JP6298026B2 - 多関節ロボットのたわみを計測するたわみ計測システム - Google Patents

Description

本発明は、多関節ロボットの各関節軸や関節軸間のリンクなどのたわみを計測するたわみ計測システムに関する。

多関節ロボットにおいては、各関節軸や関節軸間のリンクなどの機構部が、ロボットアーム部の先端に取付けられたツールの重さによって弾性変形することが知られている。このような弾性変形はロボットアーム部の先端の位置決め精度に影響するため、従前より、関節軸やリンクなどの弾性変形を考慮してロボットアーム部の位置（角度）を制御することが行われている。

例えば、特許文献１に示される発明においては、関節軸を回転ばねとみなし、関節軸に定義された座標系の座標軸回りにおける関節軸のばね定数を予め測定し設定しておく。そして、設定されたばね定数と、座標軸回りに関節軸に作用するトルクとから、座標軸回りにおける関節軸のたわみ量を計算し、このたわみ量に基づいてロボットアーム部の目標位置を補正している。

このようにロボットアーム部の目標位置を補正する場合には、前述したように関節軸のばね定数を予め定めておく必要がある。関節軸のばね定数を事前に決定する方法については種々提案されており、以下の特許文献２、特許文献３および特許文献４などに開示されるようなばね定数の決定方法がある。

特許文献２に開示された発明においては、まず、制御装置からロボットの関節軸に目標位置として与えられる指令角度と、実際に計測した関節軸の回転角度とのずれ量を求めている。そして、関節軸に作用するトルクを求め、求めた角度のずれ量と求めたトルクとに基づいて、関節軸のばね定数を算出している。

また、特許文献３に開示された発明においては、ロボットの関節軸に連結された二つのアーム部のそれぞれに角度確認位置を設けておき、制御部により関節軸を回転させて両方のアーム部の角度確認位置を位置合わせする。そして、この位置合わせ時に制御部から関節軸に与えられた指令位置（角度）と、関節軸がたわまないとした場合に前述の位置合わせ時に必要な理論上の指令位置（角度）との差を、たわみ補正量として求める。そして、そのたわみ補正量を関節軸に作用する負荷トルクにより除算した値をばね定数として定めている。

さらに、特許文献４に開示された発明においては、まず、質量の異なる二つの重りをロボットアーム部の先端に交互に取付け、重り毎に、関節軸に連結されたロボットアーム部の回転方向のたわみ量を計測する。そして、前述した２つの重りの質量の差と、重り毎に計測されたアーム部のたわみ量の差とから、関節軸のばね定数を算出している。

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

しかしながら、特許文献２、特許文献３および特許文献４に開示されるようなばね定数の決定方法には、各々、次のような問題点がある。

特許文献２に開示される発明については、制御部から関節軸に指令角度が与えられたときに関節軸の実際の回転角度を計測する必要がある。このため、回転角度センサを関節軸に取付ける作業が生じ、またコストもかかる。

特許文献３に開示される発明においては、ばね定数を算出するために何らかの方法によって関節軸の理論上の指令位置（角度）を予め決定しておく必要がある。しかし、そのような理論上の指令位置を適切に決定するのは当業者にとって簡単ではない。

特許文献４に開示される発明については、ばね定数の算出のために重りをロボットアーム部の先端に取付ける作業が必要である。このため、重りを取付ける作業は作業者にとって重労働となる。

そこで本発明は、上述したような問題点に鑑み、安価で、かつ作業者にとって簡易な作業により、前述したばね定数を求めることができるたわみ計測システムを提供することを目的とする。

本発明の第一態様によれば、多関節ロボットの機構部のたわみを計測するたわみ計測システムであって、
多関節ロボットの任意の機構部と該多関節ロボットから離れた位置とのうちの一方に配置された被測定マークと、
多関節ロボットの任意の機構部と該多関節ロボットから離れた位置とのうちの他方に配置されていて、被測定マークの位置を測定する位置計測器と、
多関節ロボットと位置計測器をそれぞれ制御する制御装置とを備え、
制御装置は、
被測定マークと位置計測器の相対的位置関係が機構部のたわみによってのみ変化するように、多関節ロボットの姿勢を変更させるロボット制御部と、
前記の姿勢の変更前と変更後のそれぞれにおいて位置計測器により被測定マークの位置を計測し、前記の姿勢の変更前に計測された被測定マークの位置と前記の姿勢の変更後に計測された被測定マークの位置との間の移動量に基づいて、機構部の実たわみ量を算出するたわみ量計算部とを有する、たわみ計測システムが提供される。

本発明の第二態様によれば、多関節ロボットの機構部のたわみを計測するたわみ計測システムであって、
それぞれ、多関節ロボットの異なる二つのリンクに配置された二つの被測定マークと、
多関節ロボットから離れた位置に二つの被測定マークの両方と相対して配置されていて、二つの被測定マークの両方の位置を測定する位置計測器と、
多関節ロボットと位置計測器をそれぞれ制御する制御装置とを備え、
制御装置は、
二つの被測定マークの相対的位置関係が二つのリンクを連結する関節軸のたわみによってのみ変化するように、多関節ロボットの姿勢を変更させるロボット制御部と、
前記の姿勢の変更前と変更後のそれぞれにおいて、位置計測器により二つの被測定マークの両方の位置を計測し、前記の姿勢の変更前に計測された二つの被測定マークのうちの一方の被測定マークの位置と前記の姿勢の変更後に計測された当該一方の被測定マークとの間の移動量と、前記の姿勢の変更前に計測された二つの被測定マークのうちの他方の被測定マークの位置と前記の姿勢の変更後に計測された当該他方の被測定マークとの間の移動量との差に基づいて、関節軸の実たわみ量を算出するたわみ量計算部とを有する、たわみ計測システムが提供される。

本発明の第三態様によれば、第一態様または第二態様のたわみ計測システムであって、
制御装置は、
前記の姿勢の変更前と変更後のそれぞれにおいて、機構部もしくは関節軸に作用するトルクを取得し、
前記の姿勢の変更前に取得されたトルクと前記の姿勢の変更後に取得されたトルクとの間の変化量を算出し、
算出された実たわみ量、トルクの変化量、および空間上の被測定マークの位置に基づき、機構部を回転ばねとみなしたときの当該機構部のばね定数を算出するばね定数算出部とをさらに有する、たわみ計測システムが提供される。

本発明の第四態様によれば、第一態様から第三態様のいずれかのたわみ計測システムであって、被測定マークは発光体である、たわみ計測システムが提供される。

本発明の第五態様によれば、第一態様から第四態様のいずれかのたわみ計測システムであって、位置計測器はＣＣＤカメラである、たわみ計測システムが提供される。

本発明の第六態様によれば、第一態様から第四態様のいずれかのたわみ計測システムであって、位置計測器はＰＳＤである、たわみ計測システムが提供される。

本発明の第一態様、第二態様、第四態様、第五態様および第六態様によれば、作業者はロボットおよびその近傍に被測定マークとカメラなどの位置計測器とを設置することにより、多関節ロボットの姿勢に応じてリンクや関節軸などの機構部に発生する実たわみ量を取得することができる。このような被測定マークと位置計測器の設置作業は、従来技術のように重りや回転角度センサの取付け作業と比べ、作業者にとって簡易な作業となる。

さらに、本発明の第一態様および第二態様によれば、多関節ロボットの姿勢を変更させたときの被測定マークの位置の変化から、機構部の実たわみ量を求めるようにしている。さらに言えば、被測定マークの位置の変化を位置計測器により測定する際に、被測定マークと位置計測器の相対的位置関係が機構部のたわみによってのみ変化するように多関節ロボットの姿勢を変更させるようにしている。このことにより、当該機構部に発生している実たわみ量のみが正確に得られる。

さらに、本発明の第三態様によれば、前述したように正確に得られた実たわみ量を機構部のばね定数の算出に利用しているため、当該ばね定数を精度よく定めることができる。

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

第一実施形態のたわみ計測システムの全体構成を示した図である。 第一実施形態のたわみ計測システムにおける制御構成要素を示したブロック図である。 図１に示されたロボットの構造をより詳しく説明するための図である。 図３に示されたロボットを構成する各関節軸と各アーム部をモデル化して表した図である。 第一実施形態のたわみ計測システムによってロボットのたわみを計測するときの処理フローを示すフローチャートである。 図１に示されたロボットの１番目の計測姿勢を示した図である。 図１に示されたロボットの２番目の計測姿勢を示した図である。 図６に示されたロボットの１番目の計測姿勢のときにカメラにより撮像された画像と、図７に示されたロボットの２番目の計測姿勢のときにカメラにより撮像された画像とを重ねあわせたものを表した図である。 第二実施形態のたわみ計測システムの全体構成を示した図である。 第二実施形態のたわみ計測システムによってロボットのたわみを計測するときの処理フローを示すフローチャートである。 図９に示されたロボットの１番目の計測姿勢を示した図である。 図９に示されたロボットの２番目の計測姿勢を示した図である。

次に、本発明の実施形態について図面を参照して説明する。以下の図面において、同じ部材には同じ参照符号が付けられている。そして、異なる図面において同じ参照符号が付されたものは同じ機能を有する構成要素であることを意味するものとする。また、理解を容易にするために、これらの図面は縮尺を適宜変更している。また本発明は、図面に描かれた形態に限定されるものでない。

まず、本発明の概要について述べる。
多関節ロボットにおいては、ロボットアーム部の先端に取付けられたツールの重さが各関節軸や関節軸間のリンクに作用して関節軸やリンクがたわむ場合がある。このように関節軸やリンクがたわむ場合、例えば、ロボットアーム部の先端に取付けられたツールを当該ツールの目標位置に基づいて位置決めしようとしても、位置決めされたツールの実位置は目標位置からずれてしまう。したがって、このようにロボットアーム部の先端に取付けられたツールを位置決め制御するときには、関節軸やリンクのたわみ量を推定し、このたわみ量の推定値に基づいて、ツールの目標位置を補正することが必要となる。

このようなツールの位置決め制御において前述した関節軸やリンクのたわみ量を推定する際、従前より、関節軸またはリンクを回転ばねとみなして、この回転ばねのばね定数を算定することが行われている。回転ばねのばね定数が定まると、当該回転ばねの回転軸回りに作用する負荷トルクの値を当該ばね定数の値で除算することにより回転ばねのたわみ量（たわみ角）を求めることができる。つまり、回転ばねとみなした関節軸またはリンクの回転軸回りに所定の負荷トルクが作用したときの、当該関節軸またはリンクの回転軸回りのたわみ量（たわみ角）を推定できるようになる。また、前述したばね定数をより正確に定められれば、関節軸またはリンクの回転軸回りのたわみ量（たわみ角）もより正確に推定できるようになり、前述したロボットアーム部の先端のツールの位置決め精度を向上させることができる。

そこで、本願においては、前述したロボットアーム部の位置決め制御において関節軸またはリンクのたわみ量を推定するのに必要なばね定数を正確に算定することが可能な発明を提案することとする。特に、本願においては、ロボットアーム部を位置決め制御したときに実際に関節軸またはリンクに発生するたわみ量を位置計測器により計測し、計測された実際のたわみ量（以下、実たわみ量と称す。）に基づいて、前述のばね定数を正確に求める発明を提案することとする。

よって、以下に説明する各実施形態においては、多関節ロボットの関節軸またはリンクの実たわみ量を計測することが可能なたわみ計測システムを示すこととする。さらに、以下の各実施形態においては、関節軸またはリンクの実たわみ量を用いて、関節軸またはリンクのばね定数を決定することが可能なたわみ計測システムも示すこととする。

（第一実施形態）
図１は、第一実施形態のたわみ計測システムの全体構成を示した図である。
図１に示されるように、第一実施形態のたわみ計測システム１０は、ロボット１１の所定の機構部に取付けられた被測定マーク１２と、被測定マーク１２の位置を計測する位置計測器としてのカメラ１３と、ロボット１１およびカメラ１３をそれぞれ制御する制御装置１４と、を備える。

さらに、制御装置１４は、ロボット制御部３１と実たわみ量計算部３２とばね定数算出部３３とを含んでいる。ロボット制御部３１は、被測定マーク１２とカメラ１３の相対的位置関係が上記の機構部のたわみによってのみ変化するように、ロボット１１の姿勢を変更させる。そして、たわみ計算部３２は、ロボット１１の姿勢の変更前と変更後のそれぞれにおいてカメラ１３により被測定マーク１２の位置を計測し、ロボット１１の姿勢の変更前に計測された被測定マーク１２の位置とロボット１１の姿勢の変更後に計測された被測定マーク１２の位置との間の移動量に基づいて、上記の機構部の実たわみ量を算出する。さらに、ばね定数計算部３３は、上記のロボット１１の姿勢の変更前と変更後のそれぞれにおいて、上記の機構部に作用するトルクを取得し、取得されたトルクの変化量を算出し、算出された実たわみ量、トルクの変化量、および空間上の被測定マーク１２の位置に基づき、上記の機構部を回転ばねとみなしたときの当該機構部のばね定数を算出する。

図１に示されるロボット１１は、典型的な垂直多関節ロボットである。ロボット１１と制御装置１４とはケーブル２２を介して互いに接続されている。さらに、カメラ１３と制御装置１４とはケーブル２３を介して接続されている。

被測定マーク１２は、ロボット１１の所定の機構部、例えばリンクもしくは関節軸、におけるたわみ量を計測するために使用される。本実施形態においては、被測定マーク１２は、ロボット１１における第一関節軸と第二関節軸との間のリンク（図３の符号１１ｂ１参照）に取付けられている。

カメラ１３は、ロボット１１から離れた位置に設置および固定されている。さらに、カメラ１３は、カメラ１３により撮像された画像内に被測定マーク１２の全体像が表示されるように、ロボット１１上の被測定マーク１２と相対して設置および固定される。カメラ１３の設置には例えば三脚が用いられる。さらに、ロボット１１がどのように動作してもカメラ１３の位置が変わらないように、カメラ１３はロボット１１の動作範囲より外側に設置されている。

図２は、たわみ計測システム１０における制御構成要素を示したブロック図である。
前述したロボット１１は、ｎ個の関節軸と、それぞれの関節軸間を連結する（ｎ−１）個のリンクとからなるロボットアーム部を備える。そして、図２に示されるようにｎ個のモータ１５−１〜１５−ｎがそれぞれｎ個の関節軸に設けられている。なお、ｎは自然数であり、本実施形態においては、ｎは６である。また、各リンクはアーム部とも呼ばれている。

図２に示されるように、制御装置１４内には、ＣＰＵ１６と、ＲＡＭ、ＲＯＭ、不揮発性メモリなどを含むメモリ１７と、ｎ個のサーボ制御器１８−１〜１８−ｎと、ｎ個のサーボアンプ１９−１〜１９−ｎと、画像処理プロセッサ２０と、カメラインタフェイス（カメラＩ／Ｆ）２１とが備えられている。このような構成要素は、バス２４を通じて相互接続されている。なお、図２に示されるＣＰＵ１６が、前述したロボット制御部３１、実たわみ量計算部３２、およびばね定数算出部３３に対応するものとなっている。

前述したメモリ１７のＲＯＭには、ロボット１１と制御装置１４の基本機能を支えるシステムプログラムが格納されている。さらに、このＲＯＭには、カメラ１３により撮像されたロボット１１の画像から被測定マーク１２を検出する画像処理プログラム、および公知のＮｅｗｔｏｎ−Ｅｕｌｅｒ法によってロボット１１の各関節軸に作用する負荷トルクを計算するプログラムも格納されている。

前述したメモリ１７の不揮発性メモリには、ロボット１１の動作プログラムと、後述するロボット１１のたわみ量を計算する際に用いられるパラメータ、例えば被測定マーク１２の寸法とが格納されている。

前述したメモリ１７のＲＡＭは、カメラ１３により撮像されたロボット１１の画像を生成する映像信号のデータや、ＣＰＵ１６が実行する各種演算処理におけるデータなどを一時記憶する記憶領域として使用される。

サーボ制御器１８−１〜１８−ｎは、ＣＰＵ１６により、ロボット１１を制御するための演算処理、例えばロボット１１の各関節軸に作用するトルクの計算、逆変換の計算など、を経て作成された移動指令を受取る。そして、サーボ制御器１８−１〜１８−ｎは、受取った移動指令と、ロボット１１の各関節軸に付属するパルスコーダ（図示せず）から受取るフィードバック信号とを比較しつつ、サーボアンプ１９−１〜１９−ｎにそれぞれトルク指令を出力する。そして、各サーボアンプ１９−１〜１９−ｎは、各トルク指令に基づいて電流を各関節軸のモータ１５−１〜１５−ｎに供給して各モータ１５−１〜１５−ｎを駆動する。

また、図２に示されるカメラ１３は、例えばＣＣＤカメラであり、制御装置１４内のカメラインタフェイス２１と接続されている。ＣＣＤカメラは、撮像対象を受光デバイス面、例えばＣＣＤアレイ面により二次元画像として検出する機能を有する。

図２に示された制御装置１４においては、ＣＰＵ１６からカメラインタフェイス２１を介してカメラ１３に撮像指令が送られた時に、カメラ１３に設定された電子シャッタ機能により撮影が実行される。そして、カメラインタフェイス２１を介して映像信号、すなわちカメラ１３の撮影した画像がメモリ１７のＲＡＭに格納される。ＲＡＭに格納された映像信号は、画像処理プロセッサ２０を利用して解析される。さらに、ＲＡＭ内の画像処理プログラムによって、カメラ１３の撮影した画像上の被測定マーク１２の位置と寸法が求められる。

図３は、図１に示されたロボット１１の構造をより詳しく説明するための図である。また、図４は、図３に示されたロボット１１の各関節軸と各リンクをモデル化して、それぞれの関節軸の位置や回転方向を表した図である。
本実施形態のロボット１１は、図３および図４に示されるように、第一関節軸１１ａ１、第二関節軸１１ａ２、第三関節軸１１ａ３、第四関節軸１１ａ４、第五関節軸１１ａ５、および第六関節軸１１ａ６を有している。また、それぞれの関節軸間にリンクが存在する。また、各々の関節軸１１ａ１〜１１ａ６は、隣接する二つのリンク同士を互いに連結しているロボット部位であり、隣接する二つのリンクはリンク間の関節軸を中心として相対的に回転することができる。

そして、図３および図４から分かるように、第二関節軸１１ａ２、第三関節軸１１ａ３および第五関節軸１１ａ５はそれぞれ、関節軸回りに負荷トルクが作用する軸部である。また、第一関節部１１ａ１と第二関節部１１ａ２との間のリンク１１ｂ１もまた、垂直方向に位置する第一関節軸１１ａ１の上端部を回転支点とした負荷トルクが作用するロボット部位と考えられる。

そこで、第一実施形態においては、そのような負荷トルクの作用によりリンク１１ｂ１がたわむと想定し、上述したたわみ計測システム１０により、リンク１１ｂ１の実たわみ量を計測することとする。このため、被測定マーク１２は、リンク１１ｂ１に取付けられている。

さらに、図３に示されるように、実空間でのリンク１１ｂ１上に、以下の条件１）〜３）を満たすワールド座標系（ＸＹＺ座標系）が設けられており、このワールド座標系を用いてリンク１１ｂ１の実たわみ量が計測されるようにする。
１）Ｚ軸は第一関節軸１１ａ１の回転軸と一致する。
２）Ｙ軸は第二関節軸１１ａ２の回転軸と平行である。
３）第二関節軸１１ａ２の回転軸はＸＹ平面上にある。

また、図３中の符号Ｏ_wは、リンク１１ｂ１上に定義されたワールド座標系の原点を表している。この原点Ｏ_wを基準とするＸＹＺ軸の方向は、図３中に拡大図によって示されている。図３に示されたワールド座標系においては、図３の右方向に＋Ｘ軸、図３の上方向に＋Ｚ軸、図３の紙面に対して奥行方向に＋Ｙ軸をそれぞれ定義している。

なお、以降の各実施形態において記述される「ワールド座標系」という用語は、すべて、前述したようにロボット１１上に定義されたＸＹＺ座標系を指すものとする。

次に、第一実施形態のたわみ計測システム１０の動作を説明する。但し、以下では、図３に示されたＸＺ平面でのリンク１１ｂ１の実たわみ量を計測する場合を例にして説明することとする。

図５は、第一実施形態のたわみ計測システム１０によってロボット１１のたわみを計測するときの処理フローを示すフローチャートである。

ロボット１１のたわみ計測を開始したとき、まず、図５のステップＳ１１に示されるように、制御装置１４は、ロボット１１の計測姿勢の順番をカウントするカウンタ（図示せず）の値Ｎを１にセットする。さらに、制御装置１４のロボット制御部３１は、ロボット１１の計測姿勢がＮ番目（すなわちＮ←１）の計測姿勢になるように、ロボット１１を移動させる。（図５のステップＳ１２）。

図６はロボット１１の１番目の計測姿勢を示した図であり、図７はロボット１１の２番目の計測姿勢を示した図である。
第一実施形態においては、図６に示されたロボット１１の１番目の計測姿勢と、図７に示されたロボット１１の２番目の計測姿勢とにおいて、第一関節軸１１ａ１のモータに与えられる回転指令角度の値は同じ値にしている。つまり、図６に示された１番目の計測姿勢から図７に示された２番目の計測姿勢にロボット１１の姿勢を変更させる際、第一関節軸１１ａ１は軸回りに回転しないようにする。これによって、ロボット１１のリンク１１ｂ１に取付けられた被測定マーク１２と、ロボット１１上の被測定マーク１２と相対して設置されたカメラ１３（図１参照）との相対的な位置関係は、ロボット１１のリンク１１ｂ１のたわみの影響のみによって変化することになる。

再び図５を参照すると、上述したステップＳ１２の後、制御装置１４は、ロボット１１の姿勢が前述した１番目の計測姿勢であるときにカメラ１３により被測定マーク１２を撮像する（図５のステップＳ１３）。なお、図６および図７から分かるように、本実施形態の被測定マーク１２は円形の図柄とされている。

さらに、制御装置１４はカメラ１３の撮像した画像に対して画像処理を実施することによって、画像上の被測定マーク１２の位置ｐ_Nと寸法ｄ_Nを取得する（図５のステップＳ１４）。続いて、前述したＮの値を一つ大きくする（図５のステップＳ１５）。その後、制御装置１４によりＮの値が２より大きいか否かを判断し（図５のステップＳ１６）、仮にＮの値が２より大きくない場合は上述したステップＳ１２〜ステップＳ１５を繰返す。

これにより、図６に示されたロボット１１の１番目の計測姿勢のときのカメラ画像上の被測定マーク１２の位置ｐ₁と寸法ｄ₁、ならびに、図７に示されたロボット１１の２番目の計測姿勢のときのカメラ画像上の被測定マーク１２の位置ｐ₂と寸法ｄ₂、が取得される。なお、上述したステップＳ１６においてＮの値が２より大きい場合は、制御装置１４はカメラ１３による被測定マーク１２の計測を終了する。続いて、制御装置１４は、取得された被測定マーク１２の位置ｐ₁および位置ｐ₂から、カメラ画像上における被測定マーク１２の移動量ｐ’を計算する（図５のステップＳ１７）。

そして、ステップＳ１７の後、制御装置１４は、前述したカメラ画像上の被測定マーク１２の移動量ｐ’と寸法ｄ₁、ｄ₂から、ワールド座標系での被測定マーク１２の実移動量Ｐを計算する（図５のステップＳ１８）。

ここで、ステップＳ１７およびステップＳ１８について、より具体的に説明する。
図８は、図６に示されたロボット１１の１番目の計測姿勢のときにカメラ１３により撮像された画像と、図７に示されたロボット１１の２番目の計測姿勢のときにカメラ１３により撮像された画像とを重ねあわせたものを表している。

さらに、図８においては、ロボット１１の１番目の計測姿勢のときに撮像された被測定マーク１２が被測定マーク１２−１として表示され、ロボット１１の２番目の計測姿勢のときに撮像された被測定マーク１２が被測定マーク１２−１として表示されている。また、図８において、被測定マーク１２−１の中心Ｃ１から被測定マーク１２−２の中心Ｃ２に向かう矢印Ｑの長さは、画像１３ａ上における被測定マーク１２−１と被測定マーク１２−２との間の移動量ｐ’を表している。なお、本実施形態のカメラ１３による画像１３ａは、図８に示されるように長方形の画像である。

前述したステップＳ１７にて被測定マーク１２の移動量ｐ’を計算するには、まず、制御装置１４は、図８に示された画像１３ａから、前述した被測定マーク１２−１の位置ｐ₁と、前述した被測定マーク１２−２の位置ｐ₂とをそれぞれ取得する。このとき、被測定マーク１２−１の位置ｐ₁として被測定マーク１２−１の中心Ｃ１の座標値が取得され、また、被測定マーク１２−２の位置ｐ₂としては被測定マーク１２−２の中心Ｃ２の座標値が取得される。

なお、図８に示される長方形の画像１３ａ上の座標値は、画像１３ａの左上角を原点Ｏ_cとし、原点Ｏ_cから図８の右方向に＋Ｙ軸、原点から図８の下方向に＋Ｘ軸を定義した二次元座標系により得られるものとする。このため、前述の被測定マーク１２−１の中心Ｃ１の座標値は（ｘ₁，ｙ₁）として表され、前述の被測定マーク１２−２の中心Ｃ２の座標値は（ｘ₂，ｙ₂）として表される。それらの座標値は画像１３ａを構成している画素での座標値（単位：ピクセル）とする。

そして、制御装置１４は、画像１３ａ上の被測定マーク１２−２の中心Ｃ２の座標値（ｘ₂，ｙ₂）から、画像１３ａ上の被測定マーク１２−１の中心Ｃ１の座標値（ｘ₁，ｙ₁）を減算することにより、画像１３ａ上の被測定マーク１２の移動量ｐ’（＝（ｐ_x，ｐ_y））を算出する。なお、ｐ_xは画面１３ａの座標系のＸ方向における移動量であり、ｘ₂からｘ₁を減算した値である。また、ｐ_yは画面１３ａの座標系のＹ方向における移動量であり、ｙ₂からｙ₁を減算した値である。

続いて、前述したステップＳ１８、すなわち、ワールド座標系での被測定マーク１２の実移動量Ｐを計算する処理を説明する。

被測定マーク１２の実移動量Ｐは、ワールド座標系の座標値（Ｐ_x，Ｐ_y，Ｐ_z）を用いて表されるものとする。そして、制御装置１４は以下の式（１）を用いることにより、画像１３ａ上の被測定マーク１２の移動量ｐ’を表す座標値（ｐ_x，ｐ_y）を、前述したワールド座標系の座標値（Ｐ_x，Ｐ_y，Ｐ_z）に変換することができる。

上記の式（１）においては、（２Ｄ／（ｄ₁＋ｄ₂））は、被測定マーク１２のワールド座標系での寸法Ｄと、画像１３ａ上における被測定マーク１２−１の寸法ｄ₁と被測定マーク１２−２の寸法ｄ₂との平均値との間の倍率を表している。

また本実施形態においては、寸法Ｄ、ｄ₁、およびｄ₂はそれぞれ、円形とした被測定マーク１２の直径（単位：ｍｍ）である。寸法ｄ₁と寸法ｄ₂の値は、上述した図５のステップＳ１２〜ステップＳ１５により取得される。寸法Ｄの値は、作業者により事前に取得して制御装置１４のメモリ１７に記憶されているものとする。

また、上記の式（１）から分かるように、図８に示された画像１３ａの座標系の＋Ｙ軸と＋Ｘ軸がそれぞれ図３に示されたワールド座標系の＋Ｘ軸と−Ｚ軸に対応するものとして、被測定マーク１２の実移動量Ｐを算出する。そして、上記の式（１）によって算出されるＰ_x，Ｐ_y，Ｐ_zの値が、図６に示されたロボット１１の計測姿勢から図７に示されたロボット１１の計測姿勢に変更されたときの被測定マーク１２の実移動量Ｐを表すものとなる。

但し、本実施形態においては、２次元のカメラ画像上の被測定マーク１２の移動量ｐ’をワールド座標系での座標値に変換するため、上記式（１）によって算出されるＰ_yの値は０となる。したがって、前述した実移動量Ｐを求める際には、上記の式（１）により算出されたＰ_xの値とＰ_zの値を用いて、次式（２）より、実移動量Ｐを算出することができる。以上の計算は、制御装置１４内のたわみ量計算部３２により行われる。

なお、前述した被測定マーク１２の実移動量Ｐを正確に取得するためには、カメラ画像上の被測定マーク１２の移動量ｐ’を正確に計測する必要がある。そのため、第一実施形態においては、図８に示された画像１３ａ内に被測定マーク１２−１と被測定マーク１２−２の両方が可能な限り大きく現れるように、カメラ１３が設置されていることが好ましい。

以上のようにして、第一実施形態のたわみ計測システム１０は、上記の式（１）および式（２）を用い、ワールド座標系での被測定マーク１２の実移動量Ｐを計算している。なお、第一実施形態においては、算出された実移動量Ｐは、図３に示されたワールド座標系のＸＺ平面でのリンク１１ｂ１の実たわみ量に相当する。

続いて、再び図５を参照すると、上述したステップＳ１８の後、制御装置１４は、上記式（１）による被測定マーク１２の実移動量Ｐと、後述するトルクの変化量、およびワールド座標系での被測定マーク１２の座標値などを用いて、リンク１１ｂ１のばね定数を決定する（図５のステップＳ１９）。

ここで、ステップＳ１９の内容（ばね定数の決定処理）を詳述する。

第一実施形態においては、前述したようにリンク１１ｂ１のたわみ量を考慮してロボット１１の先端のツールを位置決め制御するために、リンク１１ｂ１を回転ばねとみなして、この回転ばねのばね定数を事前に決定しておく。回転ばねのばね定数が定まれば、当該回転ばねの回転軸回りに作用する負荷トルクの値を当該ばね定数の値で除算することにより、回転ばねとしてのリンク１１ｂ１のたわみ量（たわみ角）を推定することができる。そして、前述したばね定数をより正確に定められれば、リンク１１ｂ１のたわみ量（たわみ角）もより正確に推定できるようになる。また、前述したツールの位置決め制御を実施する際、正確に推定されたリンク１１ｂ１のたわみ量に基づいてツールの目標位置を補正することにより、前述したロボットの先端のツールの位置決め精度を向上させることができる。

以上のことより、本実施形態のたわみ計測システム１０は、制御装置１４内のばね定数算出部３３によりリンク１１ｂ１のばね定数を決定する処理を行えるようにしている。
但し、第一実施形態においては、回転ばねとみなしたリンク１１ｂ１の回転軸はワールド座標系のＹ軸（図３参照）に一致するものとしている。つまり、第一実施形態においては、前述したワールド座標系でのＹ軸回りのリンク１１ｂ１のたわみ量を推定するためのばね定数を決定するものとする。

そこで、まず、ワールド座標系のＹ軸回りの負荷トルクによりリンク１１ｂ１が当該Ｙ軸回りにたわむとした場合の、Ｙ軸回りのリンク１１ｂ１の回転移動量（回転角度）を推定する。この回転移動量は、次式（３）のように、リンク１１ｂ１に作用するワールド座標系のＹ軸回りの負荷トルクの変化量をリンク１１ｂ１のばね定数で除算することにより求めることができる。
Ｍ＝（Ｔ２−Ｔ１）／ｋ₁ ・・・（３）

上記の式（３）において、Ｍは、リンク１１ｂ１の回転移動量の推定値を表している。ｋ₁は、求めるべきリンク１１ｂ１のばね定数である。

また、（Ｔ２−Ｔ１）は、リンク１１ｂ１に作用するワールド座標系のＹ軸回りの負荷トルクの変化量を表している。具体的には、（Ｔ２−Ｔ１）は、図７に示されたロボット１１の計測姿勢のときにリンク１１ｂ１に作用するワールド座標系のＹ軸回りの負荷トルクＴ２から、図６に示されたロボット１１の計測姿勢のときにリンク１１ｂ１に作用するワールド座標系のＹ軸回りの負荷トルクＴ１を減算した値とする。

さらに、上記の負荷トルクＴ１、Ｔ２の値は、制御装置１４に事前に記憶されているものとする。例えば、図６に示されたロボット１１の計測姿勢のときと図７に示されたロボット１１の計測姿勢のときに、制御装置１４は、公知のＮｅｗｔｏｎ−Ｅｕｌｅｒ法によってロボット１１の第一関節軸１１ａ１に作用する負荷トルクのワールド座標系のＹ軸回りの成分を計算し、それぞれの値をワールド座標系のＹ軸回りの負荷トルクＴ１、Ｔ２として制御装置１４のメモリ１７内に記憶させておく。なお、これまでの時点においては、ばね定数ｋ₁の値は未だ定まらない。そのため、上記式（３）により推定される回転移動量Ｍは、未定のばね定数ｋ₁を含む変数である。

続いて、ワールド座標系のＹ軸回りの負荷トルクがリンク１１ｂ１に作用することによりリンク１１ｂ１が当該Ｙ軸回りにたわむとした場合の、Ｙ軸回りの被測定マーク１２の移動量Ｐ’を推定する。以降、この推定される被測定マーク１２の移動量Ｐ’を、被測定マーク１２の推定移動量Ｐ’と呼ぶ。

このＹ軸回りの被測定マーク１２の推定移動量Ｐ’については、下記式（４）より算出することができる。つまり、前述した回転移動量Ｍ（回転角度）は微小であるとし、当該回転移動量を中心角とした円弧の長さが推定移動量Ｐ’であるとしている。
Ｐ’＝ｒ×Ｍ ・・・（４）

上記式（４）において、ｒは、図３に示されたワールド座標系の原点Ｏ_wからロボット１１上の被測定マーク１２の中心位置までの距離である。

上記の距離ｒの値については、作業者が、図３に示されたワールド座標系の原点Ｏ_wからロボット１１上の被測定マーク１２の中心位置までの距離ｒを実際に計測して、制御装置１４のメモリ１７に予め記憶させている。また、距離ｒを実測する時のロボット１１の姿勢は、前述したロボット１１の１番目の計測姿勢であってもよいし、２番目の計測姿勢であってもよい。

さらに、前述した推定移動量Ｐ’を算出する上記の式（４）については、上記式（３）を用いて、未定のばね定数ｋ₁を含む次式（５）に置換することができる。
Ｐ’＝ｒ×（（Ｔ２−Ｔ１）／ｋ₁） ・・・（５）
なお、上記の式（５）において、ばね定数ｋ₁以外のｒやＴ１、Ｔ２の値は上述したように制御装置１４のメモリ１７内に記憶されている。

そして、第一実施形態においては、上記式（５）の右辺の計算式の結果が上記式（２）により計算された実移動量Ｐの値と等しくなるように、リンク１１ｂ１のばね定数ｋ₁の値を決定する。つまり、前述した推定移動量Ｐ’と実移動量Ｐとの差がゼロになるように、ばね定数ｋ₁の値を決定している。このことにより、リンク１１ｂ１のばね定数ｋ₁は、リンク１１ｂ１の実たわみ量に則した正確な値となる。

また、第一実施形態においては、上記式（３）を用いることにより、ロボット１１のワールド座標系でのＹ軸回りに所定の負荷トルクが作用するときの、当該Ｙ軸回りのリンク１１ｂ１の回転移動量、すなわちたわみ量を推定することができる。このとき、前述のように算出されたばね定数ｋ₁の値を上記式（３）に事前に定めておくことにより、たわみ量の推定値もまた、リンク１１ｂ１の実たわみ量に則した正確な値として得られる。これにより、前述したようにロボット１１の先端のツールを位置決め制御するときに、そのようなたわみ量の推定値に基づいて、ツールの目標位置を正確に補正できるようになる。

なお、第一実施形態の制御装置１４のＣＰＵ１６は、上述したようにリンク１１ｂ１のばね定数ｋ₁を算出するばね定数算出部３３を備えている。そして、ＣＰＵ１６は、当該ばね定数算出部３３により算出したリンク１１ｂ１のばね定数ｋ₁をメモリ１７内に記憶させ、ロボット１１の制御時にメモリ１７から読出すものであることが好ましい。

また、上述した第一実施形態においては、ワールド座標系でのＹ軸回りのリンク１１ｂ１のたわみ量を実際に測定し、この実たわみ量に基づいて、リンク１１ｂ１のＹ軸回りのばね定数を決定している。しかし、本発明は、ワールド座標系でのＹ軸回りのリンク１１ｂ１の実たわみ量とばね定数を求めるものに限定されない。つまり、本発明においては、第一実施形態のロボット１１上の被測定マーク１２の取付位置を別の位置に変更して、ロボット１１のワールド座標系でのＸ軸回りまたはＺ軸回りのリンク１１ｂ１の実たわみ量とばね定数を求めるようにしてもよい。勿論、実たわみ量の計測対象やばね定数の算出対象においても、ロボット１１のリンク１１ｂ１以外のリンクでもよい。

また、上述した第一実施形態において、ロボット１１に取付けられる被測定マーク１２の数は一つに限られない。本発明においては、ばね定数ｋ₁の値をより精度良く決定するために、少なくとも一つの被測定マーク１２がロボット１１に取付けられていることが好ましい。また、カメラ１３の数については、少なくとも一つのカメラ１３が有ればよい。

さらに、上述した第一実施形態においては、被測定マーク１２はロボット１１に取付けられ、カメラ１３はロボット１１から離れた位置に固定されているが、本発明はこのような態様に限定されない。つまり、本発明においては、カメラ１３がロボット１１に取付けられ、被測定マーク１２がロボット１１から離れた位置に固定されていてもよい。

また、第一実施形態の被測定マーク１２は印刷した線の組合せからなるマークであるが、このようなマークに本発明は限定されない。また、本発明においては、被測定マーク１２として発光体が用いられていてもよい。被計測マーク１２として発光体を用いる場合は、位置計測器としてカメラ１３の代わりに、ＰＳＤ（Position sensitive detector）が用いられてもよい。つまり、上述した第一実施形態でのたわみ計測と同様に、ロボット１１の姿勢を変更させたときのＰＳＤ上の発光体の移動量を取得し、当該移動量からロボット１１の実たわみ量を計測すればよい。

（第二実施形態）
次に、本発明の第二実施形態について説明する。但し、ここでは、第一実施形態と同じ構成要素については同一の符号を使用して説明を割愛する。よって、第一実施形態の構成要素に対して異なる点のみを以下に述べる。

第二実施形態においては、図３に示されたワールド座標系での第二関節軸１１ａ２の軸回りの実たわみ量とばね定数を求める場合を例示することとする。

図９は、第二実施形態のたわみ計測システムの全体構成を示した図である。
前述した第二関節軸１１ａ２の軸回りのたわみを計測するため、二つの被測定マーク１２Ａ、１２Ｂがロボット１１に取付けられている。より具体的には、図９に示されるように、一方の被測定マーク１２Ａは、第二関節軸１１ａ２と第三関節軸１１ａ３との間のリンク１１ｂ２に配置されている。そして、他方の被測定マーク１２Ｂは、第一関節軸１１ａ１と第二関節軸１１ａ２との間のリンク１１ｂ１に配置されている。但し、図９に示されるように、２つの被測定マーク１２Ａ、１２Ｂ間の距離が可能な限り短くなるように、被測定マーク１２Ｂは第二関節軸１１ａ２の近傍に配置されていることが好ましい。

さらに、二つの被測定マーク１２Ａ、１２Ｂのそれぞれの寸法は等しく、また、両被測定マーク１２Ａ、１２Ｂの図柄も同じである。そして、カメラ１３の画像内に被測定マーク１２Ａと被測定マーク１２Ｂの両方の全体像が収まるように、カメラ１３は設置されている。

また、第二実施形態のロボット制御部３１は、二つの被測定マーク１２Ａ、１２Ｂの相対的位置関係が二つのリンク１１ｂ１、１１ｂ２を連結する第二関節軸１１ａ２のたわみによってのみ変化するように、ロボット１１の姿勢を変更させる。そして、第二実施形態のたわみ量計算部３２は、ロボット１１の姿勢の変更前と変更後のそれぞれにおいてカメラ１３により二つの被測定マーク１２Ａ、１２Ｂの両方の位置を計測し、ロボット１１の姿勢の変更前と変更後での二つの被測定マーク１２Ａ、１２Ｂの相対的な移動量に基づいて、第二関節軸１１ａ２の実たわみ量を算出する。さらに、第二実施形態のばね定数計算部３３は、上記のロボット１１の姿勢の変更前と変更後のそれぞれにおいて、第二関節軸１１ａ２に作用するトルクを取得し、取得されたトルクの変化量を算出し、算出された実たわみ量、トルクの変化量、および空間上の被測定マーク１２Ａの位置に基づき、上記の第二関節軸１１ａ２を回転ばねとみなしたときの当該第二関節軸１１ａ２のばね定数を算出する。

以上の点を除いた構成は、上述した第一実施形態のたわみ計測システム１０と同じである。また、ロボット１１の三次元位置を定義するワールド座標系についても、図３に示されたワールド座標系と同じように定義されているものとする。また、カメラ１３により撮像された画像や、当該画像に定義されるｘｙ座標系についても、上述した第一実施形態のカメラ１３の画像１３ａ（図８参照）と同じものとする。

次に、第二実施形態のたわみ計測システム１０の動作を説明する。

図１０は、第二実施形態のたわみ計測システム１０によってロボット１１のたわみを計測するときの処理フローを示すフローチャートである。

ロボット１１のたわみ計測を開始したとき、まず、図１０のステップＳ２１に示されるように、制御装置１４は、ロボット１１の計測姿勢の順番をカウントするカウンタ（図示せず）の値Ｎを１にセットする。さらに、制御装置１４のロボット制御部３１は、ロボット１１の計測姿勢がＮ番目（すなわちＮ←１）の計測姿勢になるように、ロボット１１を移動させる。（図１０のステップＳ２２）。

図１１は図９に示されたロボットの１番目の計測姿勢を示した図であり、図１２は図９に示されたロボットの２番目の計測姿勢を示した図である。
第二実施形態においては、図１１に示されたロボット１１の１番目の計測姿勢と、図１２に示されたロボット１１の２番目の計測姿勢とにおいて、ロボット１１の第一関節軸１１ａ１と第二関節軸１１ａ２のそれぞれのモータ（図示せず）に与えられる回転指令角度の値は同じ値にしている。つまり、ロボット１１の計測姿勢を図１１に示された１番目の計測姿勢から図１２に示された２番目の計測姿勢に変更させる際、第一関節軸１１ａ１と第二関節軸１１ａ２のそれぞれは軸回りに回転しないようにする。これによって、前述した被測定マーク１２Ａと被測定マーク１２Ｂとの相対的な位置関係は、第二関節軸１１ａ２の軸回りのたわみの影響のみによって変化することになる。

再び図１０を参照すると、上述したステップＳ２２の後、制御装置１４は、ロボット１１の姿勢が前述した１番目の計測姿勢であるときにカメラ１３により２つの被測定マーク１２Ａ、１２Ｂを同時に撮像する（図１０のステップＳ２３）。なお、第二実施形態の被測定マーク１２Ａ、１２Ｂは同じ円形の図柄からなる。また、被測定マーク１２Ａの形状および寸法はそれぞれ被測定マーク１２Ｂの形状および寸法と同じである。

さらに、制御装置１４はカメラ１３の撮像した画像に対して画像処理を実施することによって、画像上の被測定マーク１２Ａの位置ｐ^A _Nと寸法ｄ^A _N、および画像上の被測定マーク１２Ｂの位置ｐ^B _Nと寸法ｄ^B _Nを取得する（図１０のステップＳ２４）。続いて、前述したＮの値を一つ大きくする（図１０のステップＳ２５）。その後、制御装置１４によりＮの値が２より大きいか否かを判断し（図１０のステップＳ２６）、仮にＮの値が２より大きくない場合は上述したステップＳ２２〜ステップＳ２５を繰返す。

以上の計測処理により、図１１に示されたロボット１１の１番目の計測姿勢のときのカメラ画像上の被測定マーク１２Ａの位置ｐ^A ₁と寸法ｄ^A ₁および被測定マーク１２Ｂの位置ｐ^B ₁と寸法ｄ^B ₁が取得される。さらに、図１２に示されたロボット１１の２番目の計測姿勢のときのカメラ画像上の被測定マーク１２Ａの位置ｐ^A ₂と寸法ｄ^A ₂および被測定マーク１２Ｂの位置ｐ^B ₂と寸法ｄ^B ₂が取得される。なお、上述したステップＳ２６においてＮの値が２より大きい場合は、制御装置１４はカメラ１３による二つの被測定マーク１２Ａ、１２Ｂの計測を終了する。

続いて、制御装置１４は、取得された被測定マーク１２Ａの位置ｐ^A ₁と位置ｐ^A _2、および取得された被測定マーク１２Ｂの位置ｐ^B ₁と位置ｐ^B ₂から、カメラ画像上の二つの被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”を計算する（図１０のステップＳ２７）。

そして、ステップＳ２７の後、制御装置１４は、取得されたカメラ画像における二つの被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”と寸法ｄ^A ₁、ｄ^A ₂、ｄ^B ₁、ｄ^B ₂から、ワールド座標系での二つの被測定マーク１２Ａ、１２Ｂ間の相対的な実移動量Ｐを計算する（図１０のステップＳ２８）。

ここで、ステップＳ２７およびステップＳ２８について、より具体的に説明する。

前述したステップＳ２７において被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”を計算する。このとき、まず、制御装置１４は、前述したカメラ画像上の被測定マーク１２Ａの位置ｐ^A ₁と位置ｐ^A ₂として、各計測姿勢でのカメラ画像上の被測定マーク１２Ａの中心の座標値をそれぞれ取得する。前述した被測定マーク１２Ｂの位置ｐ^B ₁と位置ｐ^B ₂についても、制御装置１４は、各計測姿勢でのカメラ画像上の被測定マーク１２Ｂの中心の座標値をそれぞれ取得する。このとき、前述した１番目の計測姿勢でのカメラ画像上の被測定マーク１２Ａの中心の座標値は（ｘ^A ₁，ｙ^A ₁）として表され、前述した２番目の計測姿勢でのカメラ画像上の被測定マーク１２Ａの中心の座標値は（ｘ^A ₂，ｙ^A ₂）として表されるものとする。さらに、前述した１番目の計測姿勢での被測定マーク１２Ｂの中心の座標値は（ｘ^B ₁，ｙ^B ₁）として表され、前述した２番目の被測定マーク１２Ｂの中心の座標値は（ｘ^B ₂，ｙ^B ₂）として表されるものとする。

そして、制御装置１４は、前述した２番目の計測姿勢でのカメラ画像上の座標値（ｘ^A ₂，ｙ^A ₂）から、前述した１番目の計測姿勢でのカメラ画像上の座標値（ｘ^A ₁，ｙ^A ₁）を減算することにより、カメラ画像上での被測定マーク１２Ａの移動量ｐ^Aを算出する。

さらに、制御装置１４は、前述した２番目の計測姿勢でのカメラ画像上の座標値（ｘ^B ₂，ｙ^B ₂）から、前述した１番目の計測姿勢でのカメラ画像上の座標値（ｘ^B ₁，ｙ^B ₁）を減算することにより、カメラ画像上での被測定マーク１２Ｂの移動量ｐ^Bを算出する。

そして、制御装置１４は、算出された移動量ｐ^Aの座標値から、算出された移動量ｐ^Bを減算することにより、被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”を算出している。この算出結果もまた、上述した第一実施形態と同様にカメラ画像の座標系を用いて、ｐ”＝（ｐ_x，ｐ_y）と表されるものとする。なお、ｐ_xはカメラ画像の座標系でのＸ方向における移動量であり、（ｘ^A ₂−ｘ^A ₁）から（ｘ^B ₂−ｘ^B ₁）を減算した値である。また、ｐ_yはカメラ画像の座標系でのＹ方向における移動量であり、（ｙ^A ₂−ｙ^A ₁）から（ｙ^B ₂−ｙ^B ₁）を減算した値である。以上により、カメラ画像上における二つの被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”が取得される。

続いて、前述したステップＳ２８、すなわち、二つの被測定マーク１２Ａ、１２Ｂ間の相対的な実移動量Ｐを計算する処理を説明する。
被測定マーク１２の実移動量Ｐは、ロボット１１の三次元位置を定義するワールド座標系の座標値（Ｐ_x，Ｐ_y，Ｐ_z）を用いて表されるものとする。このワールド座標系の定義は、上述した第一実施形態と同じものとする（図３参照）。

本実施形態において、以下の式（６）を用いることにより、カメラ画像上における二つの被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”を表す座標値（ｐ_x，ｐ_y）を、前述したワールド座標系の座標値（Ｐ_x，Ｐ_y，Ｐ_z）に変換することができる。

上記の式（６）においては、（４Ｄ／（ｄ^A ₁＋ｄ^A ₂＋ｄ^B ₁＋ｄ^B ₂））は、同一寸法からなる被測定マーク１２Ａ、１２Ｂのワールド座標系での寸法Ｄと、カメラ画像上における寸法ｄ^A ₁、ｄ^A ₂、ｄ^B ₁およびｄ^B ₂の平均値との間の倍率を表している。また第二実施形態において、寸法Ｄ、ｄ^A ₁、ｄ^A ₂、ｄ^B ₁およびｄ^B ₂はそれぞれ被測定マークの直径（単位：ｍｍ）であるとする。寸法ｄ^A ₁、ｄ^A ₂、ｄ^B ₁およびｄ^B ₂のそれぞれの値は、上述した図１０のステップＳ２２〜ステップＳ２５により取得される。また、寸法Ｄの値は作業者により事前に測定されて制御装置１４のメモリ１７に記憶されているものとする。

また、上記の式（６）から分かるように、図８に示された画像１３ａの座標系の＋Ｙ軸と＋Ｘ軸がそれぞれ図３に示されたワールド座標系の＋Ｘ軸と−Ｚ軸に対応すると仮定して、二つの被測定マーク１２Ａ、１２Ｂ間の相対的な実移動量Ｐを算出する。そして、上記の式（６）によって算出されるＰ_x，Ｐ_y，Ｐ_zの値が、図１１に示されるロボット１１の計測姿勢から図１２に示されるロボット１１の計測姿勢に変更されたときの二つの被測定マーク１２Ａ、１２Ｂ間の相対的な実移動量Ｐを表すものとなる。

但し、本実施形態においては、上述したように２次元のカメラ画像上における二つの被測定マーク１２Ａ、１２Ｂ間の相対移動量ｐ”をワールド座標系での座標値に変換するため、上記式（６）によって算出されるＰ_yの値は０となる。したがって、実移動量Ｐの値を求める際には、上記の式（６）により算出されたＰ_xの値とＰ_zの値を用いて、実移動量Ｐを算出することができる。この計算式は、上述の第一実施形態に記載されている式（２）と同じである。そして、第二実施形態においては、算出された実移動量Ｐは、図１２に示される第二関節軸１１ａ２の軸回りのたわみ量に相当することになる。

以上のようにして、第二実施形態のたわみ計測システム１０は、カメラ１３により計測されたロボット１１の上の二つの被測定マーク１２Ａ、１２Ｂ間の相対位置の変化量に基づいて、ロボット１１の第二関節軸１１ａ２の軸回りのたわみ量を取得している。また、このたわみ量の計算は、制御装置１４のＣＰＵ１６内のたわみ量計算部３２により行われる。

続いて、再び図１０を参照すると、上述したステップＳ２８の後、制御装置１４は、前述した二つの被測定マーク１２Ａ、１２Ｂ間の相対的な実移動量Ｐと、後述するトルクの変化量、およびワールド座標系での被測定マーク１２Ａの位置などを用いて、第二関節軸１１ａ２のばね定数を決定する（図１０のステップＳ２９）。

ここで、上記ステップＳ２９の内容（ばね定数の決定処理）を詳述する。

第二実施形態においては、前述したように第二関節軸１１ａ２の軸回りのたわみ量を考慮してロボット１１の先端のツールを位置決め制御するために、第二関節軸１１ａ２を回転ばねとみなして、この回転ばねのばね定数を事前に決定しておく。回転ばねのばね定数が定まれば、当該回転ばねの回転軸回りに作用する負荷トルクの値を当該ばね定数の値で除算することにより、回転ばねとしての第二関節軸１１ａ２の軸回りのたわみ量（たわみ角）を推定することができる。そして、前述したばね定数をより正確に定められれば、第二関節軸１１ａ２の軸回りのたわみ量（たわみ角）もより正確に推定できるようになる。そして、前述したツールの位置決め制御を実施する際、正確に推定された第二関節軸１１ａ２の軸回りのたわみ量に基づいてツールの目標位置を補正することにより、前述したロボットの先端のツールの位置決め精度を向上させることができる。

以上のことより、第二実施形態のたわみ計測システム１０は、制御装置１４内のばね定数算出部３３により、第二関節軸１１ａ２の軸回りのばね定数を決定する処理を行えるようにしている。

まず、第二関節軸１１ａ２の軸回りに負荷トルクが作用する場合の、当該第二関節軸１１ａ２の軸回りの回転移動量（回転角度）を推定する。この回転移動量は、次式（７）のように、第二関節軸１１ａ２の軸回りに作用する負荷トルクの変化量を第二関節軸１１ａ２の軸回りのばね定数で除算することにより求めることができる。
Ｍ＝（Ｔ２−Ｔ１）／ｋ₂ ・・・（７）

上記の式（７）において、Ｍは、第二関節軸１１ａ２の軸回りの回転移動量の推定値を表している。ｋ₂は、求めるべき第二関節軸１１ａ２のばね定数である。

また、上記の式（７）中の（Ｔ２−Ｔ１）は、第二関節軸１１ａ２の軸回りに作用する負荷トルクの変化量を表している。具体的には、（Ｔ２−Ｔ１）の値は、図１２に示されたロボット１１の計測姿勢のときに第二関節軸１１ａ２の軸回りに作用する負荷トルクＴ２から、図１１に示されたロボット１１の計測姿勢のときに第二関節軸１１ａ２の軸回りに作用する負荷トルクＴ１を減算した値とする。

さらに、上記の負荷トルクＴ１、Ｔ２の値は、制御装置１４に事前に記憶されているものとする。例えば、図１１に示されたロボット１１の計測姿勢のときと図１２に示されたロボット１１の計測姿勢のときに、制御装置１４は、第二関節軸１１ａ２を回転駆動するモータを流れる電流値からトルク値をそれぞれ取得して制御装置１４のメモリ１７内に記憶させておく。なお、これまでの時点においては、第二関節軸１１ａ２のばね定数ｋ₂の値は未だ定まらない。そのため、上記式（７）により推定される回転移動量Ｍは、未定のばね定数ｋ₂を含む変数である。

続いて、第二関節軸１１ａ２の軸回りに負荷トルクが作用することにより第二関節軸１１ａ２の軸回りに第二関節軸１１ａ２がたわんだとした場合の、二つの被測定マーク１２Ａ、１２Ｂ間の相対的な移動量Ｐ’を推定する。

この二つの被測定マーク１２Ａ、１２Ｂ間の相対的な推定移動量Ｐ’については、前述した回転移動量Ｍ（回転角度）を中心角とする円弧の長さが推定移動量Ｐ’であると仮定して、下記式（８）より算出することができる。式（８）は、上述した第一実施形態の式（４）と同じである。
Ｐ’＝ｒ×Ｍ ・・・（８）

但し、上記式（８）において、ｒは、ワールド座標系での第二関節軸１１ａ２の軸芯の位置から被測定マーク１２Ａの中心位置までの距離を表している。

上記の距離ｒの値については、作業者が、ワールド座標系での第二関節軸１１ａ２の軸芯の位置から被測定マーク１２Ａの中心位置までの距離ｒを実際に計測して、制御装置１４のメモリ１７に予め記憶させている。また、距離ｒを実測する時のロボット１１の姿勢は、前述したロボット１１の１番目の計測姿勢であってもよいし、前述した２番目の計測姿勢であってもよい。

さらに、前述した推定移動量Ｐ’を算出する上記の式（８）については、上記式（７）を用いて、未定のばね定数ｋ₂を含む次式（９）に置換することができる。
Ｐ’＝ｒ×（（Ｔ２−Ｔ１）／ｋ₂） ・・・（９）
なお、上記の式（９）において、ばね定数ｋ₂以外のｒやＴ１、Ｔ２の値は上述したように既に取得されていて制御装置１４のメモリ１７内に記憶されている。

そして、第二実施形態においては、上記式（９）の右辺の計算式の結果が上記の式（６）および式（２）により計算された実移動量Ｐの値と等しくなるように、第二関節軸１１ａ２のばね定数ｋ₂の値を決定する。つまり、前述した推定移動量Ｐ’と実移動量Ｐとの差がゼロになるように、前述のばね定数ｋ₂の値を決定している。このことにより、第二関節軸１１ａ２のばね定数ｋ₂は、第二関節軸１１ａ２の軸回りの実たわみ量に則した正確な値となる。

また、第二実施形態においては、上記式（７）を用いることにより、ワールド座標系での第二関節軸１１ａ２の軸回りに負荷トルクが作用する場合の、当該軸回りの第二関節軸１１ａ２の回転移動量、すなわちたわみ量を推定することができる。このとき、前述のように算出されたばね定数ｋ₂の値を上記式（７）に事前に定めておくことにより、たわみ量の推定値もまた、第二関節軸１１ａ２の軸回りの実たわみ量に則した正確な値として得られる。これにより、前述したようにロボット１１の先端のツールを位置決め制御するときに、そのようなたわみ量の推定値に基づいて、ツールの目標位置を正確に補正できるようになる。

なお、第二実施形態の制御装置１４のＣＰＵ１６は、上述したように第二関節軸１１ａ２のばね定数ｋ₂を算出するばね定数算出部３３を備えている。そして、ＣＰＵ１６は、当該ばね定数算出部３３により算出した第二関節軸１１ａ２のばね定数ｋ₂をメモリ１７内に記憶させ、ロボット１１の制御時にメモリ１７から読出すものであることが好ましい。

また、上述した第二実施形態においては、ワールド座標系での第二関節軸１１ａ２の軸回りのたわみ量を実際に測定し、この実たわみ量に基づいて、第二関節軸１１ａ２のばね定数を決定している。しかし、本発明は、ワールド座標系での第二関節軸１１ａ２の軸回りの実たわみ量とばね定数を求めるものに限定されない。つまり、本発明においては、第二実施形態のロボット１１上の二つの被測定マーク１２Ａ、１２Ｂの取付位置を別の関節軸、例えば第三関節軸１１ａ３や第五関節軸１１ａ５などの近傍に変更して、第三関節軸１１ａ３や第五関節軸１１ａ５などの実たわみ量とばね定数を求めるようにしてもよい。

さらに、上述した第二実施形態においては、二つの被測定マーク１２Ａ、１２Ｂの相対的な実移動量を取得できればよいので、ロボット１１の１回目の計測姿勢と２回目の計測姿勢においてカメラ１３の位置が変わってしまってもたわみ量を計算することができる。したがって、上述した第二実施形態においては、カメラ１３を固定せずに、作業者がカメラ１３を持ってロボット１１上の二つの被測定マーク１２Ａ、１２Ｂを撮影してもよい。

また、第二実施形態の二つの被測定マーク１２Ａ、１２Ｂは印刷した線の組合せからなるマークであるが、このようなマークに本発明は限定されない。また、本発明においては、各被測定マーク１２Ａ、１２Ｂとして発光体が用いられていてもよい。各被測定マーク１２Ａ、１２Ｂとして発光体を用いる場合は、位置計測器としてカメラ１３の代わりに、ＰＳＤ（Position sensitive detector）が用いられてもよい。つまり、上述した第一実施形態でのたわみ計測と同様に、ロボット１１の姿勢を変更させたときのＰＳＤ上の発光体の移動量を取得し、当該移動量からロボット１１の所定の機構部の実たわみ量を計測すればよい。

さらに、以上に説明したような第一実施形態および第二実施形態においては、上述したようにリンク１１ｂ１や第二関節軸１１ａ２などの機構部の実たわみ量やばね定数を取得するのに、作業者は被測定マークとカメラなどの位置計測器とを設置するだけで済む。つまり、第一実施形態および第二実施形態によるたわみ計測システムによれば、作業者にとって比較的簡易な作業により、機構部の実たわみ量やばね定数を取得することができる。

さらに、上述した第一実施形態および第二実施形態においては、カメラやＰＳＤなどの位置計測器により被測定マークの位置の変化を測定する際に、被測定マークと位置計測器の相対的位置関係が機構部のたわみによってのみ変化するようにロボット１１の姿勢を変更させるようにしている。このことにより、当該機構部に発生している実たわみ量のみが正確に得られる。さらに、このように正確に得られた実たわみ量を機構部のばね定数の算出に利用しているため、当該ばね定数を精度よく定めることができる。

以上、多関節ロボットを例にして本発明を説明したが、本発明は、多関節ロボットのたわみを計測する場合に限定されず、複数の関節軸により連結されたリンク機構を備える機械全般のたわみ計測に適用できる。

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

１０ たわみ計測システム
１１ ロボット
１１ａ１ 第一関節軸
１１ａ２ 第二関節軸
１１ａ３ 第三関節軸
１１ａ４ 第四関節軸
１１ａ５ 第五関節軸
１１ａ６ 第六関節軸
１１ｂ１、１１ｂ２ リンク
１２、１２−１、１２−２、１２Ａ、１２Ｂ 被測定マーク
１３ カメラ
１４ 制御装置
１５−１〜１５−ｎ モータ
１６ ＣＰＵ
１７ メモリ
１８−１〜１８−ｎ サーボ制御器
１９−１〜１９−ｎ サーボアンプ
２０ 画像処理プロセッサ
２１ カメラインタフェイス
２２、２３ ケーブル
２４ バス
３１ ロボット制御部
３２ たわみ量計算部
３３ ばね定数算出部

Claims (6)

1. 多関節ロボット（１１）の機構部のたわみを計測するたわみ計測システム（１０）であって、
   前記多関節ロボット（１１）の任意の機構部と前記多関節ロボット（１１）から離れた位置のうちの一方に配置された被測定マーク（１２）と、
   前記多関節ロボット（１１）の任意の機構部と前記多関節ロボット（１１）から離れた位置のうちの他方に配置されていて、前記被測定マーク（１２）の位置を測定する位置計測器（１３）と、
   前記多関節ロボット（１１）と前記位置計測器（１３）をそれぞれ制御する制御装置（１４）とを備え、
   前記制御装置（１４）は、
   前記被測定マーク（１２）と前記位置計測器（１３）の相対的位置関係が前記機構部のたわみによってのみ変化するように、前記多関節ロボット（１１）の姿勢を変更させるロボット制御部（３１）と、
   前記姿勢の変更前と変更後のそれぞれにおいて前記位置計測器（１３）により前記被測定マーク（１２）の位置を計測し、前記姿勢の変更前に計測された前記被測定マーク（１２）の位置と前記姿勢の変更後に計測された前記被測定マーク（１２）の位置との間の移動量に基づいて、前記機構部の実たわみ量を算出するたわみ量計算部（３２）とを有する、たわみ計測システム。
2. 多関節ロボット（１１）の機構部のたわみを計測するたわみ計測システム（１０）であって、
   それぞれ前記多関節ロボット（１１）の異なる二つのリンクに配置された二つの被測定マーク（１２Ａ、１２Ｂ）と、
   前記多関節ロボット（１１）から離れた位置に前記二つの被測定マーク（１２Ａ、１２Ｂ）の両方と相対して配置されていて、前記二つの被測定マーク（１２Ａ、１２Ｂ）の両方の位置を測定する位置計測器（１３）と、
   前記多関節ロボット（１１）と前記位置計測器（１３）をそれぞれ制御する制御装置（１４）とを備え、
   前記制御装置（１４）は、
   前記二つの被測定マーク（１２Ａ、１２Ｂ）の相対的位置関係が前記二つのリンクを連結する関節軸のたわみによってのみ変化するように、前記多関節ロボット（１１）の姿勢を変更させるロボット制御部（３１）と、
   前記姿勢の変更前と変更後のそれぞれにおいて前記位置計測器（１３）により前記二つの被測定マーク（１２Ａ、１２Ｂ）の両方の位置を計測し、
   前記姿勢の変更前に計測された前記二つの被測定マーク（１２Ａ、１２Ｂ）のうちの一方の被測定マーク（１２Ａ）の位置と前記姿勢の変更後に計測された当該一方の被測定マーク（１２Ａ）との間の移動量と、前記姿勢の変更前に計測された前記二つの被測定マーク（１２Ａ、１２Ｂ）のうちの他方の被測定マーク（１２Ｂ）の位置と前記姿勢の変更後に計測された当該他方の被測定マーク（１２Ｂ）との間の移動量との差に基づいて、前記関節軸の実たわみ量を算出するたわみ量計算部（３２）とを有する、たわみ計測システム。
3. 前記制御装置（１４）は、
   前記姿勢の変更前と変更後のそれぞれにおいて、前記機構部もしくは前記関節軸に作用するトルクを取得し、
   前記姿勢の変更前に取得されたトルクと前記姿勢の変更後に取得されたトルクとの間の変化量を算出し、
   算出された前記実たわみ量、前記トルクの変化量、および空間上の前記被測定マーク（１２、１２Ａ、１２Ｂ）の位置に基づき、前記機構部を回転ばねとみなしたときの当該機構部のばね定数を算出するばね定数算出部（３３）をさらに有する、請求項１または請求項２に記載のたわみ計測システム。
4. 前記被測定マーク（１２、１２Ａ、１２Ｂ）は発光体である、請求項１から３のいずれか一項に記載のたわみ計測システム。
5. 前記位置計測器（１３）はＣＣＤカメラである、請求項１から４のいずれか一項に記載のたわみ計測システム。
6. 前記位置計測器（１３）はＰＳＤである、請求項１から４のいずれか一項に記載のたわみ計測システム。

Images