WO1990008987A1 - Procede de servocommande utilisant un detecteur d'evaluation de perturbation - Google Patents

Description

明 細 書

外乱推定オ ブザー バに よ る サ ー ボ制御方法

技 術 分 野

本発明は、 各種機械の耐外乱性を向上可能なサ ー ボ制 御方法に関 し、 特に、 イ ナ — シ ャ 変動の大 き い機械にお いて も、 広い周波数領域にわた る各種外乱を外乱推定ォ ブザー バによ り 適正に推定でき る サ ー ボ制御方法に関す る 0

背 景 技 術

片持ち梁構造の ロ ボ ッ ト を含む低剛性の各種機械は、 低周波帯域での耐振動特性が悪 く、 ま た、 低周波の外乱 に弱い。 そ こ で、 従来は、 積分器を含むサ ー ボ系を機械 に搭載 して比例積分制御を行う こ と によ り サ — ボ系を含 む機械全体の剛性を増大 させ、 振動発生防止及び耐外乱 性の向上を図 っ ている。 しか しなが ら、 積分器を備え る サ — ポ系は応答性が悪 く、 周期的な外乱の影響を受け易 い。

ま た、 外乱推定オ ブザー バを用いて外乱を推定 し、 推 定外乱を用いて外乱除去 （ゼ ロ イ ン グ） を行 う こ と が知 られて い る。 外乱推定オ ブザー バで得た外乱推定値は低 周波領域で位相回転を来す こ と がな く、 換言すれば、 外 乱推定ォ ブザ— バを装備 したサ ー ボ系は周期的な外乱に 強い。 し か し、 イ ナ ー シ ャ変動の大き い機械例えばロ ボ ッ ト に外乱推定オ ブザー バを搭載 した場合、 外乱推定ォ ブザー バの外乱推定モ デルに おいて イ ナ — シ ャ に関連 し かつ固定的に設定されるパラ メ 一 タ の値が機械のィ ナ ー シ ャ と不適合にな り易 く、 外乱の推定を適正に行えない こ とがあ る。 ま た、 外乱推定オブザーバには、 外乱推定 値が高周波領域で位栢遅れ （位相回転） を来し易い と云 う不具合があ る。

発 明 の 開 示

本発明の目的は、 イ ナ ー シ ャ変動の大き い機械を含む 各種機械において外乱推定ォブザーバによ る外乱推定を 適正に行え、 各種機械の耐外乱性を向上可能なサー ポ制 御方法を提供する こ と にある。

本発明の別の 目的は、 外乱推定ォブザー パで得た外乱 推定値の位相回転を広い周波数帯域と く に高周波帯域に おいて防止して各種周波数帯域の外乱を適正に推定し、' も っ て、 外乱除去を確実に行える サー ボ制御方法を提供 する こ と にあ る。

上述の 目的を達成する ため、 本発明によ れば、 外乱推 定ォブザ—バに よ るサー ボ制御方法が提供 され、 こ のサ — ボ制御方法は、 サ— ボ系を装備 した機械の、 当該機械 の運転状態に応 じて変化する ィ ナ ー シ ャ を周期的に決定 する工程 （ a ) と、 斯 く 決定したイ ナ — シ ャ に基づいて かつ外乱推定オ ブザー バを用いて前記機械に加わ る外乱 を推定 して推定外乱を決定する工程 （ b ) と、 前記サー ボ系に おいて前記推定外乱を除去する工程 （ c ) と を備

X- Ό o

好ま し く は、 前記工程 （ b ) において前記推定外乱の 高周波成分を除去する こ と によ り 補正推定外乱を決定 し- 前記工程 （ c ) で前記サ ー ボ系において前記補正推定外 乱を除去する。

上述のよ う に、 本発明によれば、 外乱推定オ ブザー バ を用いて機械に加わる外乱を推定， 除去す る よ う に した ので、 サ — ボ系を含む機械全体の剛性が増大 し、 ま た、 外乱推定値の位相が低周波帯域において回転 しない。 結 果と し て、 機械は周期的な外乱につ いての耐外乱性が向 上する。 ま た、 サ — ボ系を装備 した機械の、 当該機械の 運転状態に応 じ て変化す る イ ナ — シ ャ を周期的に決定 し、 斯 く 決定 したイ ナ — シ ャ に基づいて外乱を推定する よ う に し た ので、 外乱推定ォ ブザ— バの外乱推定モ デルにお いてィ ナ ー シ ャ と 関連 し て用い ら れるパ ラ メ 一 夕 が機械 の イ ナ 一 シ ャ に常に適合する よ う に当該パ ラ メ 一 夕 を可 変設定で き、 外乱推定を適正に行える。 さ ら に、 推定外 乱の高周波成分を除去す る こ と に よ り 決定 した補正推定 外乱を除去する よ う に し たので、 外乱推定値の位相回転 を高周波帯域を含む広い周波数帯域において防止で き、 外乱推定が適正化される。

