(57)【特許請求の範囲】
【請求項１】
位置制御されるアームの回転軸に作用する実出力トルクを検出するトルク検出方法であって、
前記アームが重力方向とは異なる方向を向くような第１の位置に前記アームが位置するときに、前記アームの位置制御を停止させた状態で前記回転軸に作用する重力トルクを検出し、
前記重力トルク及び前記第１の位置に基づいて、前記アームの位置に応じた前記重力トルクを算出するための重力係数を算出し、
前記アームの位置制御時に前記回転軸に作用する動作トルクを検出し、
前記位置制御時に前記アームの位置を検出し、
前記位置制御時に検出された前記アームの位置と、前記重力係数とに基づいて、前記位置制御時の前記重力トルクを算出し、
前記位置制御時に検出された前記動作トルクと、前記位置制御時に算出した前記重力トルクと、に基づいて、前記位置制御時の前記実出力トルクを算出するトルク検出方法。
【請求項２】
前記アームの位置が第２の位置である状態で、前記回転軸にかかる変化前トルクを検出し、
前記アームを前記第２の位置から第３の位置まで回転させた後、前記アームを前記第３の位置から再度前記第２の位置まで回転させた状態で、前記回転軸に作用する変化後トルクを検出し、
前記変化前トルク及び前記変化後トルクに基づいて、補正値を算出し、
前記動作トルクと、前記重力トルクと、前記補正値とに基づいて、前記位置制御時の前記実出力トルクを算出する請求項１に記載のトルク検出方法。
【請求項３】
前記補正値には、前記アームの第１の回転方向に対応する第１の補正値と、前記アームの第２の回転方向に対応する第２の補正値と、が含まれ、
前記第１及び第２の補正値のうち、前記アームの回転方向に対応する補正値に基づいて、前記実出力トルクを算出する請求項２に記載のトルク検出方法。
【請求項４】
前記位置制御時に前記アームの回転方向が前記第１の回転方向から前記第２の回転方向に切り替わった直後において、前記第１の補正値と前記第２の補正値との間の値である中間補正値に基づいて、前記実出力トルクを算出する請求項３に記載のトルク検出方法。
【請求項５】
前記中間補正値は、前記アームの回転速度の変化に応じて増減する請求項４に記載のトルク検出方法。
【請求項６】
前記重力係数を前記補正値に基づいて算出する請求項２〜５のいずれか一項に記載のトルク検出方法。
【請求項７】
アームと、
前記アームを回転駆動させる駆動手段と、
前記アームの回転軸に作用するトルクを検出するトルク検出手段と、
前記アームの位置を検出する位置検出手段と、
前記駆動手段に制御信号を出力することにより前記アームの位置制御を行う制御手段と、を備え、
前記トルク検出手段は、前記アームが重力方向とは異なる方向を向くような第１の位置に前記アームが位置するときに、前記制御手段による前記アームの位置制御が停止した状態で前記回転軸に作用する重力トルクを検出し、
前記制御手段は、前記重力トルク及び前記第１の位置に基づいて、前記アームの位置に応じた前記重力トルクを算出するための重力係数を算出し、前記アームの位置制御時に前記位置検出手段により検出された前記アームの位置と、前記重力係数とに基づいて、前記位置制御時の前記重力トルクを算出し、前記位置制御時に前記トルク検出手段により検出された前記回転軸に作用する動作トルクと、前記位置制御時に算出された前記重力トルクと、に基づいて、前記位置制御時に前記回転軸に作用する実出力トルクを算出し、当該実出力トルクに基づいて、前記駆動手段に前記制御信号を出力し、前記アームを回転駆動させるアーム装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はトルク検出方法及びアーム装置に関する。
【背景技術】
【０００２】
トルクセンサを用いて検出した力（トルク）を利用して、単軸のアームに対してサーボ制御を行う場合、自重トルクの作用、トルクセンサ装着時のメカ的な偏り、及び回転動作時の摩擦力等を考慮した補償を行う必要がある。
【０００３】
特許文献１には、力制御を行う制御系の重力補償法が開示されている。特許文献１の重力補償法は、アームの質量及び重心位置に基づいて、予めアーム角度に対する重力係数を算出しておく。そして、アームの動作中に、アーム角度に対応する重力係数を用いて、重力補償トルクを求め、重力補償を行う。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開昭６２−６００１０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１に記載の重力補償法においては、アームの角度に応じて変化するアームの自重トルク（重力トルク）を正確に補償するために、アームの質量及び重心位置を正確に把握しなければならず、煩わしいという問題があった。
【０００６】
本発明は、このような問題を解決するためになされたものであり、簡易に重力補償を行うことができるトルク検出方法及びアーム装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の一態様にかかるトルク検出方法は、制御手段により位置制御されるアームの回転軸に作用する実出力トルクを検出するトルク検出方法であって、前記アームが重力方向とは異なる方向を向くような第１の位置に前記アームが位置するときに、前記アームの位置制御を停止させた状態で前記回転軸に作用する重力トルクを検出し、前記重力トルク及び前記第１の位置に基づいて、前記アームの位置に応じた前記重力トルクを算出するための重力係数を算出し、前記アームの位置制御時に前記回転軸に作用する動作トルクを検出し、前記位置制御時に前記アームの位置を検出し、前記位置制御時に検出された前記アームの位置と、前記重力係数とに基づいて、前記位置制御時の前記重力トルクを算出し、前記位置制御時に検出された前記動作トルクと、前記位置制御時に算出された前記重力トルクと、に基づいて、前記位置制御時の前記実出力トルクを算出するものである。これにより、アームの質量及び重心位置を把握することなく、重力補償を実現することができる。
【０００８】
また、前記アームの位置が第２の位置である状態で、前記回転軸にかかる変化前トルクを検出し、前記アームを前記第２の位置から第３の位置まで回転させた後、前記アームを前記第３の位置から再度前記第２の位置まで回転させた状態で、前記回転軸に作用する変化後トルクを検出し、前記変化前トルク及び前記変化後トルクに基づいて、補正値を算出し、前記動作トルクと、前記重力トルクと、前記補正値とに基づいて、前記位置制御時の前記実出力トルクを算出してもよい。これにより、メカの偏りに起因するトルク誤差を除去することができる。そのため、高精度に実出力トルクを算出することができる。
【０００９】
また、前記補正値には、前記アームの第１の回転方向に対応する第１の補正値と、前記アームの第２の回転方向に対応する第２の補正値と、が含まれ、前記第１及び第２の補正値のうち、前記アームの回転方向に対応する補正値に基づいて、前記実出力トルクを算出してもよい。これにより、回転方向に応じて、メカの偏りが異なる場合であっても、高精度に実出力トルクを算出することができる。
【００１０】
また、前記位置制御時に前記アームの回転方向が前記第１の回転方向から前記第２の回転方向に切り替わった直後において、前記第１の補正値と前記第２の補正値との間の値である中間補正値に基づいて、前記実出力トルクを算出してもよい。これにより、回転方向が切り替わった場合に、補正値が急変することを防止できる。そのため、実出力トルクの滑らかな変化を実現できる。
【００１１】
また、前記中間補正値は、前記アームの回転速度の変化に応じて増減してもよい。これにより、実出力トルクのより滑らかな変化を実現することができる。
【００１２】
また、前記重力係数を前記補正値に基づいて算出してもよい。これにより、重力係数の算出においても、メカの偏りに起因する誤差を除去することができる。そのため、より高精度に実出力トルクを算出することができる。
【００１３】
本発明の一態様にかかるアーム装置は、アームと、前記アームを回転駆動させる駆動手段と、前記アームの回転軸に作用するトルクを検出するトルク検出手段と、前記アームの位置を検出する位置検出手段と、前記駆動手段に制御信号を出力することにより前記アームの位置制御を行う制御手段と、を備え、
前記トルク検出手段は、前記アームが重力方向とは異なる方向を向くような第１の位置に前記アームが位置するときに、前記制御手段による前記アームの位置制御が停止した状態で前記回転軸に作用する重力トルクを検出し、前記制御手段は、前記重力トルク及び前記第１の位置に基づいて、前記アームの位置に応じた前記重力トルクを算出するための重力係数を算出し、前記アームの位置制御時に前記位置検出手段により検出された前記アームの位置と、前記重力係数とに基づいて、前記位置制御時の前記重力トルクを算出し、前記位置制御時に前記トルク検出手段により検出された前記回転軸に作用する動作トルクと、前記位置制御時に算出された前記重力トルクと、に基づいて、前記位置制御時に前記回転軸に作用する実出力トルクを算出し、当該実出力トルクに基づいて、前記駆動手段に前記制御信号を出力し、前記アームを回転駆動させるものである。これにより、アームの質量及び重心位置を把握することなく、重力補償を実現することができる。
【発明の効果】
【００１４】
本発明により、簡易に重力補償を行うことができるトルク検出方法及びアーム装置を提供することができる。
【図面の簡単な説明】
【００１５】
【図１】実施の形態にかかるアーム装置の模式図である。
【図２】実施の形態にかかるアーム装置のブロック図である。
【図３】実施の形態にかかるメカの偏り検出を説明するための図である。（ａ）は、回転軸の回転角度が０°の場合を示す図である。（ｂ）は、回転軸の回転角度が４５°の場合を示す図である。（ｃ）は、回転軸の回転角度が０°の場合を示す図である。
【図４】実施の形態にかかる重力係数の算出を説明するための図である。（ａ）は、回転軸の回転角度を４５°に変化させる動作を示す図である。（ｂ）は、モータ制御を停止している状態を示す図である。（ｃ）重力トルクのみを検出している動作を示す図である。
【図５】実施の形態にかかる実出力トルクの算出を説明するための図である。（ａ）は、アームが右方向に動作している状態を示す図である。（ｂ）は、アームの動作が停止している状態を示す図である。（ｃ）は、アームが左方向に動作した直後の状態を示す図である。（ｄ）は、アームが左方向に動作している状態を示す図である。
【発明を実施するための形態】
【００１６】
以下、図面を参照して本発明の実施の形態について説明する。本実施の形態にかかるアーム装置１の構成を示す模式図を図１に示す。また、アーム装置１のブロック図を図２に示す。アーム装置１は、エンコーダ１１と、モータ１２と、減速機１３と、トルクセンサ１４と、回転軸１５と、アーム１６と、制御部１７と、を備える。図１に示すように、アーム装置１は、単軸アームである。本実施の形態においては、単軸制御系の重力補償方法について説明する。
【００１７】
エンコーダ１１（位置検出手段）は、回転軸１５の角度（位置）、回転量、角速度等を検出する。エンコーダ１１は、検出した角度、回転量、角速度等をエンコーダ信号として、制御部１７に出力する。これにより、アーム１６の角度等が検出される。
【００１８】
モータ１２には、制御部１７から出力されるモータ駆動電流が入力される。モータ１２は、モータ駆動電流に応じた出力トルク、回転速度で回転駆動する。モータ１２の出力トルクや回転速度は、電流センサ信号として、制御部１７に出力される。
【００１９】
減速機１３は、モータ１２の出力トルク、回転速度を、減速比に応じて変換する。そして、減速機１３は、変換後の出力トルク、回転速度で回転軸１５を回転させる。なお、減速機１３の減速比は、アーム装置１の使用者によって適宜設定される。
【００２０】
トルクセンサ１４（トルク検出手段）は、アーム１６が受ける外力によって、回転軸１５に作用するトルクを検出する。例えば、重力トルクや、アーム１６が物体の位置を移動させた場合（アーム１６が物体に仕事した場合）に作用するトルク等である。なお、重力トルクとは、アーム１６にかかる重力（自重）の影響で回転軸１５に作用するトルクのことを意味する。
【００２１】
回転軸１５には、減速機１３により変換された出力トルクが作用する。回転軸１５とアーム１６とは一体的に連結されており、回転軸１５が回転すると、アーム１６も回転軸１５回りに回転動作を行う。
【００２２】
制御部１７は、例えばＣＰＵ(Central Processing Unit)等であり、図示しないメモリに格納された制御プログラムに応じて、アーム装置１の各ブロックの動作を制御する。制御部１７は、エンコーダ信号に基づいて、アーム１６の角度（位置）を検出する。また、制御部１７は、回転軸１５に作用する実出力トルクを算出する。さらに、制御部１７は、アーム１６の角度、電流センサ信号及び実出力トルクに基づいて、モータ駆動電流を調整し、モータ１２の回転駆動を制御する。つまり、制御部１７は、モータ１２が所望のトルクを出力するように、フィードバック制御を行う。これにより、アーム１６が所望の角度となるような位置制御（サーボ制御）が実現される。
【００２３】
続いて、本実施の形態にかかるアーム装置１の動作例について説明する。本実施の形態にかかるトルク検出方法は、大きく分けて、「メカの偏り検出」、「重力係数の算出」、及び「実出力トルク算出」の３つのフローを含む。以下において、各フローについて詳細に説明する。なお、「メカの偏り検出」及び「重力係数の算出」は、アーム１６のサーボ制御が行われる前に予め１回ずつ行われる。
【００２４】
（メカの偏り検出）
初めに、メカの偏り検出を行う。具体的には、アーム１６の回転動作において生じる機械的、物理的な偏り（例えば、回転軸１５の回転時の摩擦やベアリングの偏り、トルクセンサ１４組み付け位置の非対称性等）を検出する。メカの偏り検出フローの動作を説明するための図を図３に示す。図３は、図１に示した模式図を正面（回転軸１５の軸方向）から見た図である。なお、アーム装置１においては、アーム１６は、向かって右方向及び左方向の双方向に回転可能である。
【００２５】
まず、制御部１７からのモータ駆動電流により、アーム１６が最も自重を受けない（重力トルクが回転軸１５に作用しない）角度にアーム１６を移動させる。そして、アーム１６が最も自重を受けない角度にある状態おいて、エンコーダ及びトルクセンサ値をリセットする。アーム１６が最も重力を受けない角度にある状態とは、アーム１６の向きが重力方向を向いている状態である（図３（ａ）参照）。つまり、アーム１６の向き（長手方向）と重力方向とが一致する状態である。この状態において、回転軸１５の回転角度を±０°（第２の位置）、トルクセンサ値を±０（変化前トルク）に設定する。なお、第２の位置とは、メカの偏り検出フローにおけるトルク検出の基準となるアーム１６の位置を意味する。つまり、第２の位置とは、メカの偏り検出フローにおいて、アーム１６の回転の前後に、トルクセンサ値が検出されるアーム１６の位置を意味する。変化前トルクとは、メカの偏り検出フローにおいて、アーム１６の位置を第２の位置から変化させる前に、トルクセンサ１４により検出されるトルクを意味する。なお、本実施の形態においては、重力トルクの影響を極力排除し、計算処理を簡便なものにするために、第２の位置をアーム１６の回転角度０°とし、変化前トルクを±０としているが、これに限られるものではない。
【００２６】
次に、制御部１７によるモータ１２の制御により、アーム１６の角度を０°（第２の位置）から４５°（第３の位置）まで回転させる（図３（ｂ））。図３（ｂ）においては、アーム装置１を正面視した状態で、アーム１６が向かって右方向に移動する方向（第１の回転方向）に、アーム１６（回転軸１５）を４５°回転させる。なお、第３の位置とは、メカの偏りに起因する誤差を発生させるために、第２の位置からアーム１６を移動させる際に、アーム１６の移動先となる位置である。第３の位置は、第２の位置と異なる位置であればよく、４５°に限られない。しかし、回転角度が小さすぎると、検出結果に誤差が生じてしまい、メカの偏りを精度よく検出できない。そのため、回転角度は４５°以上が好ましい。
【００２７】
次に、制御部１７によるモータ１２の制御により、アーム１６の角度を４５°（第３の位置）から０°（第２の位置）まで回転させる（図３（ｃ））。つまり、アーム装置１を正面視した状態で、アーム１６が向かって左方向に移動する方向（第２の回転方向）に、アーム１６（回転軸１５）を４５°回転させる。言い換えると、アーム１６の回転動作前の位置（角度０°）にアーム１６を戻す。
【００２８】
そして、アーム１６の角度が再び０°となった状態で、トルクセンサ１４は、回転軸１５に作用しているトルク値（変化後トルク）を検出する。そして、トルクセンサ１４は、検出したトルク値（変化後トルク）を制御部１７に出力する。変化後トルクとは、アーム１６を第２の位置から第３の位置まで移動させ、再度第２の位置にアーム１６を戻した状態で、回転軸１５に作用するトルクのことを意味する。つまり、変化後トルクとは、メカの偏り検出フローにおいて、アーム１６の位置を第２の位置から変化させた後、再度第２の位置において検出されるトルクのことを意味する。制御部１７は、検出したトルク値が差し引き０となるような補正値（オフセット値）を算出する。つまり、アーム１６の回転前に設定したトルク値０と、アーム１６の回転後のトルク値との差を算出する。当該差がメカの偏りにより発生する誤差であり、上記の補正値となる。
【００２９】
なお、図３（ａ）〜（ｃ）においては、アーム１６が始めに右方向に回転した場合の補正値を算出したが、アームが始めに左方向に回転した場合についても同様の計算を行う。つまり、右方向の回転方向に対応する補正値が１つ、左方向の回転方向に対応する補正値が１つ、計２つの補正値が算出される。
【００３０】
算出された２つの補正値は、回転方向と対応付けられて、制御部１７の図示しないメモリに格納される。以上の動作により、メカの偏り検出のフローは終了する。
【００３１】
（重力係数の算出）
次に、重力係数の算出を行う。具体的には、アーム１６の角度に対応した重力トルクを算出するための重力係数を算出する。重力係数の算出フローの動作を説明するための図を図４に示す。まず、制御部１７によるモータ１２の制御により、アーム１６の角度を４５°（第１の位置）まで回転させる（図４（ａ）参照）。なお、このときの回転角度も４５°に限られないが、アーム１６の自重による重力トルクを明確に検出するために、４５°以上が好ましい。
【００３２】
そして、アーム１６が４５°まで回転した時点で、制御部１７は、モータ駆動電流の出力を一時的に停止する（もしくは電流制御そのものを停止する）。つまり、制御部１７は、アーム１６の位置制御を停止する。これにより、モータ１２の動作が停止し、回転軸１５に作用するトルクには、重力トルク以外の余分なトルクが一切含まれなくなる（図４（ｂ）参照）。
【００３３】
トルクセンサ１４は、モータ駆動電流の出力が停止している状態で、回転軸１５に作用するトルク値を検出する。なお、上記の通り、このとき検出されるトルク値は、重力トルク（補正前重力トルク）のみである。これにより、トルクセンサ１４は、回転軸１５に作用している重力トルクのみを検出できる。
【００３４】
さらに、制御部１７は、トルクセンサ１４が検出した補正前重力トルクから、上述したメカの偏り検出フローにおいて算出した補正値を減算する。これにより、制御部１７は、メカの偏りによる影響が除去された重力トルクを算出できる（図４（ｃ）参照）。
【００３５】
このとき、補正前重力トルクから減算される補正値は、補正前重力トルク検出の際の回転方向に対応する補正値が用いられる。図４（ｃ）においては、アーム１６が向かって右方向に回転しているため、右方向に対応する補正値が用いられる。
【００３６】
そして、制御部１７は、補正前重力トルクを検出したときの回転軸１５の軸角度（アーム１６の角度）を用いて、重力係数を算出する。具体的には、以下の式（１）を用いて、重力係数を算出する。なお、下記の式において、トルク値とは、モータ駆動電流の出力が停止している状態で、トルクセンサ１４により検出されたトルク値であり、つまり、補正前重力トルクのことを意味する。制御部１７は、算出した重力係数を図示しないメモリに格納する。
【００３７】
【数１】
0005360254.tif 000002
【００３８】
以上の動作により、重力係数の算出のフローは終了する。なお、図４（ａ）〜（ｃ）においては、重力係数を算出するために、アーム１６を向かって右方向に回転させているが、これに限られるものではなく、左右どちらの方向に回転させてもよい。また、「メカの偏り検出」及び「重力係数の算出」フローは、サーボ制御が行われる前に（初回のみ）行われればよい。
【００３９】
（実出力トルクの検出）
最後に、実出力トルクの検出を行う。具体的には、サーボ制御時におけるアーム１６の重力補償を行い、実出力トルクを検出する。ここで、実出力トルクとは、トルクセンサ１４が検出した出力トルクから、サーボ制御時にアーム１６のメカの偏りに起因する誤差のトルクと、アーム１６の重力トルクとを除去したトルクのことを意味する。言い換えると、実出力トルクとは、メカの偏りによって生じる誤差のトルク及び重力トルクが補償されたトルクであり、実質的に回転軸１５に作用しているトルクを意味する。
【００４０】
より詳細には、制御部１７は、以下の式（２）〜（４）に示す手順によって、実出力トルクを算出する。まず、サーボ制御時の動作トルクからメカの偏り検出フローにて算出した補正値（又は中間補正値）を差し引く（式（２））。なお、動作トルクとは、サーボ制御時に回転軸１５に作用するトルクのことを意味する。言い換えると、動作トルクとは、サーボ制御時において、トルクセンサ１４により検出されたトルクセンサ値のことを意味する。そのため、動作トルクには、実出力トルク、重力トルク、及びメカの偏りに起因する誤差のトルクの全てが含まれる。以下では、動作トルクのことをトルクセンサ値と称す。次に、制御部１７は、重力係数の算出フローにて算出された重力係数に、サーボ制御時のアーム１６の角度を乗算して、アーム１６の重力トルクを算出する（式（３））。さらに、式（２）において補正値を差し引いた補正トルクから、重力トルクを差し引くことにより、実出力トルクを算出する（式（４））。
【００４１】
【数２】
0005360254.tif 000003
【００４２】
【数３】
0005360254.tif 000004
【００４３】
【数４】
0005360254.tif 000005
【００４４】
ここで、サーボ制御時におけるトルクセンサ値から、メカの偏りに起因するトルク誤差を除去する方法（式（２）の手順）について具体的に説明する。具体的には、制御部１７は、アーム１６の回転方向に基づいて、トルクセンサ値（動作トルク）からメカの偏り検出フローにて算出した補正値を減算する。
【００４５】
原則として、制御部１７は、トルクセンサ値からアーム１６の回転方向に対応する補正値を減算することにより、メカの偏りに起因するトルク誤差を除外した出力トルクを算出する。
【００４６】
例えば、アーム１６が向かって右方向に動作している場合（速度が右に＋の場合）、制御部１７は、エンコーダ信号に基づいて、アーム１６の回転方向を判定し、回転方向を保持する。そして、制御部１７は、メカの偏り検出フローにて算出した補正値のうち、保持した回転方向（右方向）に対応する補正値を、トルクセンサ値から減算する（図５（a）参照）。
【００４７】
次に、回転方向が切り替わる場合の動作について説明する。制御部１７は、エンコーダ信号に基づいて、右方向への速度が±０になったことを判定する。このとき、制御部１７は、継続して右方向に対応する補正値を、トルクセンサ値から減算する（図５（ｂ）参照）。
【００４８】
アーム１６の回転方向が右方向から左方向に切り替わった直後（速度が左に＋変化した直後）、制御部１７は、エンコーダ信号に基づいて、アーム１６の回転方向を判定し、切り替わり後の回転方向（左方向）を保持する。それと共に、制御部１７は、中間補正値を算出し、右方向に対応する補正値に代えて、当該中間補正値をトルクセンサ値から差し引く（図５（ｃ）参照）。このときの制御部１７によるアーム１６の回転方向の判定は、例えば、一定レベル以上の逆方向に対するモータ速度を検出したことに基づいて判定される。
【００４９】
ここで、中間補正値とは、メカの偏り検出フローにて算出された左方向及び右方向に対応する２つの補正値の間の値を意味する。具体的には、中間補正値とは、右方向から左方向に回転方向が切り替わる場合、右方向に対応する補正値から左方向に対応する補正値まで段階的に変化する値である。より詳細には、制御部１７は、モータドライバの処理周期に基づいて、中間補正値を段階的に増減させることにより、右方向に対応する補正値から左方向に対応する補正値に近づけていく。言い換えると、アーム１６の回転速度が±０から左方向へ加速するにつれて、中間補正値は増減する。つまり、アーム１６の回転速度に応じて、中間補正値が増減する。そして、中間補正値が左方向に対応する補正値に達すると、その後は、左方向に対応する補正値が用いられる（図５（ｄ）参照）。言い換えると、アーム１６の回転方向が右方向から左方向に切り替わった直後においては、制御部１７は、補正値を左方向に対応する補正値に瞬間的に切り替えない。
【００５０】
例えば、左方向に対応する補正値が８５、右方向に対応する補正値が９５であるとする。また、中間補正値は、モータドライバの周期毎に１ずつ変化する（変化量が１）とする。このとき、アーム１６の回転方向が左方向から右方向に切り替わったとすると、右方向に切り替わってからモータドライバの１周期経過後において、中間補正値は８５＋１＝８６となる。そして、制御部１７は、モータドライバの周期毎に中間補正値を１ずつ加算させていく。アーム１６の回転方向が右方向に切り替わってから、モータドライバの周期が１０周期経過すると、中間補正値は８５＋１０＝９５となり、右方向に対応する補正値に達する。その後は、回転方向が右方向から左方向に切り替わるまで、右方向に対応する補正値９５が用いられる。
【００５１】
一方、アーム１６の回転方向が右方向から左方向に切り替わったとすると、左方向に切り替わってからモータドライバの１周期経過後において、中間補正値は９５−１＝９４となる。そして、制御部１７は、モータドライバの周期毎に中間補正値を１ずつ減算させていく。アーム１６の回転方向が左方向に切り替わってから、モータドライバの周期が１０周期経過すると、中間補正値は９５−１０＝８５となり、左方向に対応する補正値に達する。その後は、回転方向が左方向から右方向に切り替わるまで、左方向に対応する補正値８５が用いられる。このように、アーム１６の回転方向の切り替わりの直後において、補正値を段階的に変化させることにより、補正値の急な変化を防止でき、実出力トルクの滑らかな変化を実現できる。
【００５２】
なお、上記の例においては、中間補正値はモータドライバの周期毎に１ずつ変化したが、変化量はこれに限られず、適宜設定可能である。また、中間補正値の変化の周期として、モータドライバの周期を用いたが、回転速度の増加毎や、所定の時間毎、回転軸１５の所定の回転角度毎に中間補正値を変化させてもよい。また、中間補正値を固定値（例えば、左方向に対応する補正値と右方向に対応する補正値との平均値）としてもよい。さらに、急激な逆制動（回転方向の急な切り替え）の場合には、中間補正値を使用せずに、瞬間的に現在の回転方向に対応する補正値から反対方向に対応する補正値に切り替えてもよい。
【００５３】
以上のように、本実施の形態にかかるトルク検出方法の構成によれば、トルクセンサ１４により検出された重力トルクに基づいて、重力係数を算出する。また、サーボ制御時のアーム１６の角度及び重力係数に基づいて、重力トルクを検出する。そして、サーボ制御時のトルクセンサ値（動作トルク）と重力トルクとに基づいて、サーボ制御時の実出力トルクを算出する。そのため、重力係数を算出する際にアーム１６の質量及び重心位置を把握する必要がなく、簡易に重力補償を実現することができる。
【００５４】
また、重力補償を行う際に、アーム１６の質量及び重心位置を把握しておく必要がないため、回転軸１５に装着したアーム１６を変更したり、他の部品や装置を追加したりした場合であっても、容易に重力係数を取得しなおすことができる。
【００５５】
さらに、メカの偏りに起因するトルク誤差を予め算出しておき、重力係数の算出時及び実出力トルクの検出時においてトルク誤差を除去する。これにより、より高精度に実出力トルクを検出することができる。
【００５６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更及び組み合わせをすることが可能である。
【符号の説明】
【００５７】
１  アーム装置
１１  エンコーダ
１２  モータ
１３  減速機
１４  トルクセンサ
１５  回転軸
１６  アーム
１７  制御部
【図１】
0005360254.tif 000006
【図２】
0005360254.tif 000007
【図３】
0005360254.tif 000008
【図４】
0005360254.tif 000009
【図５】
0005360254.tif 000010