JP2018036230A - たわみ角検出装置 - Google Patents

Abstract

【課題】既存の構成からの変更を抑えつつ、たわみ角を正確に検出することのできるたわみ角検出装置を提供する。【解決手段】入力軸エンコーダ４０と、減速機３０の出力軸３１に取り付けられたディスク５１、及びディスクに対して互いに異なる位置に配置されてディスクの角度位置をそれぞれ検出する検出素子５２（５２Ａ，５２Ｂ，５２Ｃ）を有する出力軸エンコーダ５０と、を備えるたわみ角検出装置であって、入力軸エンコーダ４０により検出された入力軸の角度位置に基づいて出力軸の基本角度位置を算出し、検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたディスクの角度位置に基づいて、出力軸の第１角度位置，第２角度位置，第３角度位置を算出し、基本角度位置とそれぞれ第１角度位置，第２角度位置，第３角度位置との差分である第１差分，第２差分，第３差分、及びディスクに対する検出素子の配置に基づいて、出力軸のたわみ角を算出する。【選択図】 図１

Description

本発明は、ロボット等の回転軸のたわみ角を検出するたわみ角検出装置に関する。

従来、ロボットの手先位置指令値に対応するロボットの姿勢から各回転軸にかかる関節トルクを求め、関節トルクとたわみ角との関係を示す変位モデルを用いて、関節トルクにより各回転軸のたわみ角を求めるものがある（特許文献１参照）。

特開平４−２３３６０２号公報

しかしながら、特許文献１に記載のものにおいて、変位モデルにおける関節トルクとたわみ角との関係が実際の関係からずれていたり、ロボットの回転軸に外力が作用したりする場合は、回転軸のたわみ角を正確に求めることができない。

また、高性能カメラや専用のたわみ角センサによりたわみ角を検出することも考えられるが、高精度のセンサ等を新たに追加する必要があり、既存の構成からの変更が大きくなる。

本発明は、こうした課題を解決するためになされたものであり、その主たる目的は、既存の構成からの変更を抑えつつ、たわみ角を正確に検出することのできるたわみ角検出装置を提供することにある。

上記課題を解決するための第１の手段は、
減速機の入力軸の角度位置を検出する入力軸エンコーダと、前記減速機の出力軸に取り付けられたディスク、及び前記ディスクに対して互いに異なる位置に配置されて前記ディスクの角度位置をそれぞれ検出する第１検出素子，第２検出素子，第３検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
前記第１検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
前記第２検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第２角度位置を算出する第２角度位置算出部と、
前記第３検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第３角度位置を算出する第３角度位置算出部と、
前記基本角度位置算出部により算出された前記基本角度位置と、それぞれ前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分、前記第２角度位置算出部により算出された前記第２角度位置との差分である第２差分、前記第３角度位置算出部により算出された前記第３角度位置との差分である第３差分を算出する差分算出部と、
前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子，前記第３検出素子の配置に基づいて、前記出力軸のたわみ角を算出するたわみ角算出部と、
を備える。

上記構成によれば、入力軸エンコーダにより、減速機の入力軸の角度位置が検出される。また、出力軸エンコーダにおいて、減速機の出力軸に取り付けられたディスクに対して互いに異なる位置に配置された第１検出素子，第２検出素子，第３検出素子により、ディスクの角度位置がそれぞれ検出される。

そして、基本角度位置算出部により、入力軸エンコーダにより検出された入力軸の角度位置に基づいて、出力軸の角度位置である基本角度位置が算出される。出力軸の角度位置は、入力軸の角度位置と減速機の減速比とに基づいて算出することができる。この基本角度位置は、たわみ及び弾性変形によるねじれの影響を受けていない状態での出力軸の角度位置に相当する。また、第１角度位置算出部、第２角度位置算出部、第３角度位置算出部によって、それぞれ第１検出素子、第２検出素子、第３検出素子により検出されたディスクの角度位置に基づいて、出力軸の角度位置である第１角度位置、第２角度位置、第３角度位置がそれぞれ算出される。さらに、差分算出部により、算出された基本角度位置と第１角度位置との差分である第１差分、算出された基本角度位置と第２角度位置との差分である第２差分、算出された基本角度位置と第３角度位置との差分である第３差分が算出される。

ここで、出力軸がたわんで傾いたり、出力軸が弾性変形によりねじれたりした場合は、出力軸に取り付けられたディスクとディスクの角度位置を検出する検出素子との相対位置が変化する。そして、第１検出素子，第２検出素子，第３検出素子は、ディスクに対して互いに異なる位置に配置されている。このため、出力軸に取り付けられたディスクが出力軸のたわみにより傾いた場合は、差分算出部により算出される第１差分、第２差分、第３差分は、それぞれ異なった差分になる。これらの第１差分、第２差分、第３差分と、ディスクに対する第１検出素子、第２検出素子、第３検出素子の配置と、出力軸のたわみ角、たわみ方向、弾性変形によるねじれ角とは相関を有しており、これらの間にはそれぞれ関係式が成立する。ディスクに対する第１検出素子，第２検出素子，第３検出素子の配置は既知であり、算出された差分の値と、出力軸のたわみ角、たわみ方向、弾性変形によるねじれ角との相関は、第１検出素子、第２検出素子、第３検出素子のそれぞれにおいて成立する。すなわち、関係式の数が未知の状態量（たわみ角、たわみ方向、弾性変形によるねじれ角）の数以上になっている。したがって、差分算出部により算出された第１差分、第２差分、第３差分、及びディスクに対する第１検出素子，第２検出素子，第３検出素子の配置に基づいて、出力軸のたわみ角を算出することができる。

その結果、出力軸エンコーダの検出素子数を増加させるだけで、外力が出力軸に作用する場合であっても、入力軸エンコーダ及び出力軸エンコーダの３つの検出素子により検出された角度位置に基づいて、出力軸のたわみ角を検出することができる。したがって、既存の構成からの変更を抑えつつ、たわみ角を正確に検出することができる。

第２の手段では、前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子，前記第３検出素子の配置に基づいて、前記出力軸のたわみ方向を算出するたわみ方向算出部を備える。

上述したように、成立する関係式の数が、未知の状態量（たわみ角、たわみ方向、弾性変形によるねじれ角）の数以上になっている。したがって、上記構成によれば、差分算出部により算出された第１差分、第２差分、第３差分、及びディスクに対する第１検出素子，第２検出素子，第３検出素子の配置に基づいて、出力軸のたわみ方向を算出することができる。

第３の手段では、前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子，前記第３検出素子の配置に基づいて、前記出力軸の弾性変形によるねじれ角を算出するねじれ角算出部を備える。

上述したように、成立する関係式の数が、未知の状態量（たわみ角、たわみ方向、弾性変形によるねじれ角）の数以上になっている。したがって、上記構成によれば、差分算出部により算出された第１差分、第２差分、第３差分、及びディスクに対する第１検出素子，第２検出素子，第３検出素子の配置に基づいて、出力軸の弾性変形によるねじれ角を算出することができる。

第４の手段では、前記第１検出素子，前記第２検出素子，前記第３検出素子は、前記ディスクの周方向に沿って等間隔で配置されている。

上記構成によれば、第１検出素子，第２検出素子，第３検出素子は、ディスクの周方向に沿って等間隔で配置されている。このため、特定方向へのたわみの影響だけを、検出素子が強く受けることを抑制することができる。その結果、出力軸のたわみ方向にかかわらず、出力軸の状態量を正確に検出することができる。

