JP2005038311A - 機械制御装置 - Google Patents

Abstract

【課題】移動する制御対象が複数の部材を結合して構成されている場合にも、十分な振動抑制作用を得ることができる機械制御装置を提供する。
【解決手段】直交ロボットのように、それぞれ異なる方向に移動可能な複数の部材が機械的に結合され、かつ、一方向に沿って複数の部材が一体的に移動可能な制御対象を、位置指令値に基づき所定位置に移動させて停止させるための機械制御装置であって、機械固有振動の補正量を加えて制御対象の振動を抑制する手段を備えた機械制御装置に関する。例えば、スライド移動部３及びアーム２をＸ軸方向に沿って移動させた時の重量物１のＺ軸方向の位置情報を制振用データテーブル１１に入力し、パラメータ２４を変更することによって位置指令値に加える機械固有振動の補正量を変更する。
【選択図】 図１

Description

本発明は、制御対象を目標位置へ移動させて停止させるための機械制御装置に関し、特に、停止時の慣性力による制御対象の振動を抑制するようにした機械制御装置に関するものである。

図１０は従来の機械制御装置の構成を示しており、この機械制御装置は、長尺のアーム先端に搭載された重量物を搬送する機械を制御するためのものである。この機械制御装置は、重量物１と、アーム２と、スライド移動部３と、スライドレール部４と、モータ５と、カップリング６と、送りねじ７と、位置制御装置８と、位置指令装置９とを備えている。このうち、重量物１、アーム２、スライド移動部３、スライドレール部４、モータ５、カップリング６、及び送りねじ７は、制御対象機械として包括されるものである。

スライドレール部４は、スライド移動部３が図１０の左右方向（Ｘ軸方向）にのみ移動するように拘束する。スライドレール部４とスライド移動部３との接触部は低摩擦であり、スライド移動部３がスライドレール部４上を滑らかに移動できるようになっている。
また、モータ５の出力軸はカップリング６を介して送りねじ７と連結され、この送りねじ７は、スライド移動部３に取り付けられた図示しないねじ部内に螺挿されている。

モータ５が送りねじ７を回転駆動することにより、スライド移動部３が図１０の左右方向へ移動する。モータ５には制御回路及びモータドライバを含む位置制御装置８が接続されており、この位置制御装置８は位置指令装置９から送られた位置指令値に応じてスライド移動部３の位置制御を行う。
スライド移動部３に取り付けられた長尺のアーム２の先端には重量物１が取り付けられており、スライド移動部３の位置制御によりアーム２の先端の重量物１も位置制御されることとなる。

この機械制御装置においてスライド移動部３を移動させる場合、図１１（ａ）に示すように、時間ａ〜ｂでは所定加速度でスライド移動部３の移動速度を増大させ、時間ｂ〜ｃでは一定速度でスライド移動部３を移動させ、時間ｃ〜ｄでは所定減速度でスライド移動部３の移動速度を低下させ、最終的に時間ｄで目標位置に達したスライド移動部３を停止させる。位置指令装置９は、スライド移動部３が上記速度で移動するように時々刻々と変化する位置指令値Ｕ(ｓ)を出力する。なお、Ｕ(ｓ)はラプラス変換変数を用いる関数として表現する。

位置指令装置９からの位置指令値Ｕ(ｓ)は、位置制御装置８へ入力される。スライド移動部３の位置をＸｍ(ｓ)と表し、また、位置制御装置８からモータ５、送りねじ７、スライド移動部３までを含む伝達関数をＧ(ｓ)とすると、Ｘｍ(ｓ)は次式のように表される。

［数１］
Ｘｍ(ｓ)＝Ｇ(ｓ)・Ｕ(ｓ)

上記機械制御装置において、スライド移動部３は、位置指令値Ｕ(ｓ)に応じて図１１（ａ）に示した速度で移動して時間ｄで停止することとなり、重量物１が所定の目標位置に搬送される。
この機械制御装置では、位置制御装置８及び制御対象機械の伝達関数Ｇ(ｓ)を最適に設計することでスライド移動部３を目標位置へ正確に移動させる制御を行っているが、スライド移動部３に取り付けられたアーム２はたわみが生じた状態で移動し、スライド移動部３が目標位置で停止しても、アーム２の先端にある重量物１は自らの慣性力によりアーム２をたわませて指令位置より先に進むため、その後も即座に停止せずに振動する場合がある。このため、アーム２の先端に取り付けられた重量物１の位置を制御することが難しい。

