JP3220589B2

JP3220589B2 - メカニカルシステムの制御装置

Info

Publication number: JP3220589B2
Application number: JP04727894A
Authority: JP
Inventors: 清石前川
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 1994-03-17
Filing date: 1994-03-17
Publication date: 2001-10-22
Anticipated expiration: 2016-10-22
Also published as: JPH07261843A

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は、ロボット等のメカニ
カルシステムをサーボ制御する制御装置に関し、特に、
伝達機構の剛性が低いメカニカルシステムをも制御でき
るメカニカルシステムの制御装置に関するものである。

【０００２】

【従来の技術】一般に、ロボット等のメカニカルシステ
ムの制御を行う場合、各軸毎に独立に電流制御系、速度
制御系および位置制御系を構成する。すなわち、各軸毎
に、図１６に示すように、ロボット等を駆動するモータ
５の変位を検出するエンコーダなどの変位検出器４が設
けられ、変位検出器４の出力であるモータ変位量からモ
ータ速度を算出する差分要素２２が設けられる。位置制
御系は、位置決め指令値にモータ変位量をフィードバッ
クするための比較要素１２および位置ループゲインを与
える比例要素８で構成される。速度制御系は、位置制御
系の出力にモータ速度をフィードバックするための比較
要素１１および速度ループゲインを与える比例積分要素
７で構成される。速度制御系の出力は電流制御系６に入
力され、電流制御系６からの駆動電流がモータ５に与え
られる。

【０００３】図１６に示す構成では、比較要素１２に入
力される位置決め指令値とフィードバック値であるモー
タ変位とが一致するように、また、比例積分要素７に入
力される比例要素８からの速度指令とフィードバック値
であるモータ速度とが一致するようにフィードバック制
御がなされる。しかし、速度制御系において、モータ速
度のみがフィードバックされている。すなわち、負荷側
の状態量がフィードバックされないセミクローズドルー
プになっている。速度制御系にモータ速度のみがフィー
ドバックされる場合には、伝達機構の剛性が低いロボッ
ト等を高加減速で駆動すると振動が発生する。従って、
制御系のゲインを小さくするとともに、低加減速で駆動
しなければならない。

【０００４】そこで、速度制御系において、モータ速度
に加えて負荷側の状態量もフィードバックする全状態フ
ィードバックを行う手法が提案されている。そのような
手法の一例として、電学論Ｄ，第１０７巻第８号（昭和
６２年）の１０１８頁〜１０２５頁に記載された制御方
式がある。図１７はその方式の構成を示すブロック図で
ある。負荷側の状態量をフィードバックする場合、状態
量を直接センサで検出することが考えられる。しかし、
センサの取り付け場所の選定やセンサ出力のフィルタリ
ングの設計が難しい。そこで、図１７に示すように、フ
ィードバックすべき状態量を推定するオブザーバが組み
込まれる。

【０００５】上記文献に記載された方式では、オブザー
バ部２１は、電流指令値（トルク指令値）とモータ速度
とからばね要素のひずみ量とその微分値とを推定する。
オブザーバ部２１で推定されたばね要素のひずみ量とそ
の微分値とは、状態フィードバック部３を介して電流指
令値にそのままフィードバックされる。また、モータ速
度およびモータ変位もそのままフィードバックされる。

【０００６】

【発明が解決しようとする課題】従来のメカニカルシス
テムの制御装置は以上のように構成されているので、オ
ブザーバ部２１で負荷側の状態量を推定しているもの
の、推定された状態量、モータ速度およびモータ変位は
そのままフィードバックされる。制御対象がロボットな
どの場合には、負荷の慣性等のパラメータが大きく変動
する。また、重力や他の軸からの干渉項が外乱として作
用する。このように、制御対象のパラメータ変動が大き
い場合や大きな外乱が作用する場合には、オブザーバ部
２１の特性は著しく低下し、その結果、速度制御系の特
性も著しく低下する。そのために、やはり、振動を十分
に抑制することができない。従って、低加減速でロボッ
ト等を駆動しなければならず、モータ５の性能が良くて
もモータ５の能力を十分に発揮させることができない。
すなわち、モータ５の性能限界よりも低い性能しか引き
出せない。従って、ロボット等の動作に要する時間が長
くなり、ロボット等を用いた作業時間を短縮できないと
いう問題点があった。

【０００７】この発明は上記のような問題点を解消する
ためになされたもので、制御対象のパラメータ変動が大
きく、しかも、大きな外乱が作用する場合にも、伝達剛
性が低いメカニカルシステムを高加減速で駆動すること
を可能にし、ロボット等のメカニカルシステムを用いた
作業時間を短縮できるメカニカルシステムの制御装置を
得ることを目的とする。

【０００８】

【課題を解決するための手段】請求項１記載の発明に係
るメカニカルシステムの制御装置は、モータの出力に関
する情報ならびに電流制御系の入力もしくは出力または
入力および出力に関する情報を用いて、モータ側の状態
量と負荷側の状態量を出力する状態オブザーバ部と、モ
ータ周りの外乱オブザーバ、伝達機構周りの外乱オブザ
ーバおよび負荷周りの外乱オブザーバのうちの少なくと
も１つを有し、それらの外乱オブザーバによって、状態
オブザーバ部が推定した状態量から外乱トルクを推定す
るとともに状態オブザーバ部による状態量を修正する外
乱オブザーバ部と、外乱オブザーバ部が推定した外乱ト
ルクの推定値をトルク指令にフィードバックする外乱ト
ルクフィードバック手段と、外乱オブザーバ部が修正し
た状態量をフィードバックする状態量フィードバック手
段とを備えたものである。

