技術 負荷イナーシャ推定システム、負荷イナーシャ推定プログラムを記録した媒体及びロボットの動作プログラムを記録した媒体

0001

本発明は、工作機械の送り軸やロボットのアーム等を駆動するサーボモータの負荷イナーシャ推定システム、負荷イナーシャ推定プログラムを記録した媒体及びロボットの動作プログラムを記録した媒体に関する。

0002

サーボモータによって負荷を制御する場合、制御部に実機モデルを有する制御を適用することによって振動や応答を改善する方法がよく用いられる。それらの制御手法の効果をあげるためには制御部のモデルの負荷イナーシャ値が実機と一致していることが望ましい。そのためには、実機の負荷イナーシャを正確に同定することが必要になる。ここで、もし、実機負荷イナーシャとモデル負荷イナーシャが大きな誤差を含む場合、その制御性能が劣化するだけでなく、逆に悪影響をおよぼしたり、制御系が不安定になり、発振する可能性がある。

0003

このような問題に対し、従来では、モデル部の負荷イナーシャには設計計算値を使用するか、実機を実際に数回動作させることによって各種データをとり、そのデータから負荷イナーシャの真値を計算するという方法をとっていた。

0004

図１３は特開平８−１４０３８６号公報に開示された従来の負荷イナーシャの推定方法を説明するフローチャートである。以下に説明する。

0005

図１３において、はじめにサーボモータを定速駆動する（ステップＳ１）。この定速駆動は以下のステップＳ２とステップＳ３において摩擦分をキャンセルするためのトルクＦと重力分をキャンセルするためのトルクＧを求めるために行う。次に、負荷に重力がかからない状態で一定速度で運転し、そのトルクを測定し、摩擦キャンセル分のトルクＦを求める（ステップＳ２）。次に、負荷に重力がかかる状態で一定速度で運転し、そのトルクを測定し、摩擦分をキャンセルするためのトルクＦを差し引いて、重力分をキャンセルするためのトルクＧを求める（ステップＳ３）。

0006

次に、サーボモータを加速駆動する（ステップＳ４）。ここで、この加速駆動は、例えば、サーボモータに速度零の状態において、ある設定した速度値の速度指令を与え、速度が零から設定速度に達するまで駆動するものである。

0007

この加速駆動において、サーボモータに印加されるトルクコマンドＴｃを求めることによって加速駆動に必要なトルクＴの時間変化を求める（ステップＳ５）。また、同様の加速駆動において、サーボモータのフィードバック速度ｖを求めることによって加減速駆動における速度の時間的変化を求めることができ、さらにこの測定結果から、加速駆動における加速度の時間変化を求めることができる（ステップＳ６）。

0008

次に、前記測定により得られる加速駆動に必要なトルクＴの時間変化と加速駆動における速度の時間変化とから、加速駆動における速度ｖとトルクＴとの関係を求める。また、前記加速駆動における加速度の時間変化から加速駆動における速度ｖと加速度Ａとの関係を求める。速度ｖとトルクＴとの関係は、サーボモータのイナーシャを加速するために必要なトルクＴＪを求めるためであり、また、速度ｖと加速度Ａとの関係は、サーボモータのイナーシャを求めるための加速度を定めるためである（ステップＳ７）。

0009

前記ステップＳ７において求めた加速駆動に必要なトルクＴはイナーシャを加速するために必要なトルクＴＪと摩擦分をキャンセルするために必要なトルクＦと、重力分をキャンセルするために必要なトルクＧの和（Ｔ＝ＴＪ＋Ｆ＋Ｇ）であるため、イナーシャを加速するために必要なトルクＴＪはＴＪ＝Ｔ−Ｆ−Ｇで求める（ステップＳ８）。

0010

サーボモータのモータ及び機械系のイナーシャＪは、イナーシャを加速するために必要なトルクＴＪと加速度Ａとの比（Ｊ＝ＴＪ／Ａ）によって求められる（ステップＳ９）。

0011

これによって、サーボモータのイナーシャを加速するために必要なトルクと、摩擦分および重力分キャンセルするために必要なトルクからサーボモータや機械系のイナーシャを推定することができる。

0012

上述した従来の技術は、以下の問題点があった。

0013

設計計算値を使用する従来の方法では計算誤差や、オペレータの入力ミスによる制御効果の劣化という問題があり、また、実機による数回の動作でのデータどりには多くの人手と時間が必要であるという問題があった。

0014

また、実機による数回の動作でのデータどりには多くの人手と時間が必要であるという問題があった。

0015

また、両者に共通して、アーム先端につく負荷が変わる度に複雑な計算または時間と人手のかかるデータ取りをやり直す必要があるという問題があった。

0016

また、たとえ、実機によるデータ取りを自動的に実機に動作をさせて、データを取得させ、その値を使用して自動的に負荷のイナーシャを推定計算させたとしても、実機は、特定の動作を少なくとも１回以上おこなう必要があり、実機に無駄な動作を行わせ、無駄な時間を消費するという問題があった。

