JP6009397B2 - 位置制御装置のイナーシャ推定方法およびイナーシャ推定装置 - Google Patents

Description

本発明は、工作機械等の位置制御装置、特に重力作用軸におけるイナーシャ推定方法、および、イナーシャ推定装置の改良に関する。

工作機械などの送り軸をサーボモータで駆動する場合、位置・速度制御系を構成し、数値制御装置からの指令に従い、モータやモータに取り付けられた負荷の位置・速度を制御する。モータや負荷の位置・速度を正確に制御するためには、モータや負荷のイナーシャに基づいて制御パラメータを決定する必要があり、負荷のイナーシャは工作物によって変動することから、種々のイナーシャ推定方法が提案されている。

図１３は、特許文献１に開示の技術を示している。この技術では、モータを複数の異なる速度毎に、一定速駆動し（Ｓｔｅｐ１）、速度毎に摩擦分をキャンセルするために要するトルク、重力分をキャンセルするために要するトルクを測定する（Ｓｔｅｐ２，３）。次に、サーボモータを加減速駆動し（Ｓｔｅｐ４）、速度−トルク、速度−加速度の関係を求める（Ｓｔｅｐ５〜Ｓｔｅｐ７）。そして、速度−トルクの関係から、予め測定した摩擦分をキャンセルするために要するトルク、重力分をキャンセルするために要するトルクを減算し、イナーシャを加速するのに要するトルクを算出後（Ｓｔｅｐ８）、加減速駆動で求めた加速度で除することによってイナーシャを算出する（Ｓｔｅｐ９）。

図１４は、特許文献２に開示の技術を示している。この技術では、イナーシャの推定に必要な情報を取得する際のモータ位置を、複数回の測定の間で常に同一とすることで、モータ位置によって重力の影響が変動するような機械構成であっても、同じ条件下でイナーシャを算出できるようにしている。

図１５は、特許文献２においてイナーシャを推定するための制御構成を示した図である。速度指令値Ｖｃと速度検出値Ｖｄとの誤差に従い、速度制御器１１にてモータ１２へのトルク指令値Ｔｃを生成する。一方、モータモデル１５は、モータ１２のトルク指令と回転速度の関係を数式で表したものであり、剛体モデルや２慣性モデルにより表現される。速度制御器１４は、速度制御器１１と等価な内部構成とし、出力されたトルク指令Ｔ’ｃをモータモデル１５に与え、速度検出値Ｖ’ｄを得て、速度指令値Ｖｃと速度検出値Ｖ’ｄとの誤差を速度制御器１４の入力とする。

速度指令値Ｖｃは、図１４のＳｔｅｐ１２の速度波形とし、１回目の動作では往復動作を行い、２回目の動作では１回目の動作速度の正負を反転させた逆順往復動作を行う。また、１回目の動作と２回目の動作の間にθＩだけ移動する過程を設けることで１回目と２回目の動作位置を同一とする。速度指令値Ｖｃが与えられると速度制御器１１，１４からトルク指令値Ｔｃ，Ｔ’ｃが出力され、積分器１３，１６により、図１４のＳｔｅｐ１２の積分区間において、それぞれの値が積算される。積分区間が終了した時点で、下記式１の演算を行うことによりイナーシャを算出する。なお、式１でＪはイナーシャ推定値、Ｊ’はモータモデル１５に用いた仮イナーシャ値である。

Ｊ＝｛∫（Ｔｃ）ｄｔ／∫（Ｔ’ｃ）ｄｔ｝×Ｊ’ ・・・式１

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

特許文献１の技術では、加減速駆動に必要なトルクから摩擦分をキャンセルするために要するトルク、及び、重力分をキャンセルするために要するトルクを減算するため、速度に対し、これらのトルクが一定となる状況下では、正確にイナーシャを算出することができる。しかし、摩擦分をキャンセルするために要するトルク、重力分をキャンセルするために要するトルク、それぞれを測定する上で、モータを複数の異なる一定速度で移動方向別に駆動する必要があるため、推定に時間がかかる。また、重力分をキャンセルするために要するトルクはモータ位置に関わらず一定と見なしているため、図１６のように、モータ位置によって重力の影響が異なる機械構成に対してはイナーシャを正確に推定することができないという課題がある。

図１６において、モータ２１は、アーム２２によって強固に結合されたテーブル２３をモータ軸中心で回転させる。モータ軸中心とテーブル２３の重心位置間の距離をＬ０、重心位置にかかる重力をＦｇ、Ｆｇのうちモータ軸中心方向と直行する方向の成分をＦｒ、重力分をキャンセルするために要するトルクをＴｇとすると、Ｆｒ，Ｔｇはモータ２１の回転角θを用いて下記式２、式３のように表現できる。
Ｆｒ＝Ｆｇ×ｓｉｎθ ・・・式２
Ｔｇ＝Ｆｒ×Ｌ０＝Ｆｇ×Ｌ０×ｓｉｎθ ・・・式３

即ち、重力分をキャンセルするために要するトルクＴｇはモータ２１の回転角θに基づき正弦波状に変動するため、一定速度でモータを駆動していても、変動するトルクの影響で正確にイナーシャを算出することができない。