第５の手段では、前記たわみ角算出部は、前記第１検出素子，前記第２検出素子，前記第３検出素子の配置された所定平面への投影において、前記出力軸の中心線を回転中心とした、前記第１検出素子の配置角度α１，前記第２検出素子の配置角度α２，前記第３検出素子の配置角度α３，前記出力軸のたわみ方向を示すたわみ方向角度θｄ，前記出力軸の弾性変形によるねじれ角Δθｚ、及び前記たわみ角βに対応するたわみ量ｄ、前記回転中心から各検出素子までの距離Ｒ、前記第１差分Δθ１２、前記第２差分Δθ２２、前記第３差分Δθ３２に関して、ｔａｎ（α１＋Δθ１２＋Δθｚ）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）、ｔａｎ（α２＋Δθ２２＋Δθｚ）＝（Ｒｓｉｎα２−ｄｓｉｎθｄ）／（Rｃｏｓα２−ｄｃｏｓθｄ）、及びｔａｎ（α３＋Δθ３２＋Δθｚ）＝（Ｒｓｉｎα３−ｄｓｉｎθｄ）／（Rｃｏｓα３−ｄｃｏｓθｄ）が成立するとして、前記たわみ方向角度θｄ、前記たわみ量ｄ、及び前記ねじれ角Δθｚを算出し、前記たわみ量ｄに基づいて前記たわみ角βを算出する。

たわみ方向（たわみ方向角度θｄ）、たわみ量ｄ、及びねじれ角Δθｚに関して、第１検出素子，第２検出素子，第３検出素子の配置された所定平面への投影において、上記関係式が成立する。上記関係式では、所定平面への投影を用いているため、近似計算を含んでいる。しかしながら、たわみ角βは微小な角度であるため、近似計算による誤差は微小であり、近似計算を用いることによる演算負荷の軽減効果が優勢となる。

第６の手段は、
減速機の入力軸の角度位置を検出する入力軸エンコーダと、前記減速機の出力軸に取り付けられたディスク、及び前記ディスクに対して互いに異なる位置に配置されて前記ディスクの角度位置をそれぞれ検出する第１検出素子，第２検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
前記第１検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
前記第２検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第２角度位置を算出する第２角度位置算出部と、
前記基本角度位置算出部により算出された前記基本角度位置と、それぞれ前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分、前記第２角度位置算出部により算出された前記第２角度位置との差分である第２差分を算出する差分算出部と、
前記出力軸の弾性変形によるねじれの影響がないと仮定して、前記差分算出部により算出された前記第１差分、前記第２差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子の配置に基づいて、前記出力軸のたわみ角を算出するたわみ角算出部と、
を備える。

上記構成によれば、第１の手段と比較して、第３検出素子が省略されている。ここで、出力軸の弾性変形によるねじれは、出力軸をねじる方向の外力が作用しない限り生じにくい。また、出力軸の弾性変形によるねじれの影響を、予め補正により取り除いておくこともできる。このため、そのような状況では、出力軸の弾性変形によるねじれの影響がないと仮定することができる。そして、出力軸の弾性変形によるねじれの影響がないと仮定すれば、ディスクに対する第１検出素子，第２検出素子の配置は既知であり、算出された差分（第１差分、第２差分）の数、すなわち成立する関係式の数が未知の状態量（たわみ角、たわみ方向）の数以上になる。したがって、出力軸の弾性変形によるねじれの影響がないと仮定して、差分算出部により算出された第１差分、第２差分、及びディスクに対する第１検出素子，第２検出素子の配置に基づいて、出力軸のたわみ角を算出することができる。

その結果、出力軸エンコーダの検出素子数を増加させるだけで、外力が出力軸に作用する場合であっても、入力軸エンコーダ及び出力軸エンコーダの２つの検出素子により検出された角度位置に基づいて、出力軸のたわみ角を検出することができる。したがって、既存の構成からの変更を抑えつつ、たわみ角を正確に検出することができる。

第７の手段では、前記差分算出部により算出された前記第１差分、前記第２差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子の配置に基づいて、前記出力軸のたわみ方向を算出するたわみ方向算出部を備える。

上述したように、成立する関係式の数が、未知の状態量（たわみ角、たわみ方向）の数以上になっている。したがって、上記構成によれば、差分算出部により算出された第１差分、第２差分、及びディスクに対する第１検出素子，第２検出素子の配置に基づいて、出力軸のたわみ方向を算出することができる。

第８の手段では、前記たわみ角算出部は、前記第１検出素子，前記第２検出素子の配置された所定平面への投影において、前記出力軸の中心線を回転中心とした、前記第１検出素子の配置角度α１，前記第２検出素子の配置角度α２，前記出力軸のたわみ方向を示すたわみ方向角度θｄ、及び前記たわみ角βに対応するたわみ量ｄ、前記回転中心から各検出素子までの距離Ｒ、前記第１差分Δθ１１、前記第２差分Δθ２１に関して、ｔａｎ（α１＋Δθ１１）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）、及びｔａｎ（α２＋Δθ２１）＝（Ｒｓｉｎα２−ｄｓｉｎθｄ）／（Rｃｏｓα２−ｄｃｏｓθｄ）が成立するとして、前記たわみ方向角度θｄ、及び前記たわみ量ｄを算出し、前記たわみ量ｄに基づいて前記たわみ角βを算出する。

たわみ方向（たわみ方向角度θｄ）及びたわみ量ｄに関して、第１検出素子，第２検出素子の配置された所定平面への投影において、上記関係式が成立する。上記関係式では、所定平面への投影を用いているため、近似計算を含んでいる。しかしながら、たわみ角βは微小な角度であるため、近似計算による誤差は微小であり、近似計算を用いることによる演算負荷の軽減効果が優勢となる。

第９の手段は、
減速機の入力軸の角度位置を検出する入力軸エンコーダと、ディスク及び前記ディスクの角度位置を検出する第１検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
前記第１検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
前記基本角度位置算出部により算出された前記基本角度位置と、前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分を算出する差分算出部と、
前記出力軸の弾性変形によるねじれの影響がないと仮定して、前記差分算出部により算出された前記第１差分、及び前記出力軸に対する前記第１検出素子の配置に基づいて、所定方向に対する前記出力軸のたわみ角成分を算出するたわみ角成分算出部と、
を備える。

上記構成によれば、第５の手段と比較して、第２検出素子が省略されている。ここで、所定方向に対する出力軸のたわみ角成分のみを考慮すれば、出力軸のたわみ方向を固定したことに等しくなる。第１差分と、出力軸に対する第１検出素子の配置と、出力軸のたわみ角成分とは相関を有しており、これらの間には関係式が成立する。そして、出力軸の弾性変形によるねじれの影響がないと仮定すれば、出力軸に対する第１検出素子の配置は既知であり、算出された差分（第１差分）の数、すなわち成立する関係式の数が未知の状態量（たわみ角成分）の数以上になる。したがって、出力軸の弾性変形によるねじれの影響がないと仮定して、差分算出部により算出された第１差分、及び出力軸に対する第１検出素子の配置に基づいて、所定方向に対する出力軸のたわみ角成分を算出することができる。

その結果、外力が出力軸に作用する場合であっても、入力軸エンコーダ及び出力軸エンコーダの第１検出素子により検出された角度位置に基づいて、出力軸のたわみ角成分を検出することができる。したがって、既存の構成からの変更を抑えつつ、たわみ角成分を正確に検出することができる。

第１０の手段では、前記ディスクは、前記減速機を固定する第１所定部材に取り付けられており、前記第１検出素子は、前記出力軸と一体で回転する第２所定部材に取り付けられており、前記第２所定部材の長手方向と前記出力軸の軸線方向とを含む平面に沿った方向と、前記出力軸から前記第１検出素子への方向とが垂直になっている。

出力軸がたわむと、出力軸エンコーダのディスクと第１検出素子との相対位置が変化する。この相対位置の変化は、出力軸のたわみ方向と出力軸から第１検出素子への方向とが垂直になっている場合に最も大きくなる。また、出力軸は、出力軸と一体で回転する部材の長手方向と出力軸の軸線方向とを含む平面に沿ってたわみ易い。