具体的には、図１１（ａ）で示す如く、所定減速度で減速させて最終的に時間ｄで目標位置で停止させるようにスライド移動部３を制御すると、スライド移動部３は図１１（ｂ）の斜め方向に降りる実線で示すように指令位置で停止するにも拘わらず、重量物１は図１１（ｂ）の斜め方向に降りる点線で示すように目標位置を中心とした振動を生じてしまう。
このような重量物１の位置であるＸｗ(ｓ)は、スライド移動部３の位置であるＸｍ(ｓ)を用いて伝達関数として表示すると次式のようになる。

［数２］
Ｘｗ(ｓ)＝Ｋ_Ｓ・Ｘｍ(ｓ)／（Ｗｓ^２＋Ｋ_Ｓ ）

ここでＫ_Ｓ はアーム２のバネ定数であり、Ｗは重量物１の質量である。
以上の数式１，２をまとめて、機械制御装置の全体システムを表すと次式のようになる。

［数３］
Ｘｗ(ｓ)＝Ｋ_Ｓ・Ｇ(ｓ)・Ｕ(ｓ)／（Ｗｓ^２＋Ｋ_Ｓ ）

この重量物１の移動に関する伝達関数は、ラプラス平面上で以下に示すような極を有している。

［数４］
ｓ＝±√（Ｋ_Ｓ／Ｗ）

伝達関数が数式４のような極を有する場合、重量物１の位置は正弦波状の振動波形で移動することが知られ、次式で示すような周波数で振動する。

［数５］
ｆ＝（１／２π）√（Ｋ_Ｓ／Ｗ）

このように、重量物１は図１１（ｂ）における点線で示すように目標位置に到達した後も振動し、すぐには整定しない。
従って、スライド移動部３が目標位置で停止したときに、スライド移動部３の元位置は指令通りに停止するが、アーム２の先端位置にある重量物１は、目標位置を中心とした振動を生じてしまう。つまり、重量物１が直ちに停止しないため、停止するまでの時間が損失時間となる。
上記の点に鑑み、本出願人は、停止時における重量物の振動を抑制可能な機械制御装置を出願しており、この出願は後述の特許文献１として既に出願公開されている。

図１２は、上記特許文献１に係る機械制御装置の構成図、図１３は、図１２における補償要素１０の説明図、図１４は機械制御装置の動作説明図である。
この機械制御装置が図１０と異なるのは、位置制御装置８と位置指令装置９との間に補償要素１０を設けた点にあり、この補償要素１０は、図１３に示す如く微分手段１０ａとゲイン手段１０ｂと加算手段１０ｃとを備えている。

微分手段１０ａは、位置指令装置９からの位置指令値Ｕ(ｓ)に対して２階微分を施し、２階微分値ｓ^２・Ｕ(ｓ)を出力する。ゲイン手段１０ｂは、２階微分値ｓ^２・Ｕ(ｓ)に対してゲイン定数Ｋを乗算し、補正量Ｋ・ｓ^２・Ｕ(ｓ)を出力する。
加算手段１０ｃは、上記補正量Ｋ・Ｕ(ｓ)・ｓ^２と位置指令値Ｕ(ｓ)とを加算して補償位置指令値、すなわちＵ(ｓ)＋Ｋ・Ｕ(ｓ)・ｓ^２を出力する。
補償要素１０が出力する補償位置指令値は、（１＋Ｋ・ｓ^２）・Ｕ(ｓ)となり、このゲイン定数ＫをＫ＝Ｗ／Ｋ_Ｓとすると、以下の式で表される補償位置指令値Ｕ'(ｓ)が出力される。

［数６］
Ｕ'(ｓ)＝（１＋Ｗ・ｓ^２／Ｋ_Ｓ）・Ｕ(ｓ)

