WO1993016423A1 - Adaptive sliding mode control method for control object including spring system - Google Patents

Description

明 細 書

パネ系を含む制御対象に対する適応的スライ ディ ング モー ド制御方法

技 術 分 野

本発明は、 サ一ポモータで駆動されるロボヅ ト等のバ ネ系を含む制御対象の制御方法に関するもので、 特に、 制御対象に適応的なスライ ディ ングモー ド制御方法に閧 する。

背 景 技 術

サーボモ一夕で駆動される襪械等の制御において、 . 制 御対象の機械をパネ系と考えず、 これを剛性があるもの とみな したう えで、 P I (比例， 積分） 制御等の線形制 御を実行 している。

しか し、 ロ ボヅ ト は通常、 片持は り構成になっている ため、 ロボッ ト の姿勢に応 じて大きなイ ナー シ ャ変動が 生 じ、 サ一ボ系の応答性の変化が生 じる。 また、 減速器 などのパネ要素も閧係 して低剛性で共振周波数が低い。 そのため、 ロ ボッ トの先端 （ ツールセンタポイ ン ト ） を 位置決めする よ うな際、 サ一ボモータ 'による位置決め終 了後、 ロ ボヅ ト先端が振動する。 そ こで、 従来はロボヅ ト の低剛性によるパネ系及びイ ナ一シャ変動を無視 し、 その代わ り にサ一ボゲイ ンを小さ く して制御 した り、 位 置決め終了後タ イ マーを作動させ、 所定時間経過して 口 ボッ トの先端の振動が停止 した後作業を行う等の制御を 行って いた。 そ こで、 本願発明者等は、 サ一ボ乇一夕で駆動される 制御対象に対 して、 スライ ディ ングモー ド制御を適用 し. 機械可動部の振動を防止する発明を提案 して きた （特関 平 3 — 9 2 9 1 1号公報， 特開平 3 — 1 1 8 6 1 8号公 報等参照） 。

スラ イ ディ ングモ一 ド制御と適応制御を組み合わせる と非常にロバス ト性が高く なる （ すなわち、 パラ メ 一夕 変動に強 く、 外乱抑圧性が高く なる ） ので、 利用価値が 高いこ とが知 られている。 しか し、 この技術を ロ ボヅ ト のよう な柔軟な構造物の機械等に適用する と推定パラ メ 一夕 （制御対象のイ ナ一シャ， 動摩擦、 重力項等） が旨 く 収束 しな く、 機械可動部が振動 して しま う と いう問題 がある。

発 明 の 開 示

そ こで、 本発明の目的は、 上記推定パラ メ 一夕の収束 性を向上させ、 制振性のの優れた適応的スライ ディ ング モー ド制御方法を提供する こ と にある。

上記目的を達成するために、 本発明-は、 適応的スライ デ イ ングモー ド制御方法において、 サーボモータの位置と 該サーボモータで駆動される制御対象の可動部の位置と の差をスライ ディ ングモー ド制御の位相面にフ ィ ー ドバ ヅ ク して、 該位相面が 「 0 」 に収束する よ う にサーボモ —夕への トルク指令を制御する。

好ま し く は、 適応的スラ イ ディ ングモー ド制御方法に おいて、 指令位置とサ一ボモー夕の位置と位置偏差を £ . 速度偏差を ε 、 サ一ボ乇一夕の位置と該サ一ボモ一夕で 駆動さ れる制御対象の可動部の位置との差を £ t、 位置 ループゲイ ンを Kp、 サーボモー夕の位置と該サーボモー 夕で駆動される制御対象の可動部の位置との差のフ ィ ー ドバッ クゲイ ンを K1と した と き、 スライ ディ ングモー ド 制御の位相面 Sufを、

Suf= ε + Κρ · ε — Kl · ε t

と し、

イ ナ一シャの項， 動摩擦係数， 重力項の各推定値を Jhat, Ahat, Grhat、 位置指令の速度， 加速度を夫々 0 r， Θ r 、 サ一ボモータの位置と該サ一ボモ一夕で駆動され る制御対象の可動部の位置との差の微分値を £ t 、 切換 入力を r l と した と き、 該切換入力て 1 を位相面 Sufが 「 0」 以上の ときは外乱の最大値、 「 0」 よ り小さ いと き は外乱の最小値と してサーボモ一夕への トルク指令てを r = K2 · Suf + Jhat ( θ τ - Κρ² ε + Κρ · Kl · ε t

— Kl · ε t ) + Ahat - Θ + Grhat+ て 1

から求める。

なお、 上記サ一ボモータの位置と該サ一ボモ一夕で駆 動される制御対象の可動部の位置との差 £ t及びその微 分値 £ t はオブザーバの処理によって求める。

また、 本発明の一態様では、 ロ ボッ ト をパネ系を含む 制御対象 と考え、 このロボヅ ト に対し、 以下の工程か ら なる適応的スライ ディ ングモー ド制御を適用する。 すな わち ： サーボ回路のメ モ リ 内に、 予想される外乱の最大 値 D i s ( m ax )及び最小値 D i s ( m i n )のほか、 スラ イ ディ ング モー ド制御処理及びオブザーバ処理に必要な各種の定数 を設定 し入力する ； ロ ボヅ 卜 の動作を開始させ、 各周期 ごと に位置指令 0 r 及び位置フ ィ ― ドバッ ク量 S を読み 取 り、 こ れら読み取った値か ら、 乇一夕速度 0、 指令速 度 0 r 、 指令加速度 0 r 、 位置指令 > r と位置フ ィ ー ド パッ ク量 0 との差である位置偏差 、 及び速度偏差を έ を算出する ； その求めたモ一夕速度 に基づきォブザ一 バ処理を して、 モ一夕 と ロ ボ 4 ッ ト先端の推定ね じれ量 etf及び推定ね じれ速度 tfを求め、 さ ら にこれ ら偏差 を伝達関数のフ ィ ルタ処理 してフ ィ ルタ処理を行った と ころのモ一夕 と ロボ ヅ ト先端の推定ね じれ量 £ t 及び推 定ね じれ速度 t を求める ； この位置偏差 ε及び速度偏 差 £並びにフ ィ ル夕処理を行った と ころの乇一夕 と ロ ボ ヅ ト先端の推定ね じれ量 に基づき、 位相面 Sui を求 める ； その求めた位相面 Suf に基づき重力項の推定値 Gr hat を、 また、 上記位相面 Suf 及び上記モータ速度 S に 基づき動摩擦係数の推定値 Ahatを、 さ ら に上記位相面 Suf 、 上記位置偏差 £ 及び指令加速度 Θ r 、 推定ね じれ 量 £ t 及び推定ね じれ速度 £ t に基づきィ ナ一シャ項の 推定値 Jhatを求める ； 上記位相面 Suf の値が [0] 以上 であるか否かを判別 して、 [ 0〕 以上である と切換入力 て 1 を予め入力 した外乱の最大値 Dis(max)と し、 ま た [ 0 ] 以下である と切換入力 r l を予め入力 した外乱の最小値 Dis(min)とする ； 上記モ一夕速度 Θ 位置偏差 ε 及び指 令加速度 0 r、 上記フ ィ ル夕処理を行ったと ころのモー 夕 と 口ポゾ ト先端の推定ね じれ量 及び推定ね じれ速 度 t 、 上記位相面 S u f 及び上記イ ナ一シャ項の推定値 Jh at、 動摩擦係数の推定値 Ah at、 重力項の推定値 G rh at 、 さ ら に上記切換入力て 1 か ら、 モー夕への トルク指令て を求める工程 ； その求めた トルク指令をサ一ボ回路の電 流ループに引 き渡す工程。

上述のよ う に、 本発明によれば、 サ一ボモ一夕の位置 と該サーボモータで駆動される制御対象の可動部の位置 との差をスライ ディ ングモー ド制御の位相面にフ ィ 一 ド バッ ク して、 位相面が 「 0」 に収束する よ う に、 モ一夕へ の トルク指令が算出されてサーポモー夕 を該 トルク指令 で駆動するから、 位置決め—終了後の制御対象の可動部の 振動は減少する。

図 面 の 簡 単 な 説 明

図 1 は本発明の一実施例を実施するサーボ制御系のブ ロ ヅク図、

図 2 は制御対象をばね—モデルと したと きのモデルプロ ヅ ク図、

図 3 は本発明の一実施例において、 所定周期毎にディ ジタルサーポ回路のプロセ ヅサが実施する処理のフ ロ ー チヤ一 卜 の一部、

図 4 は図 3 のフローチヤ一 卜の続き、

図 5 は本発明の効果を見るためのシ ミ ュ レーシ ョ ンを 行った と きのモデルの周波数特性を示す図、 図 6 は上記シ ミ ュ レ一シ ョ ンの入力波形を表す図、 図 7 は上記シ ミ ュ レーシ ョ ンで本発明の方法を適用 し ない と きの各データの波形を表す図、 及び、

図 8 は上記シ ミ ュ レーシ ョ ンで本発明を適用 した と き の各データの波形を表す図である。

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

まず、 スライ ディ ングモー ド制御の位相面 Sufにモ一夕 の位置と機械可動部 （ ロ ボッ トの先端位置） の位置との 差、 すなわち、 捩じれ量 £ t (詳 し く は、 後述するよ う に、 推定位置偏差に伝達関数 （ S+ Kp) / ( S+ A) を乗 じた値） をフ ィ ー ドバッ クする もの と して、 該位相面を次の （ 1 ) 式とする。 ま た、 ロ ボッ ト等の制御対象の運動方程式を 立てる と次の （ 2 ) 式となる。

