JP3657718B2

JP3657718B2 - Control system, acceleration pattern setting method and parameter setting method in control system

Info

Publication number: JP3657718B2
Application number: JP30451396A
Authority: JP
Inventors: 好高飯田; 次人丸山
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 1996-11-15
Filing date: 1996-11-15
Publication date: 2005-06-08
Anticipated expiration: 2016-11-15
Also published as: JPH10143249A

Description

【０００１】
【発明の属する技術分野】
本発明は，生産機械，プリンタ，磁気ディスク装置等の移動機構もしくはロボット等の移動制御をする制御システムにおいてサーボモータを駆動するための加速度パターンの設定方法に関するものである。
【０００２】
【従来の技術】
従来，サーボモータを駆動するための速度と加速度の最適パターンは，設計した速度と加速度の組合せをもとにサーボモータの台形波駆動指令（速度，加速度のパターンを台形パターンで表す）もしくはあるいはＳ字加減速駆動指令（加速度のパターンを時間軸に対して正側と負側の波形状パターン（Ｓ字パターン）で表す）を与えて，サーボモータを駆動し，実際の振動を計測して振動が少なくなるように速度，加速度の組合せを手作業で少しずつ変え，安定して最短時間で駆動できる加速度パターンを求めていた（以後，この操作をチューニングと称する）。また，加速度パターンと別に，サーボモータを高精度に動作させるためには制御ゲインの調整が必要であるが，従来，この調整は加速度パターンとは別にステップ入力等により行い，加速度のチューニングとは別の処理で行っていた。
【０００３】
【発明が解決しようとする課題】
従来の加速度パターンのチューニング方法は，速度，加速度を経験的に与え，オッシロスコープ等で動作状態を観察しながら調整していたので試行錯誤による部分が多く，調整に時間のかかるものであった。特に，ロボットでは６個のモータについて一つずつ加速度パターンを調整しなければならないので，長期間（１か月程度）を必要とする作業であった。さらに，判断を作業者が下すために調整された結果にバラツキを生じやすかった。また，設計においては非線形項を考慮していないため速度軌跡の計算値と実測値に差がでることがしばしばあり，この理由からも速度，加速度のチューニングは困難な作業であった。
【０００４】
本発明は，サーボモータ等の駆動装置を駆動するための最適加速度パターンの設定を容易に行うことができる制御システムおよび加速度パターンの設定方法，さらにそのように設定した最適加速度パターンのもとに，連続処理で制御ゲインの調整を行うことのできるパラメータ設定方法を提供することを目的とする。
【０００５】
【課題を解決するための手段】
本発明は，与えられた加速度パターンに従って被制御物の移動制御をする制御システムにおいて，被制御物に振動検出手段を備え，加速度パターンを発生し，振動検出手段の検出した振動の振動値を変数として評価関数を演算し，加速度パターンを変更して評価値を求め，該評価値に従って該振動が最小となる最適加速度パターンを求めて自動設定するようにした。
【０００６】
図１は本発明の基本構成の説明図である。
図１ (a)は基本構成である。
図１において，
１は被制御物である。
【０００７】
２は振動検出手段であり，例えば加速度センサであって，被制御物１が受ける振動を測定するものである。
５は加速度パターン設定手段であって，被制御物１に生じる振動が最小になるような加速度のパターンを求めるものである。
【０００８】
６は加速度パターン調整部であって，振動が最小になるように，加速度パターンを調整するものである（例えば，図１ (b)，図１ (c)等の加速度パターンの速度，加速度，加加速度，減速度，減減速度等を調整する）。
【０００９】
７は評価値算出部であって，振動検出手段２で測定された振動値を変数として評価値を求めるものである。
８は加速度パターン変更部であって，評価値算出部７の評価値に従って，速度，加速度を変更するものである。
【００１０】
９は加速度パターン発生部であって，加速度パターン調整部６の結果に従って，調整された加速度パターンを発生するものである。
１１は駆動制御手段であって，加速度パターンに従って駆動装置１２の駆動制御を行うものである。
【００１１】
１２は駆動装置であって，被制御物１を駆動するものである。
図１ (b)は加速度パターンの例であり，台形パターンの場合であり，横軸は時間，縦軸は速度である。
【００１２】
スタート（原点）から時刻ｔ₁までは定加速度で加速し，時刻ｔ₁から時刻ｔ₂までは定速度（加速度０）で制御し，時刻ｔ₂から時刻ｔ₃（目標位置）までは定加速度（定減速度）で制御する加速度パターンである。
【００１３】
図１ (c)は加速度パターンの別の例であり，Ｓ字パターンの場合である。横軸は時間，縦軸は加速度である。
スタート（原点）から時刻ｔ₁までは定加加速度で制御し，時刻ｔ₁から時刻ｔ₂までは定加速度で制御し，時刻ｔ₂から時刻ｔ₃までは定減減速度で制御する。さらに，ｔ₃からｔ₄までは，加速度０（定速度）で制御し，時刻ｔ₄からｔ₅までは定減減速度で制御し，さらに時刻ｔ₅からｔ₆までは定減速度で制御し時刻ｔ₆から時刻ｔ₇（目標値）では，定加加速度で制御するものである。
【００１４】
図１の本発明の原理図の動作を説明する。
加速度パターン発生部９は初期条件で与えられ加速度パターンを発生し，その加速度パターンで被制御物１は動作しているとする。加速度パターンは，例えば，図１ (b)， (c)のようなパターンである。制御装置Ａはその加速度パターンに従って被制御物１を駆動する。被制御物１は制御装置Ａで駆動され，スタート位置から目標位置に向かって移動するが，移動経路上で振動を生じる。振動検出手段２はその振動を測定し，評価値算出部７は，振動値と制御物の現在位置をもとに評価関数を計算する。評価関数は振動値を変数とするものであって，例えば，振動数が小さい程，評価値が小さくなるような関数である（以下，振動数が小さいほど評価値が小さくなる評価関数を例として説明する）。加速度パターン変更部８は速度，加速度を変更する。最小評価値は，例えば，評価目標値を定め，一定時間（以下整定時間と称する）の範囲でその評価目標値に最も近いものを求める。加速度パターン発生部９は，変更された加速度パターンを発生する。駆動制御手段１１は生成された加速度パターンに従って駆動装置１２の制御量（サンプリング毎の移動目標値）を生成し，その制御量に従って駆動装置１２は被制御物１を駆動する。
【００１５】
以上の処理を繰り返し，加速度パターン設定手段５は被制御物１の振動が最小となる加速度パターンを求める。
本発明によれば，従来手作業で行っていた最適加速度パターンの設定を自動化し，最適な速度・加速度の組合せを短時間に求めることができる。また，手作業で最適加速パターンを求める場合には，作業者により，あるいは作業毎に調整結果にバラツキを生じやすいが，本発明では，計算機により振動が最小になる加速度パターンを求めるので，調整された結果にバラツキがなく均一な特性が得られる。さらに，振動が少なく目標位置に短時間で移動する制御システムを得ることができる。
【００１６】
【発明の実施の形態】
図２は本発明の実施例のシステム構成であり，被制御物がロボットアームの場合を例として示す。
【００１７】
図２ (a)は本発明のシステム構成である。
図２ (a)において，
Ａは制御装置である。
【００１８】
５は加速度パターン設定手段である。
６は加速度パターン調整部である。
９は加速度パターン発生部である。
【００１９】
２１はモータ制御部である（図１の駆動制御手段に相当する）。
２２はサーボモータである（図１の駆動装置に相当する）。
２５はロボットアームであって，被制御物である。
【００２０】
２６は加速度センサであって，ロボットアーム２５の先端等に取りつけられていて，ロボットアーム２５の先端部の加速度を検出するものであり，振動を測定するものである。
【００２１】
２７は振動値入力部であって，加速度センサ２６の求めた加速度に基づく振動値（ロボットアームの先端に生じる加速度）を入力するものである。
２８は位置検出部であって，サーボモータの回転数等をエンコードし，ロボットアーム２５の位置を判定するものである。
【００２２】
３０は入出力装置であって，最大速度，最大加速度，加速度パターンの種類等の初期値を入力したり，あるいは求めた加速度パターン，応答特性等を出力するものである。
【００２３】
図２ (b)は加速度パターン設定手段，モータ制御部の実施例の構成である。
３３はＣＰＵ，３４はＲＡＭである。３６は加速度パターン発生プログラムである。３７は加速度パターン調整プログラムである。３８は入出力制御プログラムである。３９はモータ制御プログラムである。
【００２４】
加速度パターン調整部６で評価計算をする関数の例は次のとおりである。