図１２の構成においてスライド移動部３を移動させる場合、その時間−速度線図は、図１４（ａ）に示す如く図１１（ａ）と同様になる。
図１２の位置指令装置９は、スライド移動部３が所定の速度となるように時々刻々と変化する位置指令値Ｕ(ｓ)を出力する。この位置指令値Ｕ(ｓ)は補償要素１０へ入力されて補償位置指令値Ｕ'(ｓ)に変換され、位置制御装置８へ入力される。スライド移動部３の位置をＸｍ(ｓ)とし、位置制御装置８からモータ５、送りねじ７、スライド移動部３までを含む伝達関数をＧ(ｓ)とすると、Ｘｍ(ｓ)は次式のようになる。

［数７］
Ｘｍ(ｓ)＝Ｇ(ｓ)・Ｕ'(ｓ)

更に、アーム２のたわみを考慮した重量物１の位置Ｘｗ(ｓ)は、スライド移動部３の位置をＸｍ(ｓ)を用いた伝達関数として表示すると、次式のようになる。

［数８］
Ｘｗ(ｓ)＝Ｋ_Ｓ・Ｘｍ(ｓ)／（Ｗｓ^２＋Ｋ_Ｓ ）

これらの数式からＸｍ(ｓ)、Ｕ'(ｓ)を消去して、機械制御装置の全体システムを表すと次式のようになる。

［数９］
Ｘｗ(ｓ)＝Ｇ(ｓ)・Ｕ(ｓ)

上述した補償要素１０により、数式８の分母部分にあったＷｓ^２＋Ｋ_Ｓ を消去する極零相殺を行って、振動要素をなくしている。これにより、図１４（ｂ）に示すように、スライド移動部３の元位置（実線にて示す）及び重量物１の位置（点線にて示す）は、振動することなく目標位置で停止し、整定する。
上記のように、スライド移動部３の元位置と共にアーム２の先端位置にある重量物１は振動することなく停止するため、従来の損失時間を無くし、高速な制御を実現することができるようになる。

図１５は上記特許文献１に係る機械制御装置の機能ブロック図である。図１５と図１２との対応関係について述べると、図１２における位置指令装置９、補償要素１０及び位置制御装置８が図１５におけるブロック２１〜２９，３１に相当し、図１２のカップリング６以降の機械系が図１５の対象機械５０に相当する。

図１５において、位置指令ブロック２１は位置指令値を出力するブロック、補正ブロック２２は位置指令値から生成した２階微分値（加速度指令値）に基づいて補正量を生成するブロックである。
この補正ブロック２２は、図１３の微分手段１０ａに相当する加速度検出手段２３と、制御対象の固有振動数などに基づいて設定された図１３のゲイン定数Ｋに相当するパラメータ２４と、図１３のゲイン手段（乗算手段）１０ｂに相当する補正項演算手段２５とから構成され、補正ブロック２２から出力される補正量は、図１３の加算手段１０ｃに相当する加算器２６において前記位置指令ブロック２１からの位置指令値に加算される。

図１５の加算器２６からは補償位置指令値が出力され、この補償位置指令値は位置調節器２７に入力される。位置調節器２７では、モータ５に取り付けられたモータエンコーダ３１からの位置検出値が補償位置指令値に一致するように調節動作が行われ、その出力が速度指令値として速度調節器２８に送られる。速度調節器２８では、モータエンコーダ３１からの速度検出値が速度指令値に一致するように調節動作が行われ、その出力がトルク指令値としてトルク調節器２９に送られる。
トルク調節器２９ではトルク指令値に従ってモータ５を駆動し、対象機械５０のスライド移動部３を所定位置に移動させて停止させるような制御が実行される。

なお、他の従来技術として、電動機制御系における機械振動を抑制する制振制御装置が下記の特許文献２に記載され、電動機のねじり振動を抑制する制御方法が特許文献３に記載されている。

特開２００３−７６４２６号公報（請求項２，３、［００３１］〜［００４５］、図１〜図３、図６等） 特開平７−３３７０５７号公報（請求項２，３、［００２３］〜［００３１］、図７〜図９等） 特開平８−１６８２８１号公報（請求項１〜３、［００５４］〜［００７０］、図１，図２，図４等）