Suf = 1 + Κρ · ε - Kl · ε t … （ 1 ) r = J - θ + - έ + Gr+ Dis '·· ( 2 ) なお、 上記 （ 1 ) 式， （ 2 ) 式において、 ε はモー夕 への指令位置とモー夕の実際の位置との差である位置偏 差であ り、 0はモー夕の実際の位置を意味する。 また、 Κρは位置ループゲイ ン， K1は状態量である制御対象の機 械の捩 じれ量 £ t のフ ィ 一 ド ノ ッ ク ゲイ ンで、 て は ト ル ク， J はイ ナ一シャ， A は動摩擦係数， Grは重力項，

D i sは外乱である。

また、 モ一夕への位置指令を 0 r とする と、 次の式が 成立する。

ε = Θ で 一 Θ ε = θ τ - θ

»» ·» 参♦

ε = θ r - θ … （ 3 ) また、 モー夕への トルク指令 r を次の式とする。

て = K2 - Suf + Jhat { Θ T- ² £ + Kp · Kl · ε t 一 Κ1 · ε t ) + Ahat · θ + Grhat+ r 1

… ( 4 ) こ こで、 K2は後述する よ う にィ ナ一シャの最大値によつ て決ま る定数、 Jhatはイ ナ一シャ項の推定値、 Ahatは動 摩擦係数の推定値、 Grhat は重力項の推定値、 て 1 は切 換入力である。