この点、上記構成によれば、第２所定部材の長手方向と出力軸の軸線方向とを含む平面に沿った方向と、出力軸から第１検出素子への方向とが垂直になっている。このため、出力軸が、第２所定部材の長手方向と出力軸の軸線方向とを含む平面に沿ってたわんだ場合に、出力軸エンコーダのディスクと第１検出素子との相対位置の変化を最も大きくすることができる。したがって、第２所定部材の長手方向と出力軸の軸線方向とを含む平面に沿った方向（所定方向）に対する出力軸のたわみ角成分を、正確に算出することができる。

さらに、出力軸エンコーダのディスクは、減速機を固定する第１所定部材に取り付けられている。また、第１検出素子は、出力軸と一体で回転する第２所定部材に取り付けられている。このため、第２所定部材が出力軸の回転に伴って回転したとしても、第１検出素子も第２所定部材と共に回転し、出力軸のたわみ方向と出力軸から第１検出素子への方向とが垂直な状態を維持することができる。したがって、出力軸の角度位置にかかわらず、所定方向に対する出力軸のたわみ角成分を正確に算出することができる。

第１１の手段では、前記たわみ角算出部は、前記第１検出素子の配置された所定平面への投影において、前記出力軸の中心線を回転中心とした、前記第１検出素子の配置角度α１，前記所定方向を示すたわみ方向角度θｄ、及び前記たわみ角成分βｍに対応するたわみ量ｄ、前記回転中心から各検出素子までの距離Ｒ、前記第１差分Δθ１１に関して、ｔａｎ（α１＋Δθ１１）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）が成立するとして、前記たわみ量ｄを算出し、前記たわみ量ｄに基づいて前記たわみ角成分βｍを算出する。

所定方向（たわみ方向角度θｄ）及びたわみ量ｄに関して、第１検出素子の配置された所定平面への投影において、上記関係式が成立する。上記関係式では、所定平面への投影を用いているため、近似計算を含んでいる。しかしながら、たわみ角βは微小な角度であるため、近似計算による誤差は微小であり、近似計算を用いることによる演算負荷の軽減効果が優勢となる。

第１２の手段では、前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記所定方向を算出し、算出された前記所定方向と算出された前記たわみ角成分とに基づいて、前記入力軸の角度位置の制御を補正する。

上記構成によれば、入力軸エンコーダにより検出された入力軸の角度位置に基づいて、所定方向が算出される。そして、算出された所定方向と算出されたたわみ角成分とに基づいて、入力軸の角度位置の制御を補正することができる。

第１３の手段は、
減速機の入力軸の角度位置を検出する入力軸エンコーダと、前記減速機の出力軸に取り付けられたディスク、及び前記ディスクに対して互いに異なる位置に配置されて前記ディスクの角度位置をそれぞれ検出する第１検出素子，第２検出素子，第３検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
前記第１検出素子により検出された前記ディスクの角度位置と前記ディスクに対する前記第１検出素子の配置とに基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
前記第２検出素子により検出された前記ディスクの角度位置と前記ディスクに対する前記第２検出素子の配置とに基づいて、前記出力軸の角度位置である第２角度位置を算出する第２角度位置算出部と、
前記第３検出素子により検出された前記ディスクの角度位置と前記ディスクに対する前記第３検出素子の配置とに基づいて、前記出力軸の角度位置である第３角度位置を算出する第３角度位置算出部と、
前記基本角度位置算出部により算出された前記基本角度位置と、それぞれ前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分、前記第２角度位置算出部により算出された前記第２角度位置との差分である第２差分、前記第３角度位置算出部により算出された前記第３角度位置との差分である第３差分を算出する差分算出部と、
前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分に基づいて、前記出力軸のたわみ角を算出するたわみ角算出部と、
を備える。

ディスクに対する第１検出素子，第２検出素子，第３検出素子のそれぞれの配置を表す角度を基準とし、その基準からの角度位置としてそれぞれ第１角度位置，第２角度位置，第３角度位置を算出することもできる。したがって、上記構成によれば、第１の手段と同様の作用効果を奏することができる。

第１４の手段では、前記減速機は、ロボットの回転軸を接続する関節に設けられており、前記ロボットのキャリブレーションにおいて前記ロボットを所定姿勢に制御して、前記たわみ角算出部により前記出力軸のたわみ角を算出させる第１制御部と、前記ロボットの動作中に前記ロボットを前記所定姿勢に制御して、前記たわみ角算出部により前記出力軸のたわみ角を算出させる第２制御部と、前記第１制御部が算出させた前記出力軸のたわみ角と、前記第２制御部が算出させた前記出力軸のたわみ角との偏差が所定偏差よりも大きい場合に、前記ロボットに異常な外力が作用していると判定する判定部と、を備える。

上記構成によれば、第１制御部は、ロボットのキャリブレーションにおいてロボットを所定姿勢に制御して、たわみ角算出部により出力軸のたわみ角を算出させる。すなわち、ロボットに異常な外力が作用していない状態において、出力軸のたわみ角が算出される。また、第２制御部は、ロボットの動作中にロボットを上記所定姿勢に制御して、たわみ角算出部により出力軸のたわみ角を算出させる。すなわち、ロボットに異常な外力が作用している可能性のある動作中に、出力軸のたわみ角が算出される。このとき、ロボットに異常な外力が作用していれば、出力軸がたわんでたわみ角が変化する。

そして、第１制御部が算出させた出力軸のたわみ角と、第２制御部が算出させた出力軸のたわみ角との偏差が所定偏差よりも大きい場合に、ロボットに異常な外力が作用していると判定される。したがって、検出した出力軸のたわみ角を利用して、ロボットに異常な外力が作用していることを判定することができる。

ロボットの関節及びロボットコントローラを示す模式図。 出力軸がたわんでいない状態での検出角度位置を示す模式図。 出力軸がたわんでいる状態での検出角度位置を示す模式図。 出力軸エンコーダの検出素子の配置を示す斜視図。 出力軸エンコーダの検出素子の配置を示す投影図。 出力軸がたわむ前後での検出角度位置を示す斜視図。 出力軸がたわむ前後での検出角度位置を示す投影図。 出力軸がたわんでねじれる前後での検出角度位置を示す斜視図。 出力軸がたわんでねじれる前後での検出角度位置を示す投影図。 第１実施形態におけるたわみ方向、たわみ量、及びねじれ角の関係を示す投影図。 出力軸にかかるモーメントとたわみ角との関係を示すグラフ。 第２実施形態におけるたわみ方向及びたわみ量の関係を示す投影図。 出力軸と検出素子との位置関係を示す投影図。 アームの重心、入力角度、及びたわみ角の関係を示すグラフ。 所定部材の長手方向、ディスク及び検出素子の配置を示す投影図。

（第１実施形態）
以下、第１実施形態について、図面を参照しつつ説明する。本実施形態は、多関節ロボットの回転軸を接続する関節において、回転軸のたわみ角を検出するたわみ角検出装置に具現化している。

図１に示すように、ロボットの関節１０の動作状態は、ロボットコントローラ８０により制御される。関節１０は、ハウジング１１、モータ２０、減速機３０、入力軸エンコーダ４０、出力軸エンコーダ５０等を備えている。

ハウジング１１は、ロボットの所定の回転軸（基端側の回転軸）に取り付けられている。ハウジング１１には、モータ２０及び減速機３０が固定されている。モータ２０の駆動軸は、減速機３０へ回転を入力する入力軸２１になっている。モータ２０の駆動状態は、ロボットコントローラ８０により制御される。モータ２０には、入力軸２１の角度位置を検出する入力軸エンコーダ４０が取り付けられている。なお、減速機３０はモータ２０に固定されていてもよい。