図 面 の 簡 単 な 説 明

第 1 図は本発明の作動原理を示すサ ー ボ系のプ ロ ッ ク 線図、 第 2 図は本発明の一実施例によ る サ ― ボ制御方法 が適用 さ れる ロ ボ ッ 卜 の要部を示す概略ブ ロ ッ ク 図、 お よ び、 第 3 図は第 2 図の デ ジタ ルサ — ボ回路のデ ジ タ ル プ ロ セ ッ サが実行する外乱推定， 除去処理の フ ロ ー チ ヤ 一 卜であ *σ o

発明を実施するための最良の形態

以下、 本発明のサー ボ制御原理を説明す る。

一般に、 ロ ボ ッ ト （図示略） を含む各種機械 （以下、 ロ ボ ッ ト と云 う ） に搭載 したサー ボモ ー 夕 （図示略） に は、 ロ ボ ッ ト の作動部 （以下、 ロ ボ ッ 卜 と云う ） が移動 開始する と き に ロ ボ ッ ト に作用する靜摩擦， ロ ボ ッ ト移 動中に作用する動摩擦， ロボ ッ ト移動方向に応 じて変化 する重力の影響な どの各種の外乱が加わる。

本発明は、 外乱推定オ ブザーバを用いて各種外乱を推 定しかつ除去す る と共に、 ロ ボ ッ ト ハ ン ドで把持してい た ワ ー ク を離す動作などに起因 して ロ ボ ッ 卜 のイ ナ— シ ャが変化 した場合に も外乱推定， 除去を適正に行う こ と を企図する。

第 1 図のブロ ッ ク線図を参照する と、 本発明によれば、 図示 し ないサー ボ回路か ら送出される電流指令 （ ト ル ク 指令） I か ら後述の補正推定外乱 y ' を減 じて得た実際 電流指令 I 一 y ， が、 サ ー ボモー タ （図示略） に関連す る伝達要素 1， 2 を介 して実際モ ー タ速度 V に変換され る。 図中、 符号 K t 及び J mは、 サ一 ボモ ー タ の ト ル ク 定数及びロ ボ ッ ト の全イ ナ一 シ ャ を夫々示 し、 T Lは外 乱 ト ル ク を示す。

外乱推定ォ ブザ一バ 5 は、 伝達関数が夫々 K t / J m， K 1 , Κ 2及び 1 / S であ る伝達要素 5 1 〜 5 4 を含み, 下記第 （ 1 ) 式及び第 （ 2 ) 式が成立する ロ ボ ッ ト運転 条件に適合する よ う に構成されている。

d T d t = 0 · · · ( 1 ) d v / d t = ( I - K t / J m ) + ( T L/ J m )

• · · ( 2 ) こ の外乱推定オ ブザー バ 5 にお いて、 実際電流指令 I - y ' を入力す る伝達要素 5 1 か ら の出力 と、 伝達要素 5 2 の出力 と、 伝達要素 5 3 の出力 X と を加算 した も の が、 伝達要素 5 4 に印加 される。 そ して、 実際モ ー 夕速 度 V か ら伝達要素 5 4 の出力 （推定モ ー タ 速度） V ' を 減 じた も のが伝達要素 5 2， 5 3 の各々 に印加される。

さ ら に、 本発明によれば、 外乱推定オ ブザー バ 5 の伝 達要素 5 3 の出力 X を入力 し フ ィ ー ドバ ッ ク ゲイ ンを調 整する た めの伝達要素 3 か ら の出力 （推定外乱） y がフ ィ ル夕 4 に印加 さ れて推定外乱 y の高周波成分が除去さ れる。 図中、 符号 y ， は フ ィ ルタ 4 か ら送出 さ れる補正 推定外乱を表 し、 符号 A はフ ィ ー ドバ ッ ク ゲイ ンを表す。