【０００９】請求項２記載の発明に係るメカニカルシス
テムの制御装置は、請求項１記載のメカニカルシステム
の制御装置において、状態オブザーバ部に与えられる電
流制御系の入力もしくは出力に関する情報を、外乱オブ
ザーバ部による外乱トルクの推定値を用いて修正する入
力修正手段をさらに備えたものである。

【００１０】請求項３記載の発明に係るメカニカルシス
テムの制御装置は、請求項１記載のメカニカルシステム
の制御装置において、外乱トルクのフィードフォワード
項を算出するフィードフォワード項演算手段と、状態オ
ブザーバ部に与えられる電流制御系の入力もしくは出力
に関する情報を、フィードフォワード項演算手段が算出
したフィードフォワード項を用いて修正する入力修正手
段をさらに備えたものである。

【００１１】そして、請求項４記載の発明に係るメカニ
カルシステムの制御装置は、請求項１〜３記載のメカニ
カルシステムの制御装置において、状態オブザーバ部、
外乱オブザーバ部および状態量フィードバック手段にお
けるパラメータを、モータの動作状態に応じて設定する
制御系パラメータ設定手段をさらに備えたものである。

【００１２】

【作用】請求項１記載の発明における外乱オブザーバ部
は、状態オブザーバ部が推定した状態量を修正するとと
もに、外乱トルクを推定してそれをトルク指令にフィー
ドバックし、制御対象であるメカニカルシステムのパラ
メータ変動とメカニカルシステムに加わる外乱の影響を
低減する。

【００１３】請求項２記載の発明における入力修正手段
は、状態オブザーバ部の推定処理の入力を外乱オブザー
バ部が推定した外乱トルクの推定値で修正して、状態オ
ブザーバ部の推定処理の精度を向上させる。

【００１４】請求項３記載の発明における入力修正手段
は、状態オブザーバ部の推定処理の入力をフィードフォ
ワード項演算手段が算出したフィードフォワード項を用
いて修正して、状態オブザーバ部の推定処理の精度を向
上させる。

【００１５】そして、請求項４記載の発明における制御
系パラメータ設定手段は、状態オブザーバ部や外乱オブ
ザーバ部等が使用するパラメータを、そのときの制御対
象の動作状態に応じて設定する。

【００１６】

【実施例】

実施例１．図１はこの発明の第１の実施例によるメカニ
カルシステムの制御装置の構成を示すブロック図であ
る。図に示すように、ロボット等を駆動するモータ５の
変位を検出する変位検出器４が設けられる。位置制御系
は、指令値にモータ変位量をフィードバックするための
比較要素１２および位置ループゲインを与える比例要素
８で構成される。速度制御系は、位置制御系の出力にモ
ータ速度をフィードバックするための比較要素１１と速
度ループゲインを与える比例積分要素７とを含む。

【００１７】そして、この場合には、速度ループにおい
て、駆動電流とモータ変位とからモータ側および負荷側
の双方の状態量を推定する状態オブザーバ部１、状態オ
ブザーバ部１が推定した状態量を修正するとともに外乱
トルクを推定する外乱オブザーバ部２、外乱オブザーバ
部２の出力に所定のゲイン（一般に定数）を与える状態
フィードバック部３、比例積分要素７の出力に状態フィ
ードバック部３の出力をフィードバックするための比較
要素１０および外乱オブザーバ部２による外乱トルクの
推定値をフィードバックするための加算要素９が設けら
れる。速度制御系の出力は電流制御系６に入力され、電
流制御系６のからの駆動電流がモータ５に与えられる。

【００１８】なお、加算要素９は外乱トルクフィードバ
ック手段を構成し、状態フィードバック部３および比較
要素１０は状態量フィードバック手段を構成する。

【００１９】以下、メカニカルシステムとしてロボット
を例にとる。ロボットの各軸のモデルは、上記文献に記
載されているように、モータ３０、ばね要素による伝達
機構３１および負荷３２による２慣性系と考えられる。
このモデルを図２に示す。また、以下のように符号を付
ける。Ｊ_M ：モータ慣性Ｊ_L ：負荷慣性Ｋ_B ：伝達機構ばね定数 τ_m ：モータ駆動トルク τ_f ：ねじれトルク ω_m ：モータ速度 ω_l ：負荷速度 θ_m ：モータ変位

【００２０】すると、図２に示すモデルは図３のブロッ
ク図で表される。このモデルの状態方程式は（１）式で
表される。ただし、（１）式は、図２に示すモデルに対
応した部分の他に、モータ変位θ_m に関する部分も含
む。すなわち、状態ベクトルｘ_b は、θ_m ，ω_m ，ω
_l ，τ_f の４次元である。

【００２１】

【数１】