一方、特許文献２の技術では、トルク指令値Ｔｃ、Ｔ’ｃの積算を常に同一区間で行うため、重力分をキャンセルするために要するトルクＴｇを複数回の測定の間で同一とすることができる。しかし、積算するトルク指令値Ｔｃには、摩擦分をキャンセルするために要するトルク、重力分をキャンセルするために要するトルク、それぞれが含まれる。このため、算出されるイナーシャ推定値には、これらのトルク分だけ誤差が生じるという課題がある。

本発明が解決しようとする課題は、モータ位置によって重力の影響が異なる機械構成では、イナーシャを正確に推定することができないことである。そして、本発明の目的は、このようなモータ位置によって重力の影響が異なる機械構成であっても、正確にイナーシャを推定することができるイナーシャ推定方法およびイナーシャ推定装置を提供することである。

本発明のイナーシャ推定方法は、位置指令値とモータまたは当該モータに結合された被駆動部の位置検出値とに基づいてトルク指令値を算出し、当該トルク指令値に従ってモータを駆動する装置に搭載された前記モータおよび前記被駆動部を含む可動部のイナーシャを推定するイナーシャ推定方法であって、前記可動部を駆動させた際に、重力の影響をキャンセルするためのトルクである重力トルクがゼロになる位置を中心として、前記重力トルクの積算値または前記可動部の加速度が対称性を持つような加速駆動条件または減速駆動条件の少なくとも一方を決定する加減速条件決定工程と、決定された加速または減速駆動条件に従ってモータを駆動した際に得られるトルク指令値の積算値と、前記モータまたは被駆動部の加速度検出値の積算値と、の比から前記可動部のイナーシャを算出するイナーシャ演算工程と、を備えることを特徴とする。

好適な態様では、前記重力トルクがゼロとなる位置は、前記被駆動体を一定速駆動したときに得られる前記トルク指令値の極性が反転するタイミングにおける前記位置検出値から特定される。他の好適な態様では、加速駆動条件または減速駆動条件は、前記被駆動体を加速駆動または減速駆動したときに、前記重力トルクがゼロになる位置を通過する前後で、前記重力トルクの積算値の大きさが等しくなるように、加速または減速の時間が調整されている。

他の好適な態様では、加減速条件決定工程は、前記被駆動体を一定速で往復駆動したときのトルク指令値を監視する工程を含み、前記重力トルクがゼロとなる位置は、往路において前記トルク指令値の極性が反転するタイミングにおける前記位置検出値と、復路において前記トルク指令値の極性が反転するタイミングにおける前記位置検出値と、の中間点から特定される。

他の好適な態様では、前記加減速条件決定工程において、前記重力トルクがゼロになる位置を中心として、前記重力トルクの積算値の大きさまたは加速度が線対称に変化するような加速駆動条件および減速駆動条件の両方を決定し、前記イナーシャ演算工程は、決定された加速駆動条件および減速駆動条件に従ってモータを駆動した際のトルク指令値および加速度検出値を監視する工程を含み、前記可動部のイナーシャは、加速駆動時のトルク指令値の積算値と加速駆動時の加速度検出値の積算値との比、および、減速駆動時のトルク指令値の積算値と減速駆動時の前記加速度検出値の積算値との比、の平均値から算出される。

他の好適な態様では、前記加減速条件決定工程において、前記重力トルクがゼロになる位置を中心として、加速度または前記重力トルクの積算値の大きさが線対称に変化するような加速駆動条件および減速駆動条件の両方を決定し、前記イナーシャ演算工程は、加速駆動条件および減速駆動条件に従ってモータを駆動した際のトルク指令値および加速度検出値を監視する工程を含み、前記可動部のイナーシャは、加速駆動時のトルク指令値の積算値と減速駆動時のトルク指令値の積算値との合算値、および、加速駆動時の加速度検出値の積算値と減速駆動時の加速度検出値の積算値との合算値、の比から算出される。

他の本発明であるイナーシャ推定装置は、位置指令値とモータまたは当該モータに結合された被駆動部の位置検出値とに基づいてトルク指令値を算出し、当該トルク指令値に従ってモータを駆動する装置に搭載され、前記モータおよび前記被駆動部を含む可動部のイナーシャを推定するイナーシャ推定装置であって、前記可動部を駆動させた際に、重力の影響をキャンセルするためのトルクである重力トルクがゼロになる位置を中心として、前記重力トルクの積算値または加速度が対称性を持つような加速駆動条件または減速駆動条件の少なくとも一方を決定する加減速条件決定部と、決定された加速または減速駆動条件に従ってモータを駆動した際に得られるトルク指令値の積算値と、前記モータまたは被駆動部の加速度検出値の積算値と、の比から前記可動部のイナーシャを算出するイナーシャ演算部と、を備え、前記重力トルクがゼロとなる位置の前後で前記トルク指令値に含まれる重力トルクを相殺した上でイナーシャを算出することを特徴とする。

本発明によれば、イナーシャ推定用にトルク指令値を積算する過程で、重力トルクの影響を相殺するよう、被駆動体を加減速駆動するため、モータ位置によって重力の影響が異なる機械構成であっても、正確にイナーシャを推定することができる。