第 1 図に示す ブ ロ ッ ク 線図か ら明 らかな よ う に、 実際 モー タ 速度 V 及び推定モ ー タ速度 V ' は下記第 （ 3 ) 式 及び第 （ 4 ) 式に よ り夫々表される。

V = { ( I - y ' ) - K t + T L} ·

( 1 / J m · S ) · · · ( 3 ) v ' = { ( i - y ' ) - ( K t / J m )

+ ( v — v ， ) · K 1 + ( v — v ， ) - ( K 2 / S ) } - ( 1 / S ) · · · ( 4 ) 第 （ 3 ) 式及び第 （ 4 ) 式か ら下記第 （ 5 ) 式を得る。 v 一 v， = ( T Lん J m ) · [ 1 / { S + K 1

+ ( K 2 X S ) } ] · · · ( 5 ) 従っ て、 伝達要素 5 3 の出力 x は下記第 （ 6 ) 式で表 ■© O

X = ( V — ν ， ） · （ Κ 2ゾ S )

= ( T LZ J m ) · [ K 2 / { S ² + K 1 · S

+ K 2 } ] · · · ( 6 ) こ こで、 極が安定する よ う に伝達関数パ ラ メ 一 夕 K 1， K 2 の値を選択する と、 下記第 （ 7 ) 式に示すよ う に、 伝達要素 5 3 の出力 （ゲイ ン調整前の推定外乱） X は外 乱 ト ル ク T Lに比例する値を と る。

X ^ T L/ J m = T L， Z J m . . . ( 7 ) この場合、 伝達要素 3 の出力 （ ゲ イ ン調整後の推定外 乱） y 及び伝達要素 1 の出力 K t · ( I - y ' ) は下記 第 （ 8 ) 式及び第 （ 9 ) 式で夫々表される。

y = x - J m - A/ t = T L' · A / K t

• · · ( 8 )

K t · ( I - y ' ) = K t · ( I 一 T L' - A / K t )

= K t - I - T L' - A

· · - ( 9 ) こ こ で、 符号 A はフ ィ ー ドバ ッ ク ゲイ ンを表す。

結局、 伝達要素 1 の出力 K t ' ( I - y ' ) と外乱 ト ルク T Lと の和は値 K t · I に近い値を と る。 すなわち、 外乱が除去される。

さ ら に、 本発明によれば、 ロ ボ ッ ト の運転状態変化に 起因す る ロ ボ ッ ト の全ィ ナ — シ ャ J mの変ィ匕に応 じて、 伝達要素 3 , 5 1 の伝達関数パ ラ メ ー タ J mが可変設定 さ れる。 こ のゲ イ ン ス ケ ジ ユ ー リ ン グに よ り 外乱推定ォ ブザー バ 5 の外乱推定モ デルがロ ボ ッ 卜 の運転状態に常 に適合 さ れ、 イ ナ ー シ ャ 変動の大き い ロ ボ ッ 卜 につ いて も外乱推定， 除去が適正に行われる。

以下、 第 2 図を参照 し て、 本発明の一実施例によ る サ — ボ制御方法を実施する ための ロ ボ ッ ト を説明する。

ロ ボ ッ ト は、 デ ジ タ ルサ 一 ボ回路 1 0 と サ 一 ボ ア ン プ 1 1 と を含むサ ー ポ制御装置と、 パルス コ ー ダ 1 3 を装 着 した サ — ボモ 一 夕 1 2 と を備えて い る。

デ ジ タ ルサ 一 ボ回路 1 0 はデ ジ タ ル シ グナ ルプ ロ セ ッ サ （図示略） を有 し、 従来公知の位置， 速度， ト ル ク 制 御機能を備えてい る （デ ジタ ルサ — ボ回路 1 0 の、 位置, 速度， ト ル ク制御機能を、 第 2 図の機能ブ ロ ッ ク 図にサ ー ボ回路部 1 0 a と して示す） 。 すなわち、 サ ー ボ回路 部 1 0 a において、 ロ ボ ッ ト を制御す る た めの数値制御