【００２２】従って、図２に示すモデルの状態方程式を
モータ変位θ_m まで拡張した場合、そのモデルに対し
て、状態ベクトルと同一次元のオブザーバは、以下のよ
うに表される。

【００２３】

【数２】

【００２４】ここで、添字ｈは推定値を示し、Ｇ₁ 〜Ｇ
₄ はオブザーバゲインである。図４は、（４）式に対応
した状態オブザーバ部１の構成を示すブロック図であ
る。状態オブザーバ部１は、モータ変位推定値θ_mhをモ
ータ変位θ_m にフィードバックするための比較要素１５
１、比較要素１５１の出力にオブザーバゲインＧ₁ 〜Ｇ
₄ を与える各ゲイン要素１０１〜１０４を有する。

【００２５】また、ゲイン要素１０１の出力とモータ速
度推定値ω_mhとを加算する加算要素１１１、加算要素１
１１の出力を積分してモータ変位推定値θ_mhを得る積分
要素１２１、ゲイン要素１０２の出力と係数要素１３１
の出力とを加算する加算要素１１２、加算要素１１２の
出力を積分してモータ速度推定値ω_mhを得る積分要素１
２２、ゲイン要素１０３の出力と係数要素１３２の出力
とを加算する加算要素１１３、加算要素１１３の出力を
積分して負荷速度推定値ω_lhを得る積分要素１２３、ゲ
イン要素１０４の出力と係数要素１３３の出力とを加算
する加算要素１１４、加算要素１１４の出力を積分して
ねじれトルク推定値τ_fhを得る積分要素１２４、モータ
駆動トルクτ_m からねじれトルク推定値τ_fhを減算する
減算要素１４１、モータ速度推定値ω_mhから負荷速度推
定値ω_lhを減算する減算要素１４２、減算要素１４１の
出力に定数（１／Ｊ_M ）を乗ずる係数要素１３１、ねじ
れトルク推定値τ_fhに定数（１／Ｊ_L ）を乗ずる係数要
素１３２、および減算要素１４２の出力に定数（ＫＢ）
を乗ずる係数要素１３３を有する。

【００２６】図５は外乱オブザーバの構成方法を説明す
るためのブロック図である。図に示すように、伝達関数
（１／ＪＳ）でモデル化される系モデル１００に、入力
ｘおよび外乱Ｔ_d が加わり、系モデル１００から出力ｙ
が出力されるとする。すると、出力ｙは、ｙ＝（ｘ＋Ｔ_d ）１／ＪＳ・・・（５）で表される。

【００２７】（５）式より、外乱Ｔ_d は、Ｔ_d ＝ＪＳｙ−ｘ・・・（６）のように、入出力から求められる。しかし、実用上、ノ
イズ等を考慮して１次遅れフィルタを用いて、Ｔ_dh＝（ＪＳｙ−ｘ）／（ＴＳ＋１）・・・（７）の式にもとづいて外乱推定値Ｔ_dhを求める。

【００２８】図６は外乱オブザーバ部２のうちの負荷周
りの外乱オブザーバの構成を示すブロック図である。こ
の外乱オブザーバは、状態オブザーバ１が推定したねじ
れトルク推定値τ_fhおよび負荷速度推定値ω_lhにもとづ
いて負荷周りの外乱トルクを推定し、負荷周りの外乱ト
ルク推定値τ_lih を出力するものである。すなわち、
（７）式において、Ｊ＝Ｊ_L ，ｘ＝τ_fh，ｙ＝ω_lh，Ｔ
_dh＝τ_fih の場合に相当する。

【００２９】従って、負荷周りの外乱オブザーバは、ね
じれトルク推定値τ_fhについて（１／ＴＳ＋１）に関す
る演算を行う演算要素２３１、負荷速度推定値ω_lhにつ
いて（Ｊ_L Ｓ／ＴＳ＋１）に関する演算を行う演算要素
２３２および演算要素２３１の出力から演算要素２３２
の出力を減算して負荷周りの外乱トルク推定値τ_lihを
得る減算要素２３３からなっている。

【００３０】図７は外乱オブザーバ部２のうちの伝達機
構周りの外乱オブザーバの構成を示すブロック図であ
る。この外乱オブザーバは、状態オブザーバ１が推定し
たモータ速度推定値ω_mhと負荷速度推定値ω_lhとの差、
および状態オブザーバ１が推定したねじれトルク推定値
τ_fhと負荷周りの外乱オブザーバが推定した負荷周りの
外乱トルク推定値τ_lih との差にもとづいて伝達機構周
りの外乱トルク推定値τ_tih を推定するものである。す
なわち、（７）式において、Ｊ＝１／Ｋ_B ，ｘ＝ω_mh−
ω_lh，ｙ＝τ_fh−τ_lih ，Ｔ_dh＝τ_tih の場合に相当す
る。

【００３１】従って、伝達機構周りの外乱オブザーバ
は、モータ速度推定値ω_mhから負荷速度推定値ω_lhを減
算する減算要素２０３、ねじれトルク推定値τ_fhから負
荷周りの外乱トルク推定値τ_lih を減算して修正された
ねじれトルク推定値τ_fha を得る減算要素２０４、減算
要素２０３の出力について（１／ＴＳ＋１）に関する演
算を行う演算要素２２１、修正されたねじれトルク推定
値τ_fha について（Ｓ／Ｋ_B （ＴＳ＋１））に関する演
算を行う演算要素２２２および演算要素２２１の出力か
ら演算要素２２２の出力を減算して伝達機構周りの外乱
トルク推定値τ_ti _h を得る減算要素２２３からなってい
る。