また、 リ アプノ フ関数候補 V を次の式とする。

V= ( 1/2) J · Suf² + ( 1/2) α . Jbar²

+ ( 1/2) β · Abar² + ( 1/2) ァ · Grbar²

… ( 5 ) なお、 こ こで α, β, ァ は適応速度を決める正の調整パ ラメ 一夕であ り、 Jbar, Abar, Grbar はイ ナ一シャ項の 推定誤差， 動摩擦係数の推定誤差， 重力項の推定誤差で あ り、 次の （ 6 ) 式の関係にある。

Jbar = J - Jhat

Abar= A- Ahat

Grbar二 Gr- Grhat ··· ( 6 ) 上記 リ アプノ フ関数候補 V は最小値が 「 0」 で常に正 になる関数である。 よって、 リ アプノ フ関数候補 V の微 分値 V が常に負になる （ よって V の単調減少） よ う なモ 一夕への トルク rを決定すれば、 リ アプノ フ関数候補 V は最小値 「 0」 に収束する。 すなわち、

Suf = 0, Jbar = 0 , Abar = 0 , Grbar = 0

とな り、 応答は従来のスラ イ ディ ングモー ド制御と同様 に、 制御対象のパラ メ ータ （イ ナ一シャ， 動摩擦係数， 重力項） に依存 しない Suf = 0の一定な応答関数によって 決ま る応答性が られる。 ま た、 各推定誤差は 「 0」 に な り、 各推定値は真値に収束する こ と になる。

そこで、 上記 （ 5 ) 式の両辺を微分する と、

V = J · Suf · Suf + a · Jbar · Jbar+ β · Abar · Abar + r - Grbar · Grbar

… ( 7 ) ま た、 （ 1 ) 式の両辺を微分する と、

Suf = i ' +'"Κρ - £ - l · έ t … （ 8 )

( 3 ) 式を （ 2 ) 式に代入 し、 £ について解く と、 ε = θ τ + ( A/J) Θ + { ( Gr+ Dis) /J} — て / J

… ( 9 )

( 9 ) 式を （ 8 ) 式に代入 し S f を求め、 （ 7 ) 式に代 入する と、 —

V = Suf ( J · θ τ + k■ θ + Gr+ Dis- r

+ J · Kp · έ - J · Kl · ε t) + a - Jbar - Jbar

+ β · Abar · Abar + r · Grbar · Grbar

… ( 1 0 ) そ こで （ 1 ) 式よ り を求めこれを上記 （ 1 0 ) 式に 代入 し、 さ ら に （ 4 ) 式によ り て を求めこれを （ 6 ) 式 によ り処理した う えで （ 1 0 ) 式に代入する と、 次の式 が成立する。

V = Suf ² ( J · Kp - Κ2)

+ Jbar { Suf ( θ τ - Kp² - ε + Kp · Kl · ε t

- Kl · ε t ) + a - Jbar}

+ Abar ( Suf · θ + β · Abar)

+ Grbar ( Suf + ァ · rbar)

+ Suf ( Dis- r 1) - ( 1 1 ) こ こで上記 リ アブノ フ関数候補の微分値 を常に負に する こ と を考える。

まず （ 1 1 ) 式の右辺第 1 項を負にする には、

J · Kp- K2< 0

とすればよいから、 係数 Κ2の値を次の式のよう に、 イ ナ ーシャの最大値 J(max)に位置ループゲイ ン Kpを乗 じた値— にすれば、 上記 （ 1 1 ) 式の第 1 項は常に負になる。

K2 = J(max) . Kp - ( 1 2 ) また、 上記 （ 1 1 ) 式の右辺第 2項は 「 0j にする。 すなわち、

Suf ( 0 r - Kp² · ε + Kp · Kl · £ t- Kl · ε t)

+ · Jbar = 0 ― ( 1 3 ) よって、

