JPH03118618A - 制振効果を持つスライディングモード制御による制御方式 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、ロボット等のバネ要素が大きく作用する制御
対象に対するスライディングモード制御方式に関する。

特に、速度及び加速度のフィードフォアード制御を行う
制御系におけるスライディングモード制御方式に関する
。

従来の技術 第３図は、位置に対し比例（Ｐ）制御、速度に対し比例
、積分（ＰＩ）制御を行うロボット等のサーボモータの
制御において、フィードフォアード制御を行うときのブ
ロック線図である。

図中、伝達関数１０のＫＰは位置ループにおける比例ゲ
イン、伝達関数１２は速度ループにおける前置補償の伝
達関数で、Ｋ１は積分定数、Ｋ２は比例定数である。ま
た、伝達関数１４はモータの伝達関数で、Ｊはイナーシ
ャであり、伝達関数１６は速度θを積分して位置θを算
出する伝達関数である。

また、伝達関数１８は位置指令値θｒを微分して速度指
令値に加算する速度のフィードフォアードの項、伝達関
数２０は伝達関数１８の出力をさらに微分し、イナーシ
ャやぼね要素の粘性項、バネ定数によって決まる定数Ｋ
Ｆを乗じて加速度を算出しトルク指令値に加算する加速
度のフィードフォアードの項である。

位置指令値θｒからフィードバックされた現在位置を減
算し、位置偏差ε（＝θｒ−〇）を求め、これに比例定
数ＫＰを乗じて速度指令値を求め、さらに位置指令値θ
ｒを微分した速度フィードフォアード項の出力を加算し
て、速度フィードフオアードされた速度指令値からフィ
ードバックされた実速度θを減じた値を速度ループによ
ってＰＩ制御を行い、伝達関数１２の出力と速度フィー
ドフォアード項の出力をさらに微分し、定数ＫＦを乗じ
て得られる加速度のフィードフォアード項の出力を加算
してトルク指令値Ｔとして求め、該トルク指令値Ｔに対
応する電流をモータに流し、モータを駆動する。

以上がフィードフォアード制御が行われるモータ制御の
概略の動作である。

上述したフィードフォアード制御の場合、加速度のフィ
ードフォアード項２ｏにイナーシャＪやぼね要素の粘性
項、ばね定数が関係して決まる定数ＫＦが関係している
ので、これらの値の変化に応じて加速度のフィードフォ
アード項２ｏの出方が大きく変動し振動を生じることと
なる。

そこで、従来、ロボットのようにサーボモータの出力を
減速機で減速して制御対象を駆動するものにおいては、
減速機やボールネジ等の伝動機構のバネ要素が大きいも
のではバネ系のバネ定数及び粘性項をあらゆる姿勢で測
定し、オンラインで従来の線形制御の方法によりダイナ
ミックに変化するフィードフォアード項を計算し、フィ
ードフォアードを掛けて制御する方式が採用されている
。

また、イナーシャ変動に対しては、本願出願人によって
イナーシャ変動に応じてトルク指令値を変えるスライデ
ィングモード制御方式がすでに特願平１−１１７５１８
号で提案されている。

発明が解決しようとする課題 上述したバネ要素のバネ定数、粘性項を測定し、この測
定値により線形制御によってフィードフォアード項を補
正する方法では、オンラインでフィードフォアード項を
変更するため、バネ定数、粘性項等の値を正確なものに
する必要があり、ロバスト性に欠けるという欠点がある
。

そこで、本発明の目的は、ロバスト性を回復しつつ完全
な追従性を得ることができるねじれ量のフィードバック
を含むスライディングモード制御による制御方式を提供
することにある。

課題を解決するための手段 本発明は、スライディングモード制御の切換え面をサー
ボモータの位置と該サーボモータで駆動される機械可動
部の位置の差であるねじれ量とねじれ速度により変更し
てスライディングモード制御を行い、振動を除去するこ
とによって上記課題を解決した。

作用 スライディングモードの切換え面にバネ系のねじれ量、
ねじれ速度を導入したので、バネ系のねじれも考慮され
て切換え面に収束するようにトルク指令値が変えられる
から、適応性をもったバネ系に対してもロバストな系を
得ることができ、機械可動部の振動を防止しつつ、完全
な指令追従性が得られる。