【００３２】図８は外乱オブザーバ部２のうちのモータ
周りの外乱オブザーバの構成を示すブロック図である。
この外乱オブザーバは、モータ駆動トルクτ_m と状態オ
ブザーバ１が推定したねじれトルク推定値τ_fhとの差、
およびモータ速度推定値ω_mhと伝達機構周りの外乱トル
ク推定値τ_tih との差にもとづいてモータ周りの外乱ト
ルク推定値τ_mih を推定するものである。すなわち、
（７）式において、Ｊ＝Ｊ_M ，ｘ＝τ_m −τ_fh，ｙ＝ω
_mh−τ_tih ，Ｔ_dh＝τ_mih の場合に相当する。

【００３３】従って、モータ周りの外乱オブザーバは、
モータ駆動トルクτ_m からねじれトルク推定値τ_fhを減
算する減算要素２０１、モータ速度推定値ω_mhから伝達
機構周りの外乱トルク推定値τ_tih を減算して修正され
たモータ速度推定値ω_mha を得る減算要素２０２、減算
要素２０１の出力について（１／ＴＳ＋１）に関する演
算を行う演算要素２１１、修正されたモータ速度推定値
ω_mha について（Ｊ_MＳ／ＴＳ＋１）に関する演算を行
う演算要素２１２および演算要素２１１の出力から演算
要素２１２の出力を減算してモータ周りの外乱トルク推
定値τ_mih を得る減算要素２１３からなっている。

【００３４】図９は、負荷周りの外乱オブザーバ、伝達
機構周りの外乱オブザーバおよびモータ周りの外乱オブ
ザーバからなる外乱オブザーバ部２の構成を示すブロッ
ク図である。

【００３５】図１０は、状態フィードバック部３の構成
を示すブロック図である。状態フィードバック部３は、
外乱オブザーバ部２によって修正されたモータ速度推定
値ω_mha から状態オブザーバ部１による負荷速度推定値
ω_lhを減算する減算要素３０１、修正されたねじれトル
ク推定値τ_fha に所定値Ｋ_1pを乗ずるゲイン要素３１
１、減算要素３０１の出力に所定値Ｋ_2pを乗ずるゲイン
要素３１２、およびゲイン要素３１１，３１２の出力を
加算する加算要素３２１とを備えている。

【００３６】次に動作について説明する。状態オブザー
バ部１は、変位測定器４からモータ変位θ_m を入力す
る。また、電流制御系６からモータの駆動電流値を入力
する。モータ駆動トルクτ_m は、駆動電流値にトルク定
数を乗ずることにより算出される。状態オブザーバ部１
は、図４に示す（４）式に応じた構成によって、モータ
変位θ_m とモータ駆動トルクτ_m とから、モータ変位、
モータ速度、負荷速度およびねじれトルクを推定する。
そして、モータ変位推定値θ_mh、モータ速度推定値
ω_mh、負荷速度推定値ω_lhおよびねじれトルク推定値τ
_fhを出力する。これらのうち、モータ速度推定値ω_mh、
負荷速度推定値ω_lhおよびねじれトルク推定値τ_fhは、
外乱オブザーバ部２に与えられる。また、モータ変位推
定値θ_mhは、モータ変位θ_m にフィードバックされる。

【００３７】外乱オブザーバ部２において、負荷周りの
外乱オブザーバは、負荷速度推定値ω_lhおよびねじれト
ルク推定値τ_fhを入力する。そして、負荷周りの外乱ト
ルクを推定する。すなわち、負荷周りの外乱トルク推定
値τ_lih を出力する。負荷周りの外乱トルク推定値τ
_lih は、後述するように、伝達機構周りの外乱オブザー
バおよびモータ周りの外乱オブザーバを介してトルク指
令にフィードバックされる。負荷周りの外乱トルク推定
値がフィードバックされるので、負荷に作用する外乱の
影響と負荷慣性Ｊ_L の変動の影響とが低減される。負荷
周りの外乱の影響は小さいと考えられるので、現実に
は、負荷慣性Ｊ_L の変動の影響が低減される。

【００３８】伝達機構周りの外乱オブザーバは、状態オ
ブザーバ１によるねじれトルク推定値τ_fhと負荷周りの
外乱オブザーバによる負荷周りの外乱トルク推定値τ
_lih との差をとって、修正されたねじれトルク推定値τ
_fha を出力する。また、状態オブザーバ１によるモータ
速度推定値ω_mhと負荷速度推定値ω_lhとの差、および修
正されたねじれトルク推定値τ_fha を用いて、伝達機構
周りの外乱トルクを推定する。すなわち、伝達機構周り
の外乱トルク推定値τ_tih を出力する。

