JPH0392911A - スライディングモード制御によるロボット制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、ロボットの制御方法に関する。

従来の技術 ロボットの制御は、一般に、ロボットの各軸を駆動する
サーボモータに取付けられたパルスコーダ等からのフィ
ードバック信号を用いて、ＰＩ制御（比例，積分制御）
等の線型制御で制御を行ってロボットの姿勢，位置を制
御している。

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

発明が解決しようとする課題 サーボゲインを小さくしたり、位置決め終了後所定時間
経過して作業を行わせる等の制御を行えば、ロボットの
作業のサイクルタイムはその分長くなり好ましくない。

そこで、本発明の目的は、ロボットの低剛性によるバネ
要素の影響及びイナーシャ変動を考慮したロボット制御
方法を提供することにある。

課題を解決するための手段 本発明は、サーボモータの回転位置による指令位置とロ
ボット先端位置の差であるねじれ量とねじれ速度を、オ
ブザーバによって推定することによって検出するか、ま
たは減速器の出力側に速度検出器等を取付け、該速度検
出器とサーボモータに取付けられた速度検出器とから、
ねじれの主成分である減速器内におけるねじれ量及びね
じれ速度を検出すること等によって検出し、該ねじれ量
及びねじれ速度が零になるような補正トルク値をスライ
ディングモード制御で求め、該補正トルク値と速度ルー
プで求められたトルク指令値を加算してサーボモータへ
のトルク指令値（電流指令値）とすることによって上記
課題を解決した。

作用 上記ねじれ量及びねじれ速度を検出し、スライディング
モード制御によって、上記ねじれ量，ねじれ速度からな
るスライディングモード切り換え面が零に収束し、ねじ
れ量２ねじれ速度が零になるような補正トルク値を求め
る。この補正トルク値を速度ループで算出されたトルク
指令値に加算した値をサーボモータへの電流指令値とし
てのトルク指令値とすれば、ねじれを考慮し、イナーシ
ャ変動が生じても応答性の変化がなく、また、振動の小
さいロボット制御が得られる。

実施例 第１図は、本発明の一実施例におけるデジタルサーボ回
路のプロセッサが行うスライディングモード制御の動作
処理フローチャートである。第２図は同実施例のロボッ
ト制御系のブロック図である。また、第３図はロボット
のモデルであり、まず、このモデルを用いて本実施例の
スライディングモード制御の作用原理を説明する。

第３図中、符号１はサーボモータ、該サーボモータ１の
ロータ軸には速度検出器２が接続されると共に減速器３
が接続され、該減速器３を介して負荷であるロボットの
アーム４が駆動されるようになっている。

減速器３のバネ定数をＫｃ，粘性係数をＢＫとし、速度
検出器２で検出される減速器３の入力側の位置，速度，
加速度をθｍ，θｍ，θｍとし、減速器３の出力側、す
なわち負荷側の位置，速度．加速度を減速器３の入力側
に換算した値をθｒ，θｒ，θｒとする。また、モータ
１の軸イナーシャをＪｍ，アーム４等の負荷イナーシャ
（モータ軸換算）をＪｒ，モータ１の出力トルクをＴと
し、減速器３の人力側で運動方程式をたてると、Ｔ　＝
　Ｊｍ＃ｍ＋ＢＫ（θｍ−θｒ）　＋Ｋｃ　（θｍ−θ
ｒ）　　−＝また、負荷側（減速器３の出力側）で運動
方程式をたてると、次の第（２）式となる。

０　＝　Ｊ　ｒ／ｊｒ＋ＢＫ（θｒ一θｍ）　＋Ｋｃ　
（ｏｒ−θｍ）　　−・・（θｍ一θｒ）はサーボモー
タのロータ回転位置に対する負荷のねじれ量を意味し、
（θｍ一θｒ）はねじれ速度を意味する。そこで、ねじ
れ量をε，ねじれ速度をｉとする。すなわち、θｍ一θ
ｒ＝ε θｍ−θｒ＝ｉ とし、上記第（１）式をＪｍで除したものから、第（２
）式をＪｒで除したものを減算すると次の第（３）式と
なる。

Ｔ／Ｊｍ＝ど＋ＨＢＫ　／Ｊｍ）　＋　（ＢＫ　／Ｊ　
ｒ）ｌ　・Ａ＋ｔ（Ｋｃ／Ｊｍ）　＋　（Ｋｃ／Ｊ　ｒ
）Ｉ　・ｅここで、スライディングモードの切り換え面
Ｓとして次の第（４）式で示すねじれ量ε，ねじれ速度
この関数からなるものを想定する。なお、パラメータＣ
は実験的に求めて設定する値である。