(図示略） に内蔵の メ イ ン プ ロ セ ッ サ か ら の移動 ί 令 と ノ、。 ノレ ス コ ー ダ 1 3 か ら の フ ィ 一 ドノ、' ッ ク ノ、。 ル ス F Ρ と に基づいて位置偏差が求め られ、 位置偏差に応 じて算 出 した指令速度 と フ ィ ー ドバ ッ ク パ ル ス F Ρ の発生周波 数に応 じて求めた実際モ ー タ速度 と の偏差が求め られ、 さ ら に、 こ の速度偏差に対応 しかつ第 1 図及び第 2 図に 符号 I で示す ト ル ク 指令 （電流指令） が算出 さ れる。

デ ジ タ ルサ ー ボ回路 1 0 は、 第 1 図の外乱推定ォ ブザ — バ 5， フ ィ ー ドバ ッ ク ゲイ ン調整用の伝達要素 3 およ びフ ィ ル タ 4 の機能を奏する （サー ポ回路 1 0 の外乱推 定， ゲイ ン調整， フ ィ ルタ機能を第 2 図に ゼ ロ イ ン グ回 路部 1 0 1) と して示す） 。 すなわち、 ゼロ イ ン グ回路部 1 0 b と して機能する サ ー ポ回路 1 0 は、 ロ ボ ッ ト に力□ わ る全イ ナー シ ャ J mを ロ ボ ッ トの運転状態に応じて算 出 し、 こ の算出ィ ナ 一 シ ャ J mをサ一 ボ回路 1 0 に内蔵 の共有メ モ リ （図示.略） に記億し、 さ ら に、 斯 く 算出， 記億したイ ナ 一 シ ャ J mに基づいて第 3 図の外乱推定， 除去処理を行う よ う にな っ ている。

以下、 上述の構成のロ ボ ッ ト の作動を説明する。

デ ジタ ルサ 一 ボ回路 1 0 がゼロ イ ン グ回路部 1 0 わ と して機能する と き、 サ 一 ボ回路 1 0 のデ ジ タ ル シ グナ ル プロ セ ッ サは、 第 3 図の外乱推定， 除去処理を周期的に 実行する。

すなわち、 外乱推定， 除去処理周期の各々 において、 プ ロ セ ッ サは、 共有メ モ リ に記憶されかつ ロ ボ ッ 卜 の現 在の全ィ ナ — シ ャ J mを表す値を共有メ モ リ か ら読出す ( ス テ ッ プ S 1 ) 。 次いで、 プ口 セ ッ サ は、 前回処理周 斯において算出 しかつ内蔵の レ ジ ス タ R ( X ) に記憶し た第 1 図の伝達要素 5 3 の出力 （ゲイ ン調整前の推定外 乱） X に対応する値を当該レ ジス タ か ら読出 し、 こ の値 X に基づきかつ上記第 （ 8 ) 式に示す演算を行う （ ス テ ッ プ S 2 ) 。 詳 し く は、 第 1 図の伝達要素 3 によ る フ ィ — ド バ ッ ク ゲイ ン調整と 同様に、 レ ジ ス タ R ( X ) の記 憶値 χ と現在の イ ナ ー シ ャ J m と フ ィ 一 ド バ ッ ク ゲ イ ン A と の積を ト ル ク 定数 K t で除す こ と によ り 伝達要素 3 の出力 （ ゲイ ン調整後の推定外乱） y に対応する値を算 出する。 結果と して、 算出推定外乱 y は、 現在の ロ ボ ッ ト運転状態に応 じて定ま る ロ ボ ッ ト イ ナ — シ ャ に適合 し た もの と な る。 換言すれば、 外乱推定オ ブザー バの外乱 推定モ デルにお いて ロ ボ ッ ト イ ナ — シ ャ に適合 したノ、。 ラ メ — 夕 が用い ら れ、 従っ て、 イ ナ ー シ ャ変動が大き い 口 ボ ッ ト において も外乱推定が適正に行われ る。

次に、 プ ロ セ ッ サは、 ゲイ ン調整後の推定外乱 y につ いて 口 一 パス フ ィ ルタ処理を行っ て推定外乱 y に含ま れ る高周波成分を除去 し、 第 1 図の フ ィ ルタ 4 の出力 y ' に対応す る補正推定外乱を求める （ス テ ッ プ S 3 ) 。 こ の結果、 高周波帯域で外乱推定値の位相回転を生 じ る と 云 う 外乱推定オ ブザ - バの欠点が解消 さ れ、 外乱推定が 適正ィヒ さ れる。

