(57)【特許請求の範囲】
【請求項１】
動力伝達機構を介してモータと接続された負荷の位置又は速度を制御する装置であって、負荷入力軸の位置又は角度の検出値から算出された負荷速度と負荷速度指令値との差を入力として受ける負荷速度コントローラを備え、
前記負荷速度コントローラは、入力された前記差に基づいて負荷軸トルク指令値を算出するとともに、算出された値にあらかじめ定められたゲインを乗算してねじり角指令値として出力するものであり、
前記負荷速度コントローラのマイナーループに、前記ねじり角指令値に基づいてモータトルク指令値を出力するねじり角コントローラと、モータ出力軸の位置又は角度の検出値と前記負荷入力軸の位置又は角度の検出値との差をねじり角として算出する第１の演算部と、前記ねじり角指令値と前記ねじり角との差を算出する第２の演算部とを備え、
該第２の演算部の算出結果を前記ねじり角コントローラに入力することを特徴とする位置又は速度制御装置。
【請求項２】
更に、前記ねじり角から前記動力伝達機構における入力、出力の差速度を演算する差速度演算部と、この差速度の演算結果にフィルタリングを行うフィルタと、フィルタリングされた差速度に所定のゲインを乗算してトルク値に変換する乗算部と、前記モータトルク指令値と前記変換されたトルク値とを加算し、前記モータの駆動電流を制御する電流コントローラに入力する第３の演算部とを備えることを特徴とする請求項１に記載の位置又は速度制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、減速器、送りネジ機構等の動力伝達機構を介してモータの出力軸と接続された負荷の位置又は速度を制御する装置に関する。
【背景技術】
【０００２】
図４を参照して、動力伝達機構の一例としての減速器を介してモータと接続された機械負荷の位置制御装置について説明する。
【０００３】
図４に示す制御装置は、一般にセミクローズド制御装置と呼ばれ、モータ位置コントローラ４１のマイナーループにモータ速度コントローラ４２を有するものである。モータ４０の出力軸に減速器４７を介して機械負荷４８が接続されている。モータ４０の出力軸にはまた、モータ軸角度を検出するセンサが設置され、検出されたモータ軸角度が位置検出値としてフィードバックされる。フィードバックされたモータ軸角度は演算器４３に入力されて位置指令値との差が算出され、モータ位置コントローラ４１に入力される。モータ位置コントローラ４１は入力された差分に基づいて速度指令値を算出、出力する。フィードバックされたモータ軸角度はまた速度演算器４４に入力されてモータ軸速度が算出され、演算器４５に入力される。演算器４５で算出された速度指令値とモータ軸速度との差はモータ速度コントローラ４２に入力され、モータトルク指令値が算出される。電流コントローラ４６は、モータトルク指令値に基づいてモータ４０の駆動電流を制御する。
【０００４】
図５は、一般にフルクローズド制御装置と呼ばれ、図４のモータ位置コントローラ４１に代えて負荷位置コントローラ５１が設けられ、そのマイナーループにモータ速度コントローラ４２を有する。減速器４７の出力側又は機械負荷４８の入力側に負荷軸角度を検出するセンサが設置され、検出された負荷軸角度が負荷位置検出値としてフィードバックされる。フィードバックされた負荷軸角度は演算器５２に入力されて位置指令値との差が算出され、負荷位置コントローラ５１に入力される。負荷位置コントローラ５１は入力された差分に基づいて速度指令値を算出、出力する。以降の構成、動作は図４の制御装置と同じである。
【０００５】
図６、図７には、上記の位置制御装置の改良例を示す。これらの改良例は、負荷側の位置を検出し、検出した負荷側の位置情報を用いて、減速器４７が介在することによるモータ４７と機械負荷４８の間の機械共振等を抑制する補償器を有する。
【０００６】
図６の位置制御装置は、図５に示したフルクローズド制御装置を基本としている。つまり、モータ軸角度と負荷軸角度とを検出してそれらの角度差を算出し、算出した値に任意のゲインを乗算したうえでモータ速度コントローラ４２からのモータトルク指令値に加算する補償系６０を加えている。補償系６０は、演算器６２、差速度演算器６３、ゲイン乗算器６４、６５、演算器６６、６７からなる。このような補償系による補償方法は、例えば特許文献１に開示されている。
【０００７】
上記の補償方法の変形例として、差速度、すなわちモータ軸と負荷軸間の速度差を検出しこの速度差に任意のゲインを乗算してＬＰＦ（ローパスフィルタ）を通し、トルク指令値に加算する補償系を加える手法も知られている。
【０００８】
図７は、図４のセミクローズド制御と図５のフルクローズド制御を融合した、ハイブリッド制御とも呼ばれる位置制御装置を示す。この制御装置は、例えば図５の負荷位置コントローラ５１に代えてハイブリッド位置コントローラ７１を有し、中高周波帯城のモータ軸角度と低周波帯城の負荷位置をハイブリッド位置コントローラ７１にフィードバックすることにより、セミクローズド制御による安定性確保とフルクローズド制御による定常偏差解消の両立を図るようにしている。負荷位置のフィードバック系はＬＰＦ７２、演算器７３を有する。このような制御形態は、例えば特許文献２、非特許文献１に開示されている。
【０００９】
しかしながら、上記の手法ではいずれも、負荷の応答追従性が十分とはいえないケースがあった。これは、安定性の確保のため図８のようにモータ速度コントローラ４２においてモータ軸速度をフィードバック量として用いたものを、マイナーループに持っているためである。
【００１０】
これに対し、例えば図９のように負荷速度コントローラ６１において負荷速度をそのまま速度制御ループヘフィードバックしてしまうと、モータ４０と機械負荷４８間の結合剛性が高いケースを除き、システムが不安定になり発散しやすくなってしまう。よって、モータ４０と機械負荷４８間の結合剛性が低いケースでは速度制御ループに負荷速度をフィードバックする形態は採用されないのが普通である。
【００１１】
【特許文献１】特開２００２−６９５９公報
【特許文献２】特公平２−３０５２２公報
【非特許文献１】「剛性が不足した機構部を含む系の精密位置決め」精密工学会誌vol 66,No 9,2000
【発明の開示】
【発明が解決しようとする課題】
【００１２】
本発明は、モータと機械負荷間が結合剛性の十分でない動力伝達機構で結合されている場合でも機械負荷の位置、速度の制御性能の向上を実現できる位置又は速度制御装置を提供しようとするものである。
【００１３】
本発明は特に、フィードバックによる速度制御ループを持つ位置又は速度制御装置において、速度制御ループに負荷速度をフィードバックしても高精度の位置又は速度制御を実現できるようにしようとするものである。
【課題を解決するための手段】
【００１４】
本発明は、動力伝達機構を介してモータと接続された負荷の位置又は速度を制御する装置であって、負荷入力軸の位置又は角度の検出値から算出された負荷速度と負荷速度指令値との差を入力として受ける負荷速度コントローラを備え、前記負荷速度コントローラは、入力された前記差に基づいて負荷軸トルク指令値を算出するとともに、算出された値にあらかじめ定められたゲインを乗算してねじり角指令値として出力するものであり、前記負荷速度コントローラのマイナーループに、前記ねじり角指令値に基づいてモータトルク指令値を出力するねじり角コントローラと、モータ出力軸の位置又は角度の検出値と前記負荷入力軸の位置又は角度の検出値との差をねじり角として算出する第１の演算部と、前記ねじり角指令値と前記ねじり角との差を算出する第２の演算部とを備え、該第２の演算部の算出結果を前記ねじり角コントローラに入力することを特徴とする。
【００１５】
本発明による位置又は速度制御装置においては、前記ねじり角から前記動力伝達機構における入力、出力の差速度を演算する差速度演算部と、この差速度の演算結果にフィルタリングを行うフィルタと、フィルタリングされた差速度に所定のゲインを乗算してトルク値に変換する乗算部と、前記モータトルク指令値と前記変換されたトルク値とを加算し、前記モータの駆動電流を制御する電流コントローラに入力する第３の演算部とを備えるようにしても良い。
【発明の効果】
【００１６】
本発明によれば、位置又は速度制御装置に、ねじり角とねじり角指令値との差に基づいてモータトルク指令値を出力するねじり角コントローラを備えたことにより、低剛性な動力伝達機構による機械負荷の駆動系において、簡便な構成で高精度の位置又は速度制御を実現できる。
【発明を実施するための最良の形態】
【００１７】
図１を参照して本発明の第１の実施形態について説明する。第１の実施形態は、モータと機械負荷との間を減速器（動力伝達機構）で結合し、負荷速度を制御する速度制御装置に適用した例である。
【００１８】
図１において、モータ１０と機械負荷１１とが減速器１２で結合されている。モータ１０は電流コントローラ１３によって駆動電流が制御される。機械負荷１１の入力軸（又は減速器１２の出力軸）にエンコーダ等の角度センサ（図示せず）が設置され、負荷軸角度（つまり、負荷軸位置）が検出される。検出された負荷軸角度はフィードバックされ、速度演算器１４で負荷速度に変換される。負荷速度は演算器１５に入力され、負荷速度指令値との差が算出される。算出された速度差は負荷速度コントローラ１６に入力される。負荷速度コントローラ１６は入力された速度差に基づいてねじり角指令値θｒｅｆを出力する。
【００１９】
第１の実施形態による速度制御装置は、負荷速度コントローラ１６のマイナーループに、動力伝達機構におけるねじり角に基づいてモータトルク指令値を出力する、動力伝達機構のねじり角（入出力角度差）コントローラ１７を持つことを特徴とする。
【００２０】
また、モータ１０の出力軸（又は減速器１２の入力軸）に設置したエンコーダ等の角度センサ（図示せず）でモータ軸角度（つまり、モータ軸位置）が検出され、演算器１８−１（第１の演算部）により負荷軸角度との差がねじり角Δθとして算出される。算出されたねじり角Δθ（角度差）は、負荷速度コントローラ１６とねじり角コントローラ１７との間に接続された演算器１８−２（第２の演算部）にフィードバックされる。演算器１８−２は負荷速度コントローラ１６からのねじり角指令値θｒｅｆとねじり角Δθとの差を算出し、算出結果をねじり角コントローラ１７に入力する。ねじり角コントローラ１７は入力された算出結果に応じてモータトルク指令値を出力する。
【００２１】
また、演算器１８−１で算出された角度差（ねじり角Δθ）を差速度演算器１８−３で動力伝達機構の入出力差速度（ねじり角速度）に変換し、これを周波数フィルタ（ローパスフィルタ）１８−４を通過させて低周波帯域の差速度を取り出した後、ゲイン乗算器１８−５において所定のゲインを掛けてトルク値に変換し、演算器１８−６（第３の演算部）に与える。演算器１８−６はゲイン乗算器１８−５の出力（トルク値）をねじり角コントローラ１７からのモータトルク指令値に加算する。このような補償系による補償により、動力伝達機構におけるねじりにダンピング効果を与え、過度のねじりによる振動を抑止できるようにしている。
【００２２】
なお、本実施形態における負荷速度コントローラ１６は、演算器１５からの差速度に基づいて負荷軸トルク指令値を算出する、負荷速度コントローラ本来の機能に加えて、算出した負荷軸トルク指令値にねじり剛性ゲインＫを乗算してねじり角指令値θｒｅｆを出力するゲイン乗算機能を有する。ここで、
ねじり角指令値＝ねじりトルク指令値×ねじり剛性ゲインＫ
であり、ねじり剛性ゲインＫは動力伝達機構のねじり剛性により決定される。
【００２３】
また、
ねじりトルク＝負荷軸トルク＝負荷イナーシャ加速トルク＋負荷軸摩擦外乱
であり、ねじり角コントローラ１７は、摩擦外乱の影響をも含めて負荷軸トルクが所望となるようにねじり角を調整するようにしている。
【００２４】
一方、電流コントローラ１３は、
モータ発生トルク＝ねじりトルク＋モータイナーシャ加速トルク＋モータ軸摩擦外乱
として要求されるねじりトルクを満足するように、モータ１０の駆動電流を制御する。
【００２５】
図２に、本発明と従来例とを比較するための時間波形を示す。図２（ａ）は図８のモータ速度制御装置で制御を行った場合の時間波形を示し、図２（ｂ）は図８のモータ速度制御装置に、図６と同様のねじり角、差速度補償を与えて制御を行った場合の時間波形を示す。一方、図２（ｃ）は図１の負荷速度制御装置で制御した場合の時間波形を示す。
【００２６】
図２（ａ）では、モータと機械負荷間の結合剛性が十分でないと、負荷速度が指令速度に対して遅れてしまい、共振により振動的になる場合がある。
【００２７】
図２（ｂ）では、共振による振動は抑制されるが、負荷速度の応答が遅れる。
【００２８】
これに対し、図２（ｃ）では、負荷軸への入力トルクの代わりにねじり角（モータ軸−負荷軸間の角度差）を制御することにより、負荷速度を直接フィードバック制御することが可能になっている。その結果、負荷速度の応答は良好である（モータ軸は負荷軸と独立しており、ある程度の振動は許容される）。すなわち、本実施形態による速度制御装置は、従来は採用されていない負荷速度検出値を速度コントローラへフィードバックする構成を容易とし、負荷速度指令値と負荷速度の偏差を小さくして応答性を向上させ、高精度の速度制御を可能にしている。
【００２９】
図３は本発明の第２の実施形態を示し、モータと機械負荷との間を減速器（動力伝達機構）で結合し、負荷位置を制御する位置制御装置に適用した例である。
【００３０】
第２の実施形態による位置制御装置は、負荷軸角度の検出値を負荷位置としてフィードバックし、演算器３１により負荷位置指令値と負荷位置との差を算出し、算出結果に基づいて負荷速度指令値を出力する負荷位置コントローラ３２を備える。負荷位置コントローラ３２のマイナーループに負荷速度コントローラ１６を有し、図１と同様、負荷速度コントローラ１６のマイナーループに動力伝達機構のねじり角（入出力角度差）コントローラ１７を持つ。また、負荷位置指令値を受けてフィードフォワード演算を行う演算部３３を有し、その演算結果である速度フィードフォワード値と負荷位置コントローラ３２からの速度指令値とを演算器３４で加算して演算器１５に入力させるようにしている。
【００３１】
この位置制御装置は、従来の制御装置（セミクローズド制御、フルクローズド制御、ハイブリッド制御）に比べ、追従制御での位置偏差を小さくすることができ、高精度の位置制御が可能となる。
【００３２】
以上説明したように、本発明によれば、簡便なコントローラにより、これまで減速器等の低剛性な動力伝達機構を介しては不可能とされてきた高精度な速度・位置制御が可能となる。
【００３３】
また、本発明のねじり角フィードバック情報は負荷速度コントローラの出力と演算されてねじり角コントローラに入力される。この方式は従来のモータコントローラの出力を補償する方式より機械負荷の制御に遅れがないという効果がある。
【００３４】
なお、本発明の実施形態を速度制御装置、位置制御装置の場合に分けて説明したが、モータ１０と機械負荷１１とを結合する動力伝達機構には減速器に限らず、変速機、送りねじ機構等が適用され、特に結合剛性が十分でない場合に適している。
【００３５】
また、上記の実施形態では、機械負荷を回転体として説明したが、機械負荷が直線運動を行う負荷であっても、センサで検出された値は正規化されて計算されるのでねじり角に換算されることになる。つまり、請求項に記載されたねじり角は、このような換算値をも含み、これにより本発明は、対象が回転運動、直線運動のいずれを行うものであるかを問わず、適用され得る。言い換えれば、本発明における機械負荷とは、半導体製造ステージの移動体やレーザ加工装置の移動体のように直線駆動されるものでもよく、回転体又はその可動子のように回転駆動されるものでも良い。つまり、駆動体と、被駆動体と、それらの間に動力伝達機構を有する装置全般に適用される。
【００３６】
このことから、本発明による位置又は速度制御装置は、ロボット、工作機械における追従制御に適している。
【図面の簡単な説明】
【００３７】
【図１】図１は、本発明の第１の実施形態による速度制御装置の構成を示すブロック図である。
【図２】図２は、本発明と従来例との制御特性を比較するための時間波形を示す。
【図３】図３は、本発明の第２の実施形態による位置制御装置の構成を示すブロック図である。
【図４】図４は、従来のセミクローズド制御方式の位置制御装置の構成を示すブロック図である。
【図５】図５は、従来のフルクローズド制御方式の位置制御装置の構成を示すブロック図である。
【図６】図６は、他の従来として、図５に示した位置制御装置の改良例を示したブロック図である。
【図７】図７は、他の従来例として、図５と図６の組み合わせによるハイブリッド型と呼ばれる位置制御装置の構成を示したブロック図である。
【図８】図８は、従来の位置制御装置の問題点を説明するための図である。
【図９】図９は、速度制御装置の問題点を説明するための図である。
【符号の説明】
【００３８】
１５、１８−１、１８−２、１８−６、３１、３４    演算器
【図１】
0005052987.tif 000002
【図２】
0005052987.tif 000003
【図３】
0005052987.tif 000004
【図４】
0005052987.tif 000005
【図５】
0005052987.tif 000006
【図６】
0005052987.tif 000007
【図７】
0005052987.tif 000008
【図８】
0005052987.tif 000009
【図９】
0005052987.tif 000010