本発明の実施例を示すブロック図である。 本発明を使用した時の位置指令、トルク指令特性図である。 被駆動体の重心位置がずれた場合の重力トルク特性図である。 本発明における加減速時間の調整方法を示す図である。 本発明を使用した時の位置指令、トルク指令特性図である。 本発明の処理手順を示すフローチャートである。 本発明の一実施例における処理手順を示すフローチャートである。 本発明の一実施例における処理手順を示すフローチャートである。 本発明の一実施例における処理手順を示すフローチャートである。 本発明の一実施例における処理手順を示すフローチャートである。 本発明の一実施例における処理手順を示すフローチャートである。 本発明の一実施例における処理手順を示すフローチャートである。 従来技術の処理手順を示すフローチャートである。 従来技術の処理手順を示すフローチャートである。 従来技術を示すブロック図である。 モータ位置による重力トルクの変化を示す図である。

本発明の実施例について説明する。従来例と同一要素には同一符号を付しており、説明を省略する。

図１は本発明の実施例を示すブロック図である。位置指令演算器３１で生成した位置指令値Ｐｃと、位置検出器３７で検出したモータ３５または負荷３６の位置検出値Ｐｄの偏差を位置制御器３２に入力すると、位置制御器３２は、速度指令値Ｖｃを出力する。一方、微分器３８は位置検出値Ｐｄを微分して速度検出値Ｖｄを出力する。速度指令値Ｖｃと速度検出値Ｖｄとの偏差を速度制御器３３に入力すると、速度制御器３３は、トルク指令値Ｔｃを出力する。電流制御器３４は、トルク指令値Ｔｃに従い、負荷３６が取り付けられたモータ３５を駆動する。

図１では、上記位置制御装置に、可動部のイナーシャを算出する際の加減速条件を決定する加減速条件決定部３９と、可動部のイナーシャを算出するイナーシャ演算部４０とから構成されるイナーシャ推定器４１が付加され、可動部のイナーシャを推定できるようになっている。

図６は、本発明の処理手順を示すフローチャートである。加減速条件決定部３９で可動部のイナーシャを算出する際の加減速条件を決定後、イナーシャ演算部４０で可動部のイナーシャを算出する。なお、加減速条件決定部３９、及び、イナーシャ演算部４０、それぞれの処理手順については、図７〜図１２のフローチャートを用いて各実施例の項目で説明する。

［実施例１］
まず、図２を用いて加減速条件決定部３９の動作を説明する。図１６のように、モータ位置によって重力の影響が異なる機械構成の場合、重力分をキャンセルするために要するトルク（重力トルク）Ｔｇは回転角θに応じて図２の最上段のように変化する。

この時、加減速条件決定部３９は、図２の２段目のような加速度指令値Ａｃを定める。即ち、Ｔｇがゼロとなる位置を中心にモータ３５や負荷３６などの被駆動体の加速度が線対称に変化するような加速指令を構成する。例えば図２のようにｔ２の間、加速度をゼロから上昇させ、ｔ１の間一定加速度で被駆動体を駆動し、Ｔｇがゼロとなる位置に到達する。その後も、ｔ１の間一定加速度で駆動を続け、ｔ２の間、加速度をゼロまで減少させて加速を終了する。このとき、Ｔｇがゼロとなる位置は機械構成等に基づき、予め定めることができるものとする。

上記のようにして定めた加速度指令値Ａｃを積分することにより、図２の３段目に示したような速度指令値Ｖｃを得ることができる。更に、速度指令値Ｖｃを積分することにより、図２の４段目に示したような位置指令値Ｐｃを得ることができる。

ここで、位置指令値Ｐｃは、Ｔｇがゼロとなる位置を原点０とすると、原点０からθｓだけ前進した位置が加速開始点となるように構成される。ここで、θｓの大きさは、加速開始時点からＴｇがゼロとなる位置を通過するまでの間の速度指令値Ｖｃを積分することによって求めることができる。

加減速条件決定部３９は上記のような位置指令値Ｐｃが位置指令演算器３１から出力されるよう、加速開始点−θｓや加減速時間ｔ１，ｔ２などの加減速条件を位置指令演算器３１に通知する。

位置指令演算器３１から図２の４段目に示したような位置指令値Ｐｃが出力されると、位置制御器３２、速度制御器３３、電流制御器３４を経てモータ３５が駆動される。

このとき、速度制御器３３から出力されるトルク指令値Ｔｃは図２の最下段のように変化する。重力の影響を受けない状況下では、図２の最下段の一点鎖線で示したように、加速度指令値Ａｃに比例したトルク指令値Ｔｃとなるが、重力の影響を受けた状況下では、トルク指令値に、被駆動体を加速するために必要な加速トルクのほか、重力分をキャンセルするために要するトルクが含まれるため、図２の最下段の実線で示したような特性となる。

この時、図２の最下段の斜線で示したトルクが重力分をキャンセルするために要するトルクＴｇに相当し、原点０を通過するタイミングでＴｇに相当するトルクがゼロとなる。

次に、イナーシャ演算部４０の動作を説明する。イナーシャ演算部４０は、位置指令値Ｐｃから加減速状態を判別し、加減速状態にあるときのトルク指令値Ｔｃ、及び位置検出値Ｐｄを二階微分して得られる加速度検出値Ａｄを積算する。なお、加速度検出値Ａｄは、モータや被駆動部に加速度センサを設けて取得するようにしてもよい。加減速終了後、トルク指令値Ｔｃの積算値を加速度検出値Ａｄの積算値で除することによって、可動部のイナーシャを算出することができる。