Jbar = - ( 1/α ) Suf ( θ τ - Kp² ' ε

+ Kp · Kl · ε t - Kl · ε t )

… ( 1 4 ) また、 （ 6 ) 式の Jbar = J- Jhat を微分して J = 0 と仮定する と、 すなわち、 イ ナーシャ J を一 定とお く と、

Jbar = 一 Jh at

であるか ら、 上記 （ 1 4 ) 式は

Jhat = { 1/ a ) Suf ( r一 Kp² - ε

+ Κρ · Kl · ε t- Kl · £ t ) - ( 1 5 ) となる。

さ ら に同様に、 動摩擦係数 A， 重力項 Grについて もそ れぞれ一定と仮定して、 A = 0, Gr= 0 と し、 （ 6 ) 式 を微分する こ と によ り、

Abar= 一 Ahat

Grbar = - Grhat

と したう えで、 上記 （ 1 1 ) 式の右辺第 3項， 第 4項に ついても 「 0」 とすれば、

Suf ■ Θ + 3 - Abar= 0 よ り、

Ahat = ( 1/ β ) Suf · 0 - ( 1 6 )

Suf + ァ · Grbar= 0 よ り、

Grhat = ( 1/ァ ） Suf - ( 1 7 ) 次に （ 1 1 ) 式の右辺第 5項を負にする よ う にする。 Suf ( Ms - て 1) < 0 "' ( 1 8 ) 上記 （ 1 8 ) 式が成立する には、 外乱の最大値及び最 小値を Dis(max) 及び Dis(min) とする と、

Suf ≥ 0の と き には

r 1 = Dis(max) ·*· ( 1 9 )

Suf < 0の と き には

て 1 = Dis(min) - ( 2 0 ) とすればよい。

以上のよ う に、 上記 （ 1 2 ) 式， （ 1 5 ) 式， （ 1 6 ) 式， （ 1 7 ) 式及び Suf の正負 に応 じて （ 1 9 ) 式か ( 2 0 ) 式が成立するよう にモ一夕 に トルク指令てずれ ば リ アプノ フ関数候補 V の微分値は常に負になる。 すな わち—、 （ 1 5 ) 式， （ 1 6 ) 式， （ 1 7 ) 式を積分すれ ば、 それぞれ推定値 Jhat, Ahat, Grhat が求め られ、 係 数 K2の値 （ （ 1 2 ) 式） を Kp · J (max)に設定し、 Suf の 正負に応 じて切換入力て 1を Dis (max) か Dis(min)に設定 すれば、 （ 4 ) 式よ り モ一夕への トルク指令てが求ま る。

なお、 （ 4 ) 式によ り トルク指令てを求める場合、 指 令位置 0 r とモータの位置 0の位置偏差 £ { = θ τ - Θ ) , 速-虔偏差 £ { = b i- Θ ) , 位置指令の速度 r， 指令の 加速度 0 r は指令位置とモー夕 に取付けたパルスェンコ ーダと によって検出できるが、 モータ位置 0 と機械可動 部の差 £ t及びその速度偏差 έ tは検出できない。 そこで、 この差 £ t及び速度偏差 £ t をオブザーバによって推定す る

図 2 はロボ ッ トのモデルであ り、 このモデルを用いて オブザーバを考える。

図 2 中、 符号 1 0 はモー夕側で符号 1 1 は機械側であ り、 その間に減速器が設け られ該減速器のパネ定数を Kc：， 粘性項を Bkとする。 ま た、 モー夕のイ ナ一シャ を Jm， 粘 性係数を Am， 位置を 0, 機械のイ ナ一 シ ャ を Jt， 位置 ( ロボヅ 卜のアーム先端位置） を 0 tとする。 モー夕の出力 トルク を T と し、 モー夕側 1 0 で運動方 程式をたてる と、

T = Jm - Θ + B { θ - Θ t) + Kc ( θ - Θ t) + km ■ θ

··· ( 2 1 ) また、 負荷側 （機械側） 1 1 で運動方程式をたてる と、 0= Jt · ^ t+ B k ( Θ t- θ ) + Ιο( θ t- θ )

… ( 2 2 )

( θ - θ t) はモ一夕のロ ー夕回転位置に対する負荷の 位置偏差 （ね じれ量） を意味 し、 （ — t) は速度偏差 (ね じれ速度） を意味する。 そこで、 この位置偏差 （ね じれ量） を ε tf， 速度偏差 （ね じれ速度） を tfとする。 すなわち、

Θ - Θ t= ε tf ノ

θ - Θ t= £ tf - ( 2 3 ) と し、 上記 （ 2 1 ) 式を Jmで除したものから、 （ 2 2 ) 式を Jtで除したものを減算する と次の式となる。

T/Jm = ε tf + {(Bk/Jm) + (Bk/Jt)} έ tf

+ {(Kc/Jm) + (Kc/Jt)} . ε tf +- ( Am/Jra) Θ

- ( 2 4 ) 上記 （ 2 4 ) 式よ り、

