JPH05216504A - バネ系を含む制御対象に対する適応的スライディングモード制御方式 - Google Patents

Abstract

(57)【要約】 【目的】 推定パラメータの収束性を向上させ、制振性
の優れた適応的スライディングモード制御方式を得る。 【構成】 指令位置とモータ位置との位置偏差ε，速度
偏差ε⁽¹⁾ 及び位置指令の加速度θｒ⁽²⁾ を求める（Ｓ
３）。オブザーバ処理及びフイルタ処理でモータ位置と
機械可動部の位置の差εｔ及びその微分値εｔ⁽¹⁾ を求
める（Ｓ４，Ｓ５）。モータ位置と機械可動部の位置の
偏差εｔをフィードバックした位相面Ｓｕｆを求める
（Ｓ７）。イナーシャの項，動作摩擦係数，重力項の各
推定値Ｊｈａｔ，Ａｈａｔ，Ｇｒｈａｔを求め（Ｓ
８）、位相面Ｓｕｆの符号に応じて切換入力τ１を外乱
の最大値Ｄｉｓmax か最小値Ｄｉｓｍｉｎにおき（Ｓ１
０，Ｓ１１）、モータへのトルク指令τを求め電流ルー
プに引き渡す（Ｓ１２，Ｓ１３）。位置決め終了後の機
械可動部の振動を減少させる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、サーボモータで駆動さ
れるロボット等のバネ系を含む制御対象の制御方式に関
するもので、特に、制御対象に適応的なスライディング
モード制御方式に関する。

【０００２】

【従来の技術】サーボモータで駆動される機械等の制御
においては、制御対象の機械に剛性があるものとし、制
御対象をバネ系と考えずに制御を行い、通常ＰＩ（比
例，積分）制御等の線形制御が行われている。しかし、
ロボットは通常、片持はり構成になっているため、ロボ
ットの姿勢に応じて大きなイナーシャ変動が生じ、サー
ボ系の応答性の変化が生じる。また、減速器などのバネ
要素も関係して低剛性で共振周波数が低い。そのため、
ロボットの先端（ツールセンタポイント）を位置決めす
るような際、サーボモータによる位置決め終了後、ロボ
ット先端が振動する。そこで、従来はロボットの低剛性
によるバネ系及びイナーシャ変動を無視し、その代わり
にサーボゲインを小さくして制御したり、位置決め終了
後タイマーを作動させ、所定時間経過してロボットの先
端の振動が停止した後作業を行う等の制御を行ってい
た。

【０００３】そこで、本願発明者等は、サーボモータで
駆動される制御対象に対して、スライディングモード制
御を適用し、機械可動部の振動を防止する発明を提案し
てきた（特開平３−９２９１１号公報，特開平３−１１
８６１８号公報等参照）。

【０００４】

【発明が解決しようとする課題】スライディングモード
制御と適応制御を組み合わせると非常にロバスト性が高
く（パラメータ変動に強く、外乱抑圧性が高い）、利用
価値が高いものとして知られている。しかし、ロボット
のような柔軟な構造物の機械等に適用すると推定パラメ
ータ（制御対象のイナーシャ，動摩擦、重力項等）が旨
く収束しなく、機械可動部が振動するという問題があ
る。そこで、本発明の目的は、上記推定パラメータの収
束性を向上させ、制振性のの優れた適応的スライディン
グモード制御方式を提供することにある。

【０００５】

【問題を解決するための手段】本発明は、サーボモータ
の位置と該サーボモータで駆動される制御対象の可動部
の位置との差をスライディングモード制御の位相面にフ
ィードバックして、該位相面が「０」に収束するように
サーボモータへのトルク指令を制御する。特にに、指令
位置とサーボモータの位置と位置偏差をε、速度偏差を
ε⁽¹⁾ 、サーボモータの位置と該サーボモータで駆動さ
れる制御対象の可動部の位置との差をεｔ、位置ループ
ゲインをＫｐ、サーボモータの位置と該サーボモータで
駆動される制御対象の可動部の位置との差のフィードバ
ックゲインをＫ１としたとき、スライディングモード制
御の位相面Ｓｕｆを次の式とし、 Ｓｕｆ＝ε⁽¹⁾ ＋Ｋｐ・ε−Ｋ１・εｔ イナーシャの項，動摩擦係数，重力項の各推定値をＪｈ
ａｔ，Ａｈａｔ，Ｇｒｈａｔ、位置指令の速度，加速度
を夫々θｒ⁽¹⁾ ，θｒ⁽²⁾ 、サーボモータの位置と該サ
ーボモータで駆動される制御対象の可動部の位置との差
の微分値をεｔ⁽¹⁾ 、切換入力をτ１としたとき、該切
換入力τ１を位相面Ｓｕｆが「０」以上のときは外乱の
最大値、「０」より小さいときは外乱の最小値としてサ
ーボモータへのトルク指令τを次の式を演算して求め、
サーボモータを制御する。