さ ら に、 プロ セ ッ サ は、 サー ボ回路 1 0 がサ ― ボ回路 部 1 0 a と して機能する と き に求めた電流指令 I か ら補 正推定外乱 y ' を減 じ る こ と に よ り 補正電流指令 I ( = I - y ' ) を算出 し、 こ の算出値を レ ジ ス 夕 R ( I ) に 格納す る と共に サ ー ボア ン プ 1 1 に送出す る （ ス テ ッ プ S 4, S 5 ) 。 結果と し て、 サ 一 ボ ア ン プ 1 1 に供給さ れる電流指令に対する外乱の影響が除ま さ れる。

次に、 次回外乱推定， 除去処理周期で用 い る ゲイ ン調 整前の推定外乱 X を求め る ために、 第 1 図の外乱推定ォ ブザー バ 5 によ る外乱推定処理と同様の処理が行われる (ス テ ッ プ S 6 ) 。 詳し く は、 プ ロ セ ッ サ は、 サー ポ回 路部 1 0 a で求めた現在の実際モ ー タ速度 V と前回外乱 推定， 除去処理周期で求めた推定速度 V ， と に基づいて 第 1 図の伝達要素 5 3 の出力 （ゲイ ン調整前の推定外乱） X に対応する値を算出す る。

ま た、 ス テ ッ プ S 6 に おいて、 プロ セ ッ サ は、 次回処 理周期で用いる推定速度 V， を算出する。 即ち、 レ ジス タ R ( I ) の記憶値 I 一 y， に基づいて第 1 図の伝達要 素 5 1 の出力に対応する値を算出する と ^に、 現在の実 際モー タ速度 V と前回処理周期で求めた推定速度 V ' と に基づいて第 1 図の伝達要素 5 2 の出力に対応する値を 算出 し、 さ ら に、 上記 2 つの算出値と上記推定外乱 X と に基づいて第 1 図の伝達要素 5 の出力に対応する推定 速度 V ， を算出する。 そ して、 上述のよ う に夫々算出 し た推定外乱 X及び推定速度 V ， を レ ジ ス タ R ( X ) , R ( V ' ) に夫々 格納 し （ ス テ ッ プ S 7 ) 、 今回周期での 第 3 図の外乱推定， 除去処理を終了する。

本発明は上記実施例に限定されず、 種々 の変形が可能 であ る。 即ち、 上記本発明の作動原理説明及び上記実施 例では本発明を ロ ボ ッ ト に適用 した場合について説明し たが、 本発明は ロ ボ ッ ト 以外の各種機械に適用可能であ る。 又、 上記実施例ではデ ジタ ルサー ボ回路によ り本発 明を実施 したが、 その他のタ イ プのサー ボ回路を用いて も良い。

Claims

請 求 の 範 囲

1. サ - ボ系を装備 した機械の、 当該機械の運転状態に 応 じて変化す る イ ナ一 シ ャ を周期的に決定する工程

( a ) と、 斯 く 決定 し た イ ナ — シ ャ に基づいてかつ外 乱推定オ ブザー バを用 いて前記機械に加わる外乱を推 定 し て推定外乱を決定する工程 （ b ) と、 前記サ ー ボ 系において前記推定外乱を除去する工程 （ c ) と を備 え る、 外乱推定オ ブザー バによ るサ ー ボ制御方法。

2. 前記工程 （ b ) にお いて前記推定外乱の高周波成分 を除去する こ と に よ り 補正推定外乱を決定 し、 前記ェ 程 （ c ) で前記サ ― ボ系において前記補正推定外乱を 除去する請求の範囲第 1 項記載の外乱推定オ ブザ - バ に よ る サ ー ポ制御方法。

3. 前記機械は前記サー ボ系か ら の電流指令に応 じて駆 動さ れるサー ボモー タ を含み、 前記工程 （ c ) におい て前記電流指令か ら前記推定外乱を除去する請求の範 囲第 1 項記載の外乱推定オ ブザー バによ る サ ー ボ制御 方法。

4. 前記機械は前記サ ー ボ系か ら の電流指令に応 じて駆 動さ れるサ ー ボモ ー タ を含み、 前記工程 （ c ) におい て前記電流指令か ら前記補正推定外乱を除去する請求 の範囲第 2 項記載の外乱推定オ ブザー バに よ るサ ー ボ 制御方法。