ε tf = — {(Bk/Jra) + (Bk/Jt)} ε tf

- {(Kc/Jm) + (Kc/Jt)} · ε tf

一 ( Ani/Jm) Θ + T/Jm

- ( 2 5 ) そこで （ 2 1 ) 式を 0 について解き （ 2 3 式を代入 する と、

θ = - ( Bk/Jin) έ tf — ( Kc/Jm) ε tf

- ( Am/Jm) Θ + T/Jm ( 2 6 ) こ、

e tf = XI

£ tf = X2

θ = 1Z

とする と、 上記 （ 2 5 ) 式と （ 2 6 ) 式よ り次の状態遷 移行列ができる。

上記状態変数 XI〜 X3の内、 観測可能な状態変数は X3で あるモータの速度 0である。 そこで、

Y = [ 0 0 11 [ XI X2 X3] … ( 2 8 ) と し、 （ 2 7 ) 式の行列 （ XI. X2. X3) にかかる行列を A, Tにかかる行列を ， （ 2 8 ) 式の行列 （ XI· X2. X3) にかかる行列を £ と し、 この系を Z変換する と、 （ 2 7 式， （ 2 8 ) 式は、

X ( n+ 1) = Αζ · X ( n) + Bz · T … （ 2 9

Y = C · X ( η) … （ 3 0 となる。 なお、 ( η) = [ XI ( η) 12 n) X3 ( n) ] でめる ο

こ こで、 Γ (η)を ( η) の推定値と して、 同一次元ォブ ザーバを組む と

' (η+ 1) = ( Αζ- Κ · C) X' (n) -I- Bz · T+ K - Y

… ( 3 1 ) とな り、 上記 （ 3 1 ) 式を計算する こ と によって （行列 A, B, £ の各要素は既知のものである ） 、 行列 I ( n ) を 推定し、

XI = tf 及び

X2 = £ tf

を求める。 なお、 は 4z— ϋ · £が安定するよ う に選定す る。

そ して、 このオブザーバの処理によって得られた £ tf， £ tfに対 して伝達関数 （ S+ Kp) / ( S+ A) のフィ ルタ処 理 して e t, «£ tを求める。

ε t = £ tf · (S+ Kp)/(S+ A) - ( 3 2 ) I t = έ tf · (S+ Kp)/(S+ A) - ( 3 3 ) こ う して、 £ t， £ tの値が求まれば、 モー夕への指令 ト ルク てが求ま る。

次に上記モータ位置と機械可動部位置をスライ ディ ン グモ一 ドの位相面にフ ィ 一 ドバヅ クする適応的スライ デ イ ングモー ド制御を行う と、 制振効果がある こ と をロ ボ ッ 卜のパネモデルを 1例と して証明する。

上記位相面 Sufが 「 0」 になった と きを考える と、 （ 1 ) 式よ り、

0 = 1 + Κρ · £ - Κ1 · ε t - ( 3 ) 上記 （ 3 4 ) 式に （ 3 ) 式， （ 3 2 ) 式及び （ 2 3 ) 式 を代入 し、 ラブラス変換し整理する と次式を得る。

0 = ( S+ A) Θ r 一 （ S+ A+ K1) θ + 11 ■ Θ t

ノ … ( 3 5 )

—方、 上記 （ 2 2 ) 式をラブラス変換して整理する と、 θ X/ Θ = ( Bk · S+ Kc) / ( Jt · S² + Bk · S+ Kc)

… ( .3 6 ) なお、 ロボッ ト は粘性項 Bkが小さ く、 またバネ定数 Kc も小さ いため、 上記 （ 3 6 ) 式か らダン ビングが悪く し かも低周波数で振動するこ とがわかる。

こ こで （ 3 5 ) 式よ り 0 を求めて （ 3 6 ) 式に代入 し 整理する と、

Θ t/ θ τ = ( Bk · S+ Kc) / { Jt · S² + Bk · S+ Kc

+ ( S² . Kl · Jt) / ( S+ A) }

… ( 3 7 ) 上記 3 7式において、 A= 0、 すなわち、 具体的にはこ のパネ系 （ ロ ボ ッ ト ） の共振周波数で s + Aが S とみなせ る まで A の値を小さ く する と （ 3 7 ) 式は次の （ 3 8 ) ； «と /よ o。

Θ \/ θ τ = ( Bk · S+ Kc) / { Jt · S²

+ ( Bk+ Kl · Jt) S + Kc}

… ( 3 8 ) この （ 3 8 ) 式か ら、 モ一夕位置と ロ ボッ ト先端位置 の差のフ ィ ー ドバヅ クゲイ ン K1と機械イ ナ一シャ Jtの積 ( K1 · Jt) がダンピング項に入 り、 ダン ビング項が大き く な り、 振動抑制効果が生 じるこ とがわかる。