【０００６】 τ＝Ｋ２・Ｓｕｆ＋Ｊｈａｔ（θｒ⁽²⁾ −Ｋｐ² ε＋Ｋｐ・Ｋ１・εｔ −Ｋ１・εｔ⁽¹⁾ ）＋Ａｈａｔ・θ⁽¹⁾ ＋Ｇｒｈａｔ＋τ１ また、上記サーボモータの位置と該サーボモータで駆動
される制御対象の可動部の位置との差の微分値εｔ及び
その微分値εｔ(1) はオブザーバの処理によって求め
る。

【０００７】

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

【０００８】

【実施例】まず、スライディングモード制御の位相面Ｓ
ｕｆにモータの位置と機械可動部（ロボットの先端位
置）の位置との差（詳しくは、推定位置偏差に伝達関数
（Ｓ＋Ｋｐ）／（Ｓ＋Ａ）を乗じた値）εｔをフィード
バックするものとして、該位相面を次の１式とする。ま
た、ロボット等の制御対象の運動方程式を立てると次の
２式となる。 Ｓｕｆ＝ε⁽¹⁾ ＋Ｋｐ・ε−Ｋ１・εｔ …（１） τ＝Ｊ・θ⁽²⁾ ＋Ａ・θ⁽¹⁾ ＋Ｇｒ＋Ｄｉｓ …（２） なお、上記１式，２式において、εはモータへの指令位
置とモータの実際の位置との差である位置偏差であり、
θはモータの実際の位置を意味する。また、Ｋｐは位置
ループゲイン，Ｋ１は状態量である制御対象の機械の捩
じれ量（上記差εｔ）のフィードバックゲインで、τは
トルク，Ｊはイナーシャ，Ａは動摩擦係数，Ｇｒは重力
項，Ｄｉｓは外乱である。さらに、記号の右型にカッコ
書きした数字はその記号の意味する内容のものを微分し
た回数を意味し、例えば、ε⁽¹⁾は位置偏差εを１回微
分したもので速度偏差を意味し、θ⁽²⁾ はモータの位置
を２回微分したものでモータの加速度を意味する。

【０００９】また、モータへの位置指令をθｒとする
と、次の３式が成立する。 ε＝θｒ−θ ε⁽¹⁾ ＝θｒ⁽¹⁾ −θ⁽¹⁾ ε⁽²⁾ ＝θｒ⁽²⁾ −θ⁽²⁾ …（３） また、モータへのトルク指令τを次の４式とする。 τ＝Ｋ２・Ｓｕｆ＋Ｊｈａｔ（θｒ⁽²⁾ −Ｋｐ² ε＋Ｋｐ・Ｋ１・εｔ −Ｋ１・εｔ⁽¹⁾ ）＋Ａｈａｔ・θ⁽¹⁾ ＋Ｇｒｈａｔ＋τ１ …（４ ） 上記４式において、Ｋ２は後述するようにイナーシ
ャの最大値によって決まる定数、Ｊｈａｔはイナーシャ
項の推定値、Ａｈａｔは動摩擦係数の推定値、Ｇｒｈａ
ｔは重力項の推定値、τ１は切換入力である。

【００１０】また、リアプノフ関数候補Ｖを次の５式と
する。 Ｖ＝（１／２）Ｊ・Ｓｕｆ² ＋（１／２）α・Ｊｂａｒ² ＋（１／２）β・Ａｂａｒ² ＋（１／２）γ・Ｇｒｂａｒ² …（５） なお、α，β，γは適応速度を決める正の調整パラメー
タであり、Ｊｂａｒ，Ａｂａｒ，Ｇｒｂａｒはイナーシ
ャ項の推定誤差，動摩擦係数の推定誤差，重力項の推定
誤差で、次の６式の関係にある。