このとき積算されるトルク指令値Ｔｃには重力分をキャンセルするために要するトルクＴｇが含まれるが、図２の最下段の斜線で示したように、原点０を通過する前後でＴｇの符号は反転する。そのため、加速時のトルク指令値Ｔｃを積算すると、原点０を通過する前後で重力分をキャンセルするために要するトルクＴｇが相殺され、トルク指令値Ｔｃの積算値は被駆動体を加速するために必要な加速トルクの積算値とほぼ等しくなる。結果、加速するために必要な加速トルクの積算値が加速度検出値Ａｄの積算値で除され、重力の影響を排除した状態での可動部のイナーシャの算出が可能となる。

なお、上記実施例では、図２を例に加速動作にて説明を行ったが、イナーシャの算出は上記動作に限定されるものではない。図２では位置−θｓからθｅまで加速する動作を示しているが、例えば、位置θｓから−θｅまで加速するなど動作方向を逆にすることも可能であるほか、位置θｅから−θｓまで減速する、あるいは、位置−θｅからθｓまで減速するなど加速動作を減速動作に変更してイナーシャを算出することも可能である。

ここで、図６の加減速条件決定部３９、イナーシャ演算部４０の処理手順を纏めると図７のフローチャートとなる。

まず、加減速条件決定部３９は［Ｓｔｅｐ Ａ１］で、Ｔｇがゼロとなる位置を中心に加減速する加速度指令値Ａｃを定める。次に、［Ｓｔｅｐ Ａ２］で、加速度指令値Ａｃを積分して速度指令値Ｖｃを定める。更に、［Ｓｔｅｐ Ａ３］で、速度指令値Ｖｃを積分して位置指令値Ｐｃを定め、位置指令演算器３１で使用する加減速条件を決定する。すると、［Ｓｔｅｐ Ａ３］で決定した加減速条件に従い、モータ３５が駆動される。

一方、イナーシャ演算部４０は［Ｓｔｅｐ Ｂ１］で、位置指令値Ｐｃから加減速状態を検出し、［Ｓｔｅｐ Ｂ２］で、加減速時のトルク指令値Ｔｃ、及び加速度検出値Ａｄを積算する。加減速終了後、［Ｓｔｅｐ Ｂ３］で、トルク指令値Ｔｃの積算値を加速度検出値Ａｄの積算値で除することによって、可動部のイナーシャＪを算出する。

以上のように、本発明によるイナーシャ推定方法によれば、イナーシャ推定用にトルク指令値を積算する過程で、重力トルクの影響を相殺するよう、被駆動体を加減速駆動するため、モータ位置によって重力の影響が異なる機械構成であっても、正確にイナーシャを推定することができる。

［実施例２］
次に、上記実施例１を改良した実施形態について説明する。ここで説明する実施形態は、上記実施例１に対し、加減速条件決定部３９の機能を拡張するものである。

実施例１において、重力分をキャンセルするために要するトルクＴｇがゼロとなる位置は機械構成等に基づき、予め定めることができるものとした。しかし、図３のようにテーブル２３上にワーク２４をアンバランスに設置した場合など、重心位置がずれた状態では、重力分をキャンセルするために要するトルクＴｇがゼロとなる位置もずれた状態となってしまう。

例えば、ワーク２４が設置されていない時の重力分をキャンセルするために要するトルクＴｇの特性が、図３の下段の点線で示した特性であったとする。一方、ワーク２４を設置した場合にバランスが保たれる機械姿勢がモータ２１の回転角でθｒだけずれた位置であったとする。この場合、Ｔｇの特性は図３の下段の実線で示した特性となり、ワーク２４を設置していない状態に対し、θｒだけずれた位置で、Ｔｇの大きさはゼロとなる。

上記ずれを含んだ状態では、実施例１のように、イナーシャ推定用にトルク指令値を積算する過程で、重力トルクの影響を相殺することができない。即ち、正確にイナーシャを推定するためには、上記ずれを補正する必要がある。そこで、本実施形態では、上記実施例１の加減速条件決定部３９に対し、以下の機能を付加する。

モータ２１の回転角θに対する重力分をキャンセルするために要するトルクＴｇの大きさは、モータを一定速駆動したときのトルク指令値Ｔｃとほぼ等しく、回転角θに対するＴｃを測定することによって定めることが可能である。そこで、加減速条件決定部３９において、モータを一定速駆動したときのＴｃを監視し、Ｔｃがゼロとなるときの位置検出値ＰｄからＴｇがゼロとなる位置を特定する。

その上で、実施例１と同様に、位置指令値Ｐｃを構成し、イナーシャ演算部４０でイナーシャを算出する。これにより、重力の影響を排除した状態での可動部のイナーシャの算出が可能となる。

ここで、図６の加減速条件決定部３９の処理手順を纏めると図８のフローチャートとなる。

まず、［Ｓｔｅｐ Ｃ１］で、モータが一定速駆動されていることを検出し、［Ｓｔｅｐ Ｃ２］で、一定速時のトルク指令値Ｔｃを測定・監視する。更に、［Ｓｔｅｐ Ｃ３］で、トルク指令値Ｔｃの極性が反転する（Ｔｃがゼロとなる）ときの位置検出値ＰｄからＴｇがゼロとなる位置を特定する。その後、［Ｓｔｅｐ Ａ１］から［Ｓｔｅｐ Ａ３］を行うことにより、位置指令演算器３１で使用する加減速条件を決定する。