上記各特許文献に記載された従来技術では、移動（または回転）する重量物や駆動軸のような制御対象の固有振動数等の情報を陰に陽に用いて、その振動を抑制している。
しかしながら、直交ロボット（Ｘ−Ｙロボット）やスカラロボット（水平多関節ロボット）のように、移動方向が異なる複数部材が結合された制御対象では、他方の部材の移動によって自方の部材の固有振動数等が変化してしまうため、十分な振動抑制作用が得られないという問題があった。

そこで本発明は、移動する制御対象が複数の部材を結合して構成されている場合にも十分な振動抑制作用を得ることができる機械制御装置を提供しようとするものである。

上記課題を解決するため、請求項１に記載した機械制御装置は、それぞれ異なる方向に移動可能な複数の部材が機械的に結合され、かつ、一方向に沿って複数の部材が一体的に移動可能な制御対象を、位置指令値に基づき所定位置に移動させて停止させるための機械制御装置であって、機械固有振動の補正量を加えることにより制御対象の振動を抑制する手段を備えた機械制御装置において、一の部材を前記一方向に沿って移動させる際に、前記補正量を他の部材の他方向に沿った位置情報を用いて変更するものである。

請求項２に記載した機械制御装置は、請求項１に記載した機械制御装置において、前記複数の部材が互いに直交する方向に移動可能であることを特徴とする。

請求項３に記載した機械制御装置は、請求項１または２に記載した機械制御装置において、他の部材の他方向に沿った位置に応じて変化する制御対象の固有振動数を用いて、前記補正量を変更するものである。

本発明によれば、例えば直交ロボットやスカラロボットのように移動方向が異なる複数部材が結合された制御対象に対しても、移動する一部の構成部材の現在位置に応じた固有振動数等の変化を考慮して補正を行うことにより、所定位置まで移動させた制御対象を振動させずに停止させることができる。

以下、図に沿って本発明の実施形態を説明する。
図１は第１実施形態の構成を示すもので、図１２と同一の構成要素には同一の参照符号を付してある。また、図１における補償要素１０は例えば前述の図１３のように構成され、位置指令装置９、補償要素１０及び位置制御装置８を実現する機能ブロック図は図２に示すとおりであって、その主要部分は前述の図１５と同一である。

図１において、モータ５により駆動される送りねじ７に沿ってＸ軸方向に移動するアーム２の基端部にはモータ１２が取り付けられており、アーム２に取り付けられた重量物１は前記モータ１２によりＺ軸（Ｘ軸に直交する軸）方向に移動可能である。なお、モータ１２の回転により重量物１をＺ軸方向に移動させるための機構は周知であるため、ここでは詳述を省略する。

位置制御装置８によって位置制御されるこの制御対象機械は、モータ５の回転によりスライド移動部３及びアーム２がＸ軸方向に駆動し、モータ１２の回転により重量物１がアーム２に沿ってＺ軸方向に移動するものである。
ここで、本実施形態における制御系には、重量物１のＺ軸方向の現在の位置情報が入力される制振用データテーブル１１が設けられており、このデータテーブル１１の出力データが補償要素１０に入力されている。

図２は、図１に示した実施形態の機能ブロック図であり、図１５と同一の構成要素には同一の参照符号を付してある。なお、図２の対象機械５０’は図１におけるカップリング６以降の部材を纏めて表示したものである。
また、図１２と図１５との対応関係と同様に、図１の位置指令装置９、補償要素１０及び位置制御装置８は図２におけるブロック２１〜２９，３１に相当し、特に図１の補償要素１０は図２の補正ブロック２２及び加算器２６に相当する。
図２が図１５と相違する点は、前述した制振用データテーブル１１の出力データが補正ブロック２２内のパラメータ２４に加えられている点である。

図１において、重量物１のＺ軸方向の位置を固定してアーム２のみをＸ軸方向に駆動する場合、アーム２のＸ軸方向の位置制御系は図１０と実質的に同様になる。その場合には、図２のパラメータ２４を制御対象（アーム２及びスライド移動部３等）の固有振動数等に基づいて設定すれば良く、これによって図１３におけるゲイン定数Ｋが決定され、補正量であるＫ・Ｕ(ｓ)・ｓ^２、ひいては補償位置指令値であるＵ'(ｓ）＝（１＋Ｋ・ｓ^２）・Ｕ(ｓ)が決定される。

