(57)【要約】【課題】運動指令からの追従遅れや動力学特性による運動指令からのズレを生じさせることなく指令通りに応答し、簡単な演算式により逐次生成され、リアルタイムで変化する指令や外乱に対しても、対応できるモータの制御装置及び制御方法を提供する。  【解決手段】モータの制御装置において、モータを模擬したモータモデルと、機構部を模擬した機構モデル部と、モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、モータ制御部及びモータ及び機構部を含む実機部及びシミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、シミュレーション部の状態量に基づいて、モータの応答が上位指令生成部からの指令と一致するように指令修正量を作成する指令修正量演算部と、指令修正量で上位指令生成部からの指令を修正する指令修正部とを備えた。【選択図】図１
2005094964.tif 000002
【特許請求の範囲】
【請求項１】
モータにより駆動される機構部を有するモータの制御装置において、
前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、
前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記シミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、
前記シミュレーション部の状態量に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように指令修正量を作成する指令修正量演算部と、
前記指令修正量で前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とするモータの制御装置。
【請求項２】
前記上位指令生成部からの指令の時間微分量の変化に応じて、前記指令修正量に１以下の減衰率を乗算する修正量調整部を備えたことを特徴とする請求項１記載のモータの制御装置。
【請求項３】
前記上位指令生成部からの指令の時間微分量の変化に応じて、前記指令修正量の位相を進ませる修正量調整部を備えたことを特徴とする請求項１記載のモータの制御装置。
【請求項４】
モータにより駆動される機構部を有するモータの制御装置において、
前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、
前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記シミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、
前記シミュレーション部の状態量と前記実機部の状態量を比較する状態量比較部を備えたことを特徴とするモータの制御装置。
【請求項５】
前記状態量比較部の出力である状態量比較値が予め設定されたしきい値を越える場合にはアラームを出力することを特徴とする請求項４記載のモータの制御装置。
【請求項６】
前記状態量比較値を元に、前記モータの応答が前記上位指令生成部からの指令と一致するように指令修正量を作成する指令修正量演算部と、前記指令修正量から前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする請求項４記載のモータの制御装置。
【請求項７】
モータにより駆動される機構部を有するモータの制御装置において、
前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部とをそれぞれ有する第１のシミュレーション部と第２のシミュレーション部とを備え、
前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記第１のシミュレーション部及び前記第２のシミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、
前記第１のシミュレーション部の状態量に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように第１の指令修正量を作成する第１の指令修正量演算部と、
実機部及び前記第２のシミュレーション部に、前記第１の指令修正量で前記上位指令生成部からの指令を修正する指令修正部と、
前記第２のシミュレーション部の状態量と前記実機部の状態量を比較する状態量比較部とを備えたことを特徴とするモータの制御装置。
【請求項８】
前記状態量比較部の出力である状態量比較値に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように第２の指令修正量を作成する第２の指令修正量演算部と、前記第１及び第２の指令修正量で前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする請求項７記載のモータの制御装置。
【請求項９】
前記上位指令生成部からの指令の時間微分量の変化に応じて、前記第１の指令修正量に１以下の減衰率を乗算する第１の修正量調整部と、前記第２の指令修正量に１以下の減衰率を乗算する第２の修正量調整部を備えたことを特徴とする請求項７又は８記載のモータの制御装置。
【請求項１０】
前記上位指令生成部からの指令の時間微分量の変化に応じて、前記第１の指令修正量の位相を進ませる第１の修正量調整部と、前記第２の指令修正量の位相を進ませる第２の修正量調整部を備えたことを特徴とする請求項７又は８記載のモータの制御装置。
【請求項１１】
制御装置に付属した操作ペンダントの画面上に、前記実機部と前記第１のシミュレーション部と前記第２のシミュレーション部の状態量を、関節軸単位あるいは直交座標で表示し、前記実機部と前記第１のシミュレーション部と前記第２のシミュレーション部の制御パラメータを調整する手段を設けた
ことを特徴とする請求項７乃至１０記載のモータ制御装置。
【請求項１２】
複数回、同一の軌跡を動作する場合に、前周回の指令修正量を記憶する指令修正量記憶部と、次周回時に前記指令修正記憶値で前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする請求項７乃至１１記載のモータの制御装置。
【請求項１３】
モータにより駆動される機構部を有するモータの制御方法において、
前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部で構成されたシミュレーション部を有し、
前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記シミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、前記モータの応答が前記上位指令生成部からの指令と一致するように指令を修正する指令修正量を作成し、前記指令修正量で前記上位指令生成部からの指令を修正することを特徴とするモータの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、モータの制御装置に関し、さらに詳しく言えば、特にモータの動作指令に対する応答特性を改善するモータの制御装置及び制御方法に関する。
【背景技術】
【０００２】
モータを使用して作業を行う場合に、モータへの動作指令を与える方法として、実時間とオフラインの２つに分けることができる。
（１）実時間指令生成
実時間での指令生成とは、入力される指令が未知で、指令通りの軌跡を取れるように、モータを実作業させながら入力された指令を逐次修正する方法である。詳しくは、モータの動作状態や外部に設けたセンサ情報に応じて、予め決められた加減速パターンや速度を逐次変化させることで、モータへの指令を実時間で生成させる方法である。例として、２台のモータの同期制御を行う際に１台目のモータの動作に合わせて２台目に対する指令をリアルタイムに生成する場合や、アーク溶接においてロボット手先に設けた距離センサで溶接点直前のワークズレを事前に検出させながら、ロボットをワークズレに対してリアルタイムに追従させる指令を生成するような場合である。
ただし、指令生成時にモータの運動特性や機構の特性を反映していない場合には、加減速パターンからの追従遅れが生じたり、指令生成以降に設けられたフィルタやサーボ系の遅れがあったりして、モータは必ずしも指令通りの軌跡で動作することができない。
そこで、サーボ系と機構を考慮した逆伝達関数による指令生成方法が考えられる。例えば、特許文献１や特許文献２に開示されたものがある。特許文献１に開示されている装置では、制御対象の逆伝達関数を考慮して参照位置関数θirefが制御量θlに一致するような位置指令θmrefを算出する方法が提案されている。即ち「制御対象の制御量の目標となる目標関数ＸT 及び当該目標関数の導関数Ｄ1 、Ｄ2 、Ｄ3 、Ｄ4 を発生する関数発生手段と、制御対象の特性を計算手順及びパラメータ（ＪL 、ＣS 、……）として保存し、当該計算手順及びパラメータ及び目標関数ＸT 及び導関数Ｄ1 、Ｄ2 、Ｄ3 、Ｄ4 を元に制御対象を制御するための制御入力ＵC を算出する演算手段とを備えるようにする。また、演算手段３２に保存されているパラメータ（ＪL 、ＣS 、……）を、制御対象の状態に応じて更新するようにする。目標関数ＸT から制御量までの伝達関数を１とするような制御をすることにより、高精度な制御を行うことができる。また微分操作のない逆システムを構成したことにより、容易かつ安定に最適制御入力ＵC を得ることができる。」という技術が開示されている（特許文献１）。
また、特許文献２に開示されている方法では、制御系の出力θlから位置指令θmrefへの関係を表す逆伝達関数の分母多項式が定数の場合、各軸の負荷軸先端の参照位置関数θirefと制御系の出力θlが一致するような位置指令θmrefを算出する方法が提案されている。即ち「教示したある点から次の教示点へ位置の補間を行い、各軸のサーボモータの制御周期毎の位置指令θmrefを作成する位置決め制御系の指令作成方法において、制御系の出力θlから位置指令θmrefへの関係を表す逆伝達関数の分母多項式が定数の場合、各軸の負荷軸先端の参照位置関数θlrefと制御系の出力θlが一致するような位置指令θmrefを算出するために、指令生成部において参照位置関数θlrefを前記逆伝達関数の分子多項式の次数より少なくとも一次は高次で、かつ位置指令θmrefを構成する前記参照位置関数θlrefとその導関数が定数項を持たないように設定する。また、逆伝達関数の分母多項式が１次以上の多項式の場合、各軸の負荷軸先端の参照位置関数θlrefと制御系の出力θlが一致するような位置指令θmrefを算出するために、指令生成部において前記参照位置関数θlrefを逆伝達関数の分子多項式の次数より少なくとも１次は高次に設定し、逆伝達関数の分母多項式に相当する伝達関数のフィルタを設ける。このようにすることで、参照位置関数θlrefと制御系の出力θlが一致するような位置指令θmrefを算出することができる。」という技術が開示されている（特許文献２）。
（２）オフライン指令生成
オフラインでの指令生成は、ティーチング・プレイバック方式のように、予め指令が既知であり、指令通りの軌跡を取るように、モータで実作業させる前に事前に指令を修正しておく方法である。
オフラインでモータに指令通りの軌跡を取らせる方法としては、例えば、特許文献３に開示されたものがある。特許文献３に開示されている方法では、ロボットを教示点通りの軌跡に沿って動作させるため、予め教示されたポイント以外に見込み点と呼ばれるポイントを、加減速と速度とコーナー点のなす角の情報から軌跡の内外側に仮想的に設ける方法が提案されている。即ち「ロボットに動作位置を教示するロボット位置教示方式において、予め教示された、コーナ部分を形成するコーナ点とその前後の２点、前記コーナ点前後での加減速時定数及び前記コーナ部分での教示速度を読み取る教示点等読み取り手段と、前記教示点等読み取り手段が読み取った前記コーナ点とその前後の２点、前記加減速時定数及び前記教示速度を用いて、前記コーナ部分での加減速開始点と加減速終了点、及び前記ロボットの実際の動作軌跡が前記コーナ部分に沿ったものとなるように前記コーナ点に代えて内部的に教示する見込み点を演算する見込み点等演算手段と、前記見込み点等演算手段が求めた前記加減速開始点、前記加減速終了点及び前記見込み点を教示点として再記憶する教示点再記憶手段と、を有することを特徴とするロボット位置教示方式」という技術が開示されている（特許文献３）。
【特許文献１】特開平５−１４３１０６号公報
【特許文献２】特開平１０−１４９２１０号公報
【特許文献３】特開平７−３２２７９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
ところが、実時間での指令生成である前記従来例の特許文献１や特許文献２公報では、指令の時間関数が既知であるという必要条件があり、位置指令が予め決められてしまい、任意の指令に対応できるものではない。また、制御対象の逆伝達関数を必要とするため、系の変更に柔軟に対応することができない。これは、例えば、減速機のバネ項が原因で振動が生じる場合、その対策としてオブザーバによる制振を実施する必要がある。しかし、オブザーバがシステムに追加されることで、逆伝達関数を解き直す必要があり、その場合の演算式も複雑になるという問題があった。
また、オフラインでの指令生成である前記従来例の特許文献３では、外部に設けたセンサ情報に応じて逐次生成される指令には対応できず、突発的に作用する不確定な外乱にも対応できないという問題があった。更には、前述の実時間とオフラインの両方の指令生成に対応できる方法は無かった。
そこで本発明はこれら問題点を解決するためになされたものであり、その目的とするところは、フィルタの遅延やサーボ系の応答遅れがある場合にも、運動指令からの追従遅れや動力学特性による運動指令からのズレを生じさせることなく指令通りに応答し、簡単な演算式により逐次生成され、リアルタイムで変化する指令や外乱に対しても、対応できるモータの制御装置及び制御方法を提供することにある。
【課題を解決するための手段】
【０００４】
本発明では、上記問題点を解決するため、請求項１記載のモータの制御装置では、モータにより駆動される機構部を有するモータの制御装置において、前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記シミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、前記シミュレーション部の状態量に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように指令修正量を作成する指令修正量演算部と、前記指令修正量で前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする。
【０００５】
また、請求項２記載のモータの制御装置では、前記上位指令生成部からの指令の時間微分量の変化に応じて、前記指令修正量に１以下の減衰率を乗算する修正量調整部を備えたことを特徴とする。
【０００６】
また、請求項３記載のモータの制御装置では、前記上位指令生成部からの指令の時間微分量の変化に応じて、前記指令修正量の位相を進ませる修正量調整部を備えたことを特徴とする。
【０００７】
また、請求項４記載のモータの制御装置では、モータにより駆動される機構部を有するモータの制御装置において、前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部とを有するシミュレーション部と、前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記シミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、前記シミュレーション部の状態量と前記実機部の状態量を比較する状態量比較部を備えたことを特徴とする。
【０００８】
また、請求項５記載のモータの制御装置では、前記状態量比較部の出力である状態量比較値が予め設定されたしきい値を越える場合にはアラームを出力することを特徴とする。
【０００９】
また、請求項６記載のモータの制御装置では、前記状態量比較値に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように指令修正量を作成する指令修正量演算部と、前記指令修正量から前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする。
【００１０】
また、請求項７記載のモータの制御装置では、モータにより駆動される機構部を有するモータの制御装置において、前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部とをそれぞれ有する第１のシミュレーション部と第２のシミュレーション部とを備え、前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記第１のシミュレーション部及び前記第２のシミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、前記第１のシミュレーション部の状態量に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように第１の指令修正量を作成する第１の指令修正量演算部と、実機部及び前記第２のシミュレーション部に、前記第１の指令修正量で前記上位指令生成部からの指令を修正する指令修正部と、前記第２のシミュレーション部の状態量と前記実機部の状態量を比較する状態量比較部とを備えたことを特徴とする。
【００１１】
また、請求項８記載のモータの制御装置では、前記第２のシミュレーション部の状態量と前記実機部の状態量を比較する状態量比較部と、前記状態量比較部の出力である状態量比較値に基づいて、前記モータの応答が前記上位指令生成部からの指令と一致するように第２の指令修正量を作成する第２の指令修正量演算部と、前記第１及び第２の指令修正量から前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする。
【００１２】
また、請求項９記載のモータの制御装置では、前記上位指令生成部からの指令の時間微分量の変化に応じて、前記第１の指令修正量に１以下の減衰率を乗算する第１の修正量調整部と、前記第２の指令修正量に１以下の減衰率を乗算する第２の修正量調整部を備えたことを特徴とする。
【００１３】
また、請求項１０記載のモータの制御装置では、前記上位指令生成部からの指令の時間微分量の変化に応じて、前記第１の指令修正量の位相を進ませる第１の修正量調整部と、前記第２の指令修正量の位相を進ませる第２の修正量調整部を備えたことを特徴とする。
【００１４】
また、請求項１１記載のモータの制御装置では、制御装置に付属した操作ペンダントの画面上に、前記実機部と前記第１のシミュレーション部と前記第２のシミュレーション部の状態量を、関節軸単位あるいは直交座標で表示し、前記実機部と前記第１のシミュレーション部と前記第２のシミュレーション部の制御パラメータを調整する
ことを特徴とする。
【００１５】
また、請求項１２記載のモータの制御装置では、複数回、同一の軌跡を動作する場合に、前周回の指令修正量を記憶する指令修正量記憶部と、次周回時に前記指令修正記憶値で前記上位指令生成部からの指令を修正する指令修正部とを備えたことを特徴とする。
【００１６】
また、請求項１３記載のモータの制御方法では、モータにより駆動される機構部を有するモータの制御装置において、前記モータを模擬したモータモデルと、前記機構部を模擬した機構モデル部と、前記モータ制御部を模擬した制御モデル部で構成されたシミュレーション部を有し、前記モータ制御部及び前記モータ及び前記機構部を含む実機部及び前記シミュレーション部は並列に配置され、上位指令生成部からの指令をそれぞれに入力し、前記モータの応答が前記上位指令生成部からの指令と一致するように指令を修正する指令修正量を作成し、前記指令修正量で前記上位指令生成部からの指令を修正することを特徴とする。
【発明の効果】
【００１７】
請求項１のモータの制御装置によれば、実機部及びシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力し、前記シミュレーション部の状態量に基づいて、モータの応答が前記指令と一致するように指令修正量を作成することにより、フィルタの遅延やサーボ系の応答遅れがある場合にも、運動指令からの追従遅れや動力学特性による運動指令からのズレが生じずに指令通りに応答し、逐次生成される指令に対応できる。
請求項２及び３記載のモータの制御装置によれば、指令の時間変化に応じて指令修正量
を減衰又は位相進みを施すことにより、サーボ系の応答遅れに対するズレ量を小さくすることができる。
請求項４及び５記載のモータの制御装置によれば、実機部及びシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力し、実機部とシミュレーション部の状態量を比較することで、補償状態の是非を自動的に判断させることもでき、状態量の比較値が設定値以上になった場合には外部にアラームを発生させることで、外乱の検出やシミュレーション部の制御パラメータ異常の検出を自動的に実施できる。
請求項６記載のモータの制御装置によれば、前記シミュレーション部と前記実機部の状態量に基づいて、モータの応答が前記指令と一致するように指令修正量を作成することにより、モデル化誤差や実機部10への外乱を補償できる。
請求項７及び８記載のモータの制御装置によれば、実機部及び第１のシミュレーション部及び第２のシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力する。第１のシミュレーション部の状態量からモータの応答が前記指令と一致するように指令修正量を作成することで、フィルタの遅延やサーボ系の応答遅れがある場合にも、運動指令からの追従遅れや動力学特性による運動指令からのズレが生じずに指令通りに応答し、逐次生成される指令にも対応できる。また、第２のシミュレーション部と実機部の状態量からモータの応答が前記指令と一致するように指令修正量を作成することで、予期せぬ外乱が作用する場合にも同時に対応できる。更に、２つのシミュレーション部を使い、第１のシミュレーション部からの指令修正量を第２のシミュレーション部に入力して、実機を動作させずに指令の補償状態を確認することで、実機の暴走の危険性がなく、補償状態の調整も容易に実施することができる。
請求項９及び１０記載のモータの制御装置によれば、指令の時間変化に応じて第１及び第２の指令修正量演算部からの指令修正量を減衰又は位相進みを施すことにより、サーボ系の応答遅れに対するズレ量を小さくすることができる。
請求項１１記載のモータの制御装置によれば、
操作ペンダント上の画面に内部状態量を表示することで、作業者が直感的定量的に補償状態を把握でき、補償パラメータを容易に調整することができる。
請求項１２記載のモータの制御装置によれば、複数回、同一の軌跡を動作する場合に、前周回の修正量を記憶する指令修正量記憶部を備えたことにより、制御装置の演算負荷が掛からずに、指令修正量を複数平均することで、より高精度に位置指令に対する応答を補償することができる。
請求項１３記載のモータの制御方法によれば、実機部及びシミュレーション部を並列に配置し、上位指令生成部から同一の指令を入力し、前記シミュレーション部の状態量に基づいて、モータの応答が前記指令と一致するように指令修正量を作成することにより、フィルタの遅延やサーボ系の応答遅れがある場合にも、運動指令からの追従遅れや動力学特性による運動指令からのズレが生じずに指令通りに応答し、逐次生成される指令に対応できる。
【発明を実施するための最良の形態】
【００１８】
本発明の第１の基本構成を図１に示して説明する。ここでは、説明の簡略化のため、本制御装置で駆動するモータは１軸とする。
図１において、実機部10は、機構部12と、前記機構部12に接続されたモータ13と、前記モータ13を制御するモータ制御部11で構成されている。シミュレーション部20は、前記モータ13の動力学系を模擬したモータモデル23と、前記機構部12の動力学系を模擬した機構モデル部22と、前記モータ制御部11を模擬した制御モデル部21で構成されている。
モータを制御する制御装置内で、前記実機部10及び前記シミュレーション部20は並列に配置されており、それぞれ上位指令生成部50から同一の指令を入力とする。前記実機部10及び前記シミュレーション部20では、入力された指令に応じて機構部12及び機構部モデル22を制御するためのトルク指令をそれぞれモータ13及びモータモデル23に出力する。
ここで、前記シミュレーション部120でトルク指令を求める演算は機構部112を剛体モデルとすると、以下の式１〜３を解くことができ、演算量が少ないために制御装置のＣＰＵの負担にならず、マルチタスク構成のＣＰＵを使用すれば、実機部の制御と並列して演算することもできる。
【００１９】
Tref = Ｊ*Kv*( Kp*(θref - θfb ) - Ｖfb)          ・・・（式１）
ここで、
Tref：トルク指令
Ｊ：モータのロータイナーシャ＋機構部のイナーシャ（減速機を含む）
Kv：速度ループゲイン
Kp：位置ループゲイン
θref：上位指令生成部50から位置指令
θfb：位置フィードバック
Ｖfb：速度フィードバック
【００２０】
Ｖfb -= Σ( dt * Tref / J )                         ・・・（式２）
ここで、
dt：演算周期時間
Σ：積分
【００２１】
θfb = Σ( dt * Ｖfb )                         ・・・（式３）
【００２２】
また、機構部112を２慣性などの高次モデルで演算したり、摩擦モデルや重力モデルを追加したりすれことで、よりシミュレーション部と実機部の一致度を上げて使用することもできる。
ここで、シミュレーション部20では実機部10のサーボ系の遅れや機構の特性などを含んだモータの運動特性を反映できているので、実機部10とシミュレーション部20の内部状態量（位置指令、位置フィードバック、位置偏差、速度指令、速度フィードバック、速度偏差など）は、ほぼ一致する。そこで、指令修正量演算部41内において、シミュレーション部20のサーボ系の遅れやモータの運動特性によって生じる指令に対する応答の偏差を求めることができる。次に指令修正部42において、この偏差を指令修正量として実機部10への指令に補償することで、上位指令生成部50からの指令に対する実機部10の応答は、偏差の無いものとすることができる。
本構成では、リアルタイムに位置指令が変化するような場合や指令軌道が予め分からないような場合でも、実機部10とシミュレーション部20とが一致していれば、実機部10の応答遅れをシミュレーション部20で補償できるため、逐次生成される指令に対しても対応することができる。同様に、産業用ロボットなどのティーチング・プレイバックによる指令生成の場合でも、幾つかのポイントを教示して、その教示点間の補間方法を指定することで、ロボット各軸のモータへの指令を発生させている。そのため、関節軸単位でのモータへの指令という点で、前述の方法で対応可能である。
また、実機側にオブザーバなど外乱や振動特性を改善させた場合でも、シミュレーション部20には外乱が作用しないために、特に制御構成を変更する必要が無い。
【実施例１】
【００２３】
以下に、本発明の第１の具体的実施例を図２に示して説明する。図２が図１と異なる部分は、モータモデル２３から出力される位置フィードバックが指令修正量演算部４１へ入力されている部分および上位指令制御部から出力される位置指令が指令修正量演算部４１へ入力されている部分である。
前記モータ制御部11と前記制御モデル部21は、例えば図１５に示す位置比例−速度比例積分制御を用いる。
本実施例では、前述の基本構成において、指令に対する応答の偏差として位置偏差を用いるものである。具体的には、シミュレーション部20に入力された位置指令と、モータモデル23における位置フィードバックを、指令修正量演算部41に入力する。指令修正量演算部41内では、位置指令から位置フィードバックを減算することで位置偏差を作成する。ここで、補償状態に応じて、位置偏差に予め決められたゲインを乗算しても良い。ゲイン乗算後の位置偏差は、指令修正量として指令修正部42に送られる。指令修正部42では、上位指令生成部50からの位置指令と前記指令修正量を加算して修正位置指令を作成する。前記修正位置指令は、実機部10に入力されてモータ制御部11によってトルク指令に変換され、モータ13が駆動され、機構部12が動作する。
次に本実施例の効果を、図３を用いて説明する。
図３(a)に示すように、実機部10やシミュレーション部20の位置速度制御ループでは、位置指令に対する位置フィードバックは、サーボ系の遅れやモータの運動特性によって生じる遅れ（位置偏差）を持っている。
【００２４】
位置偏差＝位置指令−位置フィードバック              ・・・（式４）
【００２５】
修正位置指令＝位置指令＋位置偏差                    ・・・（式５）
【００２６】
そこで、図３(b)に示すように、この位置偏差分を予め指令修正量として位置指令に加算することで、修正位置指令を作成することができる。よって、前記修正位置指令を実機部10に入力することで、修正位置指令に対する修正位置フィードバックは、図３(c)に示すように上位指令生成部50からの位置指令とほぼ一致したものとなる。
【実施例２】
【００２７】
以下に、本発明の第２の具体的実施例を、図４及び５を用いて説明する。
前記第１の実施例では、指令修正量を上位指令生成部50からの位置指令に単純に加算していた。本実施例では図４に示すように、修正量調整部43を設け、上位指令生成部からの指令の時間微分量の変化に応じて、指令修正量演算部41の出力である指令修正量に１以下の減衰率を乗算するか、又は前記指令修正量の位相を進ませるようにした点が異なっている。
実機部10とシミュレーション部20におけるサーボ系の遅れやモータの運動特性によって生じる遅れが比較的大きい場合、実機部10とシミュレーション部20を同じ時間軸で実行すると、シミュレーション部20と指令修正量演算部41で生成された指令修正量は、上位指令生成部50からの位置指令に対してある一定時間の遅れを持つようになる。この時間遅れは、位置指令と位置応答のサーボ系の応答時間によるもので、上位指令生成部50から位置指令が入力されて、実際にモータが動き出して位置応答に変化が現れるまでの遅れである。
この時間遅れを持った指令修正量を上位指令生成部50からの位置指令に加算すると、図３(c)に示すように上位指令生成部50から位置指令に対して位置応答のオーバーシュートが発生してしまう。このオーバーシュートは加減速時の軌跡ズレを生じる原因となるため、補償する必要がある。
補償方法は、上位指令生成部50から入力される指令の微分値が、連続系でない場合と連続系である場合で分けられる。
（１）連続系でない場合
指令修正量演算部41の出力である指令修正量に１以下の減衰率を乗算することで、上記時間遅れによるオーバーシュートを補償するものである。
修正量調整部43では、上位指令生成部50から位置指令の時間微分量の変化を観測し、その変化に応じて減衰率を変更する。具体的には、時間微分量の変化が一定である場合、すなわち速度変化がない場合には、減衰率を１として指令修正量演算部41の出力である指令修正量をそのまま払い出す。時間微分量の変化が一定でない場合、すなわち速度変化がある場合には、減衰率を１より小さい値として指令修正量演算部41の出力である指令修正量を減衰させながら払い出す。
図５(a)に示す、時刻ａの時点で指令がランプ状に入力されて、時刻ｂで一定になる位置指令が入力された場合を例に説明する。
速度変化がある点は、時刻ａの前後と時刻ｂの前後である。時刻ａの前後では、速度指令が０からあるステップ状の速度指令に変化する。同様に、時刻ｂの前後では、あるステップ状の速度指令から速度指令が０に変化する。このように指令に時間変化があることは、時間微分量の変化が一定であるか、そうでないかを監視することで判断できる。
ここで、図３(c)に示すように、時刻ａの前後では上位指令生成部50から位置指令に対して、修正位置フィードバックの応答はオーバーシュートにはなっていないが、時刻ｂの前後では修正位置フィードバックの応答はオーバーシュートしている。そのため、図５(b)に示すように、上位指令生成部50から位置指令を実時間で監視することで時刻ｂになることを判断し、修正量調整部43において１より小さい減衰率で指令修正量を減衰させることで、修正位置指令も時刻ｂを通過した時点で減衰を開始する。以上より、図５(c)に示すように、修正量の減衰処理後の修正位置指令に対する修正位置フィードバックは、時刻ｂの前後でオーバーシュートすることなく、上位指令生成部50からの位置指令とほぼ一致したものとなる。
（２）連続系の場合
指令修正量演算部41の出力である指令修正量を、予め設定された時間だけ位相を進ませることで、前記時間遅れによるオーバーシュートを補償するものである。
図６(a)に示すような正弦波状の位置指令が入力された場合を例に説明する。
上位指令生成部50から位置指令に対して、実機部10の位置フィードバックはサーボ系の遅れなどで、振幅の減少と位相の遅れが発生した波形となる。
前記位置指令と位置フィードバックを指令修正量演算部41に入力することで、式４により指令修正量が得られ、式５により指令修正部42で前記指令修正量を前記位置指令に加算することで、図６(b)に示す修正位置指令が得られる。この修正位置指令に対する実機部10の位置フィードバックは、破線の修正位置フィードバックとなる。ここで、修正位置フィードバックは、指令に対する振幅の減少は小さくなっているが、位相の遅れが残る。これは、指令が時間的に変化した時点からの遅れは、補償できていないためである。そこで、予め設定された時間Δｔで前記修正量の位相を進ませることで、前記位相の遅れを解消するものである。以上より、図６(c)に示すように、修正量の位相進み後の修正位置指令に対する修正位置フィードバックは、上位指令生成部50からの位置指令とほぼ一致したものとなる。
【００２８】
次に、本発明の第２の基本構成を図７に示して説明する。ここでも、説明の簡略化のため、本制御装置で駆動するモータは１軸とする。
図７において、実機部10は、機構部12と、前記機構部12に接続されたモータ13と、前記モータ13を制御するモータ制御部11で構成されている。シミュレーション部30は、前記モータ13の動力学系を模擬したモータモデル33と、前記機構部12の動力学系を模擬した機構モデル部32と、前記モータ制御部11を模擬した制御モデル部31で構成されている。
モータを制御する制御装置内で、前記実機部10及び前記シミュレーション部30は並列に配置されており、それぞれ上位指令生成部50から同一の指令を入力とする。前記実機部10及び前記シミュレーション部30では、入力された指令に応じて機構部12及び機構部モデル32を制御するためのトルク指令をそれぞれモータ13及びモータモデル33に出力する。
ここで、シミュレーション部30では実機部10のサーボ系の遅れや機構の特性などを含んだモータの運動特性を反映できていれば、実機部10とシミュレーション部30の内部状態量（位置指令、位置フィードバック、位置偏差、速度指令、速度フィードバック、速度偏差など）は、ほぼ一致する。そこで、状態量比較部41内において、前記実機部10及び前記シミュレーション部30の内部状態量を比較することで、前記実機部10及び前記シミュレーション部30の一致度を判断することができる。
【実施例３】
【００２９】
以下に、本発明の第３の具体的実施例を図８に示して説明する。
本実施例では、前述の第２の基本構成において、内部状態量を位置フィードバックとして、外部にアラームを発生させる構成が違う。
状態量比較部41において、内部状態量の比較値、例えば前記実機部10と前記シミュレーション部30の位置フィードバックの偏差が、予め決められたしきい値よりも大きくなった場合には、主に２つに理由が考えられる。
第１の理由として、前記シミュレーション部30の制御パラメータが実機部10と一致しておらず、サーボ系の遅れや機構の特性などを含んだモータの運動特性を反映できていないことが考えられるので、自動的に外部にアラームを発生させる。第２の理由として、実機部10に予期せぬ外乱が作用して、位置フィードバックの偏差が大きくなったことが考えられるので、この場合も自動的に外部にアラームを発生させる。
【実施例４】
【００３０】
以下に、本発明の第４の具体的実施例を図９に示して説明する。
本実施例では、前述の第３の具体的実施例において、状態量を比較することで、予期せぬ外乱が作用する場合でも、モータの応答が前記上位指令生成部50からの指令と一致するように指令修正量を作成する構成が違う。
具体的には、指令修正量演算部411を設ける。この指令修正量演算部411に、シミュレーション部30のモータモデル33の位置フィードバックと、実機部10のモータ13に接続された位置検出器の出力である位置フィードバックを入力し、シミュレーション部30の位置フィードバックから実機部10の位置フィードバックを減算することで位置フィードバック偏差を作成する。ここで、補償状態に応じて、位置偏差に予め決められたゲインを乗算しても良い。ゲイン乗算後の位置フィードバック偏差は、指令修正量として指令修正部42に送られる。指令修正部42では、上位指令生成部50からの位置指令と前記指令修正量を加算して修正位置指令を作成する。前記修正位置指令は、実機部10に入力されてモータ制御部11によってトルク指令に変換され、モータ13が駆動され、機構部12が動作する。
実機部10に予期せぬ外乱が作用する場合、シミュレーション部30には外乱が作用しないので、実機部10とシミュレーション部30の位置フィードバックの偏差を求めることで、外乱による位置偏差を抽出することができ、この位置偏差を指令修正量として、指令修正部42で前記上位指令生成部50からの指令に加算することで、前記モータの応答が前記上位指令生成部50からの指令と一致するようになる。
【００３１】
次に、本発明の第３の基本構成を図１０に示して説明する。
本実施例は、前記第１と第２の基本構成を組み合わせた点が異なる。ここでも、説明の簡略化のため、本制御装置で駆動するモータは１軸とする。
実機部10とシミュレーション部20は同じ位置指令を入力して、同じ内部状態量を持たせるように、シミュレーション部20を構成していたが、実機部10への指令に対して修正量調整部43や指令修正部を設けたことで、実機部10とシミュレーション部20への指令に違いが生じている。そのため、実機部10とシミュレーション部20とで、内部状態量を比較することができない。
そこで、実機部10への指令と同じ指令を入力する第２のシミュレーション部30を別途設ける。この第２のシミュレーション部30には、指令修正部422を設ける。指令修正部422において、指令修正量演算部41からの出力である指令修正量と上位指令生成部50からの位置指令を加算し、第２のシミュレーション部30の制御モデル部31に入力する。そして、状態量比較部44において実機部10と第２のシミュレーション部30との内部状態量（ここでは位置フィードバックとする）を比較することで、指令修正量演算部41による補償状態の是非を判断させることができる。
【実施例５】
【００３２】
以下に、本発明の第５の具体的実施例を図１１に示して説明する。
本実施例は、前記第３の基本構成に第２の指令修正量演算部411と第１の修正量調整部を設けた点が異なる。
実機部10と第２のシミュレーション部30を同じ構成を取らせた場合でも、モデル化誤差があったり、実機部10に外乱が作用したりして、必ずしも実機部10と第２のシミュレーション部30との内部状態量の比較結果が一致しない。そこで、第２の指令修正量演算部411において、実機部10と第２のシミュレーション部30との状態量である位置フィードバックの偏差を第２の指令修正量とする。そして、指令修正部42において、前記上位指令生成部50からの指令に加算することで、モデル化誤差や実機部10への外乱を補償し、指令に対する応答を一致させることができる。
ここで、第２の具体的実施例で述べたように、第１のシミュレーション部20に対して実機部10は、第２の具体的実施例で述べたように、時間調整部43により時間差がある。そこで、実機部10と第２のシミュレーション部30を同じ入力にするために、修正量調整部43で上位指令生成部50からの位置指令に対して、減衰又は位相進みを持たせ、実機部10と第２のシミュレーション部30と内部状態量を出来る限り、同じにする。
本実施例の効果を、図１２を用いて説明する。
図１２(a)に示すように、実機部10やシミュレーション部20の位置速度制御ループでは、位置指令に対する位置フィードバックは、サーボ系の遅れやモータの運動特性によって生じる遅れ（位置偏差）を持っている。
（１）第１の修正
具体的実施例２の処理により、図１２(b)に示すように、この位置偏差分を予め指令修正量として位置指令に加算することで、修正位置指令を作成する。この時、修正位置指令に対する修正位置フィードバックは前記上位指令生成部50からの指令に近似したものになるが、モデル化誤差や実機部10への外乱により若干のズレを生じている。
（２）第２の修正
第２の指令修正量演算部411によって、実機部10の位置フィードバックと第２のシミュレーション部30での位置フィードバックの位置フィードバック偏差から上記ズレを作成し、指令修正部42で実機部10への指令に加算する。この値は第２修正後の位置指令となる。
以上の２つの補償により、図１２(c)に示すように、第２修正後の位置指令に対する実機部10の応答（第２修正後の実機の位置フィードバック）は、オリジナルの指令と一致したものとなる。
２つのシミュレーションを設け、それぞれで指令修正を行うことにより、第１のシミュレーション部の状態量からモータの応答が前記指令と一致するように指令修正量を作成することで、フィルタの遅延やサーボ系の応答遅れがある場合にも、運動指令からの追従遅れや動力学特性による運動指令からのズレが生じずに指令通りに応答し、逐次生成される指令に対応する。更に、第２のシミュレーション部と実機部の状態量からモータの応答が前記指令と一致するように指令修正量を作成することで、予期せぬ外乱が作用する場合に対応する。
これらは独立に設定できるため、第１のシミュレーション部のみを設けて、運動指令からの追従遅れや動力学特性による運動指令からのズレを補償したり、第２のシミュレーション部のみを設けて、外乱が作用する場合の運動指令からのズレを補償したりしてもよく、また必要に応じて選択できる構成でも良い。
【実施例６】
【００３３】
以下に、本発明の第６の具体的実施例を図１３に示して説明する。
本実施例は、前記第５の具体的実施例で、前記実機部10及び前記第１のシミュレーション部20及び第２のシミュレーション部30内の内部状態量を作業者が直感的に分かりやすいように表示し、更に制御パラメータの調整を容易にした点が異なる。ここでは、第１の位置修正量演算部からの出力である第１の指令修正量のゲイン調整を例に説明する。
図１３に示すように、モータの制御装置に付属した操作ペンダント上に、上位指令生成部50からの位置指令と、本制御を有効にしない場合の実機部10の位置フィードバックと、本制御を有効にした場合の修正位置指令と位置フィードバックを表示する。ここでは、複数個のモータで構成されたロボットの操作ペンダントの一部である表示画面を示す。図１３(a)は、ＸＹ平面上で円動作を実行した際のロボット手先の軌跡（位置指令、位置フィードバック）を表示させた例である。
ここで、本制御を有効にしない場合には、運動指令からの追従遅れにより、上位指令生成部50からの位置指令に対して、実機部10の位置フィードバックはかなり内回りした軌跡を取る。そこで、本制御を有効にして、制御系の遅れを考慮して位置指令を予め外側に膨らませた修正位置指令でモータを動作させることにより、上位指令生成部50からの指令と修正位置フィードバックを一致させることができる。
また、図１３(b)は、横軸に時間、縦軸に位置を取り、モータ軸の位置指令と位置フィードバックを表示させた例である。
このように動作に応じて補償結果を表示させることで、指令修正量の補償状態の確認や修正量調整部のパラメータ調整を容易に実施することができる。
また、ロボットの教示作業時やモータの試運転時に補償の程度を調整する場合には、実機部10の制御を停止させ、実機部10の代わりに第２のシミュレーション部30を用い、シミュレーション部20と第２のシミュレーション部30の応答を比較することで、実機を動作させること無く、ロボットのプレイバック作業やモータの本運転の前に補償状態を確認することもできる。
【実施例７】
【００３４】
以下に、本発明の第７の具体的実施例を図１４に示して説明する。
本実施例は、前記第６の具体的実施例に指令修正量記憶部45を設けた点が異なる。モータが複数回、同一の軌跡を動作する場合には、同一点における毎回の修正量はほぼ一定値となる。そのため、一定周期毎における特定位置の前周回（又は初回）の修正量を記憶しておき、次周回には対応する修正量を指令に加算することで、モータ制御装置内の演算負荷を低減することができる。
指令修正量演算部41からの出力である指令修正量と、第２の指令修正量演算部411からの出力である第２の指令修正量を、一定周期毎に特定位置の指令修正量記憶部45で記憶しておく。次に、次周回で特定位置に来た場合、指令修正量記憶部45から記憶された指令修正量が払い出され、指令修正部42で位置指令と加算されることで、制御装置の演算負荷が掛からずに、位置指令に対する位置応答を補償することができる。この時、修正量の高精度化を図る場合には、同一の特定位置において複数回の平均した指令修正量を新たに指令修正量として使用する方法と、外乱に対する追従性を重視する場合には、第２の指令修正量演算部411からの出力である第２の指令修正量を実時間で使用する方法を選択できる構成にしても良い。
【産業上の利用可能性】
【００３５】
本発明は、フィルタの遅延やサーボ系の応答遅れがある場合にも、運動指令からの追従遅れや動力学特性による運動指令からのズレが生じずに指令通りに応答し、逐次生成される指令に対応できるので、高精度の産業用ロボット制御装置、位置決め制御装置、ＮＣ制御装置、半導体製造装置・液晶製造装置における精密位置決め駆動装置にも適用できる。
【図面の簡単な説明】
【００３６】
【図１】本発明の第１の基本構成図
【図２】本発明の第１の具体的実施例を表すブロック図
【図３】本発明の第１の具体的実施例における応答波形
【図４】本発明の第２の具体的実施例を表すブロック図
【図５】本発明の第２の具体的実施例における第１の応答波形
【図６】本発明の第２の具体的実施例における第２の応答波形
【図７】本発明の第２の基本構成図
【図８】本発明の第３の具体的実施例を表すブロック図
【図９】本発明の第４の具体的実施例を表すブロック図
【図１０】本発明の第３の基本構成図
【図１１】本発明の第５の具体的実施例を表すブロック図
【図１２】本発明の第５の具体的実施例における応答波形
【図１３】本発明の第６の具体的実施例を表す装置図
【図１４】本発明の第７の具体的実施例を表すブロック図
【図１５】従来の制御方式を示す図
【符号の説明】
【００３７】
１０  実機部
１１  モータ制御部
１２  機構部
１３  モータ
２０  シミュレーション部
２１  制御モデル部
２２  機構モデル部
２３  モータモデル
３０  第２のシミュレーション部
３１  第２の制御モデル部
３２  第２の機構モデル部
３３  第２のモータモデル
４１  指令修正量演算部
４１  第２の指令修正量演算部
４２  指令修正部
４２２  第２の指令修正部
４３  第１の修正量調整部
４３１  第２の修正量調整部
４４  状態量比較部
４５  指令修正量記憶部
５０  上位指令生成部
【図１】
2005094964.tif 000003
【図２】
2005094964.tif 000004
【図３】
2005094964.tif 000005
【図４】
2005094964.tif 000006
【図５】
2005094964.tif 000007
【図６】
2005094964.tif 000008
【図７】
2005094964.tif 000009
【図８】
2005094964.tif 000010
【図９】
2005094964.tif 000011
【図１０】
2005094964.tif 000012
【図１１】
2005094964.tif 000013
【図１２】
2005094964.tif 000014
【図１３】
2005094964.tif 000015
【図１４】
2005094964.tif 000016
【図１５】
2005094964.tif 000017