以上のように、本発明によるイナーシャ推定方法によれば、被駆動体の重心位置がずれた状態でも、重力トルクの影響がゼロとなる位置を測定により特定し、トルク指令値を積算する過程で、重力トルクの影響を相殺するよう、被駆動体を加減速駆動するため、モータ位置によって重力の影響が異なる機械構成であっても、正確にイナーシャを推定することができる。

［実施例３］
次に、上記実施例１を改良した実施形態について説明する。ここで説明する実施形態は、上記実施例１に対し、加減速条件決定部３９の機能を拡張するものである。

実施例１では、図２において、重力分をキャンセルするために要するトルクＴｇがゼロとなる位置を中心にモータ３５や負荷３６などの被駆動体を加速する指令を構成し、位置−θｓからθｅまでの間、加速させることを示した。しかし、加速中の速度は徐々に高くなるため、同一時間あたりの移動量は、時間経過とともに大きくなる。

即ち、図４に示すように、加速開始位置−θｓと加速終了位置θｅの大小関係はθｓ＜θｅとなり、重力分をキャンセルするために要するトルクＴｇの大きさも｜Ｔｇ（−θｓ）｜＜｜Ｔｇ（θｅ）｜となる。結果、重力の影響はＴｇがゼロとなる前後で完全に相殺することはできず、推定されたイナーシャには僅かながら重力トルクの影響が残ることとなる。そこで、本実施形態では、上記実施例１の加減速条件決定部３９に対し、以下の機能を付加する。

実施例１と同様に、位置指令値Ｐｃを構成後、モータ２１の回転角θに対する重力分をキャンセルするために要するトルクＴｇの特性をベースにして、図４の４段目のように、経過時間ｔに対するＴｇの特性を計算する。

次に、重力分をキャンセルするために要するトルクＴｇがゼロとなる位置（原点０）を通過する前後で、Ｔｇの積算値の大きさが等しくなる時刻（加速開始時点からＴｇの積算を開始し、積算値が再びゼロとなる時刻）を求める。

そして、図４の最下段のように、上記時刻にて加速が終了するよう、加速時間を短縮した加速度指令値Ａｃを定める。なお、図４ではｔ０分の加速時間の短縮が必要と判断し、一定加速度で加速している時間ｔ１をｔ０分短縮して、ｔ１’と改めている。この加速度指令Ａｃは、Ｔｇがゼロとなる位置を中心として、重力トルクＴｇの積算値が対称性を持つような指令と言える。

このように、加減速時間を調整した加速度指令値Ａｃを二階積分して、位置指令値Ｐｃを再構成する。この再構成した位置指令値Ｐｃに従い、加減速駆動し、実施例１と同様に、イナーシャ演算部４０でイナーシャを算出する。

以上の操作により、原点０を通過する前後で重力分をキャンセルするために要するトルクＴｇがより正確に相殺されるため、重力の影響を排除した状態での可動部のイナーシャの算出が可能となる。

ここで、図６の加減速条件決定部３９の処理手順を纏めると図９のフローチャートとなる。

まず、［Ｓｔｅｐ Ａ１］から［Ｓｔｅｐ Ａ３］を行うことにより、位置指令値Ｐｃを定める。次に、［Ｓｔｅｐ Ｄ１］で、時間ｔに対するＴｇの特性を計算する。更に、［Ｓｔｅｐ Ｄ２］で、Ｔｇがゼロとなる位置を通過する前後で、Ｔｇの積算値の大きさが等しくなる時刻を求める。この時刻に位置指令値Ｐｃによる加減速が完了するよう、［Ｓｔｅｐ Ｄ３］で、位置指令演算器３１で使用する加減速条件を再決定する。

以上のように、本発明によるイナーシャ推定方法によれば、重力トルクの影響がより正確に相殺されるよう、加減速時間を調整したため、モータ位置によって重力の影響が異なる機械構成であっても、より正確にイナーシャを推定することができる。

［実施例４］
次に、上記実施例２を改良した実施形態について説明する。ここで説明する実施形態は、上記実施例２に対し、加減速条件決定部３９の機能を拡張するものである。

重力分をキャンセルするために要するトルクＴｇの大きさを、一定速駆動時のトルク指令値Ｔｃを測定して定めた場合、Ｔｃに摩擦トルクの影響が含まれると、図５の最上段のような波形となる。即ち、実線で示したＴｇの真値に対し、モータ２１を正方向に回転させた場合は点線で示したような測定結果が得られ、負方向に回転させた場合は一点鎖線で示したような測定結果が得られる。

即ち、重力分をキャンセルするために要するトルクＴｇがゼロとなる位置が真値からずれてしまう。これは、移動方向によって作用する方向が異なる摩擦トルクの影響が含まれているためである。

上記ずれを含んだ状態では、実施例２のように、イナーシャ推定用にトルク指令値を積算する過程で、重力トルクの影響を相殺することができない。即ち、正確にイナーシャを推定するためには、上記ずれを補正する必要がある。そこで、本実施形態では、上記実施例２の加減速条件決定部３９に対し、以下の機能を付加する。

モータ２１の回転角θに対する重力分をキャンセルするために要するトルクＴｇの大きさを測定するにあたり、モータを正方向に一定速駆動した場合と、負方向に一定速駆動した場合、それぞれで回転角θに対するトルク指令値Ｔｃを測定する。その上で、それぞれの場合においてＴｃがゼロとなるときの位置検出値Ｐｄを特定し、その中間点をＴｇがゼロとなる位置とする。