しかし、Ｘ軸用のモータ５を駆動してアーム２をＸ軸方向に移動させると同時に、モータ１２を駆動して重量物１をＺ軸方向に移動させる場合には、重量物１を含む制御対象の固有振動数が変化するので、アーム２をＸ軸方向に移動させて停止させる際の制振制御を行うためには、変化する固有振動数に応じてパラメータ２４（ゲイン定数Ｋ）を変更する必要がある。
このため、制振用データテーブル１１は、重量物１のＺ軸方向の現在位置、この位置に応じた重量物１を含む制御対象の固有振動数、及びこの固有振動数に対応するパラメータ自体またはその補正係数等を格納したものであり、このデータテーブル１１の出力によって補正ブロック２２のパラメータ２４が変更され、補正項演算手段２５を介して加算器２６に加算される補正量としてのＫ・Ｕ(ｓ)・ｓ^２が変更される。

これにより、前述の数式６に示した補償位置指令値Ｕ'(ｓ）も変更され、図２の位置調節器２７、速度調節器２８、トルク調節器２９等のブロック（図１の位置制御装置８）は、重量物１のＺ軸方向の位置に関わらず、常にＸ軸方向の目標位置での停止時に重量物１の振動を抑制するような制御が実行される。

なお、上記第１実施形態では、位置指令値の２階微分値に所定のゲイン定数を乗じて補正量を求め、この補正量を元の位置指令値に加算して補償を行っている。つまり、位置指令値の１階微分値（速度指令値）が図３（ａ）のとおりであるとすれば、図３（ｂ）に示す位置指令値の２階微分値（加速度指令値）を元の位置指令値に加算して補正している。従って、この補正後の位置指令値の微分値、すなわち補正後の速度指令値は図３（ｃ）のようになる。
言い換えれば、図４（ａ）に示す速度指令値に対して、その２階微分値に所定のゲイン定数を乗じた図４（ｂ）の加加速度指令値を補正量として加算すれば、図４（ｃ）のような補正後の速度指令値が得られるため、図３に示した位置指令値に対する補正と同等の作用を得ることができる。

本発明の第２実施形態は上記の点に着目したものであり、図５にその制御ブロック図を示す。なお、図２と同一の構成要素には同一の符号を付してある。
図５において、補正ブロック３２には位置指令値が入力されており、その３階微分値を演算して加加速度指令値を求め、この加加速度指令値に、前記同様に制振用データテーブル１１を用いて変更された所定のゲイン定数を乗じて補正量を算出する。そして、この補正量を加算器２６において位置調節器２７の出力である速度指令値に加算することにより、速度指令値を補正する。この補正後の速度指令値は速度調節器２８に入力され、以後は図２と同様の動作となる。

図６は、本発明の第３実施形態を示す制御ブロック図である。
この実施形態では、補正ブロック３３が位置指令値を４階微分し、その４階微分値に制振用データテーブル１１を用いて変更された所定のゲイン定数を乗じることにより、補正量を算出する。そして、この補正量を加算器２６において速度調節器２８の出力であるトルク指令値に加算することにより、トルク指令値を補正する。この補正後のトルク指令値はトルク調節器２９に入力され、以後は図２と同様の動作となる。
なお、図示されていないが、前記補正量を推力指令値に加算してその加算結果に基づいて位置制御しても良い。

図７は、本発明の第４実施形態を示す制御ブロック図である。
この実施形態は、速度指令値を補正する点では図５と同様であるが、図５では位置指令値を３階微分して求めた加加速度指令値に基づいて補正量を演算するのに対し、図７では位置調節器２７から出力される速度指令値を補正ブロック３４に入力して２階微分することにより加加速度指令値を求め、この加加速度指令値に制振用データテーブル１１を用いて変更された所定のゲイン定数を乗じることにより、補正量を算出する。この補正量を位置調節器２７から出力される速度指令値に加算して補正を行う。以後の動作は図２と同様である。

図８は、本発明の第５実施形態を示す制御ブロック図である。
この実施形態では、補正ブロック３５が位置調節器２７から出力される速度指令値を３階微分し、その３階微分値に制振用データテーブル１１を用いて変更された所定のゲイン定数を乗じることにより、補正量を算出する。そして、この補正量を加算器２６において速度調節器２８の出力であるトルク指令値に加算することにより、トルク指令値を補正する。この補正後のトルク指令値はトルク調節器２９に入力され、以後は図２と同様の動作となる。
この実施形態においても、前記補正量を推力指令値に加算してその加算結果に基づき位置制御を行っても良い。