但し，Ｗ：重み係数
α（ｔ）：加速度センサ出力
ｒｅｆ：目標値もしくは理想的な応答波形（シミュレーションにより求める時）
Ｘ（ｔ）：位置の実測値である。
【００２５】
Ｔ：時間であって，スタートからの時間もしくは（暫定時間−ｔ）である。
ｉは各サンプル周期の番号を表す
図２の構成を説明する。
【００２６】
初期値で与えられる加速度パターンに従って，各部が動作しているとする。加速度センサ２６はロボットアーム２５が移動する時に生じる振動の加速度を検出する。振動値入力部２７は加速度センサ２６の検出する加速度を求める。また，位置検出部２８はサーボモータの回転数をエンコードしロボットアーム２５の移動量（移動位置）を求める。
【００２７】
加速度パターン調整部６はロボットアーム２５の先端の振動値とその位置のデータを入力する。そして，加速度パターン調整部６において，評価値算出部（図１参照）は，評価値を上記評価関数により求める。加速度パターン変更部（図１参照）は評価値が最小と判定されるかどうか判断する。そして，評価値が最小であると判定されない場合には，速度，加速度，あるいは加加速度等を変更し，加速度パターン発生部９は加速度パターンの変更された加速度パターンを発生する。
【００２８】
モータ制御部２１は変更された加速度パターンに従ってサンプリング毎の目標位置を発生し，サーボモータ２２を駆動する。サーボモータ２２に駆動されロボットアーム２５が目標位置に向かって移動する。
【００２９】
上記の処理を一定時間繰り返し，評価値が最小となる加速度パターンを求め，最適加速度パターンとして設定する。
図３は本発明の加速度パターン調整部の実施例である。
【００３０】
図３において，
５は加速度パターン設定手段である。
６は加速度パターン調整部である。
【００３１】
６’は偏差検出手段であって，ロボットアームの最終目標値ｒｅｆとロボットアームの先端の位置Ｘ（ｔ），加速度センサの振動値（加速度α（ｔ））を入力するものである。
【００３２】
７は評価値算出部であって，最終目標値ｒｅｆと位置Ｘ（ｔ）との差および，，振動値（加速度α（ｔ））に従って，前述の評価関数により加速度を求めるものである。
【００３３】
８は加速度パターン変更部であって，評価値に基づいて加速度パターンの変更をする必要の有無を判定し，変更する必要がある場合には速度，加速度，加加速度等を変更するものである。
【００３４】
９は加速度パターン発生部である。
図３の構成の動作を説明する。
偏差検出手段６’は目標値（ｒｅｆ），位置Ｘ（ｔ），振動値（加速度α（ｔ））を入力し，評価値算出部７は前述の評価関数に従って評価値を求める。
【００３５】
評価関数は，振動（加速度α（ｔ））が小さい程，関数値（評価値）が小さくなる。加速度パターン変更部８は評価値をもとに加速度パターンを変更する必要があるか，あるいはないかを判定する。加速度パターンの変更を必要とする場合には，加速度パターンを少し変更し（変更方法については後述する），加速度パターン発生部９は変更された加速度パターンを発生する。モータ制御部２１は，その加速度パターンに基づいて，サンプリング毎の移動目標値を求め，サーボモータを駆動する。
【００３６】
加速度パターン変更部における最小評価値の判定および加速度パターンの変更方法は次のとおりである。
まず，評価値の目標値を定めて置く。そして，整定時間の範囲内で加速度パターンを変更し，評価値の目標値に一番近い評価値を最小と判断する。あるいは，整定時間が終了する前に，一定の範囲内で評価値の目標値に近いものが見つかったら，その評価値を最小と判断する。そして，加速度パターンの変更は次のように行う。
【００３７】
例えば，台形の加速度パターンを求める場合，加速度，最大速度，減速度の次のような組合せを考える。そして，速度，加速度のパラメータ値を少し増加もしくは減少させた次のような組合せについて，各加速度パターンを求め，それぞれのパターンにより被制御物を駆動する。そして，それぞれの加速度パターンの評価値を算出する。
【００３８】
▲１▼ 加速度を増加する。最大速度を増加する。減速度を増加する。
▲２▼ 加速度を増加する。最大速度を減少する。減速度を増加する。
▲３▼ 加速度を減少する。最大速度を増加する。減速度を減少する。
【００３９】
▲４▼ 加速度を減少する。最大速度を減少する。減速度を減少する。
その評価値のうち，評価値が小さい一つもしくは複数の加速度パターンを求める。そして，その選んだ数点の加速度パターンについて同様の処理をくり返し，最小の評価値の加速度パターンを求め，それを最適加速度パターンとして設定する。
【００４０】
なお，上記の加速度と減速度は符号が異なるとした場合の組合わせであって，それぞれの絶対値が異なるようにする場合に，８通りの組合せが考えられ，その中から評価関数を小さくする組合せを一つもしくは複数選択する。あるいは，パラメータの一つのみを変更し，他は，変更しないようにして評価関数の変化を判定するようにしても良い。
【００４１】
図４〜図８を参照して，本発明の加速度パターンの発生方法について説明する。
図４は加速度パターンの例であり，台形パターンを示す。
【００４２】
台形パターンの生成に必要なパラメータは速度と加速度であり，最大速度（Ｖｍａｘ）と最大加速度ａ₀を条件として与える。
台形パターンを３つの領域（(1) ，(2) ，(3) ）に分割して考える。
【００４３】
台形パターンの発生のロジックは次の通りである。
(1) 加速段階
一定加速を行い，最大速度（Ｖ_max）に達すると定速度段階(2) に移行する。
但し，位置成分（例えば，ロボットアームの位置）が全工程（開始位置から目標位置まで）の１／２に達したら，規定時間定速度を維持したあと直ちに減速段階に移行する。
【００４４】
(2) 定速度段階
加速を行わず一定の速度で移動する。目標までの距離が(1) に要した距離になったら，減速段階に移行する。但し，位置成分（スタートからの移動距離）が全工程の１／２に達したら直ちに減速段階に移行する（図４ (b)参照）。
【００４５】
(3) 減速段階
一定減速を行い，速度が０（ゼロ）に達したら，加速度パターンの生成を終了する。但し，パターンの生成は，離散的な演算であり，最終目標位置に対して誤差を持つことが考えられるので，次に，目標位置調整段階の処理を行う。
【００４６】
(4) 目標位置調整段階
(3) の段階で計算された位置（Ｘ）が最終目標（ｒｅｆ）近傍（差がε）未満）に達したら，残差をｎ当分し，サンプリング毎に加算する。残差がλ未満となったら目標位置に到達したとみなし，次の制御安定待ち段階に移行する。
【００４７】
(5) 制御安定待ち段階
サンプリングされたサーボモータの回転数のエンコーダ値と目標位置との偏差（ｅ）を確認し，偏差がλ₂未満となり，かつそれが一定期間（１サンプリングの間）連続したら制御が安定したとみなし，制御を完了する。
【００４８】
図５は上記の関係をまとめたものである。
図５において，Ｖ₀は初期速度である。Ｘ₀は初期位置である。Ｖ_kはＫ番目のサンプリングの速度である。Ｖ_maxは最大速度である。ｒｅｆは最終目標位置である。Ｔは開始からの時間である。
【００４９】
図６，図７，図８によりＳ字パターンの生成方法について説明する。
図６はＳ字パターンの例である。
Ｓ字パターンの生成に必要なパラメータのうち，最大加速度（Ａ_max），加加速度Ａ’および最大速度Ｖ_maxはあらかじめ与えておく。
【００５０】
生成する加速度のパターンを７つの領域に分割して考える。
(1) 加加速段階
一定のレート（加加速度）で加加速を行い，最大加速度に達すると定加速度段階に移行する。但し，位置成分が全工程の１／２に達したら，規定時間定速度を維持したあと直ちに(8) の段階に移行する。移行に先立ち，現状の加速度を符号反転する。
【００５１】
(2) 定加速度段階
加加速を行わず，一定の加速度で移動する。速度成分（Ｖ）が最大速度に達する工程のうち(1) に要した加速期間を除いた値に達したら減減速段階に移行する。但し，位置成分が全工程の１／２に達したら，規定時間定速度を維持したあと，直ちに(7) の段階へ移行する。移行に先立ち，現状の加速度の符号を反転する。
【００５２】
(3) 減減速段階
一定の割合で減減速を行い，速度が既定値（Ｖ_max）（最大速度）に達すると，減減速段階を終了し，揃速段階に移行する。但し，位置成分が全工程のうちの１／２に達したら，規定時間だけ定速度を維持したあと直ちに(6) の段階へ移行する。移行に先立ち，現状の加速度の符号を反転する。
【００５３】
(4) 揃速度段階
速度が最大速度（Ｖ_max）になるまで，速度成分を調整する。速度成分が規定の最大速度に達したら定加速度第二段階へ移行する。
【００５４】
(5) 定速度第二段階
加速を行わず，一定の速度で移動する。最終生成パターン（位置＝Ｘ）が全工程のうち(1) に要した距離を除いた値に達したら減減速第二段階に移行する。但し，位置成分が全工程の１／２に達したら，直ちに(6) の段階へ移行する。
【００５５】
(6) 減減速度第二段階
一定の割合（加加速度）で減減速を行い，負の最大加速度（−Ａ_max）に達すると，加速度第二段階へ移行する。この段階中に位置成分が全工程の１／２に達したら，オーバーラン（位置成分の最終値が最終目標値を越える）は避けられないため，パターンの生成を続行する。
【００５６】
(7) 定加速第二段階
加加速を行わず，一定の加速度で移動する。速度成分（Ｖ）が速度ゼロに達する工程のうち(6) に要した減速期間を除いた値に達したら加加速第二段階に移行する。この段階中に位置成分が全工程の１／２に達したら，オーバラン（位置成分の最終値が最終目標値を越える）は避けられないため，パターンの生成を続行する。
【００５７】
(8) 加加速第二段階
一定の割合で加加速を行い，加速度がゼロに達したら，揃速度第二段階に移行する。この段階中に位置成分が全工程の１／２に達したら，オーバラン（位置成分の最終値が最終目標値を越える）は避けられないため，パターンの生成を続行する（(9) に移行する）。
【００５８】
(9) 揃速度第二段階
速度がゼロになるまで，速度成分を調整する。速度成分がゼロになったら目標位置調整段階に移行する。
【００５９】
(10) 目標位置調整段階
(9) の最後で計算された位置（Ｘ）が最終目標位置（ｒｅｆ）近傍（差がε未満）となったら，残差をｎ等分し，サンプリング毎に加算する。残差がλ未満となったら目標位置に到達したとみなし，制御安定待ち段階へ移行する。
【００６０】
(11) 制御安定待ち段階
サンプリングされたエンコーダ値と最終目標位置との偏差（ｅ）を確認し，偏差がλ₂となりかつ，それが一定期間（１サンプリング）連続したら制御が安定したとみなし，制御を完了する。
【００６１】
図７，図８は上記のＳ字パターンの発生方法をまとめたものである。
図９は本発明の制御量演算部のフローチャートである。
Ｓ１初期値を設定する。初期値は初期パターンの他，加速度パターンの最大速度，最大加速度，パターンの種類，制限条件等のチューニングパラメータを与える。
【００６２】
Ｓ２サーボモータの回転数のエンコーダ値を読み取る（位置Ｘ（ｔ）を求める）。
Ｓ３パターン生成ロジック（図４〜図８において説明した加速度パターン発生方法）により，加速度パターンを発生する。
【００６３】
Ｓ４求めた加速度でサーボモータを駆動するためのフィードバック制御をする。
Ｓ５加速度センサの出力の電圧（加速度（振動値））を読み取り，メモリに保持する。
【００６４】
Ｓ６最終目標位置に到達するまでＳ２〜Ｓ５を繰り返し，最終目標位置に到達したか判定し，到達していれば制御を終了する。
Ｓ７前述の評価関数を使用して評価計算をする。
【００６５】
Ｓ８最大速度，加速度（減速度），加加速度（加減速度）等の加速度パラメータを変更した，評価値を比較し，評価値が減少する条件（パラメータを変更する条件）を求める（チューニング）。
【００６６】
Ｓ９チューニング結果（評価値の比較結果）が良好であれば（評価値を最小とする加速度パターンが求まる），加速度パターンのチューニングを終了する。チューニング結果が良好でなければＳ１０に進む。
【００６７】
Ｓ１０加速度パターン調整のパラメータを調整し，Ｓ２以降の処理を繰り返す。
チューニングは，前述の加速度パターン変更部の加速度パターンの変更方法に従う（図３の説明参照）。
【００６８】
図１０は本発明の実施例２の構成である。
図１０は，複数のモータ（モータ１，モータ２，モータ３）をロボットアームの先端７４の振動が最小になるように予め定めた一定時間（整定時間）の間に各モータの加速度パターンを調整するものである。
【００６９】
図１０において，
２６は加速度センサである。
２７は振動値入力部である。
【００７０】
７１は関節１であって，モータ１で動作するものである。
７２は関節２であって，モータ２で動作するものである。
７３は関節３であって，モータ３で動作するものである。
【００７１】
７４はロボットアームの先端である。
７５はロボットアームの基台である。
８１は制御装置１であって，本発明の加速度パターン設定手段，パターン発生部，位置検出部，モータ制御部，モータ駆動装置等を含むものであって（図１参照），関節１（７１）のモータ１の加速度パターンの設定，駆動を行うものである。
【００７２】
８２は制御装置２であって，関節２（７２）のモータ２の加速度パターンの設定，駆動を行うものである。
８３は制御装置３であって，関節３（７３）のモータ３の加速度パターンの設定，駆動を行うものである。
【００７３】
図１０の構成の動作を説明する。
制御装置１（８１）は加速度パターン１を発生し，モータ１を駆動する。制御装置２（８２）は加速度パターン２を発生し，モータ２を駆動する。制御装置３（８３）は加速度パターン３を発生し，モータ３を駆動する。
【００７４】
制御装置１（８１），制御装置２（８２），制御装置３（８３）は加速度センサ２６の検出した振動値を入力する。そして，例えば，制御装置１（８１）はモータ１の振動値（α（ｔ）），位置Ｘ（ｔ）を入力し，最終目標値（ｒｅｆ）とにより評価関数を計算して評価値を求め，整定時間の間に加速度センサ２６の検出する振動値を最小にする最適加速度パターン１を求める。同様に，制御装置２（８２），制御装置３（８３）も加速度パターン１の振動値およびそれぞれの制御物の位置を入力して評価値を求め，整定時間の間にロボットアームの先端の振動数が最小になるような加速度パターン２，加速度パターン３を求める。
【００７５】
本実施例によれば，一定の整定時間の間にアームの先端７４の振動を最小にするように各関節（関節１，関節２，関節３）を駆動する加速度パターンを求めることができる。性能のよいロボットアームとすることができる。
【００７６】
図１１は本発明の実施例３であって，本発明の方法による最適加速度パターンの設定と最適制御ゲインの設定を一定の整定時間の間に行うものである（最適制御ゲインの調整方法の詳細は特開平８−１６２０５号参照）。
【００７７】
図１１において，
図１１の構成において，加速度パターン設定手段５，加速度パターン調整部６，加速度パターン発生部９，サーボモータ２２，ロボットアーム２５，振動値入力部２７，入出力装置は前述のものと同様である。
【００７８】
２１はモータ制御部（図１の駆動制御部に相当する）であって，加速度パターン発生部９の発生した加速度パターンに従ってサーボモータのサンプル毎の移動目標値を発生してサーボモータ２２を駆動するものである。そして，制御ゲイン調整部を備えて，サーボモータの応答特性が最善になるように制御ゲインを調整するものである。
【００７９】
９１は制御目標値出力部であって，サーボモータの制御信号を出力ものである。
９２は制御ゲイン調整部であって，加速度パターン発生部９の発生した加速度パターンに従ってサーボモータ２２を駆動する場合の制御量の理論値と実際に駆動して得られる制御量との差を基にサーボモータの応答特性が最善になるような制御ゲインを求め，設定するものである。
【００８０】
９３は最適加速度パターンに従って動作する場合の理論的な制御量を演算するものである。
図１１の構成の動作は後述する。
【００８１】
図１２は制御ゲイン調整部のブロック図である。
図１２において，
９２は制御ゲイン調整部である。
【００８２】
１１９は偏差検出部であって，最終目標値（ｒｅｆ）と制御値（位置検出部の検出した位置）との偏差を検出するものである。
１２０は調整演算部であって，偏差に対して比例，積分，微分の調整演算を行うものである。
【００８３】
１２０ａは比例演算部である。
１２０ｂは積分演算部である。
１２０ｃは微分演算部である。
【００８４】
１２１は係数制御部であって，調整演算部１２０の各項の係数を制御するものである。
１２２は応答特性検出部であって，実際に駆動して得られる制御量と理論的制御量との差を検出するものである。
【００８５】
１２３は評価値算出部であって，応答特性計算部９３で計算した最適加速度パターンにより駆動した場合の理論的制御量と，最適加速度パターンで実際に駆動して得られる制御量との差に基づいて評価値を算出するものである。
【００８６】
図１２において，評価値算出部１２３は，次の評価関数で偏差を応答特性を評価する。
Ｊ＝Σ｛ｔ²（Ｙｓ（ｔ）−Ｙ（ｔ））｝²＋Σ｛δ₂（ｔ）²｝
ここで，Ｙｓ（ｔ）はシュミレーション値であって，最適加速度パターンにより駆動した場合に得られる理論的制御量である。
【００８７】
Ｙ（ｔ）は，加速度パターンで駆動した時に実際に得られる制御量である。
δ₂（ｔ）は重み係数である。
比例演算部の制御ゲインの係数Ｋｐ，積分演算部の制御ゲインの係数Ｋｉ，微分演算部の制御ゲインの係数Ｋｄの各係数を増減する組合せを考え，例えば，
▲１▼ Ｋｉ増，Ｋｉ増，Ｋｄ減，▲２▼ Ｋｉ増，Ｋｉ減，Ｋｄ減等の８種類の組合せを考え，それぞれの場合について評価値を求め，評価値の小さい一つもしくは複数の組合せを求める。そして，その係数についてさらに増減する組合せを求めて，評価値を求める処理を繰り返し，整定時間の範囲で評価値を最小とする係数を求める。そして，その係数を最適制御ゲインとして設定する。
【００８８】
図１３は本発明の実施例４の構成である。図１３は，複数のモータにより駆動されるシステムにおいて，制御物の振動が最小になるように，整定時間の範囲内で，各モータの最適加速度パターン，最適制御ゲインを求めるものである。
【００８９】
制御物は図１０と同様のロボットアームである。制御装置１，制御装置２，制御装置３が，それぞれアームの先端７４の振動が最小になるようにモータ１（７１），モータ２（７２），モータ３（７３）の加速度パターンを設定，ぞれぞれのモータを駆動する点は，図１０の場合と同様である。図１３では制御装置１（８１），制御装置２（８２），制御装置３（８３）がそれぞれに，制御ゲイン調整部を備え，それぞれの最適加速度パターンのもとに，それぞれのモータの制御ゲインを最適に設定するものである。
【００９０】
図１３において，
制御装置１はアームの先端７４の先端の振動が最小になるようにモータ１の速度パターン１を設定する。制御装置２はアームの先端７４の先端の振動が最小になるようにモータ２の速度パターン２を設定する。制御装置３はアームの先端７４の振動が最小になるようにモータ３の速度パターン３を設定する。
【００９１】
制御ゲイン調整部１（１２１’）は加速度パターン１でモータ１を駆動した場合の理論的制御量と実際にパターン１で駆動して得られる制御量との差を基に評価値を計算し，評価値が最小となる制御ゲインを設定する。制御ゲイン調整部２（１２２’）は加速度パターン２でモータ２を駆動した場合の理論的制御量と実際にパターン２で駆動して得られる制御量との差を基に評価値を計算し，評価値が最小となる制御ゲインを設定する。制御ゲイン調整部３（１２３’）は加速度パターン３でモータ３を駆動した場合の理論的制御量と実際にパターン３で駆動して得られる制御量との差を基に評価値を計算し，評価値が最小となる制御ゲインを設定する。
【００９２】
図１４は台形パターンによる応答特性と振動の状態を示すものである。
図１４ (a)は台形パターンにおけるチューニング前とチューニング後の応答特性の変化を示すものである。横軸は時間であり，縦軸は位置である。
【００９３】
チューニングにより応答特性が改善されていることが示されている。
図１４ (b)は台形パターンにより発生する振動の状態を示す。横軸は時間であり，縦軸は加速度である。図示のように振動しながら目標位置に到達することを示す。
【００９４】
図１５はＳ字パターンによる応答特性と振動の状態を示すものである。
図１５ (a)はＳ字パターンにおけるチューニング前とチューニング後の応答特性の変化を示すものである。横軸は時間であり，縦軸は位置である。
【００９５】
チューニングにより応答特性が改善されていることが示されている。
図１５ (b)はＳ字パターンにより発生する振動の状態を示す。横軸は時間であり，縦軸は加速度である。図示のように振動しながら目標位置に到達することを示す。
【００９６】
【発明の効果】
本発明によれば，制御物に発生する振動を最小とする最適な速度・加速度の組合せの加速度パターンを短時間に求めることができる。また，振動が最小になる加速度パターンを計算機により評価値を計算して求めるので，調整された結果にバラツキがなく均一な特性が得られる。そのため，応答特性が高速なロボット等を容易に得ることができる。
【００９７】
また，複数のモータを備え，任意のモータが他のモータの負荷になって駆動するような場合にも，被制御物の振動を最小にするそれぞれのモータの加速度パターンを容易に求めることができる。また，求めた加速度パターンにより駆動した場合の最適制御ゲインも自動的に求めることができるので，応答特性のすぐれた制御システムを容易に実現することが可能になる。
【図面の簡単な説明】
【図１】本発明の基本構成を示す説明図である。
【図２】本発明の実施例１のシステム構成を示す図である。
【図３】本発明の実施例１の加速度パターン調整部の実施例を示す図である。
【図４】本発明の加速度パターンの例を示す図である。
【図５】本発明の台形パターンの生成条件を示す図である。
【図６】本発明の加速度パターンの例を示す図である。
【図７】本発明のＳ字パターンの生成条件を示す図である。
【図８】本発明のＳ字パターンの生成条件を示す図である。
【図９】本発明の加速度パターン設定手段のフローチャートを示す図である。
【図１０】本発明の実施例２を示す図である。
【図１１】本発明の実施例３を示す図である。
【図１２】本発明で実施例する制御ゲイン調整部の例を示す図である。
【図１３】本発明の実施例４を示す図である。
【図１４】本発明の台形パターンの応答特性と振動の例を示す図である。
【図１５】本発明のＳ字パターンの応答特性と振動の例を示す図である。
【符号の説明】
１：被制御物
２：振動検出手段
５：加速度パターン設定手段
７：評価値算出部
８：加速度パターン変更部
９：加速度パターン発生部
１１：駆動制御手段
１２：駆動装置[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a method for setting an acceleration pattern for driving a servo motor in a control system for controlling movement of a moving mechanism such as a production machine, a printer, a magnetic disk device, or a robot.
[0002]
[Prior art]
Conventionally, the optimum pattern of speed and acceleration for driving the servo motor is the trapezoidal wave drive command of the servo motor based on the combination of the designed speed and acceleration (the speed and acceleration patterns are represented by trapezoid patterns) or S Character acceleration / deceleration drive command (acceleration pattern is expressed with positive and negative waveform patterns (S-shaped pattern) with respect to the time axis), drive the servo motor, measure the actual vibration and Therefore, an acceleration pattern that can be stably driven in the shortest time by changing the combination of speed and acceleration little by little so as to reduce the frequency (hereinafter, this operation is referred to as tuning). In addition, adjustment of the control gain is necessary to operate the servo motor with high accuracy separately from the acceleration pattern. Conventionally, this adjustment is performed by step input etc. separately from the acceleration pattern, and is separate from acceleration tuning. It was done in the process.
[0003]
[Problems to be solved by the invention]
The conventional acceleration pattern tuning method was empirically given speed and acceleration, and was adjusted while observing the operating state with an oscilloscope, etc., so there were many parts due to trial and error, and adjustment took time. In particular, the robot must adjust the acceleration pattern for each of the six motors one by one, which is an operation that requires a long time (about one month). In addition, the results of adjustments made by workers to make decisions were likely to vary. Also, since the nonlinear term is not taken into consideration in the design, there is often a difference between the calculated value of the speed trajectory and the measured value. For this reason, tuning of speed and acceleration is a difficult task.
[0004]
The present invention provides a control system and an acceleration pattern setting method capable of easily setting an optimum acceleration pattern for driving a drive device such as a servo motor, and further, based on the optimum acceleration pattern thus set. It is an object of the present invention to provide a parameter setting method capable of adjusting a control gain by continuous processing.
[0005]
[Means for Solving the Problems]
The present invention provides a control system for controlling the movement of a controlled object according to a given acceleration pattern, wherein the controlled object includes a vibration detecting means, generates an acceleration pattern, and the vibration value detected by the vibration detecting means is a variable. The evaluation function is calculated, the acceleration pattern is changed to obtain the evaluation value, and the optimum acceleration pattern that minimizes the vibration is obtained and automatically set according to the evaluation value.
[0006]
FIG. 1 is an explanatory diagram of the basic configuration of the present invention.
FIG. 1 (a) shows the basic configuration.
In FIG.
Reference numeral 1 denotes a controlled object.
[0007]
Reference numeral 2 denotes vibration detection means, which is an acceleration sensor, for example, that measures the vibration that the controlled object 1 receives.
Reference numeral 5 denotes an acceleration pattern setting means for obtaining an acceleration pattern that minimizes the vibration generated in the controlled object 1.
[0008]
An acceleration pattern adjustment unit 6 adjusts the acceleration pattern so that vibration is minimized (for example, the speed, acceleration, and acceleration of the acceleration pattern shown in FIGS. 1B and 1C). Adjust acceleration, deceleration, deceleration, etc.).
[0009]
Reference numeral 7 denotes an evaluation value calculation unit, which obtains an evaluation value using the vibration value measured by the vibration detection means 2 as a variable.
An acceleration pattern changing unit 8 changes the speed and acceleration according to the evaluation value of the evaluation value calculating unit 7.
[0010]
Reference numeral 9 denotes an acceleration pattern generation unit which generates an adjusted acceleration pattern according to the result of the acceleration pattern adjustment unit 6.
Reference numeral 11 denotes drive control means for controlling the drive of the drive device 12 according to the acceleration pattern.
[0011]
A drive device 12 drives the controlled object 1.
Fig. 1 (b) shows an example of an acceleration pattern, which is a trapezoidal pattern, where the horizontal axis represents time and the vertical axis represents speed.
[0012]
Time t from start (origin) ₁ Accelerates at a constant acceleration until time t ₁ To time t ₂ Is controlled at a constant speed (acceleration 0) until time t ₂ To time t _Three Up to (target position) is an acceleration pattern controlled by constant acceleration (constant deceleration).
[0013]
FIG. 1C shows another example of the acceleration pattern, which is an S-shaped pattern. The horizontal axis is time, and the vertical axis is acceleration.
Time t from start (origin) ₁ Is controlled with a constant jerk until time t ₁ To time t ₂ Until the time t ₂ To time t _Three Until is controlled with constant deceleration. Furthermore, t _Three To t _Four Until the time t is controlled at zero acceleration (constant speed) _Four To t _Five Until the time t _Five To t ₆ Until time t ₆ To time t ₇ (Target value) is controlled by constant jerk.
[0014]
The operation of the principle diagram of the present invention in FIG. 1 will be described.
It is assumed that the acceleration pattern generator 9 generates an acceleration pattern given under initial conditions, and the controlled object 1 is operating with the acceleration pattern. The acceleration pattern is, for example, a pattern as shown in FIGS. The control device A drives the controlled object 1 according to the acceleration pattern. The controlled object 1 is driven by the control device A and moves from the start position toward the target position, but generates vibration on the movement path. The vibration detection means 2 measures the vibration, and the evaluation value calculation unit 7 calculates an evaluation function based on the vibration value and the current position of the controlled object. The evaluation function uses the vibration value as a variable. For example, the evaluation function is a function in which the evaluation value decreases as the vibration frequency decreases (hereinafter, an evaluation function in which the evaluation value decreases as the vibration frequency decreases is taken as an example). explain). The acceleration pattern changing unit 8 changes the speed and acceleration. As the minimum evaluation value, for example, an evaluation target value is determined, and a value closest to the evaluation target value within a certain time (hereinafter referred to as settling time) is obtained. The acceleration pattern generator 9 generates a changed acceleration pattern. The drive control means 11 generates a control amount (movement target value for each sampling) of the drive device 12 according to the generated acceleration pattern, and the drive device 12 drives the controlled object 1 according to the control amount.
[0015]
By repeating the above processing, the acceleration pattern setting means 5 obtains an acceleration pattern that minimizes the vibration of the controlled object 1.
According to the present invention, it is possible to automate the setting of the optimum acceleration pattern, which has been performed manually in the past, and obtain the optimum speed / acceleration combination in a short time. In addition, when the optimum acceleration pattern is obtained manually, the adjustment results are likely to vary by the operator or from work to work. However, in the present invention, the acceleration pattern that minimizes vibration is obtained by the computer. As a result, there is no variation and uniform characteristics can be obtained. Furthermore, it is possible to obtain a control system that moves to the target position in a short time with less vibration.
[0016]
DETAILED DESCRIPTION OF THE INVENTION
FIG. 2 shows a system configuration of an embodiment of the present invention, and shows an example in which the controlled object is a robot arm.
[0017]
FIG. 2 (a) shows the system configuration of the present invention.
In Fig. 2 (a),
A is a control device.
[0018]
Reference numeral 5 denotes acceleration pattern setting means.
Reference numeral 6 denotes an acceleration pattern adjustment unit.
Reference numeral 9 denotes an acceleration pattern generator.
[0019]
Reference numeral 21 denotes a motor control unit (corresponding to the drive control means in FIG. 1).
Reference numeral 22 denotes a servo motor (corresponding to the driving device of FIG. 1).
A robot arm 25 is a controlled object.
[0020]
An acceleration sensor 26 is attached to the tip of the robot arm 25 and detects the acceleration at the tip of the robot arm 25 and measures vibration.
[0021]
A vibration value input unit 27 inputs a vibration value based on the acceleration obtained by the acceleration sensor 26 (acceleration generated at the tip of the robot arm).
A position detection unit 28 encodes the number of rotations of the servo motor and determines the position of the robot arm 25.
[0022]
Reference numeral 30 denotes an input / output device that inputs initial values such as the maximum speed, maximum acceleration, and type of acceleration pattern, or outputs the obtained acceleration pattern and response characteristics.
[0023]
FIG. 2B shows the configuration of an embodiment of the acceleration pattern setting means and the motor control unit.
33 is a CPU, and 34 is a RAM. Reference numeral 36 denotes an acceleration pattern generation program. Reference numeral 37 denotes an acceleration pattern adjustment program. Reference numeral 38 denotes an input / output control program. Reference numeral 39 denotes a motor control program.
[0024]
An example of a function for performing an evaluation calculation by the acceleration pattern adjustment unit 6 is as follows.