実施例 第３図においてモータの入出力を見ると、次の第（１）
式〜第（４）式が成立する。

ＪＯ＝Ｔ　　　　　　　　　・・・・・・（１）ε＝θ
ｒ−θ　　　　　　・・団・（２）ｔ＝θｒ−θ　　　
　　　・・・・・・（３）ど＝θｒ−θ　　　　　　・
・・・・・（４）なお、Ｌ、θｒ、θは微分を、ト、θ
ｒ、θは２回微分を意味し、θｒは速度フィードフォア
ードを表わす。

ここで、切換え面Ｓとして、従来の切換え而Ｓ＝々＋Ｃ
・εに積分要素を付加し、さらに、モータで駆動される
機械のバネ要素（減速器の低剛性と粘性）のねじれ量ε
ｎ＝θ−θｔを付加し、第（５）式で示す切換え面とす
る。なお、θｔは機械の位置である。

Ｓ＝Ａ＋Ｃａｅ＋Ｄ−ｆ（Ａ＋Ｃ・ｅ）−Ｋ・εｎ　　
−（５）なお、第（５）式において、Ｃは位置偏差εと
速度偏差ｔの時定数、Ｄは積分要素の時定数、Ｋはダン
ピングを大きくするためのフィードバックゲインである
。

しかし、説明を簡単にするために、Ｄ＝０として切換え
面を次の第（５′）式として以下説明する。

ｓ＝ｉ＋ｃ−ｅ−Ｋｎ　　ε　ｎ　　　　　　　　−（
５’）また、トルク指令値（制御対象に入力されるトル
ク指令値）Ｔを次の第（６）式であるとする。

Ｔ＝ＪＯωｃ−ｉ＋ＪＯ・ωＣ−Ｃ・ε＋Ｔ１　・・・
（６）なお、第（６）式で、ＪＯは制御対象の予想され
る最小イナーシャ、Ｔ１は切換え入力値であり、ωＣは
時定数としての定数である。

リアプノフ関数候補として次の第（７）式を考える。

Ｖ＝　（１／２）−３２−・・・・・　（７）上記リア
プノフ関数■は常に正で最小値がｒＯＪであり、もし、
Ｍく０であればリアプノフ関数Ｖは最小値「０」に収束
する。また、これにより切換え面Ｓは常に収束し、応答
性がＳ＝０の一定の応答関数によって決定される。

上記第（５）式より 白＝ど＋Ｃ・々−に・Ａｎ　　　　・・・（８）また、
第（１）式と第（４）式より ど＝θｒ−Ｔ／Ｊ　　　　　　　・・・・・・（９）上
記第（９）式に第（６）式を代入して整理すると、 と＝ｏｒ　＝ＪＯωｃ　＠　ｉ：／Ｊ−ＪＯωｃ−Ｃ・
ε／Ｊ−ＴＩ／Ｊ・・・・・・（１０） 上記第（１０）式を第（８）式に代入し整理すると、Ｑ
　＝　　（Ｃ−ωｃ−ＪＯ／Ｊ）　−Ａ−ＪＯωｃ−Ｃ
・ｅ／Ｊ−に−Ａ　ｎ−Ｔｌ／　Ｊ　＋／Ｊ　ｒ　　　
　　　　・・曲（ｉｆ）上記第（５′）式をｉについて
解くと、々＝Ｓ−Ｃφε十Ｋ・εｎ　　　・・・・・・
　（１２）上記第（１２）式を第（１１）式に代入し整
理すると、 Ｑ＝（Ｃ−ωｃ−ＪＯ／Ｊ）・５ −０２・ε＋（Ｃ−ＪＯωｃ／Ｊ）・Ｋ・εｎ−ＫＡｎ
−Ｔｌ／Ｊ＋θｒ　　　　　　　　　　・・−・−第（
７）式を微分すると、 ？＝Ｓ−白　　　　　　　　川・・（１４）第（１４）
式の白に第（１３）式を代入するとｔ　＝　　（Ｃ−ω
ｃ−ＪＯ／Ｊ）　・Ｓ２［Ｃ２・ε−（Ｃ−ＪＯωｃ／
Ｊ’）　Ｋ−εｎ＋に−Ａｎ＋（Ｔｌ／Ｊ）−θｒ］・
ｓ・・曲そこで、定数ωＣを第（１６）式に示すように
決定する。