図９は、本発明の第６実施形態を示す制御ブロック図である。
この実施形態では、補正ブロック３６が速度調節器２８から出力されるトルク指令値を２階微分し、その２階微分値に制振用データテーブル１１を用いて変更された所定のゲイン定数を乗じることにより、補正量を算出する。そして、この補正量を加算器２６においてトルク指令値に加算することにより、トルク指令値を補正する。この補正後のトルク指令値はトルク調節器２９に入力され、以後は図２と同様の動作となる。
なお、トルク指令値の代わりに推力指令値の２階微分値から補正量を算出し、この補正量を推力指令値に加算してその結果に基づき位置制御を行っても良い。

なお、上述した各実施形態では、補正量の演算において完全微分演算を行い、例えば補正量をＫｓ^２として求めることとしたが、この補正量はＫｓ^２／（１＋ｓＴ）やＫｓ^２／（１＋ｓＴ_１＋ｓ^２Ｔ_２）のような不完全微分形式によって求めても良い。

前述した特許文献２，３のように制御対象の振動抑制機能がサーボ制御系の内部に組み込まれているようなシステムでは、移動する制御対象の現在位置に応じた固有振動数等を用いてサーボ制御系内部の制振パラメータ等を変更すれば良い。
更に、本発明は、図１に示したような対象機械ばかりでなく各種の直交ロボット、スカラロボット等に適用可能であり、何れにしても、移動方向が異なる複数部材が結合された制御対象の制振制御に用いることができる。

本発明の第１実施形態を示す構成図である。 図１の機能ブロック図である。 第１実施形態における位置指令値の補正原理を示す図である。 本発明の第２実施形態における速度指令値の補正原理を示す図である。 本発明の第２実施形態の機能ブロック図である。 本発明の第３実施形態の機能ブロック図である。 本発明の第４実施形態の機能ブロック図である。 本発明の第５実施形態の機能ブロック図である。 本発明の第６実施形態の機能ブロック図である。 従来技術を示す構成図である。 図１０の機械制御装置の動作説明図であり、図１１（ａ）は時間−速度線図、図１１（ｂ）は時間−位置線図である。 特許文献１に係る機械制御装置の構成図である。 図１２における補償要素の説明図である。 図１２の機械制御装置の動作説明図であり、図１４（ａ）は時間−速度線図、図１４（ｂ）は時間−位置線図である。 図１２の機能ブロック図である。

符号の説明

１ 重量物
２ アーム
３ スライド移動部
４ スライドレール部
５，１２ モータ
６ カップリング
７ 送りねじ
８ 位置制御装置
９ 位置指令装置
１０ 補償要素
１０ａ 微分手段
１０ｂ ゲイン手段
１０ｃ 加算手段
１１ 制振用データテーブル
２１ 位置指令ブロック
２２，３２，３３，３４，３５，３６ 補正ブロック
２３ 加速度検出手段
２４ パラメータ
２５ 補正項演算手段
２６ 加算器
２７ 位置調節器
２８ 速度調節器
２９ トルク調節器
３１ モータエンコーダ
５０’ 対象機械

Claims (3)

1. それぞれ異なる方向に移動可能な複数の部材が機械的に結合され、かつ、一方向に沿って複数の部材が一体的に移動可能な制御対象を、位置指令値に基づき所定位置に移動させて停止させるための機械制御装置であって、機械固有振動の補正量を加えることにより制御対象の振動を抑制する手段を備えた機械制御装置において、
   一の部材を前記一方向に沿って移動させる際に、前記補正量を他の部材の他方向に沿った位置情報を用いて変更することを特徴とする機械制御装置。
2. 請求項１に記載した機械制御装置において、
   前記複数の部材が、互いに直交する方向に移動可能であることを特徴とする機械制御装置。
3. 請求項１または２に記載した機械制御装置において、
   他の部材の他方向に沿った位置に応じて変化する制御対象の固有振動数を用いて、前記補正量を変更することを特徴とする機械制御装置。

Images