Ｓ＝Ａ＋Ｃ・ε　　　　　　・・・　（４）リアプノフ
関数候補として次の第　（５）式を考える。

Ｖ＝Ｓ’　／２　　　　　　　　　　・・・　（５）ま
た、上記第（５）式を微分すると、 ＝ｓ−ｇ　　　　　　　　　・・・　（６）Ｓは第（４
）式より Ｓ−ど＋Ｃ●ｉ　　　　　　・・・　（７）上記リアプ
ノフ関数Ｖは常に正であり、最小値が「０」である。よ
って、リアプノフ関数の微分を〈０とすれば、リアプノ
フ関数Ｖは最小値「０」に収束、第（５）式より切り換
え面Ｓは「０」に収束し、応答性がＳ＝Ｏの一定な応答
関数となる。

そこで、第（３）式をどについて解き、第（７）式に代
入すると次の第　（８）式となる。

Ｓ＝　Ｔ／Ｊｍ＋　ｆｃ　−　（ＢＫ／Ｊｍ）　−　（
ＢＫ　／Ｊ　ｒ）ｌ　・Ａ−ｔ（Ｋｃ／Ｊｍ）　＋　（
Ｋｃ／Ｊ　ｒ）ｌ　・ε（８） ωｃ　＝　ＳＱＲ［　ｆ（１／Ｊｍ）＋　（１／Ｊ　ｒ
）ｌＫｃｌとおき、第（８）式のεに第（４）式を変形
して得られる　ε＝（Ｓ−Ａ）／Ｃを代入すると、第（
９）式となる。

Ｑ　＝　Ｔ／Ｊｍ＋　ｆｃ＋（ωｃ２／Ｃ）−　（ωｃ
２ＢＫ　／Ｋｃ）ｌ　・Ａ−ωｃ２Ｓ／Ｃ・・・（９） ゆえに、第（６）式より ＝　Ｓ−Ｑ ＝−ωｃ２Ｓ２／Ｃ＋　［Ｔ／Ｊｍ＋　（Ｃ＋　（ωｃ
２／Ｃ）−　（ωｅ２ＢＫ　／Ｋｃ）ｌ　−　＃］　Ｓ
・・・（１０） 第（１０）式において、一ωｃ’ｓ２／Ｃは常に負であ
る。よって、次の第（ｌ１）式が成立すれば、＝０とな
り、切り換え面Ｓは「０」に収束する。

［Ｔ／Ｊｍ＋　ｆｃ＋（ωｃ２／Ｃ） −　（ωｃ２ＢＫ　／Ｋｃ）Ｉ　・ＡＩ　Ｓ　＜　０　
　　・＝　（ＩＩ）よって、上記第（ｌ１）式が成立す
るようにトルクＴ即ち補正トルクＴを入力すれば、Ｓが
「０」に収束し、第（４）式より、ねじれ速度ｉ，ねじ
れ量εも「０」に収束する。

第（１ｌ）式を変形すると第（ｌ２）式となる。

［Ｔ＋　（Ｊｍ／Ｃ］　［Ｃ２＋ωｃ２ｔ　１−　（Ｂ
Ｋ　Ｃ／Ｋｃ）１・ｊ］ＩＳ＜Ｏ　　・・・（ｌ２） Ｓ≧０のとき、次の第（ｌ３）式が常に成立するように
補正トルクＴを決めれば良い。

Ｔ　＜　−　（Ｊｍ／’Ｃ）　［Ｃ２＋（ＩＪＣ２（　
１　−ＢＫ　Ｃ／Ｋｃ）ｌ］　・Ａ粘性係数ＢＫは非常
に小さく、ｌ　　＞　　ＢＫ　Ｃ／Ｋｃ）であるので、
ωＣの考えられる最大値をωｃｍａｘ２とすると、 Ｔ＝一（Ｊｍ／Ｃ）−（ωＣｍａｘ２＋０２）・ＩＡと
、補正トルクＴを設定すれば、第　（１１）式（第（１
２）　，　（１３）式）が常に成立する。

また、Ｓ〈０の時は同様に、次の第（１５）式のように
補正トルクを設定すれば、第（１１）式は常に威立する
。

Ｔ　＝　　（Ｊｍ／Ｃ）　　・　（ωｃｍａｘ２＋Ｃ２
）　　・ｌ　ｊ　ｌこうして補正トルクＴを求め、該補
正トルクＴを速度ループで算出される通常のトルク指令
値に加算してサーボモータへのトルク指令値（電流指令
値）とすれば、切り換え面Ｓは「０」に収束し、ねじれ
速度ｉ，ねじれ量εも「０」に収束することとなる。す
なわち、ｔ＝０，ε＝０を第（１１）式に代入すれば、 Ｔ＝Ｊｍθｍ となる。