ωｃ＝＝Ｃ−Ｊｍａｘ／ＪＯ−（１６）なお、Ｊｍｍｘ
は制御対象において想定される最大イナーシャである。

第（１６）式に示すように定数ωＣを決定すると、第（
１５）式の右辺第１項は、 （Ｃ−ωｃ−ＪＯ／Ｊ）・Ｓ２ ＝　（Ｃ−Ｃ−Ｊｍａｘ　／Ｊ）　Ｓ２＜　０　　　−
・曲となる（Ｊｍａｘ　／Ｊ　　＞　　１であるから）
。

故に、リアプノフ関数Ｖの微分Ｍを常に？＜０とするに
は、第（１５）式より、第（１８）式が成立するように
切換え入力Ｔ１を決定すればよい。

−［Ｃ２４ε−（Ｃ−１０ωｃ／Ｊ）Ｋ・εｎ＋に−Ａ
ｎ＋　（ＴＩ／Ｊ）　−１Ｊ　ｒｌ　・Ｓ　＜　０　　
　　　・−−−−−（１８）そこで、切換え入力Ｔ１を
εの関数Ｋｌ（ε）と加速度フィードフォアードθｒの
関数に２（θｒ）と、ねじれ量εｎの関数に３（εｎ）
及びねじれ速度の関数に４（々ｎ）に分け、第（１９）
式％式％ （） （１９） 上記第（１８）式が成立するには、 （ｉ）　　　Ｓ　≧Ｏのとき Ｔ１．　＝　Ｋｌ　（ｇ）　＋に２　（θｒ）　＋に３
　（εｎ）　＋に４　（＃ｎ）〉−Ｃ２・Ｊ・ε＋Ｊθ
ｒ＋　（Ｃ−ＪＯωｃ／Ｊ）　Ｊ−Ｋｍεｎ−Ｊ−に−
Ａｎ （２０） であればよいので、 （イ）　　Ｋ１（ε）に関して ε　≧　００とき （ロ） （ハ） Ｋｌ（ε）＝−Ｃ２・ＪＯ・ε　　　・・・・・・　（
２１）ε　〈　０のとき Ｋｌ　（ε）　＝−Ｃ２ｅＪｍａｔ　・ε−（２２）Ｋ
２（ｏｒ）に関して（加速度フィードフォアードに関し
て） ｏｒ　≧　０のとき に２（ｏｒ）　＝Ｊｔｎａｘ　拳θｒ　　　　−−−−
−・（２３）１）ｒ　　＜　　Ｑのとき に２（ｏｒ）＝ＪＯ・ｏｒ　　　　　・”　−（２４）
Ｋ３（εｎ）に関して εｎ≧　０のとき Ｋ　３　（εｎ）　＝　Ｏ−（２５） εｎ＜Ｏのとき に３　（εｎ）　＝　（ＪＯ−Ｊｍａｘ）　・Ｃ−Ｋｓ
εｎ・・・・・・（２６） （ニ） Ｋ４（Ａｎ）に関して 立ｎ　≧　０のとき に４　（Ａｎ）＝−ＪＯ拳に−Ａｎ　　　−（２７）Ａ
ｎ　＜　　Ｏのとき に４　（Ａｎ）　＝−Ｊｍａｘ　−に−Ａｎ　　−（２
８）（ｉｉ） Ｓ〈０のとき Ｔｌ　＝　ＫＬ　（ε）　＋に２　（ｏｒ）＋に３　（
εｎ）＋に４　（Ａｎ）〈−Ｃ２・Ｊ・ε＋Ｊθｒ＋　
（Ｃ−ＪＯ（１）Ｃ／Ｊ）　Ｊ曇Ｋ・εｎ−ＪｌＩＫｅ
ｌｎ ・・・・・・（２９） であればよいので、 （イ）　　Ｋ１　（ε）に関して ε　≧　０のとき Ｋｌ　（５）　＝−Ｃ２−Ｊｍａｘ　・ε−（３０）ε
　く　０のとき Ｋｌ（ε）＝−Ｃ２・ＪＯ・ε　　・・・　（３１）（
ロ）　Ｋ２（ｏｒ）に関して（加速度フィードフォアー
ドに関して） ｏｒ　≧　０のとき に２（ｏｒ）＝ＪＱ　・ｏｒ・・・・・・（３２）ｏｒ
＜０のとき に２（ｏｒ）＝Ｊ＋ｎａ！　−ｏｒ　　　−・−・・・
（３３）（ハ）　　Ｋ３（εｎ）に関して εｎ≧　０のとき に３（εｎ）＝（ＪＯ−Ｊｍａり・Ｃ−Ｋ・εｎ・・・
・・・（３４） εｎ＜Ｏのとき Ｋ　３　（ｅ　ｎ）　＝　Ｏ−（３５）（ニ）　　　Ｋ
４（Ａｎ）に関して Ａｎ　≧　０のとき に４　（Ａｎ）　＝−Ｊｍａｘ　−に−Ａｎ　　−（３
６）Ａｎ　＜　　０のとき に４　（Ａｎ）＝−ＪＯ・Ｋ−Ａｎ　　−（３７）とす
れば、リアプノフ関数Ｖの微分！は常にＭく０となり、
切換え面に収束し制御系は安定する。