0017

上記従来技術の問題点に鑑み、本発明の目的は、精度よくサーボモータの負荷イナーシャを推定する負荷イナーシャ推定システム、負荷イナーシャ推定プログラムを記録した媒体及びロボットの動作プログラムを記録した媒体を提供することにある。

0018

本発明の負荷イナーシャ推定システムは、サーボモータを使用して負荷を駆動する装置を制御するシステムの負荷イナーシャの真値を推定するための負荷イナーシャ推定システムにおいて、比例積分制御部と、サーボモータと負荷の状態量を計算するオブザーバ推定計算部と、オブザーバ推定計算部で計算された状態量の推定値と実機で観測できる状態量の値との差を計算する誤差計算部と、誤差計算部からの状態量の差の出力値とオブザーバ推定計算で使用する負荷イナーシャ設定値とから負荷イナーシャの真値を求める負荷イナーシャ推定計算部とを有する。

0019

また、誤差計算部では、オブザーバ推定計算部で計算されたモータ指令トルクの推定値と実機のモータ指令トルクとの誤差を計算する手段を有してもよい。

0020

また、誤差計算部では、オブザーバ推定計算部で計算されたモータ位置の推定値と実機のモータ位置との誤差を計算する手段を有してもよい。

0021

また、誤差計算部では、オブザーバ推定計算部で計算されたモータ速度の推定値と実機のモータ速度との誤差を計算する手段を有してもよい。

0022

また、誤差計算部では、オブザーバ推定計算部で計算されたモータ加速度の推定値と実機のモータ加速度との誤差を計算する手段を有してもよい。

0023

また、負荷駆動装置制御中に、オブザーバ推定計算部と誤差計算部と負荷イナーシャ推定計算部とを作動させ、自動的に負荷イナーシャの真値を求めて、真値をメモリに保存する手段を有してもよい。

0024

また、オブザーバ推定計算部のオブザーバモデルの構成に外乱推定項を含んでもよい。

0025

また、推定された負荷イナーシャの値をリアルタイムにオブザーバ推定計算で使用する手段を有してもよい。

0026

また、比例積分制御部にフィードフォワード制御部を有し、フィードフォワード制御部は、フィードフォワード制御の制御定数の計算に推定された負荷イナーシャの値を使用してもよい。

0027

本発明の負荷イナーシャ推定プログラムを記録した媒体は、コンピュータによって、サーボモータを使用して負荷を駆動する装置を制御するシステムの負荷イナーシャの真値を推定するための負荷イナーシャ推定プログラムを記録した媒体であって、推定プログラムは、コンピュータに比例積分制御させ、サーボモータと負荷の状態量を計算するオブザーバ推定計算させ、オブザーバ推定計算で計算された状態量の推定値と実機で観測できる状態量の値との差を計算する誤差計算させ、誤差計算からの状態量の差の出力値とオブザーバ推定計算で使用する負荷イナーシャ設定値とから負荷イナーシャの真値を求める負荷イナーシャ推定計算させる。

0028

また、誤差計算では、オブザーバ推定計算で計算されたモータ指令トルクの推定値と実機のモータ指令トルクとの誤差を計算してもよい。

0029

また、誤差計算では、オブザーバ推定計算で計算されたモータ位置の推定値と実機のモータ位置との誤差を計算してもよい。

0030

また、誤差計算では、オブザーバ推定計算で計算されたモータ速度の推定値と実機のモータ速度との誤差を計算してもよい。

0031

また、誤差計算では、オブザーバ推定計算で計算されたモータ加速度の推定値と実機のモータ加速度との誤差を計算してもよい。

0032

また、負荷駆動装置制御中に、オブザーバ推定計算と誤差計算と負荷イナーシャ推定計算とを実行させ、自動的に負荷イナーシャの真値を求めて、真値をメモリに保存してもよい。

0033

また、オブザーバ推定計算のオブザーバモデルの構成に外乱推定項を含んでもよい。

0034

また、推定された負荷イナーシャの値をリアルタイムにオブザーバ推定計算で使用してもよい。

0035

また、比例積分制御にフィードフォワード制御を有し、フィードフォワード制御は、フィードフォワード制御の制御定数の計算に推定された負荷イナーシャの値を使用してもよい。

0036

本発明の負荷イナーシャ推定システムは、サーボモータを使用して負荷を駆動する装置を制御するシステムの負荷イナーシャの真値を推定する負荷イナーシャ推定システムにおいて、比例積分制御部と、負荷に作用する外乱を推定するオブザーバ推定計算部と、オブザーバ推定計算部からの出力である外乱推定値と実機の状態量を所定のサンプリング周期でメモリに保存するデータ記憶部と、データ記憶部に保存された外乱推定値を実機の状態量を使用して解析する外乱推定値解析部と、外乱推定値解析部からの出力値によって負荷イナーシャ設定値を補正するための負荷イナーシャ補正値を計算する補正値計算部と、補正値計算部の出力および予め設定しておいた負荷イナーシャ設定値から、負荷イナーシャの真値を計算する負荷イナーシヤ推定計算部とを有する。