よって、このスライディングモード制御を速度ループの
マイナーループで行うことにより、モータトルクはロー
タイナーシャとしての固定のＪｍだけに依存するように
なり、振動を妨げると同時にイナーシャ変動をも、この
ループによって補ったこととなる。

そして、このマイナーループの外側に一般の線型制御の
ループを組むことにより、イナーシャ変動及び振動の影
響を低減したサーボループができあがる。

なお、上記スライディングモード制御を行う上で、ねじ
れ量ε，ねじれ速度ｉを検出する必要があるが、このね
じれ量ε，ねじれ速度ｉの検出として、減速器３の出力
側に速度検出器を取付け、該検出器とサーボモータ１の
ロータ軸に取付けられた速度検出器で検出される位置θ
ｍ，θｒ，速度θｍ，θｒよりねじれ量ε，ねじれ速度
ｉを検出するようにしても良い。しかし、この場合は、
新らたに速度検出器を必要とするので、本実施例ではオ
プザーバによってねじれ量ε，ねじれ速度ｉを検出する
ようにする。

第　（３）式より ’ｉ＝　−ｆ（Ｂκ／　Ｊ　ｍ）　＋　（ＢＫ　／　Ｊ
　ｒ）ｌ　・Ａ−ｆ（Ｋｃ／Ｊｍ）＋　（Ｋｃ／Ｊｒ）
ｌ　●ε＋Ｔ／Ｊｍ・・・（ｌ６） 第　（１）式より θｍ　＝　−　（ＢＥ　／　Ｊ　ｍ）　（θｍ−θｒ）
−　［Ｋｃ／Ｊｍ）（θｍ−θｒ）＋Ｔ／Ｊｍゆえに、 θｍ　＝　−　（ＢＫ　／　Ｊ　ｍ）　　・ｉ−（Ｋｃ
／Ｊｍ） ・ε＋Ｔ／Ｊｍ （ｌ７） Ｅ　＝Ｘ１　，　　８　＝Ｘ２　，　　θｍ＝ｘ，とす
ると、第 （１６） 式は 交＋　　＝　　ｆ（ＢＫ／Ｊｍ）＋　（Ｋｃ／Ｊｒ）１
１）（＋ｆ（Ｋｃ／Ｊｍ） ＋ （Ｋｃ／Ｊ　ｒ）］Ｘ２　＋Ｔ／Ｊｍ （１８） ｘ２＝ｘ， （１９） 第 （ｌ７） 式より ｉｃｓ　　＝　−　（ＢＫ／Ｊｍ）　　・ｘ，−　（Ｋ
ｃ／Ｊｍ）　　・ｘ２＋Ｔ／Ｊｍ（２０） 第（１８） 〜（２０）式より また、 とし、 とし、第（２１）式の行列（ＸＩ　＋　　Ｘ２　＋　　
ｘ３　）にかかる行列をＡＳＴに係る行列をＢ（行列Ａ
，　　Ｂの構戊要素はすべて既知な値）、第（２２）式
の行列（Ｘ　＋　＊　　Ｘ　２　１　　Ｘ　３　）にか
かる行列をＤとすると、第　（２１）　．　　（２２１
式よりｄｘ　＝　Ａｘ−ＫＤｘ＋ＫＤｘ＋ＢＴｄｔ ＝　（Ａ−ＫＤ）ｘ＋Ｋｙ＋ＢＴ　　　　　　　　　・
−（２３） そこで、Ｘの推定値として として、第（Ｉ３）式より ｄ　２＝　（Ａ−ＫＤ）ｘ十Ｋｙ＋ＢＴ　　　　　　　
　　’“゜ｄｔ （２４） （なお、Ｋは　（Ａ−ＫＤ）が安定するように設定する
定数） というモデルを組むと、Ｘが状態変数Ｘの推定値となる
。即ち、次の第（２５）式が成立し、ねじれ量ｉ，ねじ
れ速度εを推定することができる。

第（２４）式において、ｙは第（２２）式よリモータト
ルクＴであるから、第（２４）式のモデルのブロック線
図は第４図のようになる。

第２図は、ロボット制御に適用したー実施例の制御系の
ブロック図である。第２図中、１１はロボッ｝１５の各
軸への移動指令を分配する数値制御装置等のホストコン
ピュータ（以下、ホストＣＰＵという）、１２はホスト
ＣＰＵから書き込まれた各軸の移動指令をデジタルサー
ボ回路１３のプロセッサへ受け渡す共有メモリである。