入力軸エンコーダ４０は、円板状のディスク４１、検出素子４２等を備えている。ディスク４１は、入力軸２１に同心状に取り付けられている。検出素子４２は、入力軸エンコーダ４０の筐体に取り付けられている。検出素子４２は、ディスク４１の検出面における外周縁部の所定位置に対向して配置されている。検出素子４２は、ディスク４１の角度位置に対応するオン・オフ信号を検出する。検出素子４２により検出されたオン・オフ信号（すなわち角度位置）は、ロボットコントローラ８０へ入力される。入力軸エンコーダ４０として、透過型光学式エンコーダや、反射型光学式エンコーダ、磁気式エンコーダ等を採用することができる。

減速機３０は、入力軸２１の回転を所定の減速比で減速して、出力軸３１の回転として出力する。出力軸３１には、ロボットの所定の回転軸（先端側の回転軸）が取り付けられている。減速機３０として、波動歯車式の減速機や、遊星歯車式の減速機等を採用することができる。ハウジング１１あるいは減速機３０には、出力軸３１の角度位置を検出する出力軸エンコーダ５０が取り付けられている。

出力軸エンコーダ５０は、円板状のディスク５１、複数の検出素子５２（５２Ａ，５２Ｂ，５２Ｃ）等を備えている。ディスク５１は、出力軸３１に同心状に取り付けられている。検出素子５２は、出力軸エンコーダ５０の筐体に取り付けられている。検出素子５２Ａ，５２Ｂ，５２Ｃ（第１検出素子，第２検出素子，第３検出素子に相当）は、ディスク５１の検出面における外周縁部の各所定位置にそれぞれ対向して配置されている。複数の検出素子５２は、ディスク５１の角度位置に対応するオン・オフ信号をそれぞれ検出する。検出素子５２により検出されたオン・オフ信号（すなわち角度位置）は、ロボットコントローラ８０へ入力される。出力軸エンコーダ５０として、透過型光学式エンコーダや、反射型光学式エンコーダ、磁気式エンコーダ等を採用することができる。検出素子５２Ａ，５２Ｂ，５２Ｃの配置の詳細については、後述する。

入力軸２１、ディスク４１、出力軸３１、及びディスク５１は、同心状に配置されている。すなわち、入力軸２１、ディスク４１、出力軸３１、及びディスク５１の中心線は、軸線Ｃ１で一致している。

ロボットコントローラ８０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、入出力ポート、駆動回路等を含むマイクロコンピュータである。ロボットコントローラ８０は、入力軸エンコーダ４０や、出力軸エンコーダ５０等の各種センサによる検出値を入力し、モータ２０等の各種アクチュエータの駆動状態を制御する。ロボットコントローラ８０は、入力軸エンコーダ４０の検出素子４２により検出されたオン・オフ信号に基づいて、ディスク４１すなわち入力軸２１の角度位置を算出する。そして、ロボットコントローラ８０（基本角度位置算出部に相当）は、入力軸２１の角度位置及び減速機３０の減速比に基づいて、出力軸３１の基本角度位置を算出する。この基本角度位置は、たわみ及び弾性変形によるねじれの影響を受けていない状態での出力軸３１の角度位置に相当する。また、ロボットコントローラ８０（第１，第２，第３角度位置算出部に相当）は、出力軸エンコーダ５０の検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたオン・オフ信号に基づいて、ディスク５１すなわち出力軸３１の第１，第２，第３角度位置をそれぞれ算出する。なお、入力軸エンコーダ４０、出力軸エンコーダ５０、及びロボットコントローラ８０により、たわみ角検出装置が構成されている。

図２は、出力軸３１がたわんでいない状態で、検出素子５２Ｃにより検出される角度位置を示す模式図である。検出素子５２Ｃは、矢印Ｓ１で示すディスク５１の位置に対応するオン・オフの信号を検出する。そして、ディスク５１の角度位置は、二重線で示す角度位置が検出素子５２Ｃに対向している状態の角度位置、すなわち二重線で示す角度位置に対応する角度位置として検出される。

図３は、出力軸３１がたわんでいる状態で、検出素子５２Ｃにより検出される角度位置を示す模式図である。ここでは、出力軸３１が矢印Ｌ１の方向へたわみ角βだけたわんでいる状態を例にして説明する。なお、同図を含めて以降の図では、たわみ角βを誇張して示している。

検出素子５２Ｃは、図２と同様に、矢印Ｓ１で示すディスク５１の位置に対応するオン・オフの信号を検出する。そして、ディスク５１の角度位置は、実線の二重線で示す角度位置が検出素子５２Ｃに対向している状態の角度位置、すなわち実線の二重線で示す角度位置に対応する角度位置として検出される。ここで、破線の二重線で示す角度位置は、図２において二重線で示した角度位置である。したがって、出力軸３１がたわんでいない状態で検出素子５２Ｃにより検出されるディスク５１の角度位置と、出力軸３１がたわんでいる状態で検出素子５２Ｃにより検出されるディスク５１の角度位置とはずれることとなる。

図４，５は、出力軸エンコーダ５０の検出素子５２Ａ，５２Ｂ，５２Ｃの配置を示す斜視図，投影図である。出力軸３１の中心線がＺ軸に一致し、検出素子５２Ａ，５２Ｂ，５２Ｃの配置された平面（所定平面に相当）がＸＹ平面に一致している状態を例にして説明する。図５は、Ｚ軸方向への図４の投影図である。

検出素子５２Ａ，５２Ｂ，５２Ｃの配置された平面（ＸＹ平面）と、ディスク５１の検出面とは平行になっている。検出素子５２Ａ，５２Ｂ，５２Ｃは、ディスク５１の検出面における外周縁部の各所定位置にそれぞれ対向して配置されている。詳しくは、Ｘ軸を基準として左回りに、角度α１の位置に検出素子５２Ａが配置され、角度α２の位置に検出素子５２Ｂが配置され、角度α３の位置に検出素子５２Ｃが配置されている。検出素子５２Ａ，５２Ｂ，５２Ｃは、ディスク５１の周方向沿って等間隔、詳しくは１２０°間隔で配置されている。すなわち、α３−α２＝α２−α１＝１２０°になっている。

この状態では、ディスク５１の角度位置は、検出素子５２Ａ，５２Ｂ，５２Ｃにより、それぞれ角度α１，α２，α３の位置の破線で示す角度位置に対応する角度位置として検出される。すなわち、検出素子５２Ａ，５２Ｂ，５２Ｃは、ディスク５１に対して互いに異なる位置に配置されてディスク５１の角度位置をそれぞれ検出する。

図６，７は、出力軸３１がたわむ前後で、検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出される角度位置を示す斜視図，投影図である。ここでは、出力軸３１が矢印Ｌ１の方向、すなわちＸ軸と角度θｄをなす方向へたわんでいる状態を例にして説明する。図７のＸＹ平面すなわちＺ軸方向への図６の投影面において、角度θｄの方向へディスク５１の中心（座標の原点）が移動した量を、出力軸３１のたわみ量ｄとする。Ｘ，Ｙ軸は、それぞれＸｂ，Ｙｂ軸に移動している。出力軸３１がたわむ前の状態を破線で示し、出力軸３１がたわんだ後の状態を実線で示す。

図７に示すように、ディスク５１は、破線で示す円から実線で示す円へ移動する。このとき、出力軸３１がたわむ前に検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出された角度位置（破線の円に破線で示した各半径）は、たわみ量ｄだけ平行移動して実線の円に破線で示した角度位置へそれぞれ移動している。これに対して、検出素子５２Ａ，５２Ｂ，５２Ｃは、出力軸エンコーダ５０の筐体に取り付けられているため、出力軸３１がたわんでも移動していない。このため、出力軸３１がたわむ前に検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出された角度位置と、出力軸３１がたわんだ後に検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出される角度位置とがずれている。