図 1 は本発明の一実施例を実施するロ ボッ ト制御系の 要部ブロ ッ ク図である。 図中 1 は口ポヅ ト を制御するホ ス トプロセッサで、 補間、 直交座標系の座標値か ら各軸 の回転角への変換， 逆変換等を行う と共に、 ロ ボッ ト の 各軸へ位置指令を分配する。 2はホス トプロセ ッサ 1 と ディ ジタルサ一ボ回路 3のプロセ ッサ間の情報の伝達を 仲介する共有 R A Mで、 ホス トプロセッサ 1 が書き込ん だ位置指令等のデータ をディ ジタルサーボ回路 3のプロ セ ヅサに受け渡 し、 ディ ジタルサ一ボ回路 3のプロセ ヅ ザが書き込んだアラーム情報等をホス トプロセ ヅサに引 き渡す機能を行う ものである。 3 はディ ジタルシグナル プロセ ッサ等で構成されるディ ジタルサーボ回路でプロ セ ヅサ， R O M, R A M等で構成され、 ロ ボッ ト の各軸 のサ一ボモータの制御を行う もので、 本発明のスラ イ デ ィ ングモ一 ド制御処理， オブザーバの処理を行う もので ある。 4 は ト ラ ンジスタイ ンバー夕等で構成されるサ一 ボアンプで、 5 はサーボモータである。 また、 6 はサー ボモ一夕 5 の位置 0 を検出するパルスコーダで、 位置 0 はディ ジタルサーボ回路にフ ィ 一 ドバッ ク されている。 なお、 サ一ボアンブ 4， サーボモ一夕 5 は 1軸のみを図 示している。

図 3, 図 4 は、 本実施例において、 上記デジタルサ一 ボ回路のプロセ ッサが実行する本発明のサーポ乇一夕制 御処理に関するフローチャー トであ り、 該プロセヅサは 所定周期毎、 図 3， 図 = に示す処理を実行する。

まず、 ディ ジタルサーボ回路 3 のメ モ リ 内にスライ デ ィ ングモ一 ド制御処理及びオブザーバ処理に必要な定数 等、 即ち、 位置ループゲイ ン Kp， 捩 じれ量のフ ィ ー ドバ ックゲイ ン K1, 外乱の最大値， 最小値 Dis(max), Dis(mi n), モータ及び機械を含めたィ ナ一シャの最大値 J(max)， ィ ナ一シャの最大値 J(max)に位置ループゲイ ン Kpを乗 じ た値である定数 Κ2の値， モータのイ ナ一シャ Jm, 調整パ ラメ一夕 α， β , ァ， 行列 4， S, の各要素の値をあ ら か じめ設定する。

そ して、 ロ ボヅ トの動作を開始させる と、 ホス トプロ セッサ 1 は各軸に対 して位置指令を分配 し、 ディ ジタル サ一ボ回路 3 のプロセ ッサは共有 H A M 2 よ り位置指令 Θ r を読み取る と共に、 パルスコーダ 6 から出力される 位置フ ィ ー ドバヅク量 0 を読取る （ステヅブ S l， S 2 ) ₍ 次に、 位置指令 0 r と位置フ ィ ー ドバッ ク量 0 よ り位置 偏差 £ を求める。 すなわち、 位置偏差を記憶する レ ジ ス 夕 に位置指令 か ら位置フ ィ ー ドバッ ク位置 0 を減 じ た値を加算し位置偏差 £ を求める。 また、 検出フ ィ ー ド パヅ ク位置 0か ら前周期で検出 したフ ィ ー ドバ ヅ ク位置 を減じてモ一夕速度 （モータ位置 0の 1 回微分） β を求 め、 さ ら に、 位置指令 0 r から前周期の位置指令を減 じ た値である指令速度 （位置指令 の 1 回微分） θ τ か ら上記モー夕速度を減 じて速度偏差 ε を求める と共に、 求めた速度指令 0 r か ら前周期で求めた指令速度を減 じ て指令の加速度 （位置指令 0 r の 2 回微分） を求め る （ステップ S 3 ) 。

次に、 モータ速度 よ り前述したオブザーバの処理を 実行 し、 モータ と口 ポッ ト先端の推定ね じれ量 £ tf, 推 定ね じれ速度 £ tfを求める （ ステップ S 4 ) 。 次に式 3 2， 式 3 3 の演算を行ってフ ィ ル夕処理を行ってフ ィ ル 夕処理が行われたモータ と ロボッ ト先端の推定ね じれ量 ε t ， 推定ね じれ速度 £ t を求める （ステップ S 5 ) 。 そ して、 ステ ップ & 3 で求めた位置偏差 £， 速度偏差 £ 及びステ ップ S 5 で求めたフ ィルタ処理が行われたモ一 夕 と ロボッ ト先端の推定ね じれ量 £ t よ り （ 1 ) 式の演 算を行って位相面 S ufを求める （ステップ S 6 ) 。 次に モータへの トルク指令てを求めるために 4式のイ ナ一シ ャの推定値 Jhatにかかる値をステ ップ S 3 で求めた位置 偏差 £， 位置指令の加速度 及びステ ップ S 5 で求め た推定ね じれ量 £ t ， 推定ね じれ速度 t よ り 求め、 こ の値を WEK と してレジス夕に記憶する （ステヅブ S 7 ) 。 すなわち、 次の式の演算を行う。