Where W: Weighting factor
α (t): Acceleration sensor output
ref: Target value or ideal response waveform (when calculated by simulation)
X (t): Actual measured value of position.
[0025]
T: Time, which is the time from the start or (provisional time-t).
i represents the number of each sample period
The configuration of FIG. 2 will be described.
[0026]
Assume that each part operates according to the acceleration pattern given by the initial value. The acceleration sensor 26 detects the acceleration of vibration generated when the robot arm 25 moves. The vibration value input unit 27 obtains the acceleration detected by the acceleration sensor 26. Further, the position detection unit 28 encodes the number of rotations of the servo motor and obtains the movement amount (movement position) of the robot arm 25.
[0027]
The acceleration pattern adjustment unit 6 inputs the vibration value and the position data of the tip of the robot arm 25. Then, in the acceleration pattern adjustment unit 6, the evaluation value calculation unit (see FIG. 1) obtains the evaluation value by the evaluation function. The acceleration pattern changing unit (see FIG. 1) determines whether or not the evaluation value is determined to be minimum. If it is not determined that the evaluation value is the minimum, the speed, acceleration, jerk, or the like is changed, and the acceleration pattern generator 9 generates an acceleration pattern in which the acceleration pattern is changed.
[0028]
The motor control unit 21 generates a target position for each sampling according to the changed acceleration pattern, and drives the servo motor 22. The robot arm 25 is driven by the servo motor 22 and moves toward the target position.
[0029]
The above processing is repeated for a certain period of time to obtain an acceleration pattern that minimizes the evaluation value and set it as the optimum acceleration pattern.
FIG. 3 shows an embodiment of the acceleration pattern adjustment unit of the present invention.
[0030]
In FIG.
Reference numeral 5 denotes acceleration pattern setting means.
Reference numeral 6 denotes an acceleration pattern adjustment unit.
[0031]
6 ′ is a deviation detecting means for inputting the final target value ref of the robot arm, the position X (t) of the tip of the robot arm, and the vibration value (acceleration α (t)) of the acceleration sensor.
[0032]
Reference numeral 7 denotes an evaluation value calculation unit which obtains acceleration by the above-described evaluation function according to the difference between the final target value ref and the position X (t) and the vibration value (acceleration α (t)).
[0033]
An acceleration pattern change unit 8 determines whether or not the acceleration pattern needs to be changed based on the evaluation value, and changes the speed, acceleration, jerk, and the like when the change is necessary.
[0034]
Reference numeral 9 denotes an acceleration pattern generator.
The operation of the configuration of FIG. 3 will be described.
The deviation detection means 6 ′ inputs the target value (ref), the position X (t), and the vibration value (acceleration α (t)), and the evaluation value calculation unit 7 obtains the evaluation value according to the above-described evaluation function.
[0035]
The evaluation function has a smaller function value (evaluation value) as vibration (acceleration α (t)) is smaller. The acceleration pattern changing unit 8 determines whether or not the acceleration pattern needs to be changed based on the evaluation value. When it is necessary to change the acceleration pattern, the acceleration pattern is slightly changed (the changing method will be described later), and the acceleration pattern generator 9 generates the changed acceleration pattern. The motor control unit 21 obtains a movement target value for each sampling based on the acceleration pattern, and drives the servo motor.
[0036]
The determination method of the minimum evaluation value and the method of changing the acceleration pattern in the acceleration pattern changing unit are as follows.
First, set a target value for the evaluation value. Then, the acceleration pattern is changed within the settling time range, and the evaluation value closest to the evaluation value target value is determined to be the minimum. Alternatively, if a value close to the target value of the evaluation value is found within a certain range before the settling time ends, the evaluation value is determined to be the minimum. The acceleration pattern is changed as follows.
[0037]
For example, when obtaining a trapezoidal acceleration pattern, consider the following combinations of acceleration, maximum speed, and deceleration. Then, each acceleration pattern is obtained for the following combinations in which the parameter values of speed and acceleration are slightly increased or decreased, and the controlled object is driven by each pattern. Then, the evaluation value of each acceleration pattern is calculated.
[0038]
(1) Increase acceleration. Increase maximum speed. Increase deceleration.
(2) Increase acceleration. Reduce maximum speed. Increase deceleration.
(3) Decrease acceleration. Increase maximum speed. Decrease the deceleration.
[0039]
(4) Decrease acceleration. Reduce maximum speed. Decrease the deceleration.
Among the evaluation values, one or a plurality of acceleration patterns having a small evaluation value are obtained. Then, the same processing is repeated for the selected several acceleration patterns to obtain the minimum evaluation value acceleration pattern, which is set as the optimum acceleration pattern.
[0040]
Note that the above acceleration and deceleration are combinations when the signs are different, and there are eight possible combinations when the absolute values are different, and the evaluation function is made smaller from these combinations. Select one or more combinations. Alternatively, only one of the parameters may be changed, and the others may not be changed, and the change in the evaluation function may be determined.
[0041]
The acceleration pattern generation method of the present invention will be described with reference to FIGS.
FIG. 4 shows an example of an acceleration pattern, which shows a trapezoid pattern.
[0042]
The parameters necessary for generating the trapezoid pattern are speed and acceleration, and maximum speed (Vmax) and maximum acceleration a ₀ Is given as a condition.
Consider a trapezoidal pattern divided into three regions ((1), (2), (3)).
[0043]
The logic for generating the trapezoid pattern is as follows.
(1) Acceleration stage
A constant acceleration is applied and the maximum speed (V _max ) Will reach the constant speed stage (2).
However, when the position component (for example, the position of the robot arm) reaches ½ of the whole process (from the start position to the target position), the process proceeds to the deceleration stage immediately after maintaining the constant speed for the specified time.
[0044]
(2) Constant speed stage
Move at a constant speed without acceleration. When the distance to the target is the distance required for (1), the process proceeds to the deceleration stage. However, as soon as the position component (movement distance from the start) reaches 1/2 of the total process, the process proceeds to the deceleration stage (see FIG. 4B).
[0045]
(3) Deceleration stage
When a constant deceleration is performed and the speed reaches 0 (zero), the generation of the acceleration pattern is terminated. However, since the pattern generation is a discrete operation and may have an error with respect to the final target position, the target position adjustment stage processing is performed next.
[0046]
(4) Target position adjustment stage
When the position (X) calculated in the stage of (3) reaches the vicinity of the final target (ref) (difference is less than ε), the residual is divided by n and added every sampling. When the residual is less than λ, it is considered that the target position has been reached, and the next control stabilization waiting stage is entered.
[0047]
(5) Control stabilization waiting stage
Check the deviation (e) between the sampled servo motor speed and the target position. ₂ If it is less than that and continues for a certain period (during one sampling), the control is considered to be stable and the control is completed.
[0048]
FIG. 5 summarizes the above relationships.
In FIG. ₀ Is the initial speed. X ₀ Is the initial position. V _k Is the speed of the Kth sampling. V _max Is the maximum speed. ref is the final target position. T is the time from the start.
[0049]
An S-shaped pattern generation method will be described with reference to FIGS.
FIG. 6 is an example of an S-shaped pattern.
Of the parameters required for generating the S-shaped pattern, the maximum acceleration (A _max ), Jerk A 'and maximum speed V _max Is given in advance.
[0050]
Consider the generated acceleration pattern divided into seven regions.
(1) Acceleration stage
Jerk acceleration is performed at a constant rate (jerk acceleration), and when the maximum acceleration is reached, a constant acceleration stage is entered. However, when the position component reaches 1/2 of the total process, the process proceeds to step (8) immediately after maintaining the constant speed for the specified time. Prior to transition, the sign of the current acceleration is reversed.
[0051]
(2) Constant acceleration stage
It moves at a constant acceleration without acceleration. When the speed component (V) reaches the maximum speed and reaches the value excluding the acceleration period required for (1), the process proceeds to the deceleration stage. However, when the position component reaches 1/2 of the total process, after maintaining the constant speed for the specified time, the process immediately proceeds to stage (7). Prior to the transition, the sign of the current acceleration is reversed.
[0052]
(3) Deceleration stage
Decelerates and decelerates at a constant rate, and the speed _max ) (Maximum speed) is reached, the deceleration stage ends and the uniform speed stage starts. However, when the position component reaches 1/2 of the total process, the constant speed is maintained for the specified time and the process immediately proceeds to step (6). Prior to the transition, the sign of the current acceleration is reversed.
[0053]
(4) Alignment speed stage
Speed is the maximum speed (V _max Adjust the speed component until) is reached. When the speed component reaches the specified maximum speed, the process proceeds to the second stage of constant acceleration.
[0054]
(5) Constant speed second stage
Move at a constant speed without acceleration. When the final generation pattern (position = X) reaches a value excluding the distance required for (1) among all the steps, the process proceeds to the second stage of deceleration. However, when the position component reaches 1/2 of the total process, the process immediately proceeds to step (6).
[0055]
(6) Deceleration 2nd stage
Decrease and decelerate at a constant rate (jerk), negative maximum acceleration (-A _max ), It will move to the second stage of acceleration. If the position component reaches ½ of the total process during this stage, overrun (the final value of the position component exceeds the final target value) is unavoidable, and pattern generation continues.
[0056]
(7) Constant acceleration second stage
It moves at a constant acceleration without acceleration. If the speed component (V) reaches a value excluding the deceleration period required for (6) in the process where the speed reaches zero, the process proceeds to the second acceleration / acceleration stage. If the position component reaches ½ of the total process during this stage, overrun (the final value of the position component exceeds the final target value) is unavoidable, and pattern generation continues.
[0057]
(8) Acceleration second stage
Acceleration is performed at a constant rate, and when the acceleration reaches zero, the alignment speed is shifted to the second stage. If the position component reaches 1/2 of the total process during this stage, overrun (the final value of the position component exceeds the final target value) is inevitable, so continue pattern generation (proceed to (9)) ).
[0058]
(9) Alignment speed second stage
Adjust the speed component until the speed reaches zero. When the speed component becomes zero, the process proceeds to the target position adjustment stage.
[0059]
(10) Target position adjustment stage
When the position (X) calculated at the end of (9) is near the final target position (ref) (difference is less than ε), the residual is divided into n equal parts and added every sampling. When the residual is less than λ, it is considered that the target position has been reached, and the process proceeds to the control stabilization waiting stage.
[0060]
(11) Control stabilization waiting stage
Check the deviation (e) between the sampled encoder value and the final target position. ₂ If this is continued for a certain period (one sampling), it is considered that the control is stable, and the control is completed.
[0061]
7 and 8 summarize the method for generating the S-shaped pattern.
FIG. 9 is a flowchart of the control amount calculation unit of the present invention.
S1 Set the initial value. In addition to the initial pattern, the initial value gives tuning parameters such as the maximum acceleration pattern speed, maximum acceleration, pattern type, and limiting conditions.
[0062]
S2 Read the encoder value of the rotation speed of the servo motor (determine the position X (t)).
S3 An acceleration pattern is generated by pattern generation logic (acceleration pattern generation method described in FIGS. 4 to 8).
[0063]
S4 Feedback control is performed to drive the servo motor with the obtained acceleration.
S5: The voltage (acceleration (vibration value)) output from the acceleration sensor is read and stored in the memory.
[0064]
S6 S2 to S5 are repeated until the final target position is reached, it is determined whether the final target position has been reached, and if it has been reached, the control is terminated.
S7 Perform an evaluation calculation using the above-described evaluation function.
[0065]
S8: Acceleration parameters such as maximum speed, acceleration (deceleration), jerk (acceleration / deceleration) are changed, evaluation values are compared, and a condition for reducing the evaluation value (condition for changing parameters) is obtained (tuning).
[0066]
S9 If the tuning result (evaluation value comparison result) is good (an acceleration pattern that minimizes the evaluation value is obtained), the tuning of the acceleration pattern is terminated. If the tuning result is not good, the process proceeds to S10.
[0067]
S10: The acceleration pattern adjustment parameters are adjusted, and the processes after S2 are repeated.
Tuning follows the acceleration pattern changing method of the acceleration pattern changing unit described above (see the description of FIG. 3).
[0068]
FIG. 10 shows the configuration of the second embodiment of the present invention.
FIG. 10 shows the adjustment of acceleration patterns of a plurality of motors (motor 1, motor 2, motor 3) during a predetermined time (settling time) so that the vibration of the tip 74 of the robot arm is minimized. To do.
[0069]
In FIG.
Reference numeral 26 denotes an acceleration sensor.
Reference numeral 27 denotes a vibration value input unit.
[0070]
Reference numeral 71 denotes a joint 1 which is operated by the motor 1.
Reference numeral 72 denotes a joint 2 which is operated by the motor 2.
Reference numeral 73 denotes a joint 3 which is operated by the motor 3.
[0071]
74 is the tip of the robot arm.
Reference numeral 75 denotes a robot arm base.
Reference numeral 81 denotes a control device 1, which includes an acceleration pattern setting means, a pattern generation unit, a position detection unit, a motor control unit, a motor drive device, etc. of the present invention (see FIG. 1), and a joint 1 (71). The acceleration pattern of the motor 1 is set and driven.
[0072]
82 is a control device 2 for setting and driving the acceleration pattern of the motor 2 of the joint 2 (72).
Reference numeral 83 denotes a control device 3, which sets and drives the acceleration pattern of the motor 3 of the joint 3 (73).
[0073]
The operation of the configuration of FIG. 10 will be described.
The control device 1 (81) generates the acceleration pattern 1 and drives the motor 1. The control device 2 (82) generates the acceleration pattern 2 and drives the motor 2. The control device 3 (83) generates the acceleration pattern 3 and drives the motor 3.
[0074]
The control device 1 (81), the control device 2 (82), and the control device 3 (83) input the vibration value detected by the acceleration sensor 26. For example, the control device 1 (81) inputs the vibration value (α (t)) and the position X (t) of the motor 1 and calculates an evaluation function based on the final target value (ref) to obtain an evaluation value. The optimum acceleration pattern 1 that minimizes the vibration value detected by the acceleration sensor 26 during the settling time is obtained. Similarly, the control device 2 (82) and the control device 3 (83) also input the vibration value of the acceleration pattern 1 and the position of each controlled object to obtain an evaluation value, and the vibration of the tip of the robot arm during the settling time. Acceleration pattern 2 and acceleration pattern 3 that minimize the number are obtained.
[0075]
According to the present embodiment, an acceleration pattern for driving each joint (joint 1, joint 2, joint 3) can be obtained so as to minimize the vibration of the arm tip 74 during a fixed settling time. A robot arm with good performance can be obtained.
[0076]
FIG. 11 shows a third embodiment of the present invention, in which the optimum acceleration pattern and the optimum control gain are set during the settling time according to the method of the present invention (details of the optimum control gain adjustment method). JP-A-8-16205).
[0077]
In FIG.
11, the acceleration pattern setting means 5, the acceleration pattern adjustment unit 6, the acceleration pattern generation unit 9, the servo motor 22, the robot arm 25, the vibration value input unit 27, and the input / output device are the same as those described above.
[0078]
Reference numeral 21 denotes a motor control unit (corresponding to the drive control unit in FIG. 1), which generates a movement target value for each sample of the servo motor according to the acceleration pattern generated by the acceleration pattern generation unit 9 and drives the servo motor 22. Is. A control gain adjustment unit is provided to adjust the control gain so that the response characteristic of the servo motor is optimized.
[0079]
A control target value output unit 91 outputs a control signal for the servo motor.
A control gain adjustment unit 92 is based on the difference between the theoretical value of the control amount when the servo motor 22 is driven in accordance with the acceleration pattern generated by the acceleration pattern generation unit 9 and the control amount obtained by actual driving. The control gain is determined and set so that the response characteristics of the servo motor are best.
[0080]
93 calculates a theoretical control amount in the case of operating according to the optimum acceleration pattern.
The operation of the configuration of FIG. 11 will be described later.
[0081]
FIG. 12 is a block diagram of the control gain adjustment unit.
In FIG.
Reference numeral 92 denotes a control gain adjustment unit.
[0082]
A deviation detection unit 119 detects a deviation between a final target value (ref) and a control value (a position detected by the position detection unit).
An adjustment calculation unit 120 performs proportional, integral, and differential adjustment calculations with respect to the deviation.
[0083]
120a is a proportional calculation unit.
Reference numeral 120b denotes an integral calculation unit.
120c is a differential operation unit.
[0084]
A coefficient control unit 121 controls the coefficient of each term of the adjustment calculation unit 120.
A response characteristic detection unit 122 detects a difference between a control amount actually obtained by driving and a theoretical control amount.
[0085]
An evaluation value calculation unit 123 is based on a difference between a theoretical control amount when driven by the optimum acceleration pattern calculated by the response characteristic calculation unit 93 and a control amount actually obtained by driving with the optimum acceleration pattern. The evaluation value is calculated.
[0086]
In FIG. 12, the evaluation value calculation unit 123 evaluates the response characteristic by using the following evaluation function.
J = Σ {t ² (Ys (t) -Y (t))} ² + Σ {δ ₂ (T) ² }
Here, Ys (t) is a simulation value, which is a theoretical control amount obtained when driven by the optimum acceleration pattern.
[0087]
Y (t) is a control amount actually obtained when driving with an acceleration pattern.
δ ₂ (T) is a weighting coefficient.
Consider a combination of increasing or decreasing each coefficient of the control gain coefficient Kp of the proportional calculation unit, the control gain coefficient Ki of the integral calculation unit, and the control gain coefficient Kd of the differential calculation unit.
(1) Ki increase, Ki increase, Kd decrease, (2) Consider eight types of combinations such as Ki increase, Ki decrease, Kd decrease, etc., obtain an evaluation value for each case, and calculate one or more small evaluation values Find a combination. And the combination which further increases / decreases about the coefficient is calculated | required, the process which calculates | requires an evaluation value is repeated, and the coefficient which minimizes an evaluation value in the range of settling time is calculated | required. Then, the coefficient is set as the optimum control gain.
[0088]
FIG. 13 shows the configuration of the fourth embodiment of the present invention. FIG. 13 shows the optimum acceleration pattern and optimum control gain of each motor within the settling time range so that the vibration of the controlled object is minimized in a system driven by a plurality of motors.
[0089]
The controlled object is a robot arm similar to that shown in FIG. The control device 1, the control device 2, and the control device 3 set the acceleration patterns of the motor 1 (71), the motor 2 (72), and the motor 3 (73) so that the vibration of the arm tip 74 is minimized. The point of driving each motor is the same as in the case of FIG. In FIG. 13, the control device 1 (81), the control device 2 (82), and the control device 3 (83) are each provided with a control gain adjustment unit, and the control gains of the respective motors based on the respective optimum acceleration patterns. Is set optimally.
[0090]
In FIG.
The control device 1 sets the speed pattern 1 of the motor 1 so that the vibration at the tip of the arm tip 74 is minimized. The control device 2 sets the speed pattern 2 of the motor 2 so that the vibration at the tip of the arm tip 74 is minimized. The control device 3 sets the speed pattern 3 of the motor 3 so that the vibration of the arm tip 74 is minimized.
[0091]
The control gain adjustment unit 1 (121 ′) calculates an evaluation value based on the difference between the theoretical control amount when the motor 1 is driven with the acceleration pattern 1 and the control amount actually obtained by driving with the pattern 1, Set the control gain that minimizes the evaluation value. The control gain adjustment unit 2 (122 ′) calculates an evaluation value based on the difference between the theoretical control amount when the motor 2 is driven with the acceleration pattern 2 and the control amount actually obtained by driving with the pattern 2, Set the control gain that minimizes the evaluation value. The control gain adjustment unit 3 (123 ′) calculates an evaluation value based on the difference between the theoretical control amount when the motor 3 is driven with the acceleration pattern 3 and the control amount actually obtained by driving with the pattern 3, Set the control gain that minimizes the evaluation value.
[0092]
FIG. 14 shows the response characteristics and vibration state of the trapezoid pattern.
FIG. 14A shows a change in response characteristics before and after tuning in a trapezoidal pattern. The horizontal axis is time, and the vertical axis is position.
[0093]
It is shown that the response characteristics are improved by tuning.
FIG. 14B shows the state of vibration generated by the trapezoid pattern. The horizontal axis is time, and the vertical axis is acceleration. It shows reaching the target position while vibrating as shown.
[0094]
FIG. 15 shows the response characteristics and vibration state by the S-shaped pattern.
FIG. 15A shows changes in response characteristics before and after tuning in the S-shaped pattern. The horizontal axis is time, and the vertical axis is position.
[0095]
It is shown that the response characteristics are improved by tuning.
FIG. 15B shows the state of vibration generated by the S-shaped pattern. The horizontal axis is time, and the vertical axis is acceleration. It shows reaching the target position while vibrating as shown.
[0096]
【The invention's effect】
According to the present invention, it is possible to obtain an acceleration pattern of an optimum speed / acceleration combination that minimizes vibration generated in a controlled object in a short time. In addition, since the acceleration pattern that minimizes the vibration is obtained by calculating the evaluation value with a computer, the adjusted result has no variation and uniform characteristics can be obtained. Therefore, it is possible to easily obtain a robot having a high response characteristic.
[0097]
In addition, even when there are multiple motors and any motor is driven by the load of another motor, the acceleration pattern of each motor that minimizes the vibration of the controlled object can be easily obtained. . In addition, since the optimum control gain when driven by the obtained acceleration pattern can be automatically obtained, a control system having excellent response characteristics can be easily realized.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram showing a basic configuration of the present invention.
FIG. 2 is a diagram showing a system configuration of Embodiment 1 of the present invention.
FIG. 3 is a diagram illustrating an example of an acceleration pattern adjustment unit according to the first embodiment of the present invention.
FIG. 4 is a diagram showing an example of an acceleration pattern according to the present invention.
FIG. 5 is a diagram showing trapezoid pattern generation conditions according to the present invention.
FIG. 6 is a diagram showing an example of an acceleration pattern according to the present invention.
FIG. 7 is a diagram showing conditions for generating an S-shaped pattern according to the present invention.
FIG. 8 is a diagram showing conditions for generating an S-shaped pattern according to the present invention.
FIG. 9 is a diagram showing a flowchart of acceleration pattern setting means of the present invention.
FIG. 10 is a diagram showing Example 2 of the present invention.
FIG. 11 is a diagram showing a third embodiment of the present invention.
FIG. 12 is a diagram illustrating an example of a control gain adjustment unit according to an embodiment of the present invention.
FIG. 13 is a diagram showing a fourth embodiment of the present invention.
FIG. 14 is a diagram showing an example of response characteristics and vibration of a trapezoid pattern of the present invention.
FIG. 15 is a diagram illustrating an example of response characteristics and vibration of an S-shaped pattern according to the present invention.
[Explanation of symbols]
1: Controlled object
2: Vibration detection means
5: Acceleration pattern setting means
7: Evaluation value calculation unit
8: Acceleration pattern change unit
9: Acceleration pattern generator
11: Drive control means
12: Drive device