出力軸３１がたわんだ後では、その状態で検出素子５２Ａ，５２Ｂ，５２Ｃに対向する角度位置、すなわち実線の円に実線で示した各半径の角度位置に対応する角度位置が、検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出される。このため、出力軸３１がたわむ前に検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたオン・オフ信号に基づいて算出された出力軸３１の第１，第２，第３角度位置と、出力軸３１がたわんだ後に検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたオン・オフ信号に基づいて算出された出力軸３１の第１，第２，第３角度位置とには、それぞれ角度Δθ１１，Δθ２１，Δθ３１の偏差が生じる。

図８，９は、出力軸３１がたわんでねじれる前後で、検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出される角度位置を示す斜視図，投影図である。ここでは、図６，７の状態からさらに、矢印Ｌ２で示すように出力軸３１が弾性変形により左回りにねじれ角Δθｚだけねじれた状態を例にして説明する。出力軸３１がたわんだりねじれたりする前の状態を破線で示し、出力軸３１がたわんでねじれた後の状態を実線で示す。

図９に示すように、ディスク５１は、破線で示す円から実線で示す円へ移動する。このとき、出力軸３１がたわんだりねじれたりする前に検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出された角度位置（破線の円に破線で示した各半径）は、たわみ量ｄだけ平行移動し且つねじれ角Δθｚだけ回転して、実線の円に一点鎖線で示した角度位置へそれぞれ移動している。これに対して、検出素子５２Ａ，５２Ｂ，５２Ｃは、出力軸エンコーダ５０の筐体に取り付けられているため、出力軸３１がたわんだりねじれたりしても移動していない。このため、出力軸３１がたわんだりねじれたりする前に検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出された角度位置と、出力軸３１がたわんでねじれた後に検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出される角度位置とがずれている。

出力軸３１がたわんでねじれた後では、その状態で検出素子５２Ａ，５２Ｂ，５２Ｃに対向する角度位置、すなわち実線の円に実線で示した各半径の角度位置に対応する角度位置が、検出素子５２Ａ，５２Ｂ，５２Ｃによりそれぞれ検出される。このため、出力軸３１がたわんだりねじれたりする前に検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたオン・オフ信号に基づいて算出された出力軸３１の第１，第２，第３角度位置と、出力軸３１がたわんでねじれた後に検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたオン・オフ信号に基づいて算出された出力軸３１の第１，第２，第３角度位置とには、それぞれ角度Δθ１２，Δθ２２，Δθ３２の偏差が生じる。

図１０は、たわみ方向（角度θｄ）、たわみ量ｄ、及びねじれ角Δθｚの関係を示す投影図である。ここで、図９の検出素子５２Ａに着目すると、以下の関係式が成立する。Ｒは、投影図１０においてディスク５１（出力軸３１）の中心から検出素子５２Ａまでの距離である。

すなわち、ｔａｎ（α１＋Δθ１２＋Δθｚ）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）が成立する。同様にして、検出素子５２Ｂ，５２Ｃについても、ｔａｎ（α２＋Δθ２２＋Δθｚ）＝（Ｒｓｉｎα２−ｄｓｉｎθｄ）／（Rｃｏｓα２−ｄｃｏｓθｄ）、及びｔａｎ（α３＋Δθ３２＋Δθｚ）＝（Ｒｓｉｎα３−ｄｓｉｎθｄ）／（Rｃｏｓα３−ｄｃｏｓθｄ）が成立する。そして、これらの３つの関係式を連立方程式として解けば、角度θｄ、たわみ量ｄ、及びねじれ角Δθｚを求めることができる。さらに、図６に示す配置から、原点からディスク５１の中心までの距離Ｄ１は既知であるため、たわみ量ｄが求まれば、たわみ角βを求めることができる。すなわち、ｓｉｎβ＝ｄ／Ｄ１の関係式が成立する。

上述したように、入力軸エンコーダ４０の検出素子４２により検出されたオン・オフ信号に基づいて算出された出力軸３１の基本角度位置は、たわみ及び弾性変形によるねじれの影響を受けていない状態での出力軸３１の角度位置に相当する。このため、出力軸３１がたわんだりねじれたりする前に、検出素子５２Ａ，５２Ｂ，５２Ｃにより検出されたオン・オフ信号に基づいて算出された出力軸３１の第１，第２，第３角度位置に相当する角度位置として、基本角度位置を用いることができる。

そして、ロボットコントローラ８０（差分算出部に相当）は、基本角度位置と、検出素子５２Ａにより検出された角度位置に基づき算出された第１角度位置との差分である第１差分を算出する。同様にして、ロボットコントローラ８０は、基本角度位置と、検出素子５２Ｂにより検出された角度位置に基づき算出された第２角度位置との差分である第２差分、検出素子５２Ｃにより検出された角度位置に基づき算出された第３角度位置との差分である第３差分をそれぞれ算出する。これらの第１差分，第２差分，第３差分が、それぞれ上記角度Δθ１２，Δθ２２，Δθ３２に相当する。したがって、ロボットコントローラ８０（たわみ方向算出部、たわみ角算出部、ねじれ角算出部に相当）は、上記関係式により、角度θｄ、たわみ角β、及びねじれ角Δθｚを算出する。なお、ディスク５１に対する検出素子５２Ａ，５２Ｂ，５２Ｃのそれぞれの配置を表す角度α１，α２，α３を基準とし、その基準からの角度位置としてそれぞれ第１角度位置，第２角度位置，第３角度位置を算出することもできる。

また、ロボットコントローラ８０（第１制御部に相当）は、ロボットのキャリブレーションにおいてロボットを所定姿勢に制御して、出力軸３１のたわみ角βを算出する。すなわち、ロボットに異常な外力が作用していない状態において、出力軸３１のたわみ角βを算出する。そして、ロボットコントローラ８０（第２制御部に相当）は、ロボットの動作中にロボットを上記所定姿勢に制御して、出力軸３１のたわみ角βを算出する。すなわち、ロボットに異常な外力が作用している可能性のある動作中に、出力軸３１のたわみ角βを算出する。このとき、ロボットに異常な外力が作用していれば、出力軸３１がたわんでたわみ角βが変化する。

そして、ロボットコントローラ８０（判定部に相当）は、ロボットのキャリブレーションにおいて算出した出力軸３１のたわみ角βと、ロボットの動作中に算出した出力軸３１のたわみ角βとの偏差が所定偏差よりも大きい場合に、ロボットに異常な外力が作用していると判定する。したがって、検出した出力軸３１のたわみ角βを利用して、ロボットに異常な外力が作用していることを判定することができる。

さらに、図１１に示すように、出力軸３１にかかるたわみ方向のモーメントを変化させてたわみ角βを算出することで、出力軸３１のたわみ剛性を算出することができる。すなわち、たわみ剛性は、出力軸３１にかかるモーメントに対するたわみ角βの傾きαの逆数となる。

また、ロボットコントローラ８０は、ねじれ角Δθｚに基づいて、出力軸３１に作用している負荷トルクを算出することができる。また、ロボットコントローラ８０は、算出した角度θｄ、たわみ角β、及びねじれ角Δθｚに基づいて、ロボットのアームの手先位置を精密に制御することができる。

以上詳述した本実施形態は、以下の利点を有する。

・出力軸３１がたわんで傾いたり、出力軸３１が弾性変形によりねじれたりした場合は、出力軸３１に取り付けられたディスク５１と、ディスク５１の角度位置を検出する検出素子５２Ａ，５２Ｂ，５２Ｃとの相対位置が変化する。そして、検出素子５２Ａ，５２Ｂ，５２Ｃは、ディスク５１に対して互いに異なる位置に配置されている。このため、出力軸３１に取り付けられたディスク５１が出力軸３１のたわみにより傾いた場合は、算出される第１差分、第２差分、第３差分は、それぞれ異なった差分になる。これらの第１差分、第２差分、第３差分と、ディスク５１に対する検出素子５２Ａ，５２Ｂ，５２Ｃの配置と、出力軸３１のたわみ角β、たわみ方向（角度θｄ）、弾性変形によるねじれ角Δθｚとは相関を有しており、これらの間には関係式が成立する。ディスク５１に対する検出素子５２Ａ，５２Ｂ，５２Ｃの配置は既知であり、算出された差分の数、すなわち成立する関係式の数が未知の状態量（たわみ角β、たわみ方向、弾性変形によるねじれ角Δθｚ）の数以上になっている。したがって、算出された第１差分、第２差分、第３差分、及びディスク５１に対する検出素子５２Ａ，５２Ｂ，５２Ｃの配置に基づいて、出力軸３１のたわみ角βを算出することができる。