0037

また、データ記憶部で保存し、外乱推定値解析部で解析する実機の状態量は、モータの速度であってもよい。

0038

また、外乱推定値解析部では、実際のモータ速度と外乱推定値の波形を比較する手段を有してもよい。

0039

また、外乱推定値解析部では、実際のモータ速度と外乱推定値の加減速時のそれぞれのデータの傾きを比較する手段を有してもよい。

0040

また、比例積分制御部により負荷を駆動する装置を制御中に、オブザーバ推定計算部とデータ記憶部と外乱推定値解析部と補正値計算部と負荷イナーシャ推定計算部とを作動させ、自動的に負荷イナーシャの真値を求めて、負荷イナーシヤの真値をメモリに保存する手段を有してもよい。

0041

また、推定された負荷イナーシャの値をリアルタイムにオブザーバ推定計算や、または、制御計算で使用する手段を有してもよい。

0042

また、推定された負荷イナーシャの値を負荷イナーシャ設定値として、オブザーバ推定計算で使用し、負荷イナーシャ設定値を使用してオブザーバ推定計算により求まった、外乱推定値を使用して、再び、負荷イナーシャ設定値を補正し、これを繰り返すことにより、負荷イナーシャ推定値の精度を向上させる手段を有してもよい。

0043

本発明の負荷イナーシャ推定プログラムを記録した媒体は、コンピュータによって、サーボモータを使用して負荷を駆動する装置を制御するシステムの負荷イナーシャの真値を推定するための負荷イナーシャ推定プログラムを記録した媒体であって、推定プログラムは、コンピュータに比例積分制御させ、負荷に作用する外乱を推定するオブザーバ推定計算をさせ、オブザーバ推定計算で計算された外乱の推定値と実機の状態量を所定のサンプリング周期でメモリに保存しデータ記憶させ、メモリの出力から、外乱推定値を実機の状態量を使用して解析させ、その結果から負荷イナーシャ補正値を計算させ、負荷イナーシャ補正値とオブザーバ推定計算で使用する負荷イナーシャ設定値とから負荷イナーシャの真値を求める負荷イナーシャ推定計算をさせる。

0044

また、データ記憶し、外乱推定値の解析で使用する実機の状態量は、モータの速度であってもよい。

0045

また、外乱推定値の解析では、実際のモータ速度と外乱推定値の波形を比較してもよい。

0046

また、外乱推定値の解析では、実際のモータ速度と外乱推定値の加速時の傾きを比較してもよい。

0047

また、比例積分制御により負荷を駆動する装置を制御中に、オブザーバ推定計算とデータ記憶と外乱推定値解析と補正値計算と負荷イナーシャ推定計算とを実行させ、自動的に負荷イナーシャの真値を求めて、負荷イナーシャの真値をメモリに保存してもよい。

0048

また、推定された負荷イナーシャの値をリアルタイムにオブザーバ推定計算や、または、制御計算で使用してもよい。

0049

また、推定された負荷イナーシャの値を負荷イナーシャ設定値として、オブザーバ推定計算で使用し、負荷イナーシャ設定値を使用してオブザーバ推定計算により求まった、外乱推定値を使用して、再び、負荷イナーシャ設定値を補正し、これを繰り返すことにより、負荷イナーシャ推定値の精度を向上させてもよい。

0050

本発明のロボットの動作プログラムを記録した媒体は、コンピュータによって、ロボットに動作を教示し、実行するためのロボットの動作プログラムを記録した媒体であって、動作プログラムは、コンピュータに、ロボットに動作を教示し、実行させ、上述の負荷イナーシャ推定プログラムを記録した媒体に記録された負荷イナーシャ推定プログラムを実行させる。

0051

また、動作プログラムは、オペレータがロボット言語命令で記述することにより作成される、ロボットの教示プログラムを、コンピュータが解読することによって生成され、ロボット言語命令の一つとして、負荷イナーシャ推定計算をする命令を有してもよい。

0052

従って、オブザーバ推定値と実機観測値との誤差を使用し、ロボットの動作中に自動的に負荷イナーシャの真値を求めるため、又は、オブザーバ外乱推定値を実機状態量の波形を使用して解析し、ロボットの任意の動作中に自動的に負荷イナーシャの真値を求めるため、適当な負荷イナーシャ計算値を予め設定しておくだけで、後はデータ測定や複雑な計算に時間や人手を消費することなく、また、ロボットに無駄な動作を実行させることなく、かつ複雑な形状の負荷の場合、机上計算で負荷イナーシャを求めるより精度よくサーボモータの負荷イナーシャ真値を推定することができる。また、アーム先端の負荷が変わった場合も簡単に負荷イナーシヤの真値を推定することができる。