【００３９】伝達機構周りの外乱トルク推定値τ_tih
は、後述するように、モータ周りの外乱オブザーバを介
してトルク指令にフィードバックされる。伝達機構周り
の外乱トルク推定値がフィードバックされるので、伝達
機構に作用する外乱の影響と伝達機構ばね定数Ｋ_B の変
動の影響とが低減される。伝達機構周りの外乱の影響は
小さいと考えられるので、現実には、伝達機構ばね定数
Ｋ_B の変動の影響が低減される。

【００４０】モータ周りの外乱オブザーバは、状態オブ
ザーバ１によるモータ速度推定値ω_mhと伝達機構周りの
外乱トルク推定値τ_tih との差をとって、修正されたモ
ータ速度推定値ω_mha を出力する。また、モータ駆動ト
ルクτ_m と状態オブザーバ１によるねじれトルク推定値
τ_fhとの差、及び修正されたモータ速度推定値ω_mhaか
らモータ周りの外乱トルクを推定する。すなわち、モー
タ周りの外乱トルク推定値τ_mih を出力する。モータ周
りの外乱トルク推定値τ_mih は、電流制御系６に対する
トルク指令にフィードバックされる。モータ周りの外乱
トルク推定値がフィードバックされるので、モータに作
用する重力、摩擦、他の軸からの干渉等の外乱の影響と
モータ慣性Ｊ_M の変動の影響とが低減される。モータ慣
性Ｊ_M の変動はごく小さいので、結局、モータに作用す
る外乱の影響が低減される。

【００４１】伝達機構周りの外乱オブザーバによる修正
されたねじれトルク推定値τ_fha 、モータ周りの外乱オ
ブザーバによる修正されたモータ速度推定値ω_mha およ
び状態オブザーバ１による負荷速度推定値ω_lhは、状態
フィードバック部３に入力する。したがって、ここで
は、負荷速度推定値ω_lhは修正されない。状態フィード
バック部３は、それらに所定のゲインＫ_1p，Ｋ_2pを与え
た後、速度ループゲインを与える比例積分要素７の出力
にフィードバックする。

【００４２】そして、比較要素１２に入力される位置決
め指令値とフィードバック値であるモータ変位とが一致
するように、比例要素８からの速度指令とフィードバッ
ク値であるモータ速度とが一致するように、また、比較
要素１１の出力と状態フィードバック部３からの状態量
とが一致するようにフィードバック制御がなされる。こ
こで、比較要素１１の一方の出力に入力されるモータ速
度として、修正されたモータ速度推定値ω_mha が用いら
れる。比較要素１２の一方の出力に入力されるモータ変
位として、変位測定器４からのモータ変位θ_m がそのま
ま用いられる。

【００４３】なお、この実施例では、状態オブザーバ１
としてモータ変位θ_m をもとに推定を行う同一次元オブ
ザーバを用いたが、モータ速度ω_m もとに推定を行うオ
ブザーバを用いてもよい。また、最小次元オブザーバな
ど他のオブザーバを用いてもよい。

【００４４】この実施例では、状態オブザーバ１におけ
るオブザーバゲインＧ₁ 〜Ｇ₄ は比例ゲインであった
が、図１１に示すように、積分ゲインを加えたゲイン要
素１０５を用いることもできる。さらに、ここでは、電
流制御系６の出力を用いたが、電流制御系６への入力を
オブザーバ入力としてもよい。

【００４５】また、この実施例では、フィードバックさ
れるモータ変位として変位測定器４からのモータ変位θ
_m をそのまま用いている。なぜなら、重力等の外乱によ
って、モータ変位推定値θ_mhがオフセットを持つ可能性
があるからである。一方、変位測定器４として用いられ
るエンコーダの分解能が粗い場合には電流値にパルス状
の雑音が乗ってしまう。よって、モータ変位推定値θ_mh
のオフセットがエンコーダの分解能に対して小さくでき
る場合には、モータ変位推定値θ_mhをフィードバックし
てもよい。

【００４６】位置制御系の出力に対してフィードバック
されるモータ速度として、修正されたモータ速度推定値
ω_mha を用いたが、エンコーダの出力の差分などによる
モータ速度を用いてもよい。その際、そのモータ速度
を、外乱オブザーバ部２に与えられるモータ速度推定値
ω_mhに代えて用いてもよい。

【００４７】実施例２．図１２はこの発明の第２の実施
例によるメカニカルシステムの制御装置における外乱オ
ブザーバ２の構成を示すブロック図である。その他の構
成は、図１に示す構成と同じである。この場合には、負
荷周りの外乱オブザーバの出力側に係数要素２４３が、
伝達機構周りの外乱オブザーバの出力側に係数要素２４
２が、そして、モータ周りの外乱オブザーバの出力側に
係数要素２４１が設けられる。

【００４８】係数要素２４３は、負荷周りの外乱トルク
推定値τ_lih を、所定の係数Ｋ_discを乗じてから伝達機
構周りの外乱オブザーバに供給する。係数要素２４２
は、伝達機構周りの外乱トルク推定値τ_tih を、所定の
係数Ｋ_disbを乗じてからモータ周りの外乱オブザーバに
供給する。そして、係数要素２４１は、モータ周りの外
乱トルク推定値τ_mih を、所定の係数Ｋ_disaを乗じてか
らトルク指令にフィードバックする。このようにする
と、電流のノイズを低減することができる。

