(57)【要約】【課題】  余分なセンサを必要とせず、状態フィードバック方式による振動を抑制した安定性確保と、目標値への追従性を積極的に行う制御装置を提供する。【解決手段】  オブザーバＣは、実際に制御したい特定部位の状態を表す状態変数αを推定する。操作入力から特定部位までの規範モデルＤを有する。オブザーバＣから出力される特定部位の状態変数の推定値α〜と規範モデルＤから出力される特定部位の理想応答値α＊との差を基に操作入力を補正する補償器Ｅを備える。この操作入力の補正により、規範モデルＤから出力である特定部位の理想応答値α＊とオブザーバＣで推定された推定値α〜が一致するように制御される。さらに、オブザーバＣの出力α〜をフィードバックして特定部位の目標指令αＲとの偏差を求めて制御器Ａで制御して特定部位が目標指令値に追従するように制御する。振動を抑制し、定常偏差がない制御ができる。【選択図】          図１
2005301508.tif 000002
【特許請求の範囲】
【請求項１】
機械の制御対象部位の状態変数を推定する手段と、前記制御対象部位までの規範モデルと、該規範モデルからの出力と前記推定された状態変数との差に基づいて補償量を算出し制御対象への操作量を補正する補償器と、前記制御対象部位の前記推定された状態変数または前記規範モデルの出力をフィードバックして指令値に追従するようにフィードバック制御して前記操作量を求めるフィードバック制御手段を備える制御装置。
【請求項２】
前記状態変数は加速度、速度または位置であり、前記補償量が補償トルクである請求項１に記載の制御装置。
【請求項３】
前記状態変数は２慣性系制御対象の負荷側の加速度、速度または位置であり、前記補償量が補償トルクである請求項１に記載の制御装置。
【請求項４】
前記補償トルクの計算が、規範モデルの出力と推定された状態変数との差分により前記規範モデルの逆特性または逆特性に更にフィルタを付加した関数をかけることにより行われる請求項２または請求項３に記載の制御装置。
【請求項５】
前記機械はロボットである請求項１乃至４の内いずれか１項に記載の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、機械の可動部位の振動を抑制する制御装置に関する。
【背景技術】
【０００２】
産業機械、工作機械、ロボット等の機械の制御対象部位の可動部は一般的に駆動源から各種伝動機構を介して駆動され、この機械可動部の位置や速度が制御されている。駆動源と機械可動部間に介在する伝動機構等の機械要素の剛性は一般的に低い。そのため、機械可動部を高速で移動させて、位置決め位置に停止させるような場合、機械可動部に振動が発生する。  
例えば、ロボットアームのアクチュエータについて考察すると、ロボットアームのアクチュエータの構成は図２１に示すように表される。図２１において、制御対象部位のロボットアーム１は歯車等で構成される減速機構２を介してモータ３によって駆動され、該モータ３に取り付けられた位置・速度検出器４によってロボットアームの位置、速度を検出し、位置、速度のフィードバック制御（セミクローズド・ループ制御）を行って、ロボットアームの位置、速度を制御している。
【０００３】
減速機構２の剛性は一般的に低く、図２１に示したロボットアーム駆動のモデルは、モータ３の電流に対してロボットアーム１の位置、速度までのブロック線図は、図２２に示すような２慣性系で表される。なお、簡単のために摩擦・粘性の影響は無視している。  
図２２において、ｉはモータ電流、Ｔｑはモータトルク、ωＭはモータ速度、θＭはモータ位置、ωＬはアーム速度、θＬはアーム位置、ＪＭはモータイナーシャ、ＪＬはアームイナーシャ、Ｋｔはモータトルク定数、Ｋは低剛性部バネ定数、ｓはラプラス演算子である。
【０００４】
このようなロボットアーム駆動系に対して、ロボットアームの位置、速度を制御する場合、図２３に示すように、セミクローズド・ループ制御が一般的に行われている。すなわち、位置・速度検出器４で検出されたモータ速度ωＭ、モータ位置θＭをフィードバックして位置、速度のループ制御を行っている。図２３において、θＲは位置指令であり、この位置指令θＲからフィードバックされたモータ位置θＭを減じて位置偏差を求め、位置制御ゲインＫｐ（関数）により位置ループ処理を行い速度指令を求め、速度指令からフィードバックされたモータ位置θＭを減じて位置偏差を求め、速度制御ゲインＫｖ（関数）により速度ループ処理を行い電流指令ｉを求めてモータ３を駆動するものである。
【０００５】
このセミクローズド・ループ制御では、モータ４に対する位置、速度のフィードバック制御であるから、モータ３の位置、速度は上手に制御できても、前述したように低剛性である減速機構２等の伝動機構の影響で、ロボットアーム１の位置が振動してしまうという問題が知られている。  
この対策として、ロボットアーム１等の機械可動部にその位置及び／または速度を検出する検出器を設けて、機械可動部の位置及び／または速度をフードバックして、図２４に示すようなフルクローズド・ループ制御が工作機械などで一般的に行われている。図２４では、図２３と比較し、アーム位置θＬ、アーム速度ωＬがモータ位置θＭ、モータ速度ωＭの代わりにフィードバックされている点が相違するのみである。このフルクローズド・ループ制御を適用する場合、ロボットアーム等の機械可動部の位置、速度を検出するための検出器を設けねばならないという問題がある。
【０００６】
また、セミクローズド・ループ制御で機械を制御し、機械の振動を抑制するために、更に、機械に速度検出器、加速度検出器を設けて、モータへのトルク指令を補正するようにした発明も知られている（特許文献１参照）。  
さらに、上述したように機械可動部の位置、速度を検出する検出器を設けることなく、その代わりに、オブザーバによって状態変数を推定し、この推定位置、速度をフィードバックして制御する方法も知られており、推定する状態変数やフィードバック方法について多くの提案がなされている。  
例えば、セミクローズド・ループ制御を行うと共に、モータと機械負荷との間のねじれ角、ねじれ角速度をオブザーバで推定し、モータへのトルク指令を補正することによって、機械の振動を抑制したものも知られている（特許文献２参照）。
【０００７】
また、リニアモータ制御において、リニアモータ可動部の重心点における位置、速度をオブザーバで推定し、この推定位置、速度をフィードバックして制御する方法も知られている（特許文献３参照）。  
さらに、制御対象の内部状態変数をオブザーバで推定し、観測出力の目標とすべき参照値から規範モデルにより規範状態量を求め、状態量推定値と規範状態量との差をフィードバックして制御する方法も知られている（特許文献４参照）。
【０００８】
【特許文献１】特開平６−１７８５７０号公報
【特許文献２】特開平７−２０９４０号公報
【特許文献３】特開２００１−１３６７６７号公報
【特許文献４】特開２００２−２８７８０４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
ロボットアーム等の制御対象部位の機械可動部の振動を抑制するために、フルクローズド・ループ制御で行う場合には、ロボットアーム等の機械可動部の位置、速度を検出する検出器を必要とする。セミクローズド・ループ制御においても、前述した特許文献１の方法で振動を抑制使用とする場合には、状態変数を検出する検出器を必要とする。検出器を設けるとなると、該検出器からのフィードバック信号を得るためにケーブル、信号受信回路などの多くの付加装置を必要とし、全体的なコストアップとなる。また、それぞれの装置のメンテナンスコストの上昇も避けられない。さらには、部品点数増加による信頼性低下の恐れもある。
【００１０】
また、状態フィードバック方式では、主に制御対象の安定化が目的であり、積極的に状態変数を目標値に追従させることはない。図２５は、状態フィードバック制御を用いた一般的な制御系のブロック図で、ロボットアーム等の機械可動部の位置、速度制御の制御系を例としたブロック図である。この例では、制御対象は、機械可動部を駆動するモータである。位置指令θＲとモータ位置θＭのフィードバック信号の偏差に基づいて位置制御ゲインＫｐにより位置ループ制御して速度指令を求め、速度指令とモータ速度ωＭのフィードバック信号の偏差と速度制御ゲインＫｖによりトルク指令を求め、該トルク指令よりオブサーバからフィードバックされて来る状態変数の補正量を減じて制御対象（モータ）の操作量であるトルク指令（電流指令）を制御対象に入力する。またオブザーバは制御対象に入力される操作量のトルク指令と制御対象のモータの速度ωＭを入力して推定した状態変数ｘ１，ｘ２を出力する。例えば、推定状態変数ｘ１＝機械可動部であるアームの位置θＬ、推定状態変数ｘ２＝機械可動部であるアームの速度ωＬ等である。この状態変数ｘ１，ｘ２に状態フィードバックゲインＫ１，Ｋ２をそれぞれ乗じてフィードバックし操作量を補正する。
【００１１】
状態フィードバック方式では、以上のような制御が一般的になされるものであり、制御対象が安定するように制御されるもので、積極的に目標値に追従させるようにしたものではない。
【００１２】
一方、特許文献３，４では、推定した状態変数を積極的に目標値に追従させるための提案がなされているが、機械によっては、制御対象が不安定で、十分なフィードバックゲインが得られないことがある。また、フィードバックゲインは機械特性毎に決定する必要があり、このフィードバックゲインの最適値を得るのが難しい。
【００１３】
そこで、本発明の目的は、余分なセンサを必要とせず、且つ、状態フィードバック方式による振動を抑制した安定性の確保と、目標値への追従性を積極的に行う制御装置を提供することにある。
【課題を解決するための手段】
【００１４】
本願請求項１に係わる発明は、機械の制御対象部位の状態変数を推定する手段と、前記制御対象部位までの規範モデルと、該規範モデルからの出力と前記推定された状態変数との差に基づいて補償量を算出し制御対象への操作量を補正する補償器と、前記制御対象部位の前記推定された状態変数または前記規範モデルの出力をフィードバックして指令値に追従するようにフィードバック制御して前記操作量を求めるフィードバック制御手段を備えた制御装置である。また、請求項２に係わる発明は、前記状態変数を加速度、速度または位置とし、前記補償量を補償トルクとしたものである。請求項３に係わる発明は、前記状態変数を、２慣性系制御対象の負荷側の加速度、速度または位置とし、前記補償量を補償トルクとしたものである。請求項４に係わる発明は、前記補償器による補償トルクの計算が、規範モデルの出力と推定された状態変数との差分により前記規範モデルの逆特性または逆特性に更にフィルタを付加した関数をかけることにより行うものとした。そして、請求項５に係わる発明は、前記機械をロボットとしたものである。
【発明の効果】
【００１５】
指令どおりに制御対象部位を制御でき、振動の内安定した応答性が得られる。また、実際に制御したい制御対象部位までの応答を規範モデルの応答と等しくできるため、規範モデルを制御対象と考えてフィードバックループが設計でき、ゲインの設計・調整が容易に行える。また、センサを必要としないから、アーム先端にセンサを設けることがほとんどないロボットの制御において、本発明は特に効果的である。
【発明を実施するための最良の形態】
【００１６】
図１は、本発明の概要ブロック図である。  
本発明においては、実際に制御したい特定部位の状態を表す状態変数αとして推定するオブザーバＣと、操作入力から上述した特定部位までの規範モデルＤと、オブザーバＣから出力される特定部位の状態変数の推定値α〜と規範モデルＤから出力される特定部位の理想応答値α＊との差を基に操作入力を補正する補償器Ｅを備えるものである。
【００１７】
特定部位への目標指令αＲとオブザーバＣで推定された特定部位の状態変数の推定値α〜の差を制御器に入力し、該制御器Ａから出力される操作量ｕに対して、補償器Ｅの出力を減じて補正された制御対象Ｂの操作入力をｖとする。そして、オブザーバＣは操作入力ｖと制御対象Ｂの制御量ｙを入力し特定部位の状態変数の推定値α〜を推定する。また規範モデルＤは操作入力ｖより制御しようとする特定部位の理想応答値α＊を求め出力する。このオブザーバＣから出力された推定値α〜から規範モデルＤで求められた特定部位の理想応答値α＊を減じてその差に基づいて補償器Ｅは、操作量の補正値を求めて、この補正量で操作量ｕを補正して操作入力ｖとする。これによって、特定部位の理想応答値α＊とオブザーバＣで推定された特定部位の状態変数の推定値α〜が一致するように制御する。さらに、オブザーバＣの出力α〜をフィードバックして特定部位の目標指令αＲとの偏差を求めて制御器Ａで制御して特定部位が目標指令値に追従するように制御する。オブザーバＣによる特定部位の状態の推定値α〜は実際の特定部位の状態変数値αに収束するものであるから、目標値に精度よく追従させることができるものである。
【００１８】
状態変数αとして、加速度を選ぶことにより、加速度を精度よく指令値に追従させることができ、同様に、速度、位置を選べば、速度、位置を指令に追従させることができる。しかも、機械可動部の先端位置の位置、速度、加速度を制御する場合にも、機械先端にセンサを設けることなく、機械可動部先端の位置、速度、加速度を、振動発生を抑制して指令どおりに追従させることができる。  
また、図１において、補償器Ｅを規範モデルＤの逆特性とすることにより、操作量ｖから制御したい特定部位の状態値αまでの応答を規範モデルＤと同じ特性にすることができる。ただし、規範モデルＤとしては通常プロパーな関数が選ばれ、その逆特性は分子の次数が分母の次数より大きくなるため、補償器Ｅには逆特性にその相対次数以上の次数を持つフィルタを含ませることが望ましい。
【００１９】
なお、図１に示した例では、オブザーバＣの出力をフィードバックして特定部位の目標指令との偏差を求めて制御器Ａで制御して特定部位が目標指令値に追従するように制御するようにしたが、規範モデルＤの出力である特定部位の理想応答値α＊とオブザーバＣで推定された特定部位の状態変数の推定値α〜は一致するように制御されるものであるから、オブザーバＣの出力の推定値α〜の代わりに規範モデルＤの出力である理想応答値α＊をフィードバックして目標指令値αＲとの偏差を求める制御するようにしてもよいものである。
【実施例】
【００２０】
以下、本発明をロボットのアーム制御に適用した実施例について以下説明する。  
図２は、本発明の各実施例のブロック図であり、制御対象Ｂとしてのロボット１０におけるロボットアームは、図２１で説明したモータ３、減速機構２，ロボットアーム１で構成されているものとする。また、制御装置２０は、全体を制御するメインＣＰＵ２１と、ＲＯＭ２６，ＲＡＭ２７，インタフェース２２，入出力回路２８、共有メモリ２３がバス２９で接続されている。また、インタフェース２２には、タッチペンダント等の表示・操作機器３０が接続され、入出力回路２８には、ロボットアーム先端に取り付けるハンド等の外部装置が接続されている。更に、共有メモリ２３には、ロボットアーム１を駆動するモータの位置、速度を制御するためのサーボＣＰＵ２４が接続されている。該サーボＣＰＵ２４にはサーボアンプ２５が接続され該サーボアンプ２５にはロボット１０のモータ３が接続されている。
【００２１】
メインＣＰＵ２１は、ＲＯＭ２６に格納されたシステムプログラムに基づいてこのシステム全体を制御し、ＲＡＭ２７に記憶されている教示プログラムに基づいてロボットの各軸への移動指令を、共有メモリ２３を介してサーボＣＰＵ２４に出力する。サーボＣＰＵ２４はこの移動指令とモータ３に取り付けられた位置・速度検出器４からの位置、速度のフィードバック信号に基づいて、位置、速度のフィードバック制御を行い電流指令をサーボアンプ２５に出力する。そしてサーボアンプは、実際にモータ３に流れる電流がこの電流指令どおりになるように制御する。
【００２２】
まず、第１の実施例として、制御する特定部位がロボットアーム先端加速度の場合のときを説明する。  
制御対象Ｂであるロボットアームの特性は図２２に記載したブロック図で示される２慣性系モデルとする。この制御対象Ｂは、次の（１）式で示される状態方程式により表現できる。  
【００２３】
【数１】
2005301508.tif 000003
（１）式においてｘ１，ｘ２，ｘ３は状態変数であり、ｘ１’，ｘ２’，ｘ３’は、各状態変数の微分値である。ｘ１はモータ速度、ｘ２はアーム速度、ｘ３はアーム加速度である。すなわち、ｘ１＝ωＭ、ｘ２＝ωＬ、ｘ３＝ａＬである。また、ｙは制御対象Ｂの制御量であり、ｙ＝ｘ１＝ωＭである。また、図１における入力操作量ｖは図２２、状態方程式のモータ電流ｉであり（ｖ＝ｉ）、制御量ｙは図２２及び状態方程式のモータ速度ωＭに対応するものである（ｙ＝ｘ１＝ωＭ）。  
オブザーバＣとしては、次の（２）式のものとする。
【００２４】
【数２】
2005301508.tif 000004
なお、（２）式において、ｘ１〜，ｘ２〜，ｘ３〜は、状態変数ｘ１，ｘ２，ｘ３の推定値であり、ｘ１〜’，ｘ２〜’，ｘ３〜’はその微分値である。また、ＪＭｎはＪＭのノミナル値、ＪＬｎはＪＬのノミナル値、ＫｎはＫのノミナル値、ＫｔｎはＫｔのノミナル値、Ｋ１，Ｋ２，Ｋ３はオブザーバパラメータである。なお、オブザーバ理論については広く一般的であり、公知のものであるから詳細は割愛する。また、外乱を状態変数に加えれば、更に推定精度が向上することが期待できるが、ここでは説明を簡単にするために外乱は考慮しないオブザーバを構成している。
【００２５】
そして、オブザーバパラメータＫ１，Ｋ２，Ｋ３をオブザーバの系が安定するように選べば、状態変数の推定値ｘ〜は実際の状態変数値ｘに収束することが知られている。また、この実施例では、制御したい制御対象Ｂの部位がアーム加速度であることから、オブザーバＣの出力としてアーム加速度ａＬである状態変数ｘ３の推定値ｘ３〜を使用する。
【００２６】
次にモデル規範制御について説明する。  
図３に示すようなモデル規範制御のブロック図において、操作量ｕから制御量ｙまでの伝達関数は次の（３）式となる。
【００２７】
【数３】
2005301508.tif 000005
なお、図３及び（３）式において、ｕは操作量、ｙは制御量であり、Ｇｐ（ｓ）は制御対象Ｂの伝達関数、Ｇｍ（ｓ）は規範モデルＤの伝達関数、Ｈ（ｓ）は補償器Ｅの伝達関数である。
【００２８】
（３）式において補償器Ｅの伝達関数Ｈ（ｓ）を規範モデルＤの伝達関数Ｇｍ（ｓ）の逆伝達関数、Ｈ（ｓ）＝１／Ｇｍ（ｓ）とすれば、操作量ｕから制御量ｙまでの伝達関数は次の（４）式となり、規範モデルＤの伝達関数Ｇｍ（ｓ）と等しいものとなる。
【００２９】
【数４】
2005301508.tif 000006
また、図４に示すようなモデル規範制御ブロック図を考えた場合、操作量ｕから制御量ｙまでの伝達関数は、次の（５）式で表される。
【００３０】
【数５】
2005301508.tif 000007
そして、補償器Ｅの伝達関数Ｈ（ｓ）として十分大きなゲイン関数を取れば、（６）式に示すように規範モデルＤの伝達関数Ｇｍ（ｓ）と等しいものとなる。  
【００３１】
【数６】
2005301508.tif 000008
よって、図３，図４のどちらのモデル規範制御を行っても、目的は達成できるが、この実施例では、図３に示したモデル規範制御を行うものとして以下説明する。
【００３２】
図２２で示したロボットアーム制御の２慣性系モデルでの電流指令ｉ（操作量ｕに対応）からアーム先端加速度ａＬまでの伝達関数は、次の（７）式で表される。
【００３３】
【数７】
2005301508.tif 000009
ここで、モータトルク定数Ｋｔ＝５［ｋｇｆ・ｃｍ／Ａ］、モータイナーシャＪＭ＝０．０１［ｋｇｆ・ｃｍ／ｓ２］、アームイナーシャＪＬ＝０．１［ｋｇｆ・ｃｍ／ｓ２］、バネ定数Ｋ＝２００［ｋｇｆ／ｒａｄ］としたとき、（７）式で示される伝達関数のボード線図は図５に示すものとなる。
【００３４】
このボード線図で示されるように、この伝達関数の応答は安定していない。そこで、安定するように、標準２次系での伝達関数の規範モデルＤとする。この規範モデルＤの伝達関数Ｇｍ（ｓ）を次の（８）式とする。
【００３５】
【数８】
2005301508.tif 000010
この（８）式において、Ｊｍは規範モデルイナーシャ、Ｋｔｍは規範モデルトルク定数で、ωｎはモデルの共振角周波数、ζはダンピング定数である。
【００３６】
この規範モデルＤの逆特性を示す、規範モデルＤの伝達関数の逆伝達関数Ｇｍ（Ｓ）−１は次の（９）式となる。
【００３７】
【数９】
2005301508.tif 000011
しかし、上記（９）式で示される逆伝達関数Ｇｍ（Ｓ）−１は、非プロパーで非常に微分要素が強い特性となり扱いにくい。そこで、モデル規範の補償器Ｅの伝達関数Ｈ（ｓ）としては、次の（１０）式とする。
【００３８】
【数１０】
2005301508.tif 000012
なお、（１０）式においてＦ（ｓ）はフィルタの伝達関数で、次の（１１）式で表される伝達関数である。なお、ωｆ、ζｆはフィルタパラメータである。
【００３９】
【数１１】
2005301508.tif 000013
そこで、規範モデルＤの伝達関数を（８）式のものとし、補償器Ｅの伝達関数を（１０）式のものとして、先の（２）式で示されるオブザーバＣを組み込んだブロック図は、図６に示すものとなる。
【００４０】
ここで、オブザーバパラメータとして制御対象Ｂのノミナル値を使用し、オブザーバ極を１２０Ｈｚパタワーズ極配置となるようにパラメータＫ１，Ｋ２，Ｋ３を決め、規範モデルＤとしては、規範トルク定数Ｋｔｍ＝５、規範モデルイナーシャＪｍ＝０．１、モデルの共振角周波数ωｎ＝２・π・２０、ダンピング定数ζ＝０．７、補償器Ｅのフィルタのパラメータωｆ＝２・π・１００［Ｈｚ］、ダンピング定数ζｆ＝１として、図６に示すブロック図の操作量ｕの電流指令ｉ’（ｉ’＝ｕ）から制御量のアーム加速度ａＬまでの伝達関数のボード線図は、図７に示すものとなる。
【００４１】
この図７から明らかのように制御対象Ｂは大きく安定化されている。  
そこで、電流指令ｉ’（＝ｕ）から制御量のアーム加速度ａＬまでの伝達関数を新たな制御対象Ｂとして、指令加速度からアーム先端加速度までの指令追従性確保のためのサーボループを構築する。ここでは簡単にするために、図８に示すように１型のサーボ系とする。このサーボ系の伝達関数は次の（１２）式となる。なお、図８，（１２）式において、ａＲは指令加速度、Ｋａは加速度ループゲインである。
【００４２】
【数１２】
2005301508.tif 000014
（１２）式において、Ｋａ＝Ｊｍ／（Ｔ・Ｋｔｍ）とすれば、この伝達関数は次の（１３）式となる。
【００４３】
【数１３】
2005301508.tif 000015
すなわち、時定数Ｔ（ｓ）の一次応答とすることができ、振動の発生がなく、定常偏差がない追従性のよいものとなる。  
この加速度制御ループを図６のモデル規範制御ループの外側に設けたものが、図９に示すブロック図である。  
図９に示す、加速度制御ループを組み込んだブロック図の、指令加速度ａＲからアーム加速度ａＬまでの伝達関数のボード線図を求めると、図１０に示すものとなる。この場合、加速度ループゲインＫａ＝Ｊｍ／Ｋｔｍ（１／２／π／１０）となるように選定されている。  
図１０に示されるように、安定した目標の制御応答が実現できる。オブザーバＣだけは制御対象Ｂの特性に合わせておけば、規範モデルＤ、補償器Ｅ、加速度制御ループゲインは、制御対象Ｂに関係なく再利用が可能となる。
【００４４】
そして、図９に示したブック図に、位置、速度ループを付加し、ロボットアーム制御の全体のブロック図は、図１１に示すものとなる。この図１１における移動指令θＲからアーム加速度ａＬを２回積分して得られるアーム先端位置θＬまでの伝達関数のボード線図は図１２に示すものとなる。また比較のために図２３で示したセミクローズド・ループ制御での移動指令θＲからアーム先端位置θＬのでの伝達関数のボード線図は図１３に示すものとなる。なお、共に、位置ループ帯域は２Ｈｚ、速度ループ帯域は５Ｈｚ（機械共振周波数７Ｈｚのため）としている。
【００４５】
この図１２，図１３を比較して分かるように、本実施例は安定した制御ができることが分かる。  
また、移動指令θＲにステップ入力を加えた場合のアーム先端位置θＬの応答は、本実施例では図１４（ａ）に示す応答となる。また図２３で示したセミクローズド・ループ制御での応答は図１４（ｂ）に示す応答となる。この図１４（ａ）と図１４（ｂ）を比較して明らかのように、従来のセミクローズド・ループ制御では振動が発生しているにも拘わらず、本実施例では、振動は発生せず、安定した制御がなされることが分かる。
【００４６】
上述した第１の実施例は、アーム加速度ａＬを制御する例を示した。次にアーム先端速度を制御する第２の実施例について、以下説明する。図２１、図２２に示したロボットのアーム１を制御対象Ｂとし、電流指令ｉからロボットのアーム先端速度ωＬまでの伝達関数は次の（１４）式となる。
【００４７】
【数１４】
2005301508.tif 000016
従って、規範モデルＤとしては次の（１５）式で示される伝達関数となる。
【００４８】
【数１５】
2005301508.tif 000017
この規範モデルＤの伝達関数Ｇｍ（ｓ）の相対次数は３次なので、補償器Ｅとして、次の（１６）式のようなものを選ぶ。
【００４９】
【数１６】
2005301508.tif 000018
この（１６）式におけるフィルタパラメータａ１〜ａ３は、系が安定するように選ぶものとする。また、オブザーバＣは、第１の実施例と同じものを使用でき、規範モデルＤの出力と比較するものとして推定アーム速度ωＬ〜に対応する推定状態変数ｘ２〜を使用する。  
又、規範モデルＤの固有角周波数ωｎが、速度制御ループの応答周波数に対して十分に高いとすれば、速度ループ設計時の制御対象Ｂとしては、次の（１７）式で示す伝達関数として扱える。
【００５０】
【数１７】
2005301508.tif 000019
従って、速度制御ゲインは、上記伝達関数Ｇ（ｓ）を制御対象Ｂとして、図１５に示されるブロック図の簡易モデルを用いて決定する。なお、ωＲは指令速度、Ｋｖは速度ループゲインである。
【００５１】
上記簡易モデルの伝達関数は次の（１８）式となる。
【００５２】
【数１８】
2005301508.tif 000020
この（１８）式において、速度ループゲインをＫｖ＝Ｊｍ／（Ｋｔｍ・Ｔ）とすれば、（１８）式で示される伝達関数は次の（１９）となり、時定数Ｔ（ｓ）の振動のない安定な応答を得ることができることを示している。
【００５３】
【数１９】
2005301508.tif 000021
以上のことから、制御したい特定部位をアーム先端速度ωＬとしたときの、指令速度ωＲからアーム先端速度ωＬまでのブロック線図は、図１６に示すものとなる。  
更に、位置ループをも組み込んだ場合には、図１７に示すブロック線図となる。
【００５４】
そこで、この図１７に示す位置ループも組み込んだ制御系において、位置指令θＲにステップ入力を加えた場合のアーム先端応答（速度）を求めると図１８に示す結果が得られた。このように安定した振動のない制御系が得られる。
【００５５】
なお、制御対象Ｂ、オブザーバＣ、規範モデルＤ内の標準２次系部パラメータ、Ｋｔｍ，Ｊｍには第１の実施例で使用した同じ値を用いた。又、フィルタパラメータａ１〜ａ３は、次の（２０）式で示される条件を満たすものとした。なお、この（２０）式において、δ＝１／ωｆであり、ωｆ＝２・π・100［Ｈｚ］とした。又速度制御ゲインＫｖ＝Ｊｍ／Ｋｔｍ／（１／２／π／５［Ｈｚ］）、位置制御ゲインＫｐ＝２・π・２［Ｈｚ］とした。
【００５６】
【数２０】
2005301508.tif 000022
次に、制御したい特定部位がアーム先端位置である場合について、第３の実施例として説明する。  
この場合、アーム先端加速度を状態変数に含むように、制御対象Ｂを状態方程式により表現すると、（２１）式で示されるものとなる。なお、（２１）式において、状態変数ｘ１はモータ速度ωＭ、ｘ２はモータ位置θＭ、ｘ３はアーム速度ωＬ、ｘ４はアーム位置θＬである。又、観可観測性の問題解決のため、制御量ｙをモータ位置ｘ２＝θＭとした。
【００５７】
【数２１】
2005301508.tif 000023
これより、オブザーバＣとしては、（２２）式で示されるものとした。
【００５８】
【数２２】
2005301508.tif 000024
（２２）式において、ｘ１〜，ｘ２〜，ｘ３〜，ｘ４〜は、状態変数ｘ１，ｘ２，ｘ３，ｘ４の推定値であり、ｘ１〜’，ｘ２〜’，ｘ３〜’ ，ｘ４〜’はその微分値である。また、ＪＭｎはＪＭのノミナル値、ＪＬｎはＪＬのノミナル値、ＫｎはＫのノミナル値、ＫｔｎはＫｔのノミナル値、Ｋ１，Ｋ２，Ｋ３，Ｋ３はオブザーバパラメータである。
【００５９】
そして、制御対象Ｂのロボットアームにおける電流指令ｉからアーム先端位置θＬまでの伝達関数は、次の（２３）式となる。
【００６０】
【数２３】
2005301508.tif 000025
従って、規範モデルＤとしては次の（２４）式で示される伝達関数のモデルとする。
【００６１】
【数２４】
2005301508.tif 000026
この規範モデルＤの伝達関数の相対字数は４次なので、補償器Ｅとして次の（２５）式で示されるような伝達関数のものとする。そして、この補償器Ｅのフィルタパラメータは安定するものを選択する。
【００６２】
【数２５】
2005301508.tif 000027
又、規範モデルＤの固有角周波数ωｎが、位置制御ループの応答周波数に対して十分に高いとすれば、位置ループ設計時の制御対象Ｂとしては、次の（２６）式で示す伝達関数として扱える。
【００６３】
【数２６】
2005301508.tif 000028
従って、位置制御ゲインは、上記伝達関数Ｇ（ｓ）を制御対象Ｂとして、図１９に示されるブロック図の簡易モデル（この実施例では、ループ伝達関数を安定化しやすいようにＰ−Ｄ制御器を使用）を用いて決定する。なお、θＲは指令位置、Ｋｐ１は位置ループ比例ゲイン、Ｋｐ２は位置ループ微分ゲインである。
【００６４】
上記簡易モデルの伝達関数は次の（２７）式となる。
【００６５】
【数２７】
2005301508.tif 000029
この（２７）式において、位置ループ比例ゲインＫｐ１を（２８）式、位置ループ微分ゲインＫｐ２を（２９）式で示すように決めれば、（２７）式で示される伝達関数は次の（３０）となり、任意の安定な応答を得ることができる。なお、（２８）〜（３０）式において、ωｃは位置制御応答の固有角周波数で、ζｃはダンピング定数である。
【００６６】
【数２８】
2005301508.tif 000030
【００６７】
【数２９】
2005301508.tif 000031
【００６８】
【数３０】
2005301508.tif 000032
以上のことから、この第３の実施例における、指令位置θＲからアーム先端位置θＬまでのブロック線図は、図２０に示したものとなる。
【図面の簡単な説明】
【００６９】
【図１】本発明の概要ブロック図である。
【図２】本発明をロボットのアーム制御に適用した一実施例の要部ブロック図である。
【図３】モデル規範制御の第１の態様のブロック図である。
【図４】モデル規範制御の第２の態様のブロック図である。
【図５】２慣性系モデルの電流指令からアーム先端加速度までの伝達関数のボード線図である。
【図６】本発明の第１の実施例における規範モデルとオブザーバを組み合わせたブロック図である。
【図７】図６において、電流指令から制御量のアーム加速度までの伝達関数のボード線図である。
【図８】指令加速度から先端加速度までのサーボループを１型の例とし、その説明図である。
【図９】図６のブロック図に加速度制御ループを付加したときのブロック図である。
【図１０】加速度制御ループを組み込んだ制御系における指令加速度からアーム加速度までの伝達関数のボード線図である。
【図１１】本発明の第１の実施例における位置、速度ループをも組み込んだ全体のブロック図である。
【図１２】移動指令からアーム先端位置までの伝達関数のボード線図である。
【図１３】従来のセミクローズド・ループ制御での移動指令からアーム先端位置までの伝達関数のボード線図である。
【図１４】本第１の実施例と従来のセミクローズド・ループ制御において、位置指令のステップ入力に対するアーム先端の応答を表す図である。
【図１５】本発明の第２実施例における指令速度からロボットアーム速度までの簡易モデルのブロック線図である。
【図１６】同第２の実施例における指令速度からロボットアーム速度までのブロック線図である。
【図１７】同第２の実施例における位置ループをも組み込んだブロック線図である。
【図１８】同第２の実施例における図１７に示す位置ループも組み込んだ制御系において、位置指令にステップ入力を加えた場合のアーム先端応答を示す図である。
【図１９】本発明の第３の実施例における位置指令からロボットアーム先端位置までの簡易モデルのブロック線図である。
【図２０】同第３の実施例における指令位置からロボットアーム先端位置までのブロック線図である。
【図２１】ロボットアームのアクチュエータの構成の説明図である。
【図２２】ロボットアーム駆動における、モータの電流に対してロボットアームの位置、速度までのブロック線図である。
【図２３】セミクローズド・ループ制御よるロボットアームの位置、速度の制御のブロック線図である。
【図２４】フルクローズド・ループ制御よるロボットアームの位置、速度の制御のブロック線図である。
【図２５】状態フィードバック制御を用いた一般的な制御系のブロック図である。
【符号の説明】
【００７０】
１０  ロボット
２０  制御装置
【図１】
2005301508.tif 000033
【図２】
2005301508.tif 000034
【図３】
2005301508.tif 000035
【図４】
2005301508.tif 000036
【図５】
2005301508.tif 000037
【図６】
2005301508.tif 000038
【図７】
2005301508.tif 000039
【図８】
2005301508.tif 000040
【図９】
2005301508.tif 000041
【図１０】
2005301508.tif 000042
【図１１】
2005301508.tif 000043
【図１２】
2005301508.tif 000044
【図１３】
2005301508.tif 000045
【図１４】
2005301508.tif 000046
【図１５】
2005301508.tif 000047
【図１６】
2005301508.tif 000048
【図１７】
2005301508.tif 000049
【図１８】
2005301508.tif 000050
【図１９】
2005301508.tif 000051
【図２０】
2005301508.tif 000052
【図２１】
2005301508.tif 000053
【図２２】
2005301508.tif 000054
【図２３】
2005301508.tif 000055
【図２４】
2005301508.tif 000056
【図２５】
2005301508.tif 000057