0053

次に、本発明の実施の形態について図面を参照して説明する。

0054

（本発明の第１の実施の形態）図１は本発明の第１の実施の形態の負荷イナーシャの推定方法を説明するフローチャートである。

0055

まず、ＳＴＥＰ１では、オブザーバの推定計算で使用するための負荷イナーシャ値（ＪＬobs ）を設定しておく。この時、ＪＬobs の値は正確である必要はなく設計計算値等、概算で求めておく。

0056

次にＳＴＥＰ２では、サーボモータを加速駆動する。

0057

この２つの作業だけで、負荷イナーシャ真値を求めることができる。

0058

以下に、ＳＴＥＰ２でどのような計算を行っているかを説明する。

0059

図２は本発明の第１の実施の形態の制御ブロック線図である。図中、実線で囲った部分１０が制御部、点線内２０が実機に相当する。図中のＫｐは位置ループ比例ゲイン、Ｋｖは速度ループ比例ゲイン、Ｋｉは速度ループ積分ゲイン、Ｊｍはモータイナーシャ、ＪＬは負荷イナーシャ、Ｋは減速機バネ定数、Ｃは減速機減衰係数、Ｎは減速比を表す。また、Ｓはラプラス演算子を表す。

0060

１は比例積分制御部、２はオブザーバ推定計算部である。２２は誤差計算部、６は負荷イナーシャ推定計算部、２１は制御系への入力である指令作成部である。また、図中１１と１２は０次ホールドを行うサンプラーを表すものとする。

0061

１のオブザーバの入力にはモータ検出速度Ｖｍとモータ指令トルクＴｒｅｆを使う。以下にこの実施の形態で使用したオブザーバの構成を数式を用いて説明する。Ａ１１からＡ３４およびＢ１、Ｌ１〜Ｌ４は各行列の要素を表す。式中Ｔｓはオブザーバ計算の１計算周期であり、ｋは計算周期の回数を表す。すなわち（ｋ＋１）番目のデータは（ｋ）番目のデータの１計算周期後のデータを表す。

0062

0063

Ａ１１＝−Ｃ／（Ｎ2 ×Ｊｍ）×Ｔｓ＋１
Ａ１２＝−Ｋ／（Ｎ×Ｊｍ）×Ｔｓ
Ａ１３＝Ｃ／（Ｎ×Ｊｍ）×Ｔｓ
Ａ２１＝Ｔｓ／Ｎ
Ａ２３＝−Ｔｓ
Ａ３１＝Ｃ／（Ｎ×ＪＬ）×Ｔｓ
Ａ３２＝Ｋ／ＪＬ×Ｔｓ
Ａ３３＝−Ｃ／ＪＬ×Ｔｓ＋１
Ａ３４＝−Ｔｓ／ＪＬ
Ｂ１＝Ｔｓ／Ｊｍ
式に示したように、状態変数は［Ｖｍobs ・θｓobs ・ＶＬobs ・Ｄ２obs ］の４つであり、Ｖｍobs はモータ速度推定値、θｓobsはねじれ角推定値、ＶＬobs は負荷速度推定値、Ｄ２obs は２次側外乱推定値を表す。

0064

この外乱推定項を含むことにより、外乱が加わっている場合もオブザーバの推定精度を上げることができ、従って、負荷イナーシャの推定精度を上げることができる。

0065

実機検出値であるモータ速度Ｖｍとオブザーバで推定計算するモータ速度推定値Ｖｍobs との差をＬ行列（Ｌ１〜Ｌ４）により補正する構成をとる。

0066

また、本発明の第１の実施の形態では、誤差計算部２２でモータ指令トルクの推定値と実機のモータ指令トルク値を比較するため、オブザーバ推定計算部２で、モータ指令トルクの推定値Ｔobs を求める必要がある。以下に説明する。

0067

図２において、モータトルク指令Ｔｒｅｆと負荷加速度ＡＬの関係は、ＡＬ：負荷加速度とＡｍ：モータ加速度とを使って、
Ｔｒｅｆ＝Ｊｍ×Ａｍ＋ＪＬ×ＡＬ／Ｎ （２）
と表すことができる。

0068

次に、モータ指令トルクＴｒｅｆおよび負荷加速度ＡＬ、モータ加速度Ａｍの推定値をそれぞれＴobs 、ＡＬobs 、Ａｍobs とし、オブザーバ推定計算に使用する負荷イナーシャをＪＬobs とすると、以下の式が成り立つ。

0069