そこで、切換え面Ｓが「０」に収束したとき、上記ねじ
れ量εｎ、ねじれ速度Ａｎがロボット等の機械のバネモ
デルに対し制振効果があることを、以下説明する。

切換え面Ｓが「０」になったとき第（５′）式より ０＝Ａ＋Ｃｅ−Ｋ・εｎ　　　　　　　　−−（３８）
上記第（３８）式に、第（２）〜（４）式を代入しプラ
ス変換して整理すると、 （Ｓ＋Ｃ）　−ｏｒ　＝　　（Ｓ＋Ｃ＋Ｋ）’θ−に−
０１−・・なお、第（３９）式及び以降に述べる第（４
０）〜（４２）式におけるＳはラプラス演算子であり、
切換え面のＳとは異なる。

減速器を中心としたバネ系を考え、Ｂｋをダンピング類
、Ｋｃをバネ係数、Ｊｔを減速器から先の負荷イナーシ
ャとすると、減速器の入力θに対する機械（ロボット）
先端の位置θｔの伝達関数は次の第　（４０）式で表さ
れる。

（３９） ・・・・・・　（４０） なお、ロボットにおいては、ダンピング類Ｂｋ。

バネ係数Ｋｃが小さいため、ダンピングが悪く低い周波
数で振動する。

第（４０）式をθについて解き、第（３９）式に代入し
て整理すると、 （４１） 上記第（４１）式において、減速器を中心とした上記バ
ネ系の共振周波数で（Ｓ＋Ｃ）がＳとみなせるまでＣの
値を小さくとると、上記第（４１）式は次の第（４２）
式となる。

（４２） 第（４１）式は、モータへの移動指令θｒに対する負荷
であるロボット等の機械の移動θｔの伝達関数であり、
Ｋがダンピング類となっていることが分かる。このよう
に、Ｋがダンピング類となっていることから、上述した
切換え面への収束処理を行えば振動抑制効果が生じるこ
とが分かる。

ところで、上記切換え面Ｓへ収束させる処理のためには
、モータへの位置指令θｒと実際のモータの位置θとの
差である位置偏差ε（＝θｒ−θ）、位置指令θｒの加
速度１ｊｒ、ねじれ量εｎ　（＝θ−θｔ）、ねじれ速
度Ａｎを検出する必要があるが、位置側、差εはサーボ
制御処理中において検出され、位置指令の加速度θｒも
、位置指令θｒを２回微分して求めることができる。ま
た、ねじれ量εｎ、ねじれ速度Ａｎを検出する方法とし
ては、減速器の出力側に速度検出器を取り付け、該検出
器とサーボモータのロータ軸に取付けられた速度検出器
で検出される位置θｔ、θ、速度θｔ。