・出力軸エンコーダ５０の検出素子数を増加させるだけで、外力が出力軸３１に作用する場合であっても、入力軸エンコーダ４０及び出力軸エンコーダ５０の３つの検出素子５２により検出された角度位置に基づいて、出力軸３１のたわみ角βを検出することができる。したがって、既存の構成からの変更を抑えつつ、たわみ角βを正確に検出することができる。

・算出された第１差分、第２差分、第３差分、及びディスク５１に対する検出素子５２Ａ，５２Ｂ，５２Ｃの配置に基づいて、出力軸３１のたわみ方向（角度θｄ）を算出することができる。

・算出された第１差分、第２差分、第３差分、及びディスク５１に対する検出素子５２Ａ，５２Ｂ，５２Ｃの配置に基づいて、出力軸３１の弾性変形によるねじれ角Δθｚを算出することができる。

・検出素子５２Ａ，５２Ｂ，５２Ｃは、ディスク５１の周方向に沿って等間隔で配置されている。このため、特定方向へのたわみの影響だけを、検出素子５２Ａ，５２Ｂ，５２Ｃが強く受けることを抑制することができる。その結果、出力軸３１のたわみ方向にかかわらず、出力軸３１の状態量を正確に検出することができる。なお、検出素子５２Ａ，５２Ｂ，５２Ｃが、ディスク５１の周方向に沿って等間隔で配置されていない場合であっても、出力軸３１のたわみ角β、たわみ方向（角度θｄ）、ねじれ角Δθｚを算出することはできる。

（第２実施形態）
以下、第２実施形態について、第１実施形態との相違点を中心に説明する。本実施形態では、検出素子５２Ｃ（第３検出素子に相当）を省略している。なお、検出素子５２Ｃの代わりに、検出素子５２Ａ又は検出素子５２Ｂを省略することもできる。

ここで、出力軸３１の弾性変形によるねじれは、出力軸３１をねじる方向の外力が作用しない限り生じにくい。また、出力軸３１の弾性変形によるねじれの影響を、予め補正により取り除いておくこともできる。また、ねじれ量を定数として式に適用てもよい。このため、本実施形態では、出力軸３１の弾性変形によるねじれの影響がないと仮定する。

図１２は、たわみ方向（角度θｄ）及びたわみ量ｄの関係を示す投影図である。ここで、図７の検出素子５２Ａに着目すると、以下の関係式が成立する。

すなわち、ｔａｎ（α１＋Δθ１１）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）が成立する。同様にして、検出素子５２Ｂについても、ｔａｎ（α２＋Δθ２１）＝（Ｒｓｉｎα２−ｄｓｉｎθｄ）／（Rｃｏｓα２−ｄｃｏｓθｄ）が成立する。そして、これらの２つの関係式を連立方程式として解けば、角度θｄ及びたわみ量ｄを求めることができる。上述したように、たわみ量ｄが求まれば、たわみ角βを求めることができる。

そして、ロボットコントローラ８０（差分算出部に相当）は、基本角度位置と、検出素子５２Ａにより検出された角度位置に基づき算出された第１角度位置との差分である第１差分を算出する。同様にして、ロボットコントローラ８０は、基本角度位置と、検出素子５２Ｂにより検出された角度位置に基づき算出された第２角度位置との差分である第２差分を算出する。これらの第１差分，第２差分が、それぞれ上記角度Δθ１１，Δθ２１に相当する。したがって、ロボットコントローラ８０（たわみ方向算出部、たわみ角算出部）は、上記関係式により、角度θｄ及びたわみ角βを算出する。

本実施形態は、以下の利点を有する。なお、ここでは、第１実施形態との相違点のみを述べる。

・出力軸３１の弾性変形によるねじれの影響がないと仮定すれば、ディスク５１に対する検出素子５２Ａ，５２Ｂの配置は既知であり、算出された差分（第１差分、第２差分）の数、すなわち成立する関係式の数が未知の状態量（たわみ角β、角度θｄ）の数以上になる。したがって、出力軸３１の弾性変形によるねじれの影響がないと仮定して、算出された第１差分、第２差分、及びディスク５１に対する検出素子５２Ａ，検出素子５２Ｂの配置に基づいて、出力軸３１のたわみ角βを算出することができる。

・出力軸エンコーダ５０の検出素子数を増加させるだけで、外力が出力軸３１に作用する場合であっても、入力軸エンコーダ４０及び出力軸エンコーダ５０の２つの検出素子５２により検出された角度位置に基づいて、出力軸３１のたわみ角βを検出することができる。したがって、既存の構成からの変更を抑えつつ、たわみ角βを正確に検出することができる。

・算出された第１差分、第２差分、及びディスク５１に対する検出素子５２Ａ，５２Ｂの配置に基づいて、出力軸３１のたわみ方向（角度θｄ）を算出することができる。

（第３実施形態）
以下、第３実施形態について、第２実施形態との相違点を中心に説明する。本実施形態では、さらに検出素子５２Ｂ（第２検出素子に相当）を省略している。なお、検出素子５２Ｂ，５２Ｃの代わりに、検出素子５２Ａ，５２Ｂ、又は検出素子５２Ａ，５２Ｃを省略することもできる。

図１３は、出力軸３１と検出素子５２Ａとの位置関係を示す投影図である。ここでは、ロボットのアームの重心が原点からＸ軸方向へ離れている距離の長い順に、重心Ｗ１，Ｗ２，Ｗ３として示している。同図に示すように、ロボットのアームの重心が原点からＸ軸方向へ離れているほど、Ｘ軸方向への出力軸３１のたわみ量ｄ（たわみ角β）は大きくなる。出力軸３１は、出力軸３１と一体で回転する所定部材（アームの出力軸３１よりも先端側部分）の長手方向と出力軸３１の軸線方向とを含む平面（同図の状態ではＹ軸に垂直でＸ軸を含む平面）に沿ってたわみ易い。なお、アームは軸（出力軸）にとって重量物であり、軸はその重量物に引っ張られる方向にたわむ、その引っ張り方向とは、すなわちアームが存在する（延びる）方向である。アームの存在する方向は前述の計算式に頼らずとも、現在の指令値や入力軸エンコーダの検出値などから、計算可能である。よって、前述の計算式からたわみ方向を削除し、指令値や入力軸エンコーダの検出値から求めたたわみ方向を使ってアーム（減速機の入力軸）の制御を補正することは可能である。

上述したように、出力軸３１がたわむと、出力軸エンコーダ５０のディスク５１と検出素子５２Ａとの相対位置、すなわちたわみ角βが変化する。検出素子５２Ａにより検出された角度位置に基づき算出されるたわみ角βは、同図に示すように、出力軸３１のたわみ方向と出力軸３１から検出素子５２Ａへの方向とが垂直になっている場合（入力角度θｍ＝０°）に最も大きくなる。そして、入力角度θｍが９０°に近付くほど、所定部材の長手方向と出力軸３１の軸線方向とを含む平面に沿ってたわんだ場合に算出されるたわみ角βは小さくなり、入力角度θｍ＝９０°で算出されるたわみ角βは０°になる。したがって、アームの重心Ｗ１，Ｗ２，Ｗ３、入力角度θｍ、及びたわみ角βの関係は、図１４で示すグラフのようになる。この算出されるたわみ角βは、Ｘ軸方向に対する出力軸３１のたわみ角成分βｘに相当する。

