(57)【要約】【課題】粘性抵抗や摺動抵抗が環境や経年変化によって変化することがあっても、イナーシャを正確に推定可能にする。【解決手段】本発明のワーク質量推定装置は、トルクの情報と加速度の情報とに基づいてワーク質量を推定するものにおいて、加速時に推定したワーク質量値と、減速時に推定したワーク質量値との平均を求め、この平均値をワーク質量とするように構成したものである。この構成の場合、粘性抵抗及び摺動抵抗をキャンセルすることができるので、粘性抵抗や摺動抵抗が環境や経年変化によって変化することがあっても、イナーシャ（ワーク質量）を正確に推定することができる。【選択図】　図１
2004054838.tif 000002
【特許請求の範囲】
【請求項１】
トルクの情報と加速度の情報とに基づいてワーク質量を推定するワーク質量推定装置において、
加速時に推定したワーク質量値と、減速時に推定したワーク質量値との平均を求め、この平均値をワーク質量とするようにしたことを特徴とするワーク質量推定装置。
【請求項２】
加速時にワーク質量を推定する場合、モータの加速度が設定値以上のときのデータを用いることを特徴とする請求項１記載のワーク質量推定装置。
【請求項３】
推定したワーク質量を、ユーザが設定したワーク質量と比較して、適切であるか否かを判断し、適切でないときには、報知して停止することを特徴とする請求項１または２記載のワーク質量推定装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、トルクの情報と加速度の情報とに基づいてワーク質量を推定するワーク質量推定装置に関する。
【０００２】
【従来の技術】
モータによりメカ機構（例えばボールねじ等）を介してワークを移動させる各種の搬送装置やロボット等においては、ワークをある位置から他の位置へ移動させるサーボ制御を行うための制御装置が設けられている。この種の制御装置においては、ワーク（制御対象）の位置指令に対して実際の位置が一致するように制御が行われる構成となっている。このような制御装置として、ユーザが設定したワーク質量設定値に応じて制御の時定数を変えるように、例えばワーク質量設定値が軽いときは時定数を短くし、重いときは時定数を長くするように構成したものがある。
【０００３】
この構成の場合、ユーザが設定したワーク質量設定値が実際のワーク質量よりも軽い場合には、時定数が適正値よりも短くなってしまい、メカ機構部品の寿命の低減や、振動増加や、メカ機構の破損等が発生する可能性がある。このため、制御装置自身が、ワーク質量を正確に推定できることが好ましい。
【０００４】
ここで、ワーク質量を推定する周知の方法について、以下簡単に述べる。まず、上記した制御系においては、次の式が成り立つ。
【０００５】
トルク＝総イナーシャ×加速度＋粘性抵抗×速度＋摺動抵抗　　　　　（１）
ここで、総イナーシャは、ワーク質量のイナーシャ換算分とワーク無し時のイナーシャの和であるから、
2004054838.tif 000003
ここで、ワーク無し時のイナーシャは、機械構成が決まれば、一義的に決まる値である。式（２）により、ワーク質量分のイナーシャを正確に求めるためには、粘性抵抗と摺動抵抗が既知である必要があるので、装置出荷時の測定値をパラメータとして予め与えておけば、イナーシャを推定することが可能である。
【０００６】
【発明が解決しようとする課題】
しかし、粘性抵抗や摺動抵抗は、装置の使用環境の温度によって大きく変化することがあるし、また、装置を使用していくうちに、メカ機構の摩耗・がたの影響によって変化することがあるので、上記した方法ではイナーシャ（即ち、ワーク質量）を正確に推定することができなかった。
【０００７】
本発明は上記事情に鑑みてなされたものであり、その目的は、粘性抵抗や摺動抵抗が環境や経年変化によって変化することがあっても、イナーシャを正確に推定することができるワーク質量推定装置を提供するにある。
【０００８】
【課題を解決するための手段】
本発明のワーク質量推定装置は、トルクの情報と加速度の情報とに基づいてワーク質量を推定するものにおいて、加速時に推定したワーク質量値と、減速時に推定したワーク質量値との平均を求め、この平均値をワーク質量とするようにした。このように構成すると、粘性抵抗及び摺動抵抗をキャンセルすることができるので、粘性抵抗や摺動抵抗が環境や経年変化によって変化することがあっても、イナーシャを正確に推定することができる。
【０００９】
また、上記構成において、加速時にワーク質量を推定する場合、モータの加速度が設定値以上のときのデータを用いることが好ましい。更に、推定したワーク質量を、ユーザが設定したワーク質量と比較して、適切であるか否かを判断し、適切でないときには、報知して停止するように構成することも好ましい構成である。
【００１０】
【発明の実施の形態】
以下、本発明を搬送装置に適用した一実施例について、図面を参照しながら説明する。まず、本発明の原理について、図１に従って説明する。ここでは、説明を簡単にするために、加速度が一定で加速及び減速を実行するものとする。
【００１１】
加速時の速度Ｎ１のときの加速度をα、トルクをＴ１とし、減速時の同速度Ｎ１のときの加速度をα、トルクをＴ２とすると、それぞれのワーク質量イナーシャは、前記式（２）より、次の２つの式で表わされる。
【００１２】
2004054838.tif 000004
従って、加速時のワーク質量イナーシャ推定値と、減速時のワーク質量イナーシャ推定値との平均は、次の式で表わされる。
【００１３】
2004054838.tif 000005
この式（５）において、Ｔ１は加速に必要なトルクＴａに摺動抵抗分トルクと粘性抵抗分トルクとを足したものである。また、Ｔ２は減速に必要なトルク−Ｔａに摺動抵抗分トルクと粘性抵抗分トルクとを足したものである。このため、上記式（５）は次のようになる。
【００１４】
2004054838.tif 000006
以上のことから、加速時のトルクと減速時のトルクの平均をとることにより、粘性抵抗及び摺動抵抗をキャンセルすることができ、加減速に使用されたトルク分だけが求まることがわかる。このトルクを加速度で割って、ワーク無し時のイナーシャを引けば、ワーク重量分のイナーシャが求まる。尚、この場合、直線加減速の例で説明したが、Ｓ字加減速の場合も同様に説明することができる。
【００１５】
また、上述した説明では、加減速の途中のある速度Ｎ１に対して説明したが、ワーク質量の推定精度を高めるために、加速度が設定値以上になった後、所定のサンプリング周期毎に複数回計算して、それらの平均を取るように構成することが好ましい。以下、このように構成した実施例を具体的に説明する。
【００１６】
まず、本実施例の搬送装置の機械的構成について図２を参照して説明する。この図２に示すように、搬送装置１は、ベース２と、このベース２上に設けられたリニアガイド３と、このリニアガイド３上に移動可能に設けられたテーブル４と、このテーブル４をリニアガイド３のボールねじ５を介して移動駆動するモータ６とを備えて構成されている。
【００１７】
そして、上記テーブル４の上に、ワーク７が載置されて固定されている。尚、モータ６のシャフト６ａと、ボールねじ５は、カップリング８を介して連結されている。また、モータ６は、サーボモータで構成されており、エンコーダ６ｂ（図３参照）を備えている。
【００１８】
次に、上記モータ６をサーボ制御する制御装置について、図３のブロック図を参照して説明する。図３に示すように、制御装置８は、数値制御部（ＮＣ）９と、サーボアンプ１０とから構成されている。尚、サーボアンプ１０の電気的構成は、従来周知のサーボアンプの構成とほぼ同じである。
【００１９】
上記数値制御部９は、モーションコントロール部１１と、位置カウンタ１２と、ワーク質量推定部（ワーク質量推定装置）１３とから構成されている。モーションコントロール部１１は、例えば加工プログラムを解釈して位置指令Ｓａを生成し、この位置指令Ｓａをサーボアンプ１０へ送るように構成されている。これと共に、モーションコントロール部１１は、モータ６のエンコーダ６ｂから出力された位置フィードバックＳｂを位置カウンタ１２によりカウントして得られた位置情報（カウント結果）Ｓｃを受け取り、この位置情報Ｓｃに基づいて現在位置を認識し、加工プログラムを進めていくように構成されている。
【００２０】
また、ワーク質量推定部１３は、サーボアンプ１０からのトルク情報Ｔ（ｎ）と、位置カウンタ１２からの位置情報Ｓｃと、モーションコントロール部１１からの加減速情報Ｓｄとを受け取り、これらの情報に基づいて、後述するようにしてワーク重量を推定するように構成されている。
【００２１】
また、サーボアンプ１０は、モーションコントロール部１１からの位置指令Ｓａ通りモータ６が動作するように、モータ６の電流を制御するように構成されている。具体的には、サーボアンプ１０の加算器１４において、位置指令Ｓａと実際の位置（エンコーダ６ｂからの位置フィードバックＳｂ）との位置偏差Ｓｅが算出され、この位置偏差Ｓｅに位置ループゲインＫｐを乗ずることにより、速度指令Ｓｆが算出される。
【００２２】
そして、加算器１５において、上記速度指令Ｓｆと実際の速度（エンコーダ６ｂからの位置フィードバックを微分回路１８で微分して得られた速度）Ｓｇとの速度偏差Ｓｈが算出される。この速度偏差Ｓｈに速度ループ比例ゲインＫｖｐを乗ずることにより得られた電流指令（比例）Ｓｉと、上記速度指令Ｓｆを積分回路１９により積分して、その積分結果に速度ループ積分ゲインＫｖｉを乗ずることにより得られた電流指令（積分）Ｓｊとを、加算器１６により加算することにより、電流指令Ｓｋが算出される。そして、この電流指令Ｓｋにしたがうように電流制御部１７によりモータ６が通電制御される構成となっている。また、電流制御部１７は、モータ６の電流情報、即ち、トルク情報Ｔ（ｎ）を数値制御部９のワーク質量推定部１３へ与えるように構成されている。
【００２３】
次に、ワーク質量推定部１３の制御内容について、図４及び図５のフローチャートを参照して説明する。この図４及び図５のフローチャートにおいて使用されている各記号は、次の３つのグループに分類されて定義されている。まず、機械固有パラメータとして、Ｎｏｋ：ワーク質量推定計算個数許容値、Ｗｅｒｒ：ユーザ設定ワーク質量の許容誤差［ｋｇ］、αｓｔ：ワーク質量推定開始角加速度［ｒａｄ／ｓｅｃ２］、Ｊ：ワーク無し時の総イナーシャ［ｋｇ・ｍ２］、Ａ：加速度を角加速度に変換する係数［ｒａｄ／ｓｅｃ２］、Ｂ：ワーク質量をモータ軸換算イナーシャに変換するパラメータ［ｋｇ・ｍ２／ｋｇ］がある。また、ユーザ設定パラメータとして、Ｗｓｅｔ：ユーザが設定するワーク質量［ｋｇ］がある。
【００２４】
また、内部レジスタとして、Ｎａｃｃ：加速時のワーク質量計算個数カウンタ、Ｎｄｅｃ：減速時のワーク質量計算個数カウンタ、Ｗａｃｃ：加速時のワーク質量累積［ｋｇ］、Ｗｄｅｃ：減速時のワーク質量累積［ｋｇ］、Ｐ（ｎ）：ｎ時の位置カウンタ［ｐｕｌｓｅ］、Δｔ：サンプリング時間［ｓｅｃ］、Ｆ（ｎ）：ｎ時の速度［ｐｕｌｓｅ／ｓｅｃ］、α（ｎ）：ｎ時の角加速度［ｒａｄ／ｓｅｃ２］、Ｊａｌｌ（ｎ）：ｎ時の総イナーシャ推定値［ｋｇ・ｍ２］、Ｗ（ｎ）：ｎ時のワーク質量推定値［ｋｇ］、Ｗ：ワーク質量推定加減速平均［ｋｇ］がある。
【００２５】
そして、本実施例のワーク質量推定の制御においては、推定精度を上げるために、加速度がある設定値以上になったときに、ワーク質量計算を実行している。また、計算個数が少ないと、推定精度が低下するため、加減速終了後において、加速時、減速時のそれぞれの計算個数が設定値以下の場合には、ワーク質量推定処理を中断するように構成されている。以下、図４、図５のフローチャートに従って、ワーク質量推定処理の制御について説明する。
【００２６】
まず、図４のステップＳ１において、４つの内部レジスタＮａｃｃ、Ｎｄｅｃ、Ｗａｃｃ、Ｗｄｅｃをクリアする。そして、ステップＳ２へ進み、加速開始したか否かを判断する。ここで、加速開始したときには、ステップＳ２にて「ＹＥＳ」へ進み、速度Ｆ（ｎ）の計算と、角加速度α（ｎ）の計算を実行する（ステップＳ３）。
【００２７】
続いて、ステップＳ４へ進み、角加速度α（ｎ）の絶対値が設定値αｓｔ以上であるか否かを判断する。ここで、角加速度α（ｎ）の絶対値が設定値αｓｔ以上であれば、ステップＳ４にて「ＹＥＳ」へ進み、ワーク質量を推定する計算を実行する（ステップＳ５）。この場合、総イナーシャ推定の計算と、ワーク質量推定の計算と、ワーク質量累積の計算と、加速時計算回数の累積の計算とを実行するように構成されている。
【００２８】
そして、ステップＳ６へ進み、加速が終了したか否かを判断するようになっている。ここで、加速が終了していない場合には、ステップＳ６にて、「ＮＯ」へ進み、ステップＳ３へ戻るように構成されている。これにより、ワーク質量推定の計算処理（ステップＳ３、Ｓ５）が所定のサンプリング周期で複数回実行されるようになっている。尚、ステップＳ４にて、角加速度α（ｎ）の絶対値が設定値αｓｔ以上でないときには、「ＮＯ」へ進み、ステップＳ６へ進む。
【００２９】
さて、加速が終了したら、ステップＳ６にて「ＹＥＳ」へ進み、図５のステップＳ７へ進み、減速開始したか否かを判断する。ここで、減速開始したときには、ステップＳ７にて「ＹＥＳ」へ進み、速度Ｆ（ｎ）の計算と、角加速度α（ｎ）の計算を実行する（ステップＳ８）。
【００３０】
続いて、ステップＳ９へ進み、角加速度α（ｎ）の絶対値が設定値αｓｔ以上であるか否かを判断する。ここで、角加速度α（ｎ）の絶対値が設定値αｓｔ以上であれば、ステップＳ９にて「ＹＥＳ」へ進み、ワーク質量を推定する計算を実行する（ステップＳ１０）。この場合、総イナーシャ推定の計算と、ワーク質量推定の計算と、ワーク質量累積の計算と、減速時計算回数の累積の計算とを実行するように構成されている。
【００３１】
そして、ステップＳ１１へ進み、減速が終了したか否かを判断するようになっている。ここで、減速が終了していない場合には、ステップＳ１１にて、「ＮＯ」へ進み、ステップＳ８へ戻るようになっている。これにより、ワーク質量推定の計算処理（ステップＳ８、Ｓ１０）が所定のサンプリング周期で複数回実行されるようになっている。尚、ステップＳ９にて、角加速度α（ｎ）の絶対値が設定値αｓｔ以上でないときには、「ＮＯ」へ進み、ステップＳ１１へ進む。
【００３２】
さて、減速が終了したら、ステップＳ１１にて「ＹＥＳ」へ進み、加速時の推定計算回数並びに減速時の推定計算回数がそれぞれ予め設定された設定回数Ｎｏｋ（ワーク質量推定計算個数許容値）以上であるか否かを判断する（ステップＳ１２）。ここで、加減速時の推定計算回数が設定回数Ｎｏｋ以上でない場合には、ステップＳ１２にて「ＮＯ」へ進み、ワーク質量の推定処理を終了（中止）するように構成されている。
【００３３】
これに対して、ステップＳ１２にて、加減速時の推定計算回数がそれぞれ設定回数Ｎｏｋ以上であるときは、「ＹＥＳ」へ進み、加減速時の各ワーク質量の推定値の平均を計算する（ステップＳ１３）。この場合、加速時に計測した複数のワーク質量の推定値の平均を計算すると共に、減速時に計測した複数のワーク質量の推定値の平均を計算し、更に、これら２つの平均値の平均を計算しており、この平均値をワーク質量の推定値Ｗとしている。
【００３４】
そして、ステップＳ１４へ進み、上記求めたワーク質量の推定値Ｗと、ユーザが設定したワーク質量Ｗｓｅｔとを比較し、上記ワーク質量の推定値Ｗがユーザが設定したワーク質量Ｗｓｅｔよりも、予め設定された許容誤差Ｗｅｒｒ以上重いか否かを判断する。ここで、上記推定値Ｗが許容誤差Ｗｅｒｒ以上重くなければ、ステップＳ１４にて「ＮＯ」へ進み、ワーク質量推定処理を終了する。
【００３５】
これに対して、上記推定値Ｗが許容誤差Ｗｅｒｒ以上重いときには、ステップＳ１４にて「ＹＥＳ」へ進み、ワーク質量の設定エラー処理へ移行するように構成されている。この場合、上記ワーク質量の推定値Ｗがユーザが設定したワーク質量Ｗｓｅｔよりも許容誤差Ｗｅｒｒ以上重いときは、機械振動が増え、加工精度が悪化し、また、ボールねじやガイド等にダメージを与えて寿命を短くしてしまう等の不具合が発生することがあるためである。
【００３６】
そこで、このような場合には、ワーク質量の設定エラー処理へ移行し、例えば、表示装置の画面に警告を表示したり、アラーム音を鳴動させて装置の動作を停止させたりするように構成されている。尚、ユーザが設定したワーク質量設定値を実際のワーク質量値（推定値Ｗ）に自動的に書き換えると共に、実際のワーク質量に合った時定数に自動的に切り替えるように構成しても良い。
【００３７】
さて、本実施例によってワーク質量を推定（測定）した実測結果を、図６に示す。この図６において、太い実線Ｌ１はモータの速度を示し、細い実線Ｌ２はモータのトルクを示し、破線Ｌ３はモータの角加速度を示している。また、黒い小さい丸のマーク（点）はワーク質量の推定値（計算値）を示し、横線Ｈ１は加速時のワーク質量推定値の平均値を示し、横線Ｈ２は減速時のワーク質量推定値の平均値を示している。
【００３８】
上記図６によれば、加速時のワーク質量推定値の平均値（Ｈ１）が２５５ｋｇであると共に、減速時のワーク質量推定値の平均値（Ｈ２）が１２９ｋｇであるから、これら２つの平均値の平均、即ち、ワーク質量推定値は、１９２ｋｇとなる。これに対して、実際のワークの質量は、１９４ｋｇであることから、上記求めたワーク質量推定値は、かなり正確であることがわかる。
【００３９】
尚、上記実施例においては、モータの加速度（の絶対値）がある設定値以上になったときに、ワーク質量計算を実行するように構成したが、これに代えて、モータのトルクがある設定値以上になったときに、ワーク質量計算を実行するように構成しても良い。
【００４０】
【発明の効果】
本発明は以上の説明から明らかなように、加速時に推定したワーク質量値と、減速時に推定したワーク質量値との平均を求め、この平均値をワーク質量とするように構成したので、粘性抵抗及び摺動抵抗をキャンセルすることが可能となり、粘性抵抗や摺動抵抗が環境や経年変化によって変化することがあっても、イナーシャ、即ち、ワーク質量を正確に推定することができるという効果を奏する。
【図面の簡単な説明】
【図１】本発明の一実施例を示すものであり、直線加減速時の速度とトルクの関係を示す特性図
【図２】搬送装置の側面図
【図３】制御装置のブロック図
【図４】フローチャート（その１）
【図５】フローチャート（その２）
【図６】ワーク質量推定の実測値を示す図
【符号の説明】
１は搬送装置、２はベース、３はリニアガイド、４はテーブル、５はボールねじ、６はモータ、６ｂはエンコーダ、７はワーク、８は制御装置、９は数値制御部、１０はサーボアンプ、１１はモーションコントロール部、１２は位置カウンタ、１３はワーク質量推定部（ワーク質量推定装置）を示す。
【図１】
2004054838.tif 000007
【図２】
2004054838.tif 000008
【図３】
2004054838.tif 000009
【図４】
2004054838.tif 000010
【図５】
2004054838.tif 000011
【図６】
2004054838.tif 000012