Ｔobs ＝Ｊｍ×Ａｍobs ＋ＪＬobs ×ＡＬobs ／Ｎ （３）
ここで、モータ速度推定値Ｖｍobs と負荷速度推定値ＶＬobs から微分近似として差分を使って、モータ加速度推定値Ａｍobs と負荷加速度推定値ＡＬobs を計算する式を示す。

0070

Ａｍobs （ｋ）＝｛Ｖｍobs （ｋ）−Ｖｍobs （ｋ−１）｝／Ｔｓ （４）
ＡＬobs （ｋ）＝｛ＶＬobs （ｋ）−ＶＬobs （ｋ−１）｝／Ｔｓ （５）
式（３）（４）（５）より、
Ｔobs ＝Ｊｍ×｛Ｖｍobs （ｋ）−Ｖｍobs （ｋ−１）｝／Ｔｓ
＋ＪＬobs ×｛ＶＬobs （ｋ）−ＶＬobs （ｋ−１）｝／Ｔｓ（６）
となり、図２中のオブザーバ推定計算部２でモータ指令トルク推定値Ｔobs を求めることができる。

0071

誤差計算部２２では、実機モータ指令トルク値とオブザーバモータ指令トルク推定値との差ＤＬＴT を計算する。

0072

ＤＬＴT ＝Ｔｒｅｆ−Ｔobs （７）
負荷イナーシャ推定計算部６では、実機の負荷イナーシャの真値を推定する。内部の計算方法を以下に説明する。

0073

まず、式（２）（３）を式（７）に代入する。

0074

ＤＬＴT ＝ＪＬ×ＡＬ／Ｎ−ＪＬobs ×ＡＬobs ／Ｎ
＋Ｊｍ×Ａｍ−Ｊｍ×Ａｍobs （８）
ここで、オブザーバの推定開始時に推定値が真値とずれていた場合でも、オブザーバの内部の式の補正の項により、オブザーバ推定値は、ただちに真値に収束し、推定値は真値とほぼ等しい値となるため、
Ａｍ＝Ａｍobs 、ＡＬ＝ＡＬobs （９）
とおく。

0075

式（９）を、式（８）に代入して整理すると、式（８）は次のように書き換えられる。

0076

ＤＬＴT ＝（ＪＬ−ＪＬobs ）×ＡＬobs ／Ｎ
＝（ＪＬ−ＪＬobs ）×
[｛ＶＬobs （ｋ）−ＶＬobs （ｋ−１）｝／Ｔｓ]／Ｎ （１０）
（１０）式をＪＬについて解くと、
ＪＬ＝ＤＬＴT ×Ｎ／[｛ＶＬobs （ｋ）−ＶＬobs （ｋ−１）｝／Ｔｓ]
＋ＪＬobs （１１）
となる。

0077

この式（１１）を負荷イナーシャ推定計算部６で計算することにより、精度の高い負荷イナーシャ推定値を得ることができる。

0078

また、負荷駆動装置制御中に、オブザーバ推定計算部２と誤差計算部２２と負荷イナーシャ推定計算部６とを作動させ、自動的に負荷イナーシャの真値を求めて、真値をメモリに保存する機能を追加できる。従って、負荷駆動装置制御中に自動的に負荷イナーシャの真値を推定することができ、その真値をメモリに書込んで保存することにより、状態量入力の手間も省くことができる。

0079

また、推定された負荷イナーシャの値をリアルタイムにオブザーバ推定計算で使用することにより、負荷イナーシャの真値の推定精度を上げることができる。

0080

本発明の負荷イナーシャ推定プログラムは媒体に記録されており、コンピュータによって、サーボモータを使用して負荷を駆動する装置を制御するシステムの負荷イナーシャの真値を推定するためのものであり、コンピュータに比例積分制御させ、サーボモータと負荷の状態量を計算するオブザーバ推定計算させ、オブザーバ推定計算で計算された状態量の推定値と実機で観測できる状態量の値との差を計算する誤差計算させ、誤差計算からの状態量の差の出力値とオブザーバ推定計算で使用する負荷イナーシャ値とから負荷イナーシャの真値を求める負荷イナーシャ推定計算させるものである。

0081

また、ロボットに動作を教示し、実行させるためにはオペレータによって教示プログラムが作成される。教示プログラムは、特定のプログラム言語であるロボット言語によって記述されている。本発明は、そのロボット言語の命令の一つとして、上記負荷イナーシャ推定計算処理を実行させる命令を持っている。ロボットの教示プログラムを、コンピュータが解読することによって、ロボットの動作プログラムが生成される。オペレータが教示プログラムを実行するだけで、コンピュータは動作プログラムに解読し、その動作プログラムを実行するので、上記、定加速度動作ならびに負荷イナーシャ推定計算処理を自動的に実行するものである。本発明のロボットの動作プログラムは媒体に記録されている。

0082

以下に本発明を実施した時の効果を示す。

0083

