(57)【要約】【課題】  従来のベルト駆動サーボ装置ではプーリとベルトとの噛合いの変動によりモータカ移転速度に比例した周波数の駆動力リップルが発生し、可動部が振動したのを本発明により解消する。【解決手段】  モータ６と、モータのシャフトに結合されたプーリと、プーリによって駆動されるベルトと、ベルトに結合された可動部とを備えたベルト駆動サーボ装置の位置制御方法において、モータ６の速度から外乱推定オブザーバ９により外乱力を推定し、推定した外乱力推定値をモータトルク指令に減算（５）して外乱力を補正すると同時に、加速度センサ１０で可動部の加速度を計測し、フィルタ１１を通して比例加速度制御ループを構成することにより、可動部の振動を減衰させるようにした。【選択図】図２
2007004287.tif 000002
【特許請求の範囲】
【請求項１】
モータと、該モータを制御するモータ制御器と、前記モータのシャフトに結合されたプーリと、該プーリによって駆動されるベルトと、該ベルトに結合された可動部と、該可動部に設けられた加速度センサとを備えたベルト駆動サーボ装置の位置制御方法において、
前記モータの速度から外乱推定オブザーバにより外乱力を推定し、推定した外乱力推定値をモータトルク指令に減算して外乱力を補正すると同時に、
前記加速度センサで前記可動部の加速度を計測し、フィルタを通して比例加速度制御ループを構成することにより、前記プーリと前記ベルトとの噛合いの変動に起因して前記モータの回転数に比例する駆動力リップルにより発生する前記可動部の振動を減衰させる振動抑制制御方法を採用したことを特徴とするベルト駆動サーボ装置の位置制御方法。
【請求項２】
比例積分速度制御器からの出力を比例加速度制御器に入れて該比例加速度制御器からモータトルク指令を得るベルト駆動サーボ装置の位置制御方法において、
前記加速度センサからの加速度信号をバンドパスフィルタに通して前記比例積分速度制御器からの出力に負帰還することを特徴とする請求項１記載のベルト駆動サーボ装置の位置制御方法。
【請求項３】
比例積分速度制御器からの出力を比例加速度制御器に入れて該比例加速度制御器からモータトルク指令を得るベルト駆動サーボ装置の位置制御方法において、
前記加速度センサからの加速度信号をバンドパスフィルタおよび積分器に通して前記比例積分速度制御器からの出力に負帰還することを特徴とする請求項１記載のベルト駆動サーボ装置の位置制御方法。
【請求項４】
モータと、該モータを制御するモータ制御器と、前記モータのシャフトに結合されたプーリと、該プーリによって駆動されるベルトと、該ベルトに結合された可動部と、該可動部に設けられた加速度センサとを備えたベルト駆動サーボ装置の位置制御装置において、
前記モータの速度から外乱力を推定し、推定した外乱力推定値をモータトルク指令に減算して補正する外乱推定オブザーバと、
前記加速度センサで計測した前記可動部の加速度を扱う比例加速度制御ループ内に挿入されたフィルタと、を備えたことを特徴とするベルト駆動サーボ装置の位置制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ベルト駆動サーボ装置の位置制御方法に関するもので、特に可動部を位置決めするベルト駆動サーボ装置の振動抑制制御方法に関する。
【背景技術】
【０００２】
従来のベルト駆動サーボ装置は、外乱オブザーバにより外乱力を推定し、推定された外乱力をモータトルク指令に加算して振動を減衰させている（例えば、非特許文献１参照）。
また、従来のサーボ系の振動抑制制御では加速度を加速度センサで計測し、速度制御器に負帰還し、振動を減衰させている（例えば、非特許文献２参照）。
図７において、１０１は位置制御器、１０２はベルト駆動系の動特性、１０３は外乱力オブザーバ、１０４は外乱力、１０５は推定外乱力、１０６は減算器、１０７はモータトルクであり、外乱力を外乱力オブザーバで推定し、推定外乱力をモータトルクから減算することで、ベルト駆動サーボ装置の振動抑制制御を行っている。
このように、従来のベルト駆動サーボ装置では、外乱力オブザーバにより外乱力を推定しモータトルクから減算して振動抑制をするという手順がとられていた。
さらにベルト駆動ではない産業用ロボットの振動抑制制御としては、加速度を計測しモータ速度指令から減算することが行われていた．
【特許文献１】特開平９−２１２２０３号公報
【特許文献２】特開平１０−６２６１号公報
【非特許文献１】李亨、武居直行、古荘純次著、「歯付ベルト駆動サーボ系の周期的速度変動の外乱オブザーバによる低減に関する研究」、日本機械学会論文集（C編）68巻676号、3673-3680頁 (2002)、図５
【非特許文献２】S. Futami、 N. Kyura and S. Hara: Vibration absorption control of industrial robots by acceleration feedback、 IEEE Transaction on Industrial Electronics、 299-305、 Vol. IE-30、 No.3 (1983)
【０００３】
特許文献１記載の発明は、ロボットのアームの振動の抑制を図り、かつアームの位置決めの高速化を図るために、アンプに与える指令トルクを算出する制御部が、第２アームを駆動するサーボモータと、サーボモータにトルクを発生させるアンプ部及びサーボモータの角度を検出する角度検出器を備えるロボットを制御対象として、指令トルクとサーボモータの角度とに基づいて、アームの角度及び角速度、サーボモータの角速度、外乱トルクを推定するねじれ角・外乱推定オブザーバと、サーボモータの目標角度に基づいて、サーボモータの目標角速度を算出するフィードフォワード部と、サーボモータの目標角度と角度と目標角速度、第２アーム３の推定角度と推定角速度、サーボモータの推定角速度及び推定外乱トルクに基づいて指令トルクを算出し、制御部が算出した指令トルクに基づいてロボットを制御するものである。
特許文献２記載の発明は、 測定された周波数特性から推定演算に必要な定数を簡便に求めるもので、そのために、第２アームを駆動するサーボモータ、サーボモータにトルクを発生させるアンプ部及びサーボモータの角度を検出する角度検出器を備えるロボットを制御対象とし、指令トルクとサーボモータの角度とに基づいて、少なくとも第２アームの角度若しくは角速度'を推定するねじれ角・外乱推定オブザーバと、ねじれ角・外乱推定オブザーバが推定した第２アームの推定角度若しくは推定角速度に基づいて、指令トルクを算出する制御部と、第２アーム３の角度若しくは角速度を推定するための各種定数をアンプ部に与える指令トルクから角度検出器により検出されるサーボモータの角度までの伝達関数から決定する推定定数決定部とを有し、制御部が算出した指令トルクに基づいてロボットを制御するものである。
【発明の開示】
【発明が解決しようとする課題】
【０００４】
従来のベルト駆動サーボ装置の振動抑制制御方法では、外乱力オブザーバにより外乱力を推定し、推定外乱力をモータトルクから減算するという手順をとっており、さらに外乱力リップルはモータ回転数に比例して変化するため振動周波数が大幅に変化するため、高速域での速い外乱力推定ができず、大きな振動抑制効果が得られないという問題があった。
また、産業用ロボットのような場合は、加速度を加速度センサで計測し、速度制御器に負帰還することが行われていたが、ベルト駆動サーボ装置においては、速度制御系に負帰還しても振動抑制効果がないという問題もあった。
そして、特許文献１および特許文献２記載の発明は、本発明と同じく共に外乱オブザーバを用いて振動抑制を計る点で共通しているが、加速度フィードバック制御を行なうための加速度センサがないため、高い周波数領域で加速度フィードバックをする場合に弱点があった。
本発明はこのような問題点に鑑みてなされたものであり、外乱力オブザーバにより外乱力を推定し外乱力推定値をモータトルクから減ずるとともに、可動部の加速度を加速度センサにより計測し、比例加速度制御ループを構成することにより、ベルト駆動サーボ装置の振動抑制制御することができるようにすることを目的としている。
本発明によって設けた加速度フィードバック制御により、外乱オブザーバによる外乱力推定に与える影響として以下のように考えられる。
１．ベルト駆動系の振動周波数は一般に速度に比例するため、広い周波数帯域において振動抑制作用がある加速度フィードバックが必要となる。
２．外乱オブザーバは摩擦力などの比較的低周波数の動作補正に効果がある。
３．これらから、高い周波数領域で加速度フィードバックが有効であり、低い周波数領域で外乱オブザーバが有効である。
【課題を解決するための手段】
【０００５】
上記問題を解決するため、請求項１記載の発明は、ベルト駆動サーボ装置の位置制御方法に係り、モータと、該モータを制御するモータ制御器と、前記モータのシャフトに結合されたプーリと、該プーリによって駆動されるベルトと、該ベルトに結合された可動部と、該可動部に設けられた加速度センサとを備えたベルト駆動サーボ装置の位置制御方法において、前記モータの速度から外乱推定オブザーバにより外乱力を推定し、推定した外乱力推定値をモータトルク指令に減算して外乱力を補正すると同時に、前記加速度センサで前記可動部の加速度を計測し、フィルタを通して比例加速度制御ループを構成することにより、前記プーリと前記ベルトとの噛合いの変動に起因して前記モータの回転数に比例する駆動力リップルにより発生する前記可動部の振動を減衰させる振動抑制制御方法を採用したことを特徴としている。
請求項２記載の発明は、請求項１記載のベルト駆動サーボ装置の位置制御方法において、比例積分速度制御器からの出力を比例加速度制御器に入れて該比例加速度制御器からモータトルク指令を得るベルト駆動サーボ装置の位置制御方法において、前記加速度センサからの加速度信号をバンドパスフィルタに通して前記比例積分速度制御器からの出力に負帰還することを特徴としている。
請求項３記載の発明は、請求項１記載のベルト駆動サーボ装置の位置制御方法において、比例積分速度制御器からの出力を比例加速度制御器に入れて該比例加速度制御器からモータトルク指令を得るベルト駆動サーボ装置の位置制御方法において、前記加速度センサからの加速度信号をバンドパスフィルタおよび積分器に通して前記比例積分速度制御器からの出力に負帰還することを特徴としている。
請求項４記載の発明は、ベルト駆動サーボ装置の位置制御装置に係り、モータと、該モータを制御するモータ制御器と、前記モータのシャフトに結合されたプーリと、該プーリによって駆動されるベルトと、該ベルトに結合された可動部と、該可動部に設けられた加速度センサとを備えたベルト駆動サーボ装置の位置制御装置において、前記モータの速度から外乱力を推定し、推定した外乱力推定値をモータトルク指令に減算して補正する外乱推定オブザーバと、前記加速度センサで計測した前記可動部の加速度を扱う比例加速度制御ループ内に挿入されたフィルタと、を備えたことを特徴としている。
【発明の効果】
【０００６】
請求項１および４記載の発明によると、比例加速度制御ループを構成することで周波数が速度に比例する振動を広い範囲で抑制することができ、その残りの低周波数の振動をオブザーバで補正することができる。
また、請求項２記載の発明によると、抑制する振動周波数領域を限定し、高次の振動モードで発振することを防止することができる。
また、請求項３記載の発明によると、請求項１記載の効果に加えて、速度フィードバックが有効となる構造のサーボ系に適用することが可能となる。
【発明を実施するための最良の形態】
【０００７】
以下、本発明の方法の具体的実施例について、図に基づいて説明する。
図１は本発明の方法を実施するベルト駆動サーボ装置の全体システムを示す概略図である。図において、Ｍはサーボモータ、Ｒはサーボモータを制御するモータ制御器、Ｃはカップリング、Ｐ１、Ｐ１はプーリ、Ｂはベルト、Ｔはテーブルで形成された可動部、Ａは可動部に設けられた加速度センサである。図から判るように、サーボモータＭにカップリングＣを介してベルトＢと噛合うプーリＰ１が結合されている。可動部（テーブル）ＴはベルトＢに結合され、サーボモータＭの正転・反転に応じて可動部Ｔは左・右に動く。可動部Ｔの動きは加速度センサＡが検出する。
【実施例１】
【０００８】
図２は、本発明の実施例１に係る方法を実施するベルト駆動サーボ装置の構成を示すブロック図である。図において１は位置の比例制御器、２は速度の比例積分制御器、３は減算器、４は加速度の比例制御器、５は減算器、６はモータ、６１はエンコーダ、７はエンコーダ６１の検出したモータ速度からモータ位置への物理的な積分器、８はベルト駆動機構、９は外乱力オブザーバ、１０は加速度センサ、１１はフィルタとなっている。そして、比例制御器１〜減算器５、積分器７、外乱力オブザーバ９、フィルタ１１は図1のモータ制御器Ｒ内に収納される構成としている。
【０００９】
図３は、外乱力オブザーバ９の構成を示している。図において、破線から上の部分が制御対象のブロック線図で、下が外乱力オブザーバである。
ここに、
【００１０】
【数１】
2007004287.tif 000003
で、1/s は積分を表し、Jはサーボモータとベルトを合わせた慣性モーメントである。破線から下が外乱力オブザーバで、Lはオブザーバのバンド幅を決定するパラメータである。
【００１１】
図４は外乱力オブザーバおよび加速度制御を行ったときのサーボモータの回転速度の時間応答であり、図５は外乱力オブザーバおよび加速度制御が無い場合のサーボモータの回転速度の時間応答である。
両図において、縦軸は角速度（ｒａｄ／ｓ）、横軸は時間（ｓ）である。
図５では外乱力オブザーバおよび加速度制御が無いので、サーボモータの回転速度の時間応答は、図のように、約５５ヘルツの顕著な振動が発生している。この振動数は、サーボモータ回転速度とプーリの歯数とを乗じた値と等しく、プーリとベルトとの噛合いの変動で発生する振動である。
これに対して、図４では本発明に係る外乱力オブザーバおよび加速度制御を行っているので、可動部の速度は回転速度プーリの等価半径を乗じたものとなり、図のように、この応答には図５で示される振動が抑制されているのが判る。
【００１２】
以上のように、本発明によれば、外乱力オブザーバにより外乱力を推定し外乱力推定値をモータトルクから減ずるとともに、可動部の加速度を加速度センサにより計測し、比例加速度制御ループを構成するので、周波数が速度に比例する振動を比例加速度制御ループにより広い範囲で抑制することができ、その残りの低周波数の振動をオブザーバで補正することができるため、ベルト駆動サーボ装置の振動を広い範囲で抑制制御することができるようになる。
【実施例２】
【００１３】
図６は、本発明の実施例２に係る方法を実施するベルト駆動サーボ装置の構成を示すブロック図である。図における符号で図２と同じものは、同じ機能を有する同一機器なので、重複説明は省略する。
実施例２のベルト駆動サーボ装置が、図２のそれと異なるのは、フィルタ１１と減算器３との間に積分器１２を挿入して、加速度情報を速度情報に変換している点である。積分器１２も同じくモータ制御器Ｒ（図１）の中に収納されている。
これにより、比例積分速度制御器２からの出力を比例加速度制御器４に入れる際に、加速度センサ１０からの加速度信号をバンドパスフィルタ１１および積分器１２に通して加速度ではなく速度として比例積分速度制御器２からの出力に負帰還するようになるので、加速度ではなく速度フィードバックが有効となる構造のサーボ系に適用することが可能となる。
実施例２によっても、同じく外乱力オブザーバにより外乱力を推定し外乱力推定値をモータトルクから減ずるとともに、可動部の加速度を加速度センサにより計測し、比例加速度制御ループを構成するので、周波数が速度に比例する振動を比例加速度制御ループにより広い範囲で抑制することができ、その残りの低周波数の振動をオブザーバで補正することができるため、ベルト駆動サーボ装置の振動を広い範囲で抑制制御することができるようになる。
【図面の簡単な説明】
【００１４】
【図１】本発明の方法を適用するベルト駆動サーボ装置の構成を示すブロック図である。
【図２】本発明の実施例１に係る方法を適用するベルト駆動サーボ装置の外形図である。
【図３】本発明で適用する外乱力オブザーバのブロック線図である。
【図４】本発明の方法を適用したときのモータ回転速度の時間応答である。
【図５】本発明を適用しないときのモータ回転速度の時間応答である。
【図６】本発明の実施例２に係る方法を適用するベルト駆動サーボ装置の外形図である。
【図７】従来の方法を適用したベルト駆動サーボ装置の構成を示すブロック図である。
【符号の説明】
【００１５】
１  比例位置制御器
２  比例積分速度制御器
３  減算器
４  比例加速度制御器
５  加算器
６  モータ
６１  エンコーダ
７  積分器
８  ベルト駆動機構
９  外乱力オブザーバ
１０  加速度センサ
１０１  制御器
１０２  ベルト駆動系の動特性
１０３  外乱力オブザーバ
１０４  外乱力
１０５  推定外乱力
１０６  減算器
１０７  モータトルク
Ｍ  サーボモータ
Ｒ  モータ制御器
Ｃ  カップリング
Ｐ１、Ｐ１  プーリ
Ｂ  ベルト
Ｔ  テーブル（可動部）
Ａ  加速度センサ
【図１】
2007004287.tif 000004
【図２】
2007004287.tif 000005
【図３】
2007004287.tif 000006
【図４】
2007004287.tif 000007
【図５】
2007004287.tif 000008
【図６】
2007004287.tif 000009
【図７】
2007004287.tif 000010