その上で、実施例２と同様に、位置指令値Ｐｃを構成し、イナーシャ演算部４０でイナーシャを算出する。これにより、重力の影響を排除した状態での可動部のイナーシャの算出が可能となる。

ここで、図６の加減速条件決定部３９の処理手順を纏めると図１０のフローチャートとなる。

まず、［Ｓｔｅｐ Ｅ１］で、モータが正方向に一定速駆動されていることを検出し、［Ｓｔｅｐ Ｅ２］で、正方向移動一定速時のトルク指令値Ｔｃを測定・監視する。更に、［Ｓｔｅｐ Ｅ３］で、トルク指令値Ｔｃの極性が反転する（Ｔｃがゼロとなる）ときのＰｄを特定する。

一方、［Ｓｔｅｐ Ｅ４］で、モータが負方向に一定速駆動されていることを検出し、［Ｓｔｅｐ Ｅ５］で、負方向移動一定速時のトルク指令値Ｔｃを測定・監視する。更に、［Ｓｔｅｐ Ｅ６］で、トルク指令値Ｔｃの極性が反転する（Ｔｃがゼロとなる）ときの位置検出値Ｐｄを特定する。

その後、［Ｓｔｅｐ Ｅ７］で、［Ｓｔｅｐ Ｅ３］で特定した位置（Ｐｄ）と［Ｓｔｅｐ Ｅ６］で特定した位置（Ｐｄ）の中間点をＴｇがゼロとなる位置と定め、その上で［Ｓｔｅｐ Ａ１］から［Ｓｔｅｐ Ａ３］を行うことにより、位置指令演算器３１で使用する加減速条件を決定する。

なお、［Ｓｔｅｐ Ｅ１］から［Ｓｔｅｐ Ｅ３］と［Ｓｔｅｐ Ｅ４］から［Ｓｔｅｐ Ｅ６］については、その順序を入れ替えることは可能である。即ち、［Ｓｔｅｐ Ｅ４］から［Ｓｔｅｐ Ｅ６］を実施後、［Ｓｔｅｐ Ｅ１］から［Ｓｔｅｐ Ｅ３］を行っても支障ない。

以上のように、本発明によるイナーシャ推定方法によれば、摩擦トルクの影響が大きい場合でも、重力トルクの影響がゼロとなる位置を特定した上で、トルク指令値を積算する過程で、重力トルクの影響を相殺するよう、被駆動体を加減速駆動するため、モータ位置によって重力の影響が異なる機械構成であっても、正確にイナーシャを推定することができる。

［実施例５］
次に、上記実施例４を改良した実施形態について説明する。ここで説明する実施形態は、上記実施例４に対し、イナーシャ演算部４０の機能を拡張するものである。

トルク指令値Ｔｃに含まれる摩擦トルクの影響が大きく、加減速時に必要となる加速トルクに対し、無視できない大きさの場合、Ｔｃを積算してイナーシャを算出すると、摩擦トルクの影響でイナーシャ推定値に誤差が発生してしまう。そこで、本実施形態では、上記実施例４のイナーシャ演算部４０に対し、以下の機能を付加する。

摩擦トルクの影響が無視できない状況下でのトルク指令値Ｔｃには、図５の最下段に示したように、加減速トルクＴａ、重力分をキャンセルするために要するトルクＴｇ、摩擦分をキャンセルするために要するトルクＴｆが含まれる。図５の最下段において、一点鎖線はＴａとＴｇを考慮した時のＴｃ、実線はＴａとＴｇに加えてＴｆを考慮した時のＴｃをそれぞれ示している。一点鎖線で示した特性に対し、実線で示した特性は、軸駆動のために斜線で示したＴｆ分のトルクが必要になる。ここで、各トルクの関係をモータ２１の回転角θを用いて纏めると、下記式４のように表現できる。

Ｔｃ＝ Ｔａ＋Ｔｇ（θ）＋Ｔｆ ［加速時（正方向移動時）］
−Ｔａ＋Ｔｇ（θ）＋Ｔｆ ［減速時（正方向移動時）］
−Ｔａ＋Ｔｇ（θ）−Ｔｆ ［加速時（負方向移動時）］
Ｔａ＋Ｔｇ（θ）−Ｔｆ ［減速時（負方向移動時）］ ・・・式４

加減速トルクＴａは、加速時と減速時とで加速度の符号が反転するため、異符号となる。また、正方向移動時と負方向移動時とで加速方向が相反するため、異符号となる。一方、重力分をキャンセルするために要するトルクＴｇは位置によって決定されるため、同位置であれば加減速や移動方向に左右されず、同一符号となる。また、摩擦分をキャンセルするために要するトルクＴｆは軸移動に対し逆らうように作用するため、移動方向によって符号が決定される。

ここで、摩擦トルクの影響は、同一位置にて加速動作、減速動作を行うことによって、相殺することができる。例えば、加速時（正方向移動時）と減速時（正方向移動時）とで差分をとることによって、トルク指令値Ｔｃ、またはトルク指令値Ｔｃの積算値に含まれる、摩擦分をキャンセルするために要するトルクＴｆを相殺することができる。

同様に、加速時（正方向移動時）と減速時（負方向移動時）とで加算、加速時（負方向移動時）と減速時（負方向移動時）とで減算、加速時（負方向移動時）と減速時（正方向移動時）とで加算しても、摩擦分をキャンセルするために要するトルクＴｆを相殺することができる。