WRK = 0 Γ— Κρ² £ + Κρ · Κ1 · £ t— Κ1 · ε t

··· ( 3 9 ) 次に、 ステ ヅ ブ S 6， S 7 で求めた位相面 Suf の値及 び W の値よ り （ 1 5 ) 式， （ 1 6 ) 式， （. 1 7 ) 式を 積分 し、 イ ナ一シャの項の推定値 Jhat, 動摩擦係数の推 定値 Ahat, 重力項の推定値 Grhat を求める。 すなわち、 Suf · WRKの値を各周期毎算出 しこの算出された値をアキ ュム レ一夕 に積算し、 その積算値に係数 （ 1/α ) を乗 じ てイ ナ一シャの項の推定値 Jhatを求め、 同様に、 Suf · 0 の値を求めアキュムレータ に積算し、 その積算値に係数 ( 1//3 ) を乗 じて動摩擦係数の推定値 Ahatを求める。 ま た、 Sufの値をアキュム レータ に積算して、 係数 （ 1/ァ ） を乗 じて重力項の推定値 Grhatを求める。

次に位相面 Sufの値が 「 0」 以上か否か判断し （ステツ ブ S 9 ) 、 「 0」 以上な らば、 切換入力て 1と して レジス 夕 に Dis(max) を、 負な らば Dis(min) を格納し （ステツ プ S 1 0， S 1 1 ) 、 式 4の演算を行ってモー夕への ト ルク指令てを求め （ステップ S 1 2 ) 、 該 トルク指令て を電流ループに引き渡 し （ ステップ S 1 3 ) 、 当該処理 周期の処理を終了する。

以下、 各周期毎上記処理をディ ジタルサーボ回路 3 の プロセ ヅサは実行 しサーボモー夕 を制御 し、 ロボヅ ト を 駆動する こ と になる。 図 5 〜図 8 は本発明の効果を見るためのシ ミ ュ レ一 シ ヨ ンデ一夕 を示す図で、 図 5 はモデルばね系の周波数応 答を示す図であ り、 図 6 はシ ミ ュ レ一シ ョ'ンのため入力 波形を示 し、 （ a ) は加加速度、 （ b ) は加速度、 （ c ) は速度、 （ d ) は位置である。 図 7 は本発明を適用 しな い と きのシ ミ ュ レーシ ョ ン結果のデ一夕で、 図 8 は本発 明を適用 した と きの.シ ミ ュ レーシ ョ ン結果のデ一夕であ り、 夫々 （ a ) はモータ位置 0、 ( b ) はモ一夕速度 0 (1) 、 （ c ) はアーム先端位置 0 t、 （ d ) はアーム先端 位置速度 t 、 （ e ) はイ ナ一シャ値の推定値 Jhat、 ( f ) は動摩擦係数の推定値 Ahat、 ( g ) は重力項の推 定値 Grhat、 ( h ) はモ一夕への トルク指令 rである。

この図 7 と図 8の （ c ) ， ( d ) で示すアーム先端位 置及び速度を比較 して分かるよ う に本発明を適用 した場 合の方 （図 8 ) が、 位置決め終了後の振動が減少 してい る こ とが分かる。 ま た、 図 7， 図 8の （ e ) 〜 （ g ) を 比較 しても、 本発明を適用 した と きの方がイ ナ一シャ値 の推定値 Jhat、 動摩擦係数の推定値 Ahat、 重力項の推定 値 Grhatが真値にほぼ等 しいこ とが分かる。

以上説明 したよう に、 本発明の適応スライ ディ ングモ — ド制御では、 モータの位置と該モ一夕で駆動される可 動部の先端位置の差分をスライ ディ ングモー ドの位相面 にフ ィ ー ドバッ ク して、 制御対象のパラ メ ータ （ イ ナ一 シャ， 動摩擦係数， 重力項） の収束性能を改善 し、 機械 可動部の振動を減少させる こ とができる。

Claims

請 求 の 範 囲

1. 適応的スライ ディ ングモー ド制御方法において、 サ —ボモ一夕の位置と該サーボモ一夕で駆動される制御対 象の可動部の位置との差をスライ ディ ングモ一 ド制御の 位相面にフ ィ ー ドバッ ク して、 該位相面が 「 0」 に収束す るよう にサ一ボモ一夕への トルク指令を制御する こ と を 特徴とするパネ系を含む制御対象に対する適応的スライ デイ ングモー ド制御方法。 '

2. 適応的スライ ディ ングモー ド制御方法において、 指 令位置と分一ボ乇一夕の位置との位置偏差を £、 速度偏 差を £ 、 サーボモ一夕の位置と該サーボ乇一夕で駆動さ れる制御対象の可動部の位置との差を £ t、 位置ルーブゲ イ ンを ΚρΓサ一ボモータの位置と該サーボモータで駆動 される制御対象の可動部の位置との差のフ ィ ー ドバッ ク ゲイ ンを K1と したと き、 スライ ディ ングモー ド制御の位 相面 Sufを次の式と し、