図３はシミュレーションで使用した負荷イナーシャ推定計算部の制御ブロック図である。５は逆数を表す関数である。入力はＤＬＴTモータ指令トルク誤差と式（５）で得られるＡＬobs負荷加速度の２入力である。

0084

図４、図５にシミュレーション結果を示す。本発明の第１の実施の形態では、負荷の加速度の推定値を利用するため、サーボモータを加速駆動した時に負荷イナーシャを推定することができる。図４に、時間−モータ速度の波形を、図５にその時の負荷イナーシャ推定値の波形を示している。

0085

本シミュレーションの各状態量の値を説明する。

0086

ＪＬ＝２１３３ （Ｋｇ・ｍ2）
ＪＬobs ＝３１９９．５（＝ＪＬ×１．５）
Ｋ＝２６７７５００ （Ｎ・ｍ／ｒａｄ）
Ｋobs ＝３２１３０００（＝Ｋ×１．２）
Ｎ＝１６７
Ｃ＝７５５７ （Ｎ・ｍ・ｓｅｃ／ｒａｄ）
Ｋｐ＝１０ （１／ｓｅｃ）
Ｋｖ＝３．２５ （１／ｓｅｃ）
Ｋｉ＝２５ （１／ｓｅｃ）
Ｔｓ＝０．００１ （ｓｅｃ）
上記のように、オブザーバで推定計算に使用する負荷イナーシャＪＬobs は、実機負荷イナーシャの１．５倍に、また、オブザーバで推定計算に使用するバネ定数Ｋobs は、実機バネ定数Ｋの１．２倍に設定している。

0087

図５の波形を見ると分かるように、オブザーバ推定計算で使用する減速機のバネ定数Ｋobs が実機バネ定数とずれて設定してあっても、実機負荷イナーシャの真値を精度よく推定できている。

0088

また、本実施の形態では、モータ指令トルク値の誤差を使用したが、その他、モータ位置・モータ速度・モータ加速度を使用した方法でも同様の結果が得られる。

0089

また、比例積分制御部にフィードフォワード制御部（前向き補償部）を有し、フィードフォワード制御部は、フィードフォワード制御の制御定数の計算に推定された負荷イナーシャの値を使用することもできる。

0090

（本発明の第２の実施の形態）次に、本発明の第２の実施の形態について図面を参照して説明する。図６は本発明の第２の実施の形態の負荷イナーシャの推定操作を説明するフローチャートである。

0091

まず、ＳＴＥＰ１では、オブザーバの推定計算で使用するための負荷イナーシャ設定値ＪＬobs を設定しておく。この時、ＪＬobs の値は正確である必要はなく設計計算値等、概算で求めておく。

0092

次に、ＳＴＥＰ２では、サーボモータを任意の動作で駆動する。

0093

この２つのＳＴＥＰだけで、負荷イナーシャ真値を自動的に求めることができる。以上が実際にオペレータが行う操作である。

0094

以下に、ＳＴＥＰ２でどのような計算を行っているかを説明する。

0095

図７は本発明の第２の実施の形態の構成を示すブロック図である。図８は図７を制御ブロック線図にしたものである。図８中のＫｐは位置ループ比例ゲイン、Ｋｖは速度ループ比例ゲイン、Ｋｉは速度ループ積分ゲイン、Ｊｍはモータイナーシャ、ＪＬは負荷イナーシャ、Ｋは減速機バネ定数、Ｃは減速機減衰係数を表す。また、Ｓはラプラス演算子を表す。

0096

２１は制御系への入力である指令作成部である。また、図中３１と３２と３３と３４は０次ホールドを行うサンプラーを表すものとする。

0097

図７と図８に共通して、１は比例積分制御部であり、モータの位置と速度をフィードバック制御している。２はオブザーバ推定計算部である。３は実機状態量とオブザーバ外乱推定値をサンプリング周期Ｔでメモリに保存するデータ記憶部である。本実施の形態では、実機状態量として、モータ速度を使用している。４はデータ記憶部に保存された、外乱推定値とモータ速度の値から、外乱推定値を解析する、外乱推定値解析部である。５は外乱推定値解析部で解析した結果を基に、負荷イナーシャの補正値を計算する補正値計算部であり、６は補正値計算部の出力である補正値と負荷イナーシャ設定値から、負荷イナーシャ真値を計算する負荷イナーシャ推定計算部である。

0098

次に、オブザーバ推定計算部２の説明をする。２のオブザーバの入力にはモータ検出速度Ｖｍとモータ指令トルクＴｒｅｆを使う。以下にこの実施の形態で使用したオブザーバの構成を数式を用いて説明する。Ａ１１からＡ３４およびＢ１、Ｌ１〜Ｌ４は各行列の要素を表す。式中Ｔｓはオブザーバ計算の１計算周期であり、ｋは計算周期の回数を表す。すなわち（ｋ＋１）番目のデータは（ｋ）番目のデータの１計算周期後のデータを表す。