ここで、所定方向に対する出力軸３１のたわみ角成分βｍのみを考慮すれば、出力軸３１の角度θｄ（たわみ方向）を固定したことに等しくなる。このため、ｔａｎ（α１＋Δθ１１）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）から、たわみ量ｄを求めることができる。上述したように、たわみ量ｄが求まれば、たわみ角成分βｍを求めることができる。

そして、ロボットコントローラ８０（たわみ角成分算出部に相当）は、基本角度位置と、検出素子５２Ａにより検出された角度位置に基づき算出された第１角度位置との差分である第１差分を算出する。この第１差分が、上記角度Δθ１１に相当する。したがって、ロボットコントローラ８０は、上記関係式により、たわみ角成分βｍを算出する。

さらに、本実施形態では、出力軸エンコーダ５０のディスク５１は、出力軸エンコーダ５０の筐体、すなわち減速機３０を固定するハウジング１１（第１所定部材に相当）に取り付けられている。また、図１５に示すように、検出素子５２Ａは、出力軸３１と一体で回転する所定部材６１（第２所定部材に相当）に取り付けられている。そして、所定部材６１の長手方向と出力軸３１の軸線方向（Ｚ軸方向）とを含む平面に沿った方向と、出力軸３１から検出素子５２Ａへの方向とが垂直になるように、検出素子５２Ａを所定部材６１に取り付けている。

このため、図１５に破線で示すアーム（所定部材６１）の回転前の状態でも、実線で示すアームの回転後の状態でも、所定部材６１の長手方向と出力軸３１の軸線方向とを含む平面に沿った方向と、出力軸３１から検出素子５２Ａへの方向とが常に垂直になっている。したがって、検出素子５２Ａにより検出された角度位置に基づき算出されるたわみ角成分βｍは、入力角度θｍにかかわらず、図１４における入力角度θｍ＝０°に等しい特性となる。すなわち、出力軸エンコーダ５０のディスク５１と検出素子５２Ａとの相対位置の変化を最も大きくすることができる。したがって、出力軸３１のたわみ角成分βｍを、正確に算出することができる。

本実施形態は、以下の利点を有する。なお、ここでは、第１，第２実施形態との相違点のみを述べる。

・所定方向に対する出力軸３１のたわみ角成分βｍのみを考慮すれば、出力軸３１のたわみ方向を固定したことに等しくなる。第１差分と、出力軸３１に対する検出素子５２Ａの配置と、出力軸３１のたわみ角成分βｍとは相関を有しており、これらの間には関係式が成立する。そして、出力軸３１の弾性変形によるねじれの影響がないと仮定すれば、出力軸３１に対する検出素子５２Ａの配置は既知であり、算出された差分（第１差分）の数、すなわち成立する関係式の数が未知の状態量（たわみ角成分βｍ）の数以上になる。したがって、出力軸３１の弾性変形によるねじれの影響がないと仮定して、算出された第１差分、及び出力軸３１に対する検出素子５２Ａの配置に基づいて、所定方向に対する出力軸３１のたわみ角成分βｍを算出することができる。

・外力が出力軸３１に作用する場合であっても、入力軸エンコーダ４０及び出力軸エンコーダ５０の検出素子５２Ａにより検出された角度位置に基づいて、出力軸３１のたわみ角成分βｍを検出することができる。したがって、既存の構成からの変更を抑えつつ、たわみ角成分βｍを正確に検出することができる。

・所定部材６１の長手方向と出力軸３１の軸線方向とを含む平面に沿った方向と、出力軸３１から検出素子５２Ａへの方向とが垂直になっている。このため、出力軸３１が、所定部材６１の長手方向と出力軸３１の軸線方向とを含む平面に沿ってたわんだ場合に、出力軸３１エンコーダのディスク５１と検出素子５２Ａとの相対位置の変化を最も大きくすることができる。したがって、所定部材６１の長手方向と出力軸３１の軸線方向とを含む平面に沿った方向（所定方向）に対する出力軸３１のたわみ角成分βｍを、正確に算出することができる。

・出力軸エンコーダ５０のディスク５１は、減速機３０を固定するハウジング１１に取り付けられている。また、検出素子５２Ａは、出力軸３１と一体で回転する所定部材６１に取り付けられている。このため、所定部材６１が出力軸３１の回転に伴って回転したとしても、検出素子５２Ａも所定部材６１と共に回転し、出力軸３１のたわみ方向と出力軸３１から検出素子５２Ａへの方向とが垂直な状態を維持することができる。したがって、出力軸３１の角度位置にかかわらず、所定方向に対する出力軸３１のたわみ角成分βｍを正確に算出することができる。

なお、第３実施形態を、以下のように変更して実施することもできる。

・第１，第２実施形態と同様に、出力軸エンコーダ５０のディスク５１が出力軸３１に取り付けられ、検出素子５２Ａが出力軸エンコーダ５０の筐体、すなわちハウジング１１（あるいは減速機３０）に取り付けられた構成を採用することもできる。

・所定部材６１の長手方向と出力軸３１の軸線方向とを含む平面に沿った方向と、出力軸３１から検出素子５２Ａへの方向とが垂直でない構成を採用することもできる。ただし、上記平面に沿った方向と出力軸３１から検出素子５２Ａへの方向との角度が０°に近付くほどたわみ角成分βｍが０に近付くため、その角度は９０°に近い方が望ましい。

また、上記の各実施形態において、ロボットのアームに限らず、いわゆるマニプレータを採用することができる。なお、入力軸２１及び出力軸３１を有する減速機３０を備える回転機構であれば、上記の各実施形態のたわみ角検出装置を適用することができる。

２１…入力軸、３０…減速機、３１…出力軸、４０…入力軸エンコーダ、４１…ディスク、４２…検出素子、５０…出力軸エンコーダ、５１…ディスク、５２…検出素子、５２Ａ…検出素子（第１検出素子）、５２Ｂ…検出素子（第２検出素子）、５２Ｃ…検出素子（第３検出素子）、８０…ロボットコントローラ。

Claims (14)

1. 減速機の入力軸の角度位置を検出する入力軸エンコーダと、前記減速機の出力軸に取り付けられたディスク、及び前記ディスクに対して互いに異なる位置に配置されて前記ディスクの角度位置をそれぞれ検出する第１検出素子，第２検出素子，第３検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
   前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
   前記第１検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
   前記第２検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第２角度位置を算出する第２角度位置算出部と、
   前記第３検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第３角度位置を算出する第３角度位置算出部と、
   前記基本角度位置算出部により算出された前記基本角度位置と、それぞれ前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分、前記第２角度位置算出部により算出された前記第２角度位置との差分である第２差分、前記第３角度位置算出部により算出された前記第３角度位置との差分である第３差分を算出する差分算出部と、
   前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子，前記第３検出素子の配置に基づいて、前記出力軸のたわみ角を算出するたわみ角算出部と、
   を備えるたわみ角検出装置。