Suf = ε + Κρ · £ - Kl · ε t

イ ナ二シャの項， 動摩擦係数， 重力項の各推定値を Jh at, Ahat, Grhat、 位置指令の速度， 加速度を夫々

Θ r 、 サーボモー夕の位置と該サ一ボモータで駆動され る制御対象の可動部の位置との差の微分値を t、 切換入 力を て 1と した と き、 該切換入力て 1を位相面 Sufが 「 0」 以上の と きは外乱の最大値、 「 0」 よ り 小さ い と きは外乱 の最小値と してサーボモー夕への トルク指令て を次の式 を演算 して求める r = K2 · Suf + Jhat ( Θ r Kp² ε + Κρ · Κ1 · ε t - Kl · I t) + Ahat - + Grhat+ て 1 こ と を特徴とするパネ系を含む制御対象に対する適応的 スライ ディ ングモ一 ド制御方法。

3. 上記サーポ乇一夕の位置と該サ一ボモ一夕で駆動さ れる制御対象の可動部の位置との差 £ t 及びその微分値 έ t はオブザーバの処理によって求める請求の範匪第 2 項記載のパネ系を含む制御対象に対する適応的スライ デ ィ ングモ一 ド制御方法。

4. ロ ボッ ト をバネ系を含む制御対象と考え、 このロ ボ ッ ト 'に対 し以下の工程からなる適応的スライディ ングモ

― ド制御を適用する方法，

( a ) サーボ回路のメ モ リ 内に、 予想される外乱の最 大値 Dis(max)及び最小値 Dis(min)のほか、 スラ イ ディ ン グモー ド制御処理及びオブザーバ処理に必要な各種の定 数を設定 し入力する工程、

( ) ロボ'ッ トの動作を開始させ、 各周期ごと に位置 指令 0 r 及び位置フ ィ ー ドバヅ ク量 0を読みと これ ら読みと つた値から、 モ一夕速度 、 指令速度 0 r 、 指 令加速度 ¾' r 、 位置指令 0 r と位置フ ィ ー ドバッ ク量 S との差である位置偏差 ε、 及び速度偏差を を算出する 工程、

( c ) 工程 （ b ) で求めたモ一夕速度 Θ に基づきォブ ザーパ処理を して、 乇一夕 と ロボ ッ ト先端の推定ね じれ 量 e tf及び推定ね じれ速度 tiを求め、 さ ら に これ ら偏 差を伝達関数のフ ィ ルタ処理 してフ ィル夕処理を行った と こ ろのモ一夕 と ロ ボッ ト先端の推定ね じれ量 £ t 及び 推定ね じれ速度 £ t を求める工程、

( d ) 上記工程 （ b ) で求めた位置偏差 £及び速度偏 差 έ並びに上記工程 （ c ) で求めたフ ィ ルタ処理を行つ た と ころのモー夕 と ロ ボッ ト先端の推定ね じれ量 £ t に 基づき、 位相面 Suf を求める工程、

'( e ) 上記工程 （ d ) で求めた位相面 Suf に基づき重 力項の推定値 Grhat を、 ま た、 この位相面 Suf 及び上記 工程 （ b ) で求めたモータ速度 0 に基づき動摩擦係数の 推定値 Ahatを、 さ ら に上記位相面 Suf 、 上記工程 （ b ) で求めた位置偏差 £及び指令加速度 、 上記工程 （ c ) で求めた推定ね じれ量 £ t 及び推定ね じれ速度 e t に基 づきイ ナ一シャ項の推定値 Jhatを求める工程、

( f ) 上記工程 （ d ) で求めた位相面 Suf の値が [ 0 ] 以上であるか否かを判別 して、 [0] 以上である と切換入 力 て 1 を上記工程 （ a ) で入力 した外乱の最大値 Dis(ma X)と し、 また [0] 以下である と切換入力て 1 を上記工程 ( a ) で入力 した外乱の最小値 Dis(min)とする工程、

( g ) 上記工程 （ b ) で求めたモータ速度 0、 位置偏 差 ε及び指令加速度 0 r、 上記工程 （ c ) で求めたフィ ル夕処理を行ったと こ ろのモータ と ロボヅ ト先端の推定 ね じれ量 £ t 及び推定ね じれ速度 t 、 上記工程 （ d ) で求めた位相面 Suf 及び上記工程 （ e ) で求めたイ ナ一 シャ項の推定値 Jhat、 動摩 係数の推定値 Ahat、 重力項 の推定値 Grhat 、 さ ら に上記工程 （ f ) で求めた切換入 力 r l か ら、 モータへの ト ルク指令 rを求める工程

( h ) 上記工程 （ g ) で求めた ト ルク指令をサ一ボ回 路の電流ループに引 き渡す工程