【００４９】実施例３．外乱オブザーバ２が図１２に示
すように構成されている場合、以下のようにして、電源
投入時のロボットの手先部分の落下量を低減することが
できる。ｎ自由度ロボットの第ｉ軸の運動方程式は、以
下のようにかける。

【００５０】 τ_i ＝Ｊ_mi（ｄω_mi／ｄｔ）＋Ｍ_ii（ｄω_li／ｄｔ）＋｛Σ_j Ｍ_ij（ｄω_lj／ｄｔ）−Ｍ_ii（ｄω_li／ｄｔ）｝＋ｈ_i ＋ｇ_i ・・・（８）ここで、Ｍ_ijは慣性行列の要素、ｇ_i はｉ軸の重力項、
ｈ_i はｉ軸の遠心力・コリオリ力・摩擦力の項、右辺第
３項は干渉項である。

【００５１】電源投入時に、初期設定手段（図示せず）
は、そのときのロボットの関節変位とロボットの手先に
かかる手先負荷とから重力項ｇ_i の値を計算する。そし
て、速度ループゲインを与える比例積分要素７における
積分器の初期値を以下のように設定する。ｇ_i ・（１−Ｋ_disa）・ａそして、モータの起動を開始するとともにブレーキを解
放する。

【００５２】さらに、初期設定手段が、モータ周りの外
乱トルク推定値の初期値を、ｇ_i ・ｂと設定するように
してもよい。ここで、ａ，ｂは定数であり、例えば、
｛１／（減速比・トルク定数）が採用される。

【００５３】実施例４．状態オブザーバ部１が、図１１
に示すように積分ゲインを加えたゲイン要素１０５を用
いた構成になっている場合には、以下のようにして、電
源投入時のロボットの手先部分の落下量を低減すること
ができる。

【００５４】電源投入時に、初期設定手段（図示せず）
は、そのときのロボットの関節変位とロボットの手先に
かかる手先負荷とから重力項ｇ_i の値を計算する。そし
て、ゲイン要素１０５内の積分器の初期値を、（−ｇ_i
／Ｊ_Mi）・ｃと設定した後モータの起動を開始しブレー
キを解放する。ここで、ｃは定数であり例えば減速比の
逆数が採用される。

【００５５】実施例５．図１３はこの発明の第５の実施
例によるメカニカルシステムの制御装置の構成を示すブ
ロック図である。図に示すように、この場合には、状態
オブザーバ部１に与えられるモータ駆動トルクτ_m を修
正する入力修正手段１３が設けられる。

【００５６】この場合には、入力修正手段１３は、モー
タ駆動トルクτ_m と外乱オブザーバ部２からのモータ周
りの外乱トルク推定値τ_mih とを入力する。そして、τ
_mihに所定値（τ_mih の影響度に応じた値）を乗じた値
をτ_m から減算して修正モータ駆動トルクを得る。入力
修正手段１３は、修正モータ駆動トルクを状態オブザー
バ部１に与える。この場合には、状態オブザーバ部１
は、第１の実施例におけるモータ駆動トルクτ_m に代え
て修正モータ駆動トルクを用いる。その後の動作は第１
の実施例の場合と同様である。

【００５７】この場合には、状態オブザーバ部１が推定
のための入力として修正されたモータ駆動トルクを用い
るので、外乱の影響がより低減される。なお、この実施
例においても、状態オブザーバ１として、図１１に示す
構成を用いることができる。

【００５８】実施例６．メカニカルシステムの制御装置
が図１３に示すように構成され、状態オブザーバ１が図
１１に示すように構成されている場合には、以下のよう
にして、電源投入時のロボットの手先部分の落下量を低
減することができる。

【００５９】電源投入時に、初期設定手段（図示せず）
は、そのときのロボットの関節変位とロボットの手先に
かかる手先負荷とから重力項ｇ_i の値を計算する。そし
て、ゲイン要素１０５内の積分器の初期値を以下のよう
に設定する。 −ｇ_i ・（１−Ｋ_disa）・ａ／Ｊ_Ｍｉまた、モータ周りの外乱トルク推定値の初期値を、−ｇ
_ｉ・ｂと設定する。そして、モータの起動を開始する
とともにブレーキを解放する。

【００６０】実施例７．図１４はこの発明の第７の実施
例によるメカニカルシステムの制御装置の構成を示すブ
ロック図である。図に示すように、この場合には、状態
オブザーバ部１に与えられるモータ駆動トルクτ_m を修
正する入力修正手段１４と重力項や遠心力・コリオリ力
のフィードフォワード演算を行うフィードフォワード項
演算手段１５とが設けられる。

【００６１】図２に示したモデルは、（８）式の右辺第
１項および第２項が反映されているものである。従っ
て、その他の項を反映させるために、フィードフォワー
ド項演算手段１５は、例えば、位置決め指令値等を用い
て重力項ｇ_i を算出する。入力修正手段１４は、モータ
駆動トルクτ_m とｇ_i とを入力する。そして、τ_m から
ｇ_i を減算して修正モータ駆動トルクを得る。入力修正
手段１３は、修正モータ駆動トルクを状態オブザーバ部
１に与える。その後の動作は第１の実施例の場合と同様
である。