また、１３はデジタルシグナルプロセッサ等で構成され
るデジタルサーボ回路であり、ロボット１５の各軸サー
ボモータの位置，速度，電流のフィードバック制御を行
うもので、本発明のスライディングモード制御処理，オ
ブザーバ処理をも行うものである。１４は、ロボット１
５における各サーボモータの駆動電流のフィードバック
値，移動量θｍとしてのフィードバックパルスが書き込
まれる帰還信号レジスタである。

第１図は、本実施例において、上記デジタルサーボ回路
のプロセッサが実行する本発明のスライディングモード
制御処理に関するフローチャートであり、該プロセッサ
は所定周期毎、第１図に示す処理を実行する。

まず、デジタルサーボ回路１３のメモリ内にスライディ
ングモード制御処理及びオブザーバ処理に必要な定数等
、即ち、パラメータＣ，モータの軸イナーシャＪｍ，　
ωｃｍａｘの値、行列Ａ，　　Ｂ，Ｄをあらかじめ設定
する。

そして、ロボットの動作を開始させると、デジタルサー
ボ回路１３のプロセッサは所定周期毎第１図の処理を開
始し、位置指令値を共有メモリ１２から読取り、帰還信
号レジスタ１４より位置フィードバック値を読取り、従
来と同様に位置ループ処理，速度ループ処理を実行する
（ステップ１００〜ｌ０２）。次に、位置ループで算出
された速度指令θｍ，速度ループで算出された前回のト
ルク指令値Ｔに基いて第（２４）式及び第４図に示すオ
プザーバの処理を行ってねじれ量ε，ねじれ速度ｉを求
め（ステップ１０３）、求められたねじれ量ε，ねじれ
速度こより第（４）式の演算を行って切り換え面Ｓの計
算を行う（ステップ１０４）。その計算結果が零または
正であれば、第（１４）式の演算を行って補正トルクＴ
を求める。

また、Ｓ〈０であれば、第（１５）式の演算を行って補
正トルクＴを求める（ステップ１０５〜１０７）。次に
、速度ループ処理で算出されたトルク指令値にステップ
１０６，１０７で求めた補正トルクＴを加算してスライ
ディングモード制御処理により補正されたトルク指令値
（電流指令値）として電流ループへ引き渡し（ステップ
１０８）、当該周期の処理を終了する。

以上の処理によって、サーボモータの出力トルクがねじ
れ量ε，ねじれ速度ｉが零になるように制御されるから
、ロボットの姿勢が変わり、イナーシャ変動が生じても
応答性は変化せず、また、ロボット先端の振動も小さく
なる。

発明の効果 本発明は、スライディングモード制御によってサーボモ
ータの回転位置による指令位置とロボット先端位置の差
であるねじれ量及びねじれ速度が零になるようにサーボ
モータの出力トルクが制御されるから、ロボットの姿勢
が変化し、イナーシャ変動が生じても制御系の応答性は
変化せず、ロボット先端の振動も小さくなり、ロボット
先端の位置決め完了が短時間で終了し、ロボットのサイ
クルタイムを短かくすることができる。

【図面の簡単な説明】

第１図は本発明の一実施例におけるデジタルサーボ回路
のプロセッサが実行する動作処理フローチャート、第２
図は本発明の一実施例を実行するロボットの制御ブロッ
ク図、第３図は本発明の一実施例のロボットモデルの概
要図、第４図はオブザーバ処理のブロック線図である。 １・・・サーボモータ、２・・・速度検出器、３・・・
減速器、４・・・ロボットアーム。 第 ２ 口 第 ４ 因

Claims (3)

【特許請求の範囲】
1. （１）サーボモータの回転位置による指令位置とロボッ
   ト先端位置の差であるねじれ量とねじれ速度を検出し、
   該ねじれ量及びねじれ速度が零になるような補正トルク
   値をスライディングモード制御で求め、該補正トルク値
   と速度ループで求められたトルク指令値を加算してサー
   ボモータへのトルク指令値としたことを特徴とするスラ
   イディングモード制御によるロボット制御方法。
2. （２）上記ねじれ量、ねじれ速度はオブザーバーにより
   推定して求める請求項１記載のスライディングモード制
   御によるロボット制御方法。
3. （３）サーボモータのモータ軸に接続された減速器の出
   力側に速度検出器を設け、該速度検出器と上記サーボモ
   ータに取付けられた速度検出器から上記ねじれ量及びね
   じれ速度を検出するようにした請求項１記載のスライデ
   ィングモード制御によるロボット制御方法。