ただし、加速時（正方向移動時）と減速時（正方向移動時）、加速時（負方向移動時）と減速時（負方向移動時）の組合せの場合、加減速する区間を同一としても、同じ地点を通過するときの速度が異なるため、同一時間あたりの移動量に違いが出る。結果、重力分をキャンセルするために要するトルクＴｇがゼロとなる位置や、Ｔｇの変化の仕方が変わるため、Ｔｇを積算した場合に、加速時と減速時とで値に違いが出てしまう。このため、イナーシャを算出する際に重力トルクの影響が僅かながら残ってしまう。よって、好適な形態として、加速時（正方向移動時）と減速時（負方向移動時）、加速時（負方向移動時）と減速時（正方向移動時）の組合せにて、摩擦分をキャンセルするために要するトルクＴｆを相殺することを推奨する。

上記のように、摩擦分をキャンセルするために要するトルクＴｆを相殺するため、位置制御装置は、加減速条件決定部３９が定めた重力分をキャンセルするために要するトルクＴｇがゼロとなる位置を中心とした、加速動作、減速動作を行い、イナーシャ演算部４０は、加速動作、減速動作、それぞれの動作に対し、トルク指令値Ｔｃ、加速度検出値Ａｄを積算する。

加減速動作終了後、加速動作、減速動作それぞれにおいて、トルク指令値Ｔｃの積算値を加速度検出値Ａｄの積算値で除し、両者の平均値から可動部のイナーシャを算出する。この場合、加速動作から算出される可動部のイナーシャは摩擦トルクの影響分だけイナーシャが大きく算出され、減速動作から算出される可動部のイナーシャは摩擦トルクの影響分だけイナーシャが小さく算出されるが、両者の平均値をとることによって、摩擦トルクの影響分が相殺された可動部のイナーシャを算出することができる。

あるいは、加速動作、減速動作それぞれのトルク指令値Ｔｃの積算値、加速度検出値Ａｄの積算値を合算して、トルク指令積算合算値、加速度検出積算合算値を算出し、その上で、トルク指令積算合算値を加速度検出積算合算値で除することによって可動部のイナーシャを算出する。この場合、トルク指令積算合算値を算出する過程で、加速動作時のトルク指令値Ｔｃの積算値に含まれる摩擦トルク分と減速動作時のトルク指令値Ｔｃの積算値に含まれる摩擦トルク分とを相殺することができる。

ここで、図６のイナーシャ演算部４０の処理手順を纏めると図１１、あるいは図１２のフローチャートとなる。

図１１では、まず、［Ｓｔｅｐ Ｆ１］で、位置指令値Ｐｃから加速状態を検出し、［Ｓｔｅｐ Ｆ２］で、加速時のトルク指令値Ｔｃ、及び加速度検出値Ａｄを積算する。加速終了後、［Ｓｔｅｐ Ｆ３］で、トルク指令値Ｔｃの積算値を加速度検出値Ａｄの積算値で除することによって、加速イナーシャＪａを算出する。

また、［Ｓｔｅｐ Ｆ４］で、位置指令値Ｐｃから減速状態を検出し、［Ｓｔｅｐ Ｆ５］で、減速時のトルク指令値Ｔｃ、及び加速度検出値Ａｄを積算する。減速終了後、［Ｓｔｅｐ Ｆ６］で、トルク指令値Ｔｃの積算値を加速度検出値Ａｄの積算値で除することによって、減速イナーシャＪｄを算出する。

その後、［Ｓｔｅｐ Ｆ７］で、加速イナーシャＪａと減速イナーシャＪｄの平均をとり、可動部のイナーシャＪを算出する。

一方、図１２では、まず、［Ｓｔｅｐ Ｇ１］で、位置指令値Ｐｃから加速状態を検出し、［Ｓｔｅｐ Ｇ２］で、加速時のトルク指令値Ｔｃ、及び加速度検出値Ａｄを積算する。その後、［Ｓｔｅｐ Ｇ３］で、位置指令値Ｐｃから減速状態を検出し、［Ｓｔｅｐ Ｇ４］で、減速時のトルク指令値Ｔｃ、及び加速度検出値Ａｄを積算する。

次に、［Ｓｔｅｐ Ｇ５］で、加速時のトルク指令値Ｔｃの積算値と減速時のトルクし指令値Ｔｃの積算値を合算してトルク指令積算合算値を算出し、［Ｓｔｅｐ Ｇ６］で、加速時の加速度検出値Ａｄの積算値と減速時の加速度検出値Ａｄの積算値を合算して加速度検出積算合算値を算出する。その上で、［Ｓｔｅｐ Ｇ７］で、トルク指令積算合算値を加速度検出積算合算値で除することによって、可動部のイナーシャＪを算出する。

以上のように、本発明によるイナーシャ推定方法によれば、イナーシャ推定用にトルク指令値を積算する過程で、重力トルクの影響を相殺するとともに、加速動作と減速動作とを組み合わせることにより、摩擦トルクの影響を相殺するため、モータ位置によって重力の影響が異なる、かつ摩擦トルクの影響が無視できない機械構成であっても、正確にイナーシャを推定することができる。