【００６２】この場合にも、状態オブザーバ部１が、修
正されたモータ駆動トルクを推定のための入力として用
いるので、外乱の影響がより低減される。なお、この実
施例においても、状態オブザーバ１として、図１１に示
す構成を用いることができる。

【００６３】実施例８．図１５はこの発明の第８の実施
例によるメカニカルシステムの制御装置の構成を示すブ
ロック図である。図に示すように、この場合には、状態
オブザーバ部１、外乱オブザーバ部２および状態フィー
ドバック部３におけるパラメータ、速度ループゲインな
らびに位置ループゲインを設定する制御系パラメータ設
定手段１６が設けられる。制御系パラメータ設定手段１
６は、状態オブザーバ部１における各パラメータのうち
の負荷慣性Ｊ_L およびオブザーバゲインＧ₁ 〜Ｇ₄ の
値、外乱オブザーバ部２における各パラメータのうちの
負荷慣性Ｊ_L 、状態フィードバック部３におけるゲイン
Ｋ_1p，Ｋ_2p、速度ループゲインならびに位置ループゲイ
ンを決定するものである。その他のパラメータは固定値
である。

【００６４】制御系パラメータ設定手段１６は、外部か
らモータ５の駆動状態、動作開始または終了点に関する
情報および手先負荷値を含む動作条件を入力する。そし
て、ロボット駆動の加速区間では、動作開始点と手先負
荷とから加速区間におけるパラメータを決定する。ま
た、ロボット駆動の減速区間では、動作終了点と手先負
荷とから減速区間におけるパラメータを決定する。決定
されたパラメータは、状態オブザーバ部１、外乱オブザ
ーバ部２、状態フィードバック部３、速度ループゲイン
を与える比例積分要素７および位置ループゲインを与え
る比例要素８に与えられる。等速区間では、加速区間に
おけるパラメータまたは減速区間におけるパラメータが
それらに与えられる。その後の動作は、第１の実施例の
場合と同様である。

【００６５】次にパラメータの設定方法の一例について
説明する。まず、制御系パラメータ設定手段１６は、モ
ータ慣性Ｊ_M ，負荷慣性Ｊ_L ，伝達機構ばね定数Ｋ_B
と、オブザーバゲインＧ₁ 〜Ｇ₄ ，状態フィードバック
部におけるゲインＫ_1p，Ｋ_2p、速度ループゲインおよび
位置ループゲインとの間の関係式をあらかじめ求めてお
く。加速区間では、動作開始点の関節変位と手先負荷の
値から慣性行列Ｍの対角項Ｍ_iiを計算し、Ｍ_iiの値をそ
れぞれの軸の負荷慣性Ｊ_L とする。次いで、求めたＪ_L
とあらかじめ求められている関係式とから、オブザーバ
ゲインＧ₁ 〜Ｇ₄，状態フィードバック部におけるゲイ
ンＫ_1p，Ｋ_2p、速度ループゲインおよび位置ループゲイ
ンを求める。減速区間では、動作終了点の関節変位と手
先負荷の値にもとづいて同様の処理を行う。

【００６６】なお、ここでは、図１に示す構成に対して
制御系パラメータ設定手段１６が設けられた場合につい
て説明したが、図１３や図１４に示す構成に対して制御
系パラメータ設定手段１６を適用することもできる。

【００６７】また、上記各実施例では、メカニカルシス
テムとしてロボットを例にとって説明したが、ロボット
以外のメカニカルシステムに対しても本制御装置は有効
な制御環境を提供する。

【００６８】

【発明の効果】以上のように、請求項１記載の発明によ
れば、メカニカルシステムの制御装置が、モータ側の状
態量と負荷側の状態量を推定し、推定された状態量から
外乱トルクを推定するとともに推定された状態量を修正
し、外乱トルクの推定値をトルク指令にフィードバック
するとともに修正した状態推定量を速度制御系にフィー
ドバックする構成になっているので、制御対象であるメ
カニカルシステムのパラメータ変動が大きく、しかも、
大きな外乱が作用する場合にも、伝達剛性が低いメカニ
カルシステムを高加減速で駆動することを可能にし、そ
の結果、ロボット等のメカニカルシステムを用いた作業
時間を短縮できるものが得られる効果がある。

【００６９】請求項２記載の発明によれば、メカニカル
システムの制御装置が、状態オブザーバ部に与えられる
電流制御系の入力もしくは出力に関する情報を外乱トル
クの推定値を用いて修正する構成になっているので、請
求項１記載の発明の効果に加えて、状態オブザーバ部の
推定処理の精度を向上させて外乱の影響をより低減でき
るものが得られる効果がある。従って、ロボット等の目
標軌道への追従性がより向上する。

【００７０】請求項３記載の発明によれば、メカニカル
システムの制御装置が、状態オブザーバ部に与えられる
電流制御系の入力もしくは出力に関する情報をフィード
フォワード項演算手段が算出したフィードフォワード項
を用いて修正する構成になっているので、請求項１記載
の発明の効果に加えて、状態オブザーバ部の推定処理の
精度を向上させて外乱の影響をより低減できるものが得
られる効果がある。