θよりねじれ量εｎ、ねじれ速度Ａｎを検出するように
してもよい。しかし、この場合は、新らたに速度検出器
を必要とするので、オブザーバを組むことによってねじ
れ量εｎ、ねじれ速度Ａｎを検出するようにする。

このオブザーバによってねじれ量εｎ、ねじれ速度々ｎ
を検出することはすでに公知であり、また、本願出願人
による先の平成元年９月６日に出願した特許出願に詳細
に記載しているので詳細を省略する。

第２図は本発明をロボット制御に適用した一実施例の制
御系のブロック図である。第２図中、１は、ロボット５
の各軸への移動指令を分配する数値制御装置等のホスト
コンピュータ（以下、ホス）ＣＰＵという）、２はホス
トＣＰＵから書込まれた各軸の移動指令をデジタルサー
ボ回路３のプロセッサへ受は渡す共有メモリである。ま
た、３はデジタルシグナルプロセッサ等で構成されるデ
ジタルサーボ回路であり、ロボット５の各軸のサーボモ
ータの位置、速度、電流のフィードバック制御を行うも
のである。４は、ロボット５における各サーボモータの
駆動電流のフィードバック値。

移動量θとしてのフィードバックパルスが書込まれる帰
還信号レジスタである。

第１図（ａ）、（ｂ）、（ｃ）は本実施例において、上
記デジタルサーボ回路のプロセッサが実行する動作処理
フローチャートであり、該プロセッサは所定周期（移動
指令の分配周期）毎、第１図（ａ）、　　（ｂ）、　　
（ｃ）の処理を実行する。

まず、共有メモリ２及び帰還信号レジスタ４より位置の
指令値θｒ、フィードバックパルス量θを読取（ステッ
プ１００）、従来と同様に位置偏差ε（＝θｒ−〇）お
よび速度偏差ｔを算出し、さらにオブザーバ処理によっ
てねじれ量εｎを算出しくステップ１０１）、第（５′
）式の演算を行って切換え面Ｓの値を計算する（ステッ
プ１０２）。

なお、上述した定数Ｃ，フィードバックゲインＫ及び予
想されるイナーシャの最大値Ｊｍａｘ、最小値ＪＯ１及
び、これらＣ，イナーシャＪｍａｘ。

ＪＯで決まる定数ωＣの値は、予めデジタルサーボ回路
３中のメモリ内に設定されている。または、プログラム
化されている。

デジタルサーボ回路３のプロセッサはステップ１０２で
算出した切換え面Ｓの値が「０」以上か否か判断しくス
テップ１０３）、Ｓ≧０ならば、次に位置偏差εが「０
」以上か否か判断しくステップ１０４）、ε≧０ならば
、第（２１）式の演算を行って切換え入力Ｔ１の位置偏
差εの関数部Ｋｌ（ε）の値をレジスタＲ１に格納する
（ステップ１０５）。また、ステップ１０４でε〈０で
あれば、第（２２）式の演算を行ってレジスタＲ１に格
納する（ステップ１０６）。

次に、θｒの値を計算しく前周期の位置の指令θｒと今
周期の位置の指令θｒとの差によってθｒが求められ、
前周期で求められたｌｌｒと今周期で求められた０ｒの
差によってθｒの値は求められる）、該値θｒが「０」
以上か否か判断しくステップ１０７）、「０」以上なら
ば第（２３）式の演算を行ってレジスタＲ２に格納する
（ステップ１０８）。また、θｒの値が負であれば第（
２４）式の演算を行ってレジスタＲ２に格納する（ステ
ップ１０９）。

次に、オブザーバ処理によって求められたねじれ量εｎ
、ねじれ速度Ａｎを読取り（ステップ１１０）、ねじれ
量εｎが「０」以上か否か判断しくステップ１１１）、
εｎ≧０ならばレジスタＲ３に「０」を格納し、εｎが
負であれば第（２６）式の演算を行ってレジスタＲ３に
に３（εｎ）の値を格納する（ステップ１１２．１１３
）。そして、ねじれ速度Ａｎが「０」以上か否か判断し
くステップ１１４）、Ａｎ≧０であれば第（２７）式の
演算を行ってに４（Ａｎ）の値をレジスタＲ４に格納し
、Ａｎが負であれば第（２８）式の演算を行ってに４（
Ａｎ）の値をレジスタＲ４に格納する（ステップ１１５
，１１６）。