なお、上記実施例１〜５では、トルク指令値Ｔｃを積算した値を用いてイナーシャを算出することを示したが、本発明はこれに限定されるものではない。例えば、電流制御器３４内でトルク指令値Ｔｃから変換される電流指令値や、モータ３５に通電された電流を検出した電流検出値等を積算し、トルク定数等を乗じてトルク指令値Ｔｃの積算値に相当する値に変換して、イナーシャを算出することも可能である。

１１，１４，３３ 速度制御器、１２，２１，３５ モータ、１３，１６ 積分器、１５ モータモデル、２２ アーム、２３ テーブル、２４ ワーク、３１ 位置指令演算器、３２ 位置制御器、３４ 電流制御器、３６ 負荷、３７ 位置検出器、３８ 微分器、３９ 加減速条件決定部、４０ イナーシャ演算部、４１ イナーシャ推定器。

Claims (7)

1. 位置指令値とモータまたは当該モータに結合された被駆動部の位置検出値とに基づいてトルク指令値を算出し、当該トルク指令値に従ってモータを駆動する装置に搭載された前記モータおよび前記被駆動部を含む可動部のイナーシャを推定するイナーシャ推定方法であって、
   前記可動部を駆動させた際に、重力の影響をキャンセルするためのトルクである重力トルクがゼロになる位置を中心として、前記重力トルクの積算値または前記可動部の加速度が対称性を持つような加速駆動条件または減速駆動条件の少なくとも一方を決定する加減速条件決定工程と、
   決定された加速または減速駆動条件に従ってモータを駆動した際に得られるトルク指令値の積算値と、前記モータまたは被駆動部の加速度検出値の積算値と、の比から前記可動部のイナーシャを算出するイナーシャ演算工程と、
   を備えることを特徴とするイナーシャ推定方法。
2. 請求項１に記載のイナーシャ推定方法であって、
   前記重力トルクがゼロとなる位置は、前記被駆動体を一定速駆動したときに得られる前記トルク指令値の極性が反転するタイミングにおける前記位置検出値から特定される、ことを特徴とするイナーシャ推定方法。
3. 請求項１または２に記載のイナーシャ推定方法であって、
   加速駆動条件または減速駆動条件は、前記被駆動体を加速駆動または減速駆動したときに、前記重力トルクがゼロになる位置を通過する前後で、前記重力トルクの積算値の大きさが等しくなるように、加速または減速の時間が調整されている、ことを特徴とするイナーシャ推定方法。
4. 請求項１から３のいずれか１項に記載のイナーシャ推定方法であって、
   加減速条件決定工程は、前記被駆動体を一定速で往復駆動したときのトルク指令値を監視する工程を含み、
   前記重力トルクがゼロとなる位置は、往路において前記トルク指令値の極性が反転するタイミングにおける前記位置検出値と、復路において前記トルク指令値の極性が反転するタイミングにおける前記位置検出値と、の中間点から特定される、
   ことを特徴とするイナーシャ推定方法。
5. 請求項１から４のいずれか１項に記載のイナーシャ推定方法であって、
   前記加減速条件決定工程において、前記重力トルクがゼロになる位置を中心として、前記重力トルクの積算値の大きさまたは加速度が線対称に変化するような加速駆動条件および減速駆動条件の両方を決定し、
   前記イナーシャ演算工程は、決定された加速駆動条件および減速駆動条件に従ってモータを駆動した際のトルク指令値および加速度検出値を監視する工程を含み、
   前記可動部のイナーシャは、加速駆動時のトルク指令値の積算値と加速駆動時の加速度検出値の積算値との比、および、減速駆動時のトルク指令値の積算値と減速駆動時の前記加速度検出値の積算値との比、の平均値から算出される、
   ことを特徴とするイナーシャ推定方法。
6. 請求項１から４のいずれか１項に記載のイナーシャ推定方法であって、
   前記加減速条件決定工程において、前記重力トルクがゼロになる位置を中心として、加速度または前記重力トルクの積算値の大きさが線対称に変化するような加速駆動条件および減速駆動条件の両方を決定し、
   前記イナーシャ演算工程は、加速駆動条件および減速駆動条件に従ってモータを駆動した際のトルク指令値および加速度検出値を監視する工程を含み、
   前記可動部のイナーシャは、加速駆動時のトルク指令値の積算値と減速駆動時のトルク指令値の積算値との合算値、および、加速駆動時の加速度検出値の積算値と減速駆動時の加速度検出値の積算値との合算値、の比から算出される、
   ことを特徴とするイナーシャ推定方法。
7. 位置指令値とモータまたは当該モータに結合された被駆動部の位置検出値とに基づいてトルク指令値を算出し、当該トルク指令値に従ってモータを駆動する装置に搭載され、前記モータおよび前記被駆動部を含む可動部のイナーシャを推定するイナーシャ推定装置であって、
   前記可動部を駆動させた際に、重力の影響をキャンセルするためのトルクである重力トルクがゼロになる位置を中心として、前記重力トルクの積算値または加速度が対称性を持つような加速駆動条件または減速駆動条件の少なくとも一方を決定する加減速条件決定部と、
   決定された加速または減速駆動条件に従ってモータを駆動した際に得られるトルク指令値の積算値と、前記モータまたは被駆動部の加速度検出値の積算値と、の比から前記可動部のイナーシャを算出するイナーシャ演算部と、
   を備え、前記重力トルクがゼロとなる位置の前後で前記トルク指令値に含まれる重力トルクを相殺した上でイナーシャを算出することを特徴とするイナーシャ推定装置。

Images