2. 前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子，前記第３検出素子の配置に基づいて、前記出力軸のたわみ方向を算出するたわみ方向算出部を備える請求項１に記載のたわみ角検出装置。
3. 前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子，前記第３検出素子の配置に基づいて、前記出力軸の弾性変形によるねじれ角を算出するねじれ角算出部を備える請求項１又は２に記載のたわみ角検出装置。
4. 前記第１検出素子，前記第２検出素子，前記第３検出素子は、前記ディスクの周方向に沿って等間隔で配置されている請求項１〜３のいずれか１項に記載のたわみ角検出装置。
5. 前記たわみ角算出部は、前記第１検出素子，前記第２検出素子，前記第３検出素子の配置された所定平面への投影において、前記出力軸の中心線を回転中心とした、前記第１検出素子の配置角度α１，前記第２検出素子の配置角度α２，前記第３検出素子の配置角度α３，前記出力軸のたわみ方向を示すたわみ方向角度θｄ，前記出力軸の弾性変形によるねじれ角Δθｚ、及び前記たわみ角βに対応するたわみ量ｄ、前記回転中心から各検出素子までの距離Ｒ、前記第１差分Δθ１２、前記第２差分Δθ２２、前記第３差分Δθ３２に関して、ｔａｎ（α１＋Δθ１２＋Δθｚ）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）、ｔａｎ（α２＋Δθ２２＋Δθｚ）＝（Ｒｓｉｎα２−ｄｓｉｎθｄ）／（Rｃｏｓα２−ｄｃｏｓθｄ）、及びｔａｎ（α３＋Δθ３２＋Δθｚ）＝（Ｒｓｉｎα３−ｄｓｉｎθｄ）／（Rｃｏｓα３−ｄｃｏｓθｄ）が成立するとして、前記たわみ方向角度θｄ、前記たわみ量ｄ、及び前記ねじれ角Δθｚを算出し、前記たわみ量ｄに基づいて前記たわみ角βを算出する請求項１〜４のいずれか１項に記載のたわみ角検出装置。
6. 減速機の入力軸の角度位置を検出する入力軸エンコーダと、前記減速機の出力軸に取り付けられたディスク、及び前記ディスクに対して互いに異なる位置に配置されて前記ディスクの角度位置をそれぞれ検出する第１検出素子，第２検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
   前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
   前記第１検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
   前記第２検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第２角度位置を算出する第２角度位置算出部と、
   前記基本角度位置算出部により算出された前記基本角度位置と、それぞれ前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分、前記第２角度位置算出部により算出された前記第２角度位置との差分である第２差分を算出する差分算出部と、
   前記出力軸の弾性変形によるねじれの影響がないと仮定して、前記差分算出部により算出された前記第１差分、前記第２差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子の配置に基づいて、前記出力軸のたわみ角を算出するたわみ角算出部と、
   を備えるたわみ角検出装置。
7. 前記差分算出部により算出された前記第１差分、前記第２差分、及び前記ディスクに対する前記第１検出素子，前記第２検出素子の配置に基づいて、前記出力軸のたわみ方向を算出するたわみ方向算出部を備える請求項６に記載のたわみ角検出装置。
8. 前記たわみ角算出部は、前記第１検出素子，前記第２検出素子の配置された所定平面への投影において、前記出力軸の中心線を回転中心とした、前記第１検出素子の配置角度α１，前記第２検出素子の配置角度α２，前記出力軸のたわみ方向を示すたわみ方向角度θｄ、及び前記たわみ角βに対応するたわみ量ｄ、前記回転中心から各検出素子までの距離Ｒ、前記第１差分Δθ１１、前記第２差分Δθ２１に関して、ｔａｎ（α１＋Δθ１１）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）、及びｔａｎ（α２＋Δθ２１）＝（Ｒｓｉｎα２−ｄｓｉｎθｄ）／（Rｃｏｓα２−ｄｃｏｓθｄ）が成立するとして、前記たわみ方向角度θｄ、及び前記たわみ量ｄを算出し、前記たわみ量ｄに基づいて前記たわみ角βを算出する請求項６又は７に記載のたわみ角検出装置。
9. 減速機の入力軸の角度位置を検出する入力軸エンコーダと、ディスク及び前記ディスクの角度位置を検出する第１検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
   前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
   前記第１検出素子により検出された前記ディスクの角度位置に基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
   前記基本角度位置算出部により算出された前記基本角度位置と、前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分を算出する差分算出部と、
   前記出力軸の弾性変形によるねじれの影響がないと仮定して、前記差分算出部により算出された前記第１差分、及び前記出力軸に対する前記第１検出素子の配置に基づいて、所定方向に対する前記出力軸のたわみ角成分を算出するたわみ角成分算出部と、
   を備えるたわみ角検出装置。
10. 前記ディスクは、前記減速機を固定する第１所定部材に取り付けられており、
    前記第１検出素子は、前記出力軸と一体で回転する第２所定部材に取り付けられており、
    前記第２所定部材の長手方向と前記出力軸の軸線方向とを含む平面に沿った方向と、前記出力軸から前記第１検出素子への方向とが垂直になっている請求項９に記載のたわみ角検出装置。
11. 前記たわみ角算出部は、前記第１検出素子の配置された所定平面への投影において、前記出力軸の中心線を回転中心とした、前記第１検出素子の配置角度α１，前記所定方向を示すたわみ方向角度θｄ、及び前記たわみ角成分βｍに対応するたわみ量ｄ、前記回転中心から各検出素子までの距離Ｒ、前記第１差分Δθ１１に関して、ｔａｎ（α１＋Δθ１１）＝（Ｒｓｉｎα１−ｄｓｉｎθｄ）／（Rｃｏｓα１−ｄｃｏｓθｄ）が成立するとして、前記たわみ量ｄを算出し、前記たわみ量ｄに基づいて前記たわみ角成分βｍを算出する請求項９又は１０に記載のたわみ角検出装置。
12. 前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記所定方向を算出し、算出された前記所定方向と算出された前記たわみ角成分とに基づいて、前記入力軸の角度位置の制御を補正する請求項１１に記載のたわみ角検出装置。
13. 減速機の入力軸の角度位置を検出する入力軸エンコーダと、前記減速機の出力軸に取り付けられたディスク、及び前記ディスクに対して互いに異なる位置に配置されて前記ディスクの角度位置をそれぞれ検出する第１検出素子，第２検出素子，第３検出素子を有する出力軸エンコーダと、を備えるたわみ角検出装置であって、
    前記入力軸エンコーダにより検出された前記入力軸の角度位置に基づいて、前記出力軸の角度位置である基本角度位置を算出する基本角度位置算出部と、
    前記第１検出素子により検出された前記ディスクの角度位置と前記ディスクに対する前記第１検出素子の配置とに基づいて、前記出力軸の角度位置である第１角度位置を算出する第１角度位置算出部と、
    前記第２検出素子により検出された前記ディスクの角度位置と前記ディスクに対する前記第２検出素子の配置とに基づいて、前記出力軸の角度位置である第２角度位置を算出する第２角度位置算出部と、
    前記第３検出素子により検出された前記ディスクの角度位置と前記ディスクに対する前記第３検出素子の配置とに基づいて、前記出力軸の角度位置である第３角度位置を算出する第３角度位置算出部と、
    前記基本角度位置算出部により算出された前記基本角度位置と、それぞれ前記第１角度位置算出部により算出された前記第１角度位置との差分である第１差分、前記第２角度位置算出部により算出された前記第２角度位置との差分である第２差分、前記第３角度位置算出部により算出された前記第３角度位置との差分である第３差分を算出する差分算出部と、
    前記差分算出部により算出された前記第１差分、前記第２差分、前記第３差分に基づいて、前記出力軸のたわみ角を算出するたわみ角算出部と、
    を備えるたわみ角検出装置。
14. 前記減速機は、ロボットの回転軸を接続する関節に設けられており、
    前記ロボットのキャリブレーションにおいて前記ロボットを所定姿勢に制御して、前記たわみ角算出部により前記出力軸のたわみ角を算出させる第１制御部と、
    前記ロボットの動作中に前記ロボットを前記所定姿勢に制御して、前記たわみ角算出部により前記出力軸のたわみ角を算出させる第２制御部と、
    前記第１制御部が算出させた前記出力軸のたわみ角と、前記第２制御部が算出させた前記出力軸のたわみ角との偏差が所定偏差よりも大きい場合に、前記ロボットに異常な外力が作用していると判定する判定部と、
    を備える請求項１〜８，１３のいずれか１項に記載のたわみ角検出装置。

Images