そして、レジスタＲ１，Ｒ２，Ｒ３，Ｒ４に格納された
値を加算し、切換え入力Ｔ１の値を求め（ステップ１３
０）、次に第（６）式の演算を行ってトルク指令値Ｔを
算出しくステップ１３１）、このトルク指令値Ｔを電流
補償ループ処理へ受は渡す（ステップ１３２）。デジタ
ルサーボ回路３のプロセッサは、このトルク指令値及び
電流フィードバック値によって従来と同様に電流補償ル
ープ処理を行ってロボットの各軸のサーボモータへ駆動
電流を流し、サーボモータを駆動する。

一方、ステップ１０３で切換え面Ｓの値が負と判断され
ると、プロセッサは位置偏差εが「０」以上か否か判断
しくステップ１１７）、ε≧０と判断されると、第（３
０）式の演算を行ってレジスタＲ１に格納しくステップ
１１８）、さく０と判断されると第（３１）式の演算を
行ってレジスタＲ１に格納する（ステップ１１９）。

次に、θｒが「０」以上か否か判断しくステップ１２０
）、θｒ≧０ならば第（３２）式の演算を行ってレジス
タＲ２に格納し、θｒ＜Ｏならば第（３３）式の演算を
行ってレジスタＲ２に格納する（ステップ１２１，１２
２）。次に、オブザーバ処理によって検出されたねじれ
量εｎが「０」以上か否か判断し、εｎ≧０ならば第（
３４）式の演算を行ってレジスタＲ３に格納し、εｎ＜
Ｏならば「０」をレジスタＲ３に格納する。また、ねじ
れ速度Ａｎが「０」以上であれば第（３６）式の演算を
行ってに４（々ｎ）の値を、また、々ｎ＜０であれば第
（３７）式の演算を行ってに４（Ａｎ）の値をレジスタ
Ｒ４に格納する（ステップ１２３〜１２９）。

そして、前述同様、レジスタＲ１，Ｒ２，Ｒ３゜Ｒ４の
値を加算し、切換え入力Ｔ１の値を求め（ステップ１３
０）、第（６）式の演算を行ってトルク指令値Ｔを求め
る（ステップ１３１）。そして、このトルク指令値Ｔを
電流補償ループへ渡しくステップ１３２）、電流制御を
実行することとなる。

発明の効果 本発明は、スライディングモード制御の切換え面を系の
有するバネ要素のねじれ量、ねじれ速度によって変更し
、切換え面に収束させるようにスライディングモード制
御を行ったから、振動を減少させつつ、かつ、指令追従
性のよいサーボ系が実現できる。これにより、ロボット
等においては、軌跡精度の高いかつ定常速度時の振動及
び停止時の残留振動を小さくすることができる。

【図面の簡単な説明】

第１図（ａ）、（ｂ）、（ｃ）は本発明の一実施例にお
ける動作処理フローチャート、第２図は本発明を適用し
た一実施例のロボット制御系のブロック図、第３図はフ
ィードフォアード制御を行う位置、速度ループ制御系の
ブロック線図である。 θｒ・・・位置指令値、θ・・・現在位置、ε・・・位
置偏差、θ・・・実速度、Ｔ・・・トルク指令値。 第 １ 因（Ｃ）

Claims (3)

【特許請求の範囲】
1. （１）スライディングモード制御によってサーボモータ
   を制御し、該サーボモータで駆動される機械を制御する
   制御方式において、スライディングモード制御の切換え
   面を上記サーボモータの位置と該サーボモータで駆動さ
   れる機械可動部の位置の差であるねじれ量とねじれ速度
   により変更してスライディングモード制御を行い、振動
   を除去したことを特徴とする制振効果を持つスライディ
   ングモード制御による制御方式。
2. （２）上記ねじれ量、ねじれ速度はオブザーバによって
   推定する請求項１記載の制振効果を持つスライディング
   モード制御による制御方式。
3. （３）上記サーボモータで駆動される機械はロボットで
   ある請求項１または請求項２記載の制振効果を持つスラ
   イディングモード制御による制御方式。