【００１１】 Ｊｂａｒ＝Ｊ−Ｊｈａｔ Ａｂａｒ＝Ａ−Ａｈａｔ Ｇｒｂａｒ＝Ｇｒ−Ｇｒｈａｔ …（６） 上記リアプノフ関数候補Ｖは最小値が「０」で常に正に
なる関数である。よって、リアプノフ関数候補Ｖの微分
値Ｖ⁽¹⁾ が常に負になる（単調減少）ようなモータへの
トルクτを決定すれば、リアプノフ関数候補Ｖは最小値
「０」に収束する。すなわち、Ｓｕｆ＝０，Ｊｂａｒ＝
０，Ａｂａｒ＝０，Ｇｒｂａｒ＝０となり、応答は従来
のスライディングモード制御と同様に、制御対象のパラ
メータ（イナーシャ，動摩擦係数，重力項）に依存しな
いＳｕｆ＝０の一定な応答関数によって決まる応答性が
得られる。また、各推定誤差は「０」になり、各推定値
は真値に収束することになる。

【００１２】そこで、上記５式の両辺を微分すると、 Ｖ⁽¹⁾ ＝Ｊ・Ｓｕｆ・Ｓｕｆ⁽¹⁾ ＋α・Ｊｂａｒ・Ｊｂａｒ⁽¹⁾ ＋β・Ａｂａｒ・Ａｂａｒ⁽¹⁾ ＋γ・Ｇｒｂａｒ・Ｇｒｂａｒ⁽¹⁾ …（７） また、１式の両辺を微分すると、 Ｓｕｆ⁽¹⁾ ＝ε⁽²⁾ ＋Ｋｐ・ε⁽¹⁾ −Ｋ１・εｔ⁽¹⁾ …（８） ３式を２式に代入し、ε⁽²⁾ について解くと、 ε⁽²⁾ ＝θｒ⁽²⁾ ＋（Ａ／Ｊ）θ⁽¹⁾ ＋｛（Ｇｒ＋Ｄｉｓ）／Ｊ｝−τ／Ｊ …（９） ９式を８式に代入しＳｕｆ(1) を求め、７式に代入する
と、 Ｖ⁽¹⁾ ＝Ｓｕｆ（Ｊ・θｒ⁽²⁾ ＋Ａ・θ⁽¹⁾ ＋Ｇｒ＋Ｄｉｓ −τ＋Ｊ・Ｋｐ・ε⁽¹⁾ −Ｊ・Ｋ１・εｔ⁽¹⁾ ） ＋α・Ｊｂａｒ・Ｊｂａｒ⁽¹⁾ ＋β・Ａｂａｒ・Ａｂａｒ⁽¹⁾ ＋γ・Ｇｒｂａｒ・Ｇｒｂａｒ⁽¹⁾ …（１０） １式よりε⁽¹⁾ を求め上記１０式に代入すると共に、４
式を代入し、整理すると次の１１式となる。

【００１３】 Ｖ⁽¹⁾ ＝Ｓｕｆ² （Ｊ・Ｋｐ−Ｋ２） ＋Ｊｂａｒ｛Ｓｕｆ（θｒ⁽²⁾ −Ｋｐ² ・ε＋Ｋｐ・Ｋ１・εｔ −Ｋ１・εｔ⁽¹⁾ ）＋α・Ｊｂａｒ⁽¹⁾ ｝ ＋Ａｂａｒ（Ｓｕｆ・θ⁽¹⁾ ＋β・Ａｂａｒ⁽¹⁾ ） ＋Ｇｒｂａｒ（Ｓｕｆ＋γ・Ｇｒｂａｒ⁽¹⁾ ） ＋Ｓｕｆ（Ｄｉｓ−τ１） …（１１） 上記リアプノフ関数候補の微分値Ｖ⁽¹⁾ を常に負にする
ことを考える。まず１１式の右辺第１項を負にするに
は、 Ｊ・Ｋｐ−Ｋ２＜０ とすればよいから、係数ｋ２の値を次の１２式のよう
に、イナーシャの最大値Ｊmax に位置ループゲインＫｐ
を乗じた値にすれば、上記１１式の第１項は常に負にな
る。

【００１４】 Ｋ２＝Ｊmax ・Ｋｐ …（１２） また、上記１１式の右辺第２項は「０」にする。