0099

0100

Ａ１１＝−Ｃ／Ｊｍ
Ａ１２＝−Ｋ／Ｊｍ×Ｔｓ
Ａ１３＝Ｃ／Ｊｍ×Ｔｓ
Ａ２１＝Ｔｓ
Ａ２３＝−Ｔｓ
Ａ３１＝Ｃ／ＪＬ×Ｔｓ
Ａ３２＝Ｋ／ＪＬ×Ｔｓ
Ａ３３＝−Ｃ／ＪＬ×Ｔｓ＋１
Ａ３４＝−Ｔｓ／ＪＬ
Ｂ１＝Ｔｓ／Ｊｍ
上式に示したように、推定する状態変数は［Ｖｍobs・θｓobs・ＶＬobs・Ｄ２obs］ の４つであり、Ｖｍobsはモータ速度推定値、θｓobsはねじれ角推定値、ＶＬobs は負荷速度推定値、Ｄ２obs は２次側外乱推定値を表す。

0101

実機検出値であるモータ速度Ｖｍとオブザーバで推定計算するモータ速度推定値Ｖｍobsとの差をＬ行列（Ｌ１〜Ｌ４）により補正する構成をとる。

0102

次に、データ記憶部３の説明をする。

0103

データ記憶部３では、ロボットが動作中に、図９に示すようなある一つの＜加速−定速−減速＞パターン動作時のモータ速度をあるサンプリング周期Ｔｎで保存する。それをＶｍｄ（ｎ）とする。

0104

それと同時に、その動作時のオブザーバ外乱推定値の値もサンプリング周期Ｔｎで保存する。それをＤ２ｄ（ｎ）とする。

0105

（ｎはサンプリング回数：ｎ＝１，２，３．．．ｎ）次に、外乱推定値解析部４の説明をする。

0106

外乱推定値解析部４では、まず最初に、外乱推定値をモータ速度と比較しやすいように補正する。

0107

外乱推定値には、主に、重力と摩擦が外乱として推定されている。ここで、本実施の形態では、重力がかからない軸について説明する。ただし、重力がかかる軸でも、予め負荷にかかる重力を計算して、減算しておけば、以下は同じ手順で負荷イナーシャ真値を推定することが出来る。

0108

動作中の摩擦には、大きく分けて、クーロン摩擦と、粘性摩擦がある。このうち、クーロン摩擦は速度に依存せず、速度の向きにのみ依存する値である。すなわち、動作中には常に、一定の大きさで作用する。それに対して、粘性摩擦は、速度に比例する値である。

0109

従って、クーロン摩擦分を外乱推定値から減算すると、外乱として速度に依存する粘性摩擦のみが抽出できる。図１０に説明の波形を示す。

0110

さらに、定速動作時にモータの速度と外乱推定値が一致するように外乱推定値の全体にある定数を乗算する。結果を図１１に示す。

0111

図１１のようになったところで、Ｖｍｄ（ｎ）：モータ速度とＤ２ｄ（ｎ）：外乱推定値の加速時の傾きを比較する。モータ速度の傾きをＡｖｍ、外乱推定値の傾きをＡｄ２とすると、
Ａｖｍ＝｛Ｖｍｄ（ｎ）−Ｖｍｄ（ｎ−ｍ）｝／（Ｔｎ×ｍ）
（１３）
Ａｄ２＝｛Ｄ２ｄ（ｎ）一Ｄ２ｄ（ｎ−ｍ）｝／（Ｔｎ×ｍ）
（１４）
となる。

0112

ｍは自然数であり傾きはｍ個前のデータと今回のデータの差分から計算してしいる。

0113

次に、補正値計算部５の説明をする。

0114

補正値計算部５ではＡｖｍとＡｄ２の大きさの違いに応じて、補正値を決定する。

0115

以下に、本実施の形態での補正値の決定方法を説明する。

0116

ＡｖｍとＡｄ２の比をＰとする。

0117

Ｐ＝Ａｖｍ／Ａｄ２ （１５）
負荷イナーシヤ設定値ＪＬｏｂｓとＰから補正値Ｒを計算する。

0118

Ｒ＝ＪＬobs×Ｐ×Ｃ （１６） （Ｃは設定定数）
また、負荷イナーシャの誤差によっては、図１２に示すような外乱推定値になる場合がある。この場合は、外乱推定値の加速または、減速前の突起の大きさδを使用して、負荷イナーシャ補正値を計算することも可能である。

0119

次に、負荷イナーシヤ推定計算部の説明をする。

0120

負荷イナーシャ推定計算部６では、ＪＬobs とＲから、負荷イナーシャ真値ＪＬrealを計算する。

0121