【００７１】そして、請求項４記載の発明によれば、メ
カニカルシステムの制御装置が、状態オブザーバ部、外
乱オブザーバ部および状態量フィードバック手段におけ
るパラメータをモータの動作状態に応じて設定する構成
になっているので、上記各効果に加えて、動作状況に応
じたきめ細かなパラメータ設定ができ、常に良好な振動
抑制を実現するものが得られる効果がある。

【図面の簡単な説明】

【図１】この発明の第１の実施例によるメカニカルシス
テムの制御装置の構成を示すブロック図である。

【図２】ロボットの各軸のモデルの一例を示す説明図で
ある。

【図３】図２に示すモデルをブロック化したものを示す
構成図である。

【図４】状態オブザーバ部の構成を示すブロック図であ
る。

【図５】外乱オブザーバの構成方法を説明するためのブ
ロック図である。

【図６】負荷周りの外乱オブザーバの構成を示すブロッ
ク図である。

【図７】伝達機構周りの外乱オブザーバの構成を示すブ
ロック図である。

【図８】モータ周りの外乱オブザーバの構成を示すブロ
ック図である。

【図９】外乱オブザーバ部の構成を示すブロック図であ
る。

【図１０】状態フィードバック部の構成を示すブロック
図である。

【図１１】状態オブザーバ部の他の構成を示すブロック
図である。

【図１２】この発明の第２の実施例によるメカニカルシ
ステムの制御装置における外乱オブザーバの構成を示す
ブロック図である。

【図１３】この発明の第５の実施例によるメカニカルシ
ステムの制御装置の構成を示すブロック図である。

【図１４】この発明の第７の実施例によるメカニカルシ
ステムの制御装置の構成を示すブロック図である。

【図１５】この発明の第８の実施例によるメカニカルシ
ステムの制御装置の構成を示すブロック図である。

【図１６】従来のメカニカルシステムの制御装置の構成
を示すブロック図である。

【図１７】従来の他のメカニカルシステムの制御装置の
構成を示すブロック図である。

【符号の説明】

１状態オブザーバ部２外乱オブザーバ部３状態フィードバック部（状態量フィードバック手
段）５モータ６電流制御系７比例積分要素（速度制御系）８比例要素（位置制御系）９加算要素（外乱トルクフィードバック手段）１０比較要素（状態量フィードバック手段）１１比較要素（速度制御系）１２比較要素（位置制御系）１３入力修正手段１４入力修正手段１５フィードフォワード項演算手段１６制御系パラメータ設定手段

Claims

(57)【特許請求の範囲】

【請求項１】メカニカルシステムを駆動するモータ
と、前記モータに与えられる電流を制御する電流制御系
と、メカニカルシステムの位置に関する情報を位置決め
指令に対してフィードバックする位置制御系と、メカニ
カルシステムの速度に関する情報を前記位置制御系の出
力にフィードバックして前記電流制御系に与えられるト
ルク指令を生成する速度制御系とを備えたメカニカルシ
ステムの制御装置において、前記モータの出力に関する
情報ならびに前記電流制御系の入力もしくは出力または
入力および出力に関する情報を用いて、前記メカニカル
システムを駆動するモータ側の状態量と負荷側の状態量
を出力する状態オブザーバ部と、モータ周りの外乱トル
ク推定値を出力するモータ周りの外乱オブザーバ、伝達
機構周りの外乱トルク推定値を出力する伝達機構周りの
外乱オブザーバおよび負荷周りの外乱トルク推定値を出
力する負荷周りの外乱オブザーバのうちの少なくとも１
つを有し、それらの外乱オブザーバによって、前記状態
オブザーバ部による状態量から外乱トルクの推定値を推
定するとともに前記状態オブザーバ部による状態量を前
記負荷周りの外乱トルク推定値または前記伝達機構周り
の外乱トルク推定値に基づいて修正する外乱オブザーバ
部と、前記外乱オブザーバ部による外乱トルクの推定値
を前記電流制御系に対する前記トルク指令にフィードバ
ックする外乱トルクフィードバック手段と、前記外乱オ
ブザーバ部が修正した状態量を前記速度制御系の出力に
フィードバックする状態量フィードバック手段とを備え
たことを特徴とするメカニカルシステムの制御装置。
【請求項２】状態オブザーバ部に与えられる電流制御
系の入力もしくは出力に関する情報を、外乱オブザーバ
部による外乱トルクの推定値を用いて修正する入力修正
手段を備えた請求項１記載のメカニカルシステムの制御
装置。
【請求項３】外乱トルクのフィードフォワード項を算
出するフィードフォワード項演算手段と、状態オブザー
バ部に与えられる電流制御系の入力もしくは出力に関す
る情報を、前記フィードフォワード項を用いて修正する
入力修正手段を備えた請求項１記載のメカニカルシステ
ムの制御装置。
【請求項４】モータの動作状態と、あらかじめ記憶し
てあるモータ慣性モーメント、伝達機構バネ定数、負荷
慣性モーメントと状態オブザーバ部、外乱オブザーバ部
および状態量フィードバック手段におけるパラメータと
の関係式に基づいて、状態オブザーバ部、外乱オブザー
バ部および状態量フィードバック手段におけるパラメー
タを設定する制御系パラメータ設定手段を備えた請求項
１〜３のうちいずれか１項記載のメカニカルシステムの
制御装置。