Claims

In a control system that controls the movement of a controlled object according to a given acceleration pattern,
The controlled object has vibration detection means,
An acceleration pattern is generated, an evaluation function is calculated using the vibration value of the vibration detected by the vibration detection means as a variable, an evaluation value is obtained by changing the acceleration pattern, and an optimum acceleration pattern that minimizes the vibration according to the evaluation value is obtained. along with the automatically set to seek,
A plurality of driving devices that are loaded by other driving devices with respect to an arbitrary driving device that drives the controlled object, and acceleration pattern setting means for setting an acceleration pattern in each driving device;
An acceleration pattern setting method in a control system, wherein an acceleration pattern of each driving device is determined so that the vibration value is minimized .

2. The method for setting an acceleration pattern in a control system according to claim 1, wherein the shape of the function is a trapezoid pattern when the horizontal axis is time and the vertical axis is speed.

2. The method for setting an acceleration pattern in a control system according to claim 1, wherein the acceleration pattern is a wave shape in which the shape of the function is up and down with respect to the time axis when the horizontal axis is time and the vertical axis is acceleration. .

In a control system that controls the movement of a controlled object according to a given acceleration pattern,
Vibration detection means provided on the controlled object;
An acceleration that generates an acceleration pattern, calculates an evaluation function using the vibration value of the vibration detected by the vibration detection means as a variable, obtains an evaluation value by changing the acceleration pattern, obtains an optimum acceleration pattern according to the evaluation value, and automatically sets the acceleration Pattern setting means;
Deviation detecting means for detecting a deviation between a controlled variable of the controlled object and a target value of the controlled variable;
A drive control means having a proportional operation unit for performing a proportional operation with respect to the deviation, an integral operation unit for performing an integral operation, and a differential operation unit for performing a differential operation, and determining a control amount according to the operation result of each of the operation units; With
An optimum acceleration pattern that minimizes the vibration value of the controlled object is obtained, a theoretical control amount is obtained based on the acceleration pattern, a difference between the theoretical control amount and a control amount actually generated by the acceleration pattern is obtained, Using the difference as a variable, a second evaluation value is obtained by a second evaluation function, and the control gain of each arithmetic unit is optimized so that the difference is minimized according to the evaluation value of the second evaluation value calculation unit A parameter setting method in a control system, characterized in that a coefficient is determined .