ＪＬreal＝ＪＬobs ＋Ｒ （１７）
この式（１７）を計算することにより、精度の高い負荷イナーシャ推定値を得ることができる。また、比例積分制御によって負荷を駆動する装置を制御中に、前記オブザーバ推定計算部２と前記データ記憶部３と、前記外乱推定値解析部４と前記補償値計算部５と前記負荷イナーシャ推定計算部６とを作動させ、自動的に負荷イナーシャの真値を求めて、真値をメモリに保存する機能を追加できる。従って、負荷駆動装置制御中に自動的に負荷イナーシャの真値を推定することができ、その真値をメモリに書込んで保存することにより、状態量入力の手間を省くことができる。

0122

また、推定された負荷イナーシャの値をリアルタイムにオブザーバ推定計算や、その他の制御計算で使用することにより、負荷イナーシャの真値の推定精度を上げることができる。

0123

また、推定された負荷イナーシャの値を負荷イナーシャ設定値として、オブザーバ推定計算で使用し、今回の負荷イナーシャ設定値を使用してオブザーバ推定計算により求まった、外乱推定値を使用して、再び、負荷イナーシャ設定値を補正する。これを繰り返すことにより、負荷イナーシャ推定値の精度を向上させることができる。

0124

本発明の負荷イナーシャ推定プログラムは、媒体に記録されており、コンピュータによって、サーボモータを使用して負荷を駆動する装置を制御するシステムの負荷イナーシャの真値を推定するためのものである。即ち、負荷イナーシャ推定プログラムは、コンピュータに比例積分制御させ、負荷に作用する外乱を推定するオブザーバ推定計算をさせ、オブザーバ推定計算で計算された外乱の推定値と実機の状態量をあるサンプリング時間でメモリに保存させ、データ記憶部の出力から、外乱推定値を実機状態量を使用して解析し、その結果から負荷イナーシャ補正値を計算させ、その値とオブザーバ推定計算で使用する負荷イナーシャ設定値とから負荷イナーシャの真値を求める負荷イナーシャ推定計算をさせるものである。

0125

また、ロボットに動作を教示し、実行させるためにはオペレータによって教示プログラムが作成される。教示プログラムは、特定のプログラム言語であるロボット言語によって記述されている。本発明は、そのロボット言語の命令の一つとして、上記負荷イナーシャ推定計算処理を実行させる命令を持っている。ロボットの教示プログラムを、コンピュータが解読することによって、ロボットの動作プログラムが生成される。オペレータが教示プログラムを実行するだけで、コンピュータは動作プログラムに解読し、その動作プログラムを実行するので、上記、定加速度動作ならびに負荷イナーシャ推定計算処理を自動的に実行するものである。本発明のロボットの動作プログラムは媒体に記録されている。

0126

以上説明したように本発明は、オブザーバ推定値と実機観測値との誤差を使用し、ロボットの動作中に自動的に負荷イナーシャの真値を求めるため、又は、オブザーバ外乱推定値を実機状態量の波形を使用して解析し、ロボットの任意の動作中に自動的に負荷イナーシャの真値を求めるため、適当な負荷イナーシャ計算値を予め設定しておくだけで、後はデータ測定や複雑な計算に時間や人手を消費することなく、また、ロボットに無駄な動作を実行させることなく、かつ複雑な形状の負荷の場合、机上計算で負荷イナーシャを求めるより精度よくサーボモータの負荷イナーシャ真値を推定することができるという効果がある。また、アーム先端の負荷が変わった場合も簡単に負荷イナーシヤの真値を推定することができるという効果がある。

0127

図１本発明の第１の実施の形態の負荷イナーシャの推定方法を説明するフローチャートである。
図２本発明の第１の実施の形態の制御ブロック線図である。
図３本発明の第１の実施の形態の負荷イナーシャ推定計算部の制御ブロック図である。
図４本発明の第１の実施の形態のシミュレーション結果の時間−モータ速度の波形である。
図５本発明の第１の実施の形態のシミュレーション結果の時間−負荷イナーシャ推定値の波形である。
図６本発明の第２の実施の形態の負荷イナーシャの推定操作を説明するフローチャートである。
図７本発明の第２の実施の形態の構成を示すブロック図である。
図８本発明の第２の実施の形態を説明する、制御ブロック線図である。
図９加速−定速−減速時の速度波形を説明する図である。
図１０外乱推定値の補正を説明する図である。
図１１外乱推定値の解析を説明する図である。
図１２負荷イナーシャの設定値が誤差を含むときの外乱推定値の波形を説明する図である。
図１３従来の負荷イナーシャの推定方法を説明するフローチャートである。

0128

１比例積分制御部
２オブザーバ推定計算部
３データ記憶部
４外乱推定値解析部
５補正値計算部
６負荷イナーシャ推定計算部
７サーボモータ
８負荷
１０ 制御部
１１、１２、３１、３２、３３、３４ ０次ホールドを行うサンプラー
２０実機
２１指令作成部
２２誤差計算部
２５逆数を表す関数