【００１５】 Ｓｕｆ（θｒ⁽²⁾ −Ｋｐ² ・ε＋Ｋｐ・Ｋ１・εｔ−Ｋ１・εｔ⁽¹⁾ ） ＋α・Ｊｂａｒ⁽¹⁾ ＝０ …（１３） よって Ｊｂａｒ⁽¹⁾ ＝−（１／α）Ｓｕｆ（θｒ⁽²⁾ −Ｋｐ² ・ε ＋Ｋｐ・Ｋ１・εｔ−Ｋ１・εｔ⁽¹⁾ ）…（１４） また、６式のＪｂａｒ＝Ｊ−Ｊｈａｔを微分してＪ(1)
＝０（インーシャＪを一定）と仮定すると、Ｊｂａｒ
⁽¹⁾ ＝−Ｊｈａｔ⁽¹⁾ であるから、上記１４式は Ｊｈａｔ⁽¹⁾ ＝（１／α）Ｓｕｆ（θｒ⁽²⁾ −Ｋｐ² ・ε ＋Ｋｐ・Ｋ１・εｔ−Ｋ１・εｔ⁽¹⁾ ）…（１５） さらに、同様に、Ａ⁽¹⁾ ＝０，Ｇｒ⁽¹⁾ ＝０と仮定し
て、１１式の第３項，第４項についても「０」とすれ
ば、 Ａｈａｔ⁽¹⁾ ＝（１／β）Ｓｕｆ・θ^(1） …（１６） Ｇｒｈａｔ⁽¹⁾ ＝（１／γ）Ｓｕｆ …（１７） 次に１１式の右辺第５項を負にするようにする。

【００１６】 Ｓｕｆ（Ｄｉｓ−τ１）＜０ …（１８） 上記１８式が成立するには、外乱の最大値及び最小値を
Ｄｉｓmax 及びＤｉｓmin とすると、 Ｓｕｆ≧０のときには τ１＝Ｄｉｓmax …（１９） Ｓｕｆ＜０のときには τ１＝Ｄｉｓmin …（２０） とすればよい。

【００１７】以上のように、上記１２式，１５式，１６
式，１７式及びＳｕｆの正負に応じて１９式か１８式が
成立するようにモータにトルク指令τすればリアプノフ
関数候補Ｖの微分値は常に負になる。すなわち、１５
式，１６式，１７式を積分すれば、夫々推定値Ｊｈａ
ｔ，Ａｈａｔ，Ｇｒｈａｔが求められ、係数Ｋ２の値を
Ｋｐ・Ｊmax に設定し、Ｓｕｆの正負に応じて切換入力
τ１をＤｉｓmax かＤｉｓmin に設定すれば、４式より
モータへのトルク指令が求まる。

【００１８】４式よりトルク指令τを求める場合、指令
位置θｒとモータの位置θの位置偏差ε（＝θｒ−
θ），速度偏差ε⁽¹⁾ （＝θｒ⁽¹⁾ −θ⁽¹⁾ ），位置指
令の速度θｒ⁽¹⁾ ，指令の加速度θ⁽²⁾ は指令位置とモ
ータに取り付けたパルスコーダ等によって検出できる
が、モータ位置θと機械可動部の差εｔ及びその速度偏
差εｔ⁽¹⁾ は検出できない。そこで、この差εｔと速度
偏差εｔ⁽¹⁾ をオブザーバによって推定する。

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

【００２０】図２中、符号１０はモータ側で符号１１は
機械側であり、その間に減速器が設けられ該減速器のバ
ネ定数をＫｃ，粘性項をＢｋとする。また、モータのイ
ナーシャをＪｍ，粘性係数をＡｍ，位置をθ，機械のイ
ナーシャをＪｔ，位置（ロボットのアーム先端位置）を
θｔとする。

【００２１】モータの出力トルクをＴとし、モータ側１
０で運動方程式をたてると、 Ｔ＝Ｊｍ・θ⁽²⁾ ＋Ｂｋ（θ⁽¹⁾ −θｔ⁽¹⁾ ） ＋Ｋｃ（θ−θｔ）＋Ａｍ・θ⁽¹⁾ …（２１） また、負荷側（機械側）１１で運動方程式をたてると、 ０＝Ｊｔ・θｔ⁽²⁾ ＋Ｂｋ（θｔ⁽¹⁾ −θ⁽¹⁾ ）＋Ｋｃ (θｔ−θ) …（２２） （θ−θｔ）はモータのロータ回転位置に対する負荷の
位置偏差（ねじれ量）を意味し、 (θ⁽¹⁾ −θｔ⁽¹⁾ ）
は速度偏差（ねじれ速度）を意味する。そこで、位置偏
差（ねじれ量）をεtf，速度偏差（ねじれ速度）をεtf
⁽¹⁾ とする。すなわち、 θ−θｔ＝εtf θ⁽¹⁾ −θｔ⁽¹⁾ ＝εtf⁽¹⁾ …（２３） とし、上記２１式をＪｍで除したものから、２２式をＪ
ｔで除したものを減算すると次の２４式となる。

【００２２】 Ｔ／Ｊｍ＝εtf⁽²⁾ ＋{(Ｂｋ／Ｊｍ) ＋ (Ｂｋ／Ｊｒ)}εtf⁽¹⁾ ＋{(Ｋｃ／Ｊｍ) ＋ (Ｋｃ／Ｊｔ)}・εtf＋（Ａｍ／Ｊｍ）θ⁽¹⁾ …（２４） ２４式より、 εtf⁽²⁾ ＝−{(Ｂｋ／Ｊｍ) ＋ (Ｂｋ／Ｊｔ)}εtf⁽¹⁾ −{(Ｋｃ／Ｊｍ) ＋ (Ｋｃ／Ｊｔ)}・εtf −（Ａｍ／Ｊｍ）θ⁽¹⁾ ＋Ｔ／Ｊｍ …（２５） ２１式をθ⁽²⁾ についてとき２３式を代入すると、 θ⁽²⁾ ＝−（Ｂｋ／Ｊｍ）εtf⁽¹⁾ −（Ｋｃ／Ｊｍ）εtf −（Ａｍ／Ｊｍ）θ⁽¹⁾ ＋Ｔ／Ｊｍ …（２６） ここで、 εtf⁽¹⁾ ＝Ｘ１ εtf＝Ｘ２ θ⁽¹⁾ ＝Ｘ３ とすると、上記２５式と２６式より次の２７式の状態遷
移行列ができる。

【００２３】

【数式２７】上記状態変数Ｘ１〜Ｘ３の内、観測可能な
状態変数はＸ３であるモータの速度θ⁽¹⁾ である。そこ
で、 Ｙ＝［０ ０ １］［Ｘ１ Ｘ２ Ｘ３］^ｔ …（２８） とし、２７式の行列（Ｘ１．Ｘ２．Ｘ３）にかかる行列
をＡ，Ｔにかかる行列をＢ，２８式の行列（Ｘ１．Ｘ
２．Ｘ３）にかかる行列をＣとし、この系をＺ変換する
と、２７式，２８式は、 Ｘ（ｎ＋１）＝Ａｚ・Ｘ（ｎ）＋Ｂｚ・Ｔ …（２９） Ｙ＝Ｃ・Ｘ（ｎ） …（３０） となる。なお、Ｘ（ｎ）＝［Ｘ１（ｎ） Ｘ２（ｎ）
Ｘ（ｎ）］^ｔである。

【００２４】ここで、Ｘ´（ｎ）をＸ（ｎ）の推定値と
して、同一次元オブザーバを組むと Ｘ´（ｎ＋１）＝（Ａｚ−Ｋ・Ｃ）Ｘ´（ｎ）＋Ｂｚ・Ｔ＋Ｋ・Ｙ …（３１） となり、上記３１式を計算することによって（行列Ａ，
Ｂ，Ｃの各要素は既知のものである）、行列Ｘ１（ｎ）
を推定し、Ｘ１＝εtf⁽¹⁾ 及びＸ２＝εtfを求める。な
お、ＫはＡｚ−Ｋ・Ｃが安定するように選定する。

【００２５】そして、このオブザーバの処理によって得
られたεtf⁽¹⁾ ，εtfに対して伝達関数（Ｓ＋Ｋｐ）／
（Ｓ＋Ａ）のフイルタ処理してεｔ⁽¹⁾ ，εｔを求め
る。

【００２６】 εｔ＝εtf・（Ｓ＋Ｋｐ）／（Ｓ＋Ａ） …（３２） εｔ⁽¹⁾ ＝εtf⁽¹⁾ ・（Ｓ＋Ｋｐ）／（Ｓ＋Ａ） …（３３） こうして、εｔ⁽¹⁾ ，εｔの値が求まれば、モータへの
指令トルクτが求まる。

【００２７】次に上記モータ位置と機械可動部位置をス
ライディングモードの位相面にフィードバックする適応
的スライディングモード制御を行うと、制振効果がある
ことをロボットのバルモデルを１例として証明する。上
記位相面Ｓｕｆが「０」になったときを考えると、 ０＝ε⁽¹⁾ ＋Ｋｐ・ε−Ｋ１・εｔ …（３４） 上記３４式に３式，３２式及び２３式を代入し、ラプラ
ス変換し整理すると３５式を得る。

【００２８】 ０＝（Ｓ＋Ａ）θｒ−（Ｓ＋Ａ＋Ｋ１）θ＋Ｋ１・θｔ …（３５） 一方、上記２２式をラプラス変換して整理すると、 θｔ／θ＝（Ｂｋ・Ｓ＋Ｋｃ）／（Ｊｔ・Ｓ² ＋Ｂｋ・Ｓ＋Ｋｃ） …（３６） なお、ロボットは粘性項Ｂｋが小さく、またバネ定数Ｋ
ｃ小さいため、上記３６式からダンピングが悪くしかも
低周波数で振動することがわかる。３５式よりθを求め
て３６式に代入し整理すると、 θｔ／θｒ＝（Ｂｋ・Ｓ＋Ｋｃ）／｛Ｊｔ・Ｓ² ＋Ｂｋ・Ｓ＋Ｋｃ ＋（Ｓ² ・Ｋ１・Ｊｔ）／（Ｓ＋Ａ）｝ …（３７） 上記３７式において、Ａ＝０、すなわち、具体的にはこ
のバネ系（ロボット）の共振周波数でＳ＋ＡがＳとみな
せるまでＡの値を小さくすると３７式は次の３８式とな
る。 θｔ／θｒ＝（Ｂｋ・Ｓ＋Ｋｃ）／｛Ｊｔ・Ｓ² ＋（Ｂｋ＋Ｋ１・Ｊｔ）Ｓ ＋Ｋｃ｝ …（３８） この３８式から、モータ位置とロボット先端位置の差の
フィードバックゲインＫ１と機械イナーシャの積がダン
ピング項に入り、ダンピング項が大きくなり、振動抑制
効果が生じることがわかる。

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

【００３０】図３，図４は、本実施例において、上記デ
ジタルサーボ回路のプロセッサが実行する本発明のサー
ボモータ制御処理に関するフローチャートであり、該プ
ロセッサは所定周期毎、図３，図４に示す処理を実行す
る。

【００３１】まず、ディジタルサーボ回路３のメモリ内
にスライディングモード制御処理及びオブザーバ処理に
必要な定数等、即ち、位置ループゲインＫｐ，捩じれ量
のフィードバックゲインＫ１，外乱の最大値，最小値Ｄ
ｉｓmax,Ｄｉｓmin ，モータ及び機械を含めたイナーシ
ャの最大値Ｊmax ，イナーシャの最大値Ｊmax に位置ル
ープゲインＫｐを乗じた値である定数Ｋ２の値，モータ
のイナーシャＪｍ，調整パラメータα，β，γ，行列
Ａ，Ｂ，Ｃの各要素の値をあらかじめ設定する。

【００３２】そして、ロボットの動作を開始させると、
ホストプロセッサ１は各軸に対して位置指令を分配し、
ディジタルサーボ回路３のプロセッサは共有ＲＡＭ２よ
り位置指令θｒを読み取ると共に、パルスコーダ６から
出力される位置フィードバック量θを読取る（ステップ
Ｓ１，Ｓ２）。次に、位置指令θｒと位置フィードバッ
ク量θより位置偏差εを求める。すなわち、位置偏差を
記憶するレジスタに位置指令θｒから位置フィードバッ
ク位置θを減じた値を加算し位置偏差εを求める。ま
た、検出フィードバック位置θから前周期で検出したフ
ィードバック位置を減じてモータ速度（モータ位置θの
１回微分）θ⁽¹⁾ を求め、さらに、位置指令θｒから前
周期の位置指令を減じた値である指令速度（位置指令θ
ｒの１回微分）θｒ⁽¹⁾ から上記モータ速度を減じて速
度偏差ε⁽¹⁾ を求めると共に、求めた速度指令θｒ⁽¹⁾
から前周期で求めた指令速度を減じて指令の加速度（位
置指令θｒの２回微分）θｒ⁽²⁾ を求める（ステップＳ
３）。

【００３３】次に、モータ速度θ⁽¹⁾ より前述したオブ
ザーバの処理を実行し、モータとロボット先端の推定位
置偏差εtf，推定速度偏差εtf⁽¹⁾ を求める（ステップ
Ｓ４）。次に式３２，式３３の演算を行ってフイルタ処
理を行ってフィルタ処理が行われたモータとロボット先
端の推定位置偏差εt ，推定速度偏差εt ⁽¹⁾ を求める
（ステップＳ５）。そして、ステップＳ３で求めた位置
偏差ε，速度偏差ε^{(1 )} 及びステップＳ５で求めたフィ
ルタ処理が行われたモータとロボット先端の推定位置偏
差εt より１式の演算を行って位相面Ｓｕｆを求める
（ステップＳ６）。次にモータへのトルク指令τを求め
るために４式のイナーシャの推定値Ｊｈａｔにかかる値
をステップＳ３で求めた位置偏差ε，位置指令の加速度
θ⁽²⁾ 及びステップＳ５で求めたεｔ，εｔ⁽¹⁾ より求
め、この値をＷＲＫとしてレジスタに記憶する（ステッ
プＳ７）すなわち、次の３９式の演算を行う。

【００３４】 ＷＲＫ＝θｒ⁽²⁾ −Ｋｐ² ε＋Ｋｐ・Ｋ１・εｔ−Ｋ１・εｔ⁽¹⁾ …（３９） 次に、ステップＳ６，Ｓ７で求めた位相面Ｓｕｆの値及
びＷＲＫの値より１５式，１６式，１７式を積分し、イ
ナーシャの項の推定値Ｊｈａｔ，動摩擦係数の推定値Ａ
ｈａｔ，重力項の推定値Ｇｒｈａｔを求める。すなわ
ち、Ｓｕｆ・ＷＲＫの値を各周期毎算出しこの算出され
た値をアキュムレータに積算し、その積算値に係数（１
／α）を乗じてイナーシャの項の推定値Ｊｈａｔを求
め、同様に、Ｓｕｆ・θ⁽¹⁾ の値を求めアキュムレータ
に積算し、その積算値に係数（１／β）を乗じて動摩擦
係数の推定値Ａｈａｔを求める。また、Ｓｕｆの値をア
キュムレータに積算して、係数（１／γ）を乗じて重力
項の推定値Ｇｒｈａｔを求める。

【００３５】次に位相面Ｓｕｆの値が「０」以上か否か
判断し（ステップＳ９）、「０」以上ならば、切換入力
τ１としてレジスタにＤｉｓmax を、負ならばＤｉｓmi
n を格納し（ステップＳ１０，Ｓ１１）、式４の演算を
行ってモータへのトルク指令τを求め（ステップＳ１
２）、該トルク指令τを電流ループに引き渡し（ステッ
プＳ１３）、当該処理周期の処理を終了する。

【００３６】以下、各周期毎上記処理をディジタルサー
ボ回路３のプロセッサは実行しサーボモータを制御し、
ロボットを駆動することになる。

【００３７】図５〜図８は本発明の効果を見るためのシ
ミュレーションデータを示す図で、図５はモデルばね系
の周波数応答を示す図であり、図６はシミュレーション
のため入力波形を示し、（ａ）は加加速度、（ｂ）は加
速度、（ｃ）は速度、（ｄ）は位置である。図７は本発
明を適用しないときのシミュレーション結果のデータ
で、図８は本発明を適用したときのシミュレーション結
果のデータであり、夫々（ａ）はモータ位置θ、（ｂ）
はモータ速度θ⁽¹⁾ 、（ｃ）はアーム先端位置θｔ、
（ｄ）はアーム先端位置速度θｔ⁽¹⁾ 、（ｅ）はイナー
シャ値の推定値Ｊｈａｔ、（ｆ）は動摩擦係数の推定値
Ａｈａｔ、（ｇ）は重力項の推定値Ｇｒｈａｔ、（ｈ）
はモータへのトルク指令τである。

【００３８】この図７と図８の（ｃ），（ｄ）で示すア
ーム先端位置及び速度を比較して分かるように本発明を
適用した場合の方（図８）が、位置決め終了後の振動が
減少していることが分かる。また、図７，図８の（ｅ）
〜（ｇ）を比較しても、本発明を適用したときの方がイ
ナーシャ値の推定値Ｊｈａｔ、動摩擦係数の推定値Ａｈ
ａｔ、重力項の推定値Ｇｒｈａｔが真値にほぼ等しいこ
とが分かる。

【００３９】

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

【図面の簡単な説明】

【図１】本発明の一実施例を実施するサーボ制御系のブ
ロック図である。

【図２】制御対象をばねモデルとしたときのモデルブロ
ック図である。

【図３】本発明の一実施例において、所定周期毎にディ
ジタルサーボ回路のプロセッサが実施する処理のフロー
チャートの一部である。

【図４】図３のフローチャートの続きである。

【図５】本発明の効果を見るためのシミュレーションを
行ったときのモデルの周波数特性を示す図である。

【図６】上記シミュレーションの入力波形を表す図であ
る。

【図７】上記シミュレーションで本発明を適用しないと
きの各データの波形を表す図である。

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

【符号の説明】

εtf モータと機械可動部の推定位置偏差 εtf(1) モータと機械可動部の推定速度偏差 εｔ モータと機械可動部の位置偏差 εｔ(1) モータと機械可動部の速度偏差 Ｊｈａｔ イナーシャ項の推定値 Ａｈａｔ 動摩擦係数の推定値 Ｇｒｈａｔ 重力の項の推定値 Ｄｉｓ 外乱 τ１ 切換入力 τ モータへのトルク指令

【数２７】

Claims (3)

【特許請求の範囲】
1. 【請求項１】 適応的スライディングモード制御方式に
   おいて、サーボモータの位置と該サーボモータで駆動さ
   れる制御対象の可動部の位置との差をスライディングモ
   ード制御の位相面にフィードバックして、該位相面が
   「０」に収束するようにサーボモータへのトルク指令を
   制御することを特徴とするバネ系を含む制御対象に対す
   る適応的スライディングモード制御方式。
2. 【請求項２】 適応的スライディングモード制御方式に
   おいて、指令位置とサーボモータの位置と位置偏差を
   ε、速度偏差をε⁽¹⁾ 、サーボモータの位置と該サーボ
   モータで駆動される制御対象の可動部の位置との差をε
   ｔ、位置ループゲインをＫｐ、サーボモータの位置と該
   サーボモータで駆動される制御対象の可動部の位置との
   差のフィードバックゲインをＫ１としたとき、スライデ
   ィングモード制御の位相面Ｓｕｆを次の式とし、 Ｓｕｆ＝ε⁽¹⁾ ＋Ｋｐ・ε−Ｋ１・εｔ イナーシャの項，動摩擦係数，重力項の各推定値をＪｈ
   ａｔ，Ａｈａｔ，Ｇｒｈａｔ、位置指令の速度，加速度
   を夫々θｒ⁽¹⁾ ，θｒ⁽²⁾ 、サーボモータの位置と該サ
   ーボモータで駆動される制御対象の可動部の位置との差
   の微分値をεｔ⁽¹⁾ 、切換入力をτ１としたとき、該切
   換入力τ１を位相面Ｓｕｆが「０」以上のときは外乱の
   最大値、「０」より小さいときは外乱の最小値としてサ
   ーボモータへのトルク指令τを次の式を演算して求める τ＝Ｋ２・Ｓｕｆ＋Ｊｈａｔ（θｒ⁽²⁾ −Ｋｐ² ε＋Ｋｐ・Ｋ１・εｔ −Ｋ１・εｔ⁽¹⁾ ）＋Ａｈａｔ・θ⁽¹⁾ ＋Ｇｒｈａｔ＋τ１ ことを特徴とするバネ系を含む制御対象に対する適応的
   スライディングモード制御方式。
3. 【請求項３】 上記サーボモータの位置と該サーボモー
   タで駆動される制御対象の可動部の位置との差の微分値
   εｔ及びその微分値εｔ(1) はオブザーバの処理によっ
   て求める請求項２記載のバネ系を含む制御対象に対する
   適応的スライディングモード制御方式。