A plurality of drive units that are loaded with other drive units for any drive unit are provided.
The vibration value sets the optimum acceleration pattern for each driving device so as to minimize, according to claim 4, characterized in that the set seeking optimum control coefficient of each driving device in accordance with the respective optimal acceleration pattern Parameter setting method in the control system.

A controlled object,
A drive for driving the controlled object;
Drive control means for controlling the drive device;
Vibration detection means provided on the controlled object;
Acceleration pattern setting means for inputting the vibration value detected by the vibration detection means and obtaining an acceleration pattern for driving the controlled object so as to minimize the vibration value and automatically setting the acceleration pattern;
The movement of the controlled object is controlled by an acceleration pattern automatically set to minimize the vibration value of the controlled object, and
The acceleration pattern setting means generates an acceleration pattern, calculates an evaluation function using the vibration value of the vibration detected by the vibration detection means as a variable, obtains an evaluation value by changing the acceleration pattern, and determines an optimum acceleration pattern according to the evaluation value. To automatically set
Deviation detecting means for detecting a difference between a controlled variable of the controlled object and a target value of the controlled variable;
Has a differentiating unit and the proportional calculation section and the integral operation unit for the integral calculation you differential calculations for the proportional operation on the deviation, determines a control amount in accordance with the calculation results of the respective calculation unit, the driving device Drive control means for controlling
The drive control unit includes a response characteristic calculation unit that obtains a theoretical control amount based on the acceleration pattern obtained by the acceleration pattern setting unit, and a control amount that is actually generated by the control based on the theoretical control amount and the acceleration pattern. A response characteristic detecting unit for obtaining a difference between the first evaluation value, an evaluation value calculating unit for obtaining a second evaluation value according to a second evaluation function using the difference as a variable, and control of each arithmetic unit according to the second evaluation value A coefficient control unit for determining the optimum coefficient of gain,
A control system characterized in that an acceleration pattern that minimizes the vibration of the vibration detecting means is obtained, and the control gain of each arithmetic unit is determined so that the difference caused by the control of the acceleration pattern is minimized .