WO1995002214A1

WO1995002214A1 - Procede de commande d'un servomoteur permettant d'affecter de la souplesse aux coordonnees de travail

Info

Publication number: WO1995002214A1
Application number: PCT/JP1994/001012
Authority: WO
Inventors: Tetsuaki Kato; Soichi Arita; Masaru Nakamura
Original assignee: Tetsuaki Kato; Soichi Arita; Masaru Nakamura
Priority date: 1993-07-05
Filing date: 1994-06-23
Publication date: 1995-01-19
Also published as: JP3283650B2; JPH0720941A; US5587638A

Description

明細書

作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法

技 HB" 万 ST

本発明は、ロボットアーム等を駆動するサーボモータの制御に閧するものであり、位置、速度制御ループを有し、位置を制御するサーボ制御方法に関し、特に作業座標上において柔らかさを指定することのできる柔軟なサーボ制御方法に関する。

背景技術

ロボヅトのアーム等の制御において、該アームを駆動するサーボモ一夕の制御は、位置、速度ループを有し、該サーボモー夕の位置を制御することによってアーム等の被駆動体の位置を制御している。このような制御系において、サーボモー夕が制御されると、位置指合を与え、その位置に向かってロボットアーム等の被駆動体が移動している途中に、障害物があっても、その障害物を避けることが出来ず、そのまま目標位置に向かって移動し銃けるという現象が生じる。例えば、ロボットがハンドによって工作機械にワークを取り付ける作業を行なっている場合、ワークの精度が悪くて、指合した位置とヮークの位置に多少のずれが生じる場合には、ワークを工作機械に取り付けることができない。しかも、このような場合に、手動でロボットアームを移動させてワークをェ作機械の取付け位置に移動させることも困難となる。このような問題点を解決して、人力によって被駆動体を移動させ、障害物から回避させることができる柔軟なサーボ制御方法が提案されている。図 5 は、従来の柔軟なサーボ制御方法に用いるサーボ系のブロック図である。図 5 において、柔軟なサーボ制御を行なうには、ポジシヨンゲイン K p 、速度制御ループの比例ゲイン Κ 2 を設定した柔らかさの度合いに応じて低下させ、また、速度制御ループの積分器の出力を設定クランプ値にリミットする。これにより、位置偏差が増大してもトルク指会は格別大きな値とならないので、このサーボモー夕で駆動ざれる被駆動体を人力で移動させることが可能となる ο

しかしながら、前記の従来の柔軟なサーボ制御方法において、その柔らかさの程度の指定は、ロボットアーム等の被駆動体を駆動するロボットのモ一夕の有する各軸 ( 3 軸およびその軸の回り）に組まれたサ一ボ系に対するものであって、実際にロボットアーム等の被駆動体を駆動する作業座標上における指定となっていない。したがって、ロボットアーム等の被駆動体を従来の柔軟なサーボ制御方法によりその柔らかさの程度を指定しょうとすると、通常作業を行なう例えば X , Υ , Ζ座標等の作業座標上での被駆動体の移動状態を推定しながら、この被駆動体を駆動するモー夕の各軸に組まれたサーボ系に対して、ポジションゲインと速度制御ループの比例ゲインを、設定した柔らかさの度合いに応じて低下させる必要がある。

しかしながら、各軸の動作は他の軸の動作と関連しているため、モー夕の各軸に組まれたサーボ系に対する柔らかさの調整を行なうことによって作業座標上での柔らかさを設定することは、実際上困難である。

発明の開示 - 本発明の目的は、作業座標上での柔らかさの設定により、各軸に組まれたロボットのサ一ボ系のゲインの変更を可能とする柔軟なサーボ制御方法を提供することにある。

本発明の方法は、位置制御ループおよび速度制御ループを備える制御系で制御されるサーボモ一夕の制御方法において、サーボモー夕が位置する作業座標上において指定した柔らかさを、該サーボモー夕の各軸におけるサ —ボゲインに変換し、該変換したサーボゲインにより指定された柔らかさの程度に応じたサーボモータの駆動を行い、これにより被駆動体を手動で移動可能とすることができ、作業座標上において柔らかさを指定可能とするものである。

また、サーボモー夕が位置する作業座標上における柔らかさの指定は、作業座標軸方向およびその回りのパネ定数を指定することにより行なうことができる。

また、サーボモー夕の各軸におけるサーボゲインは、該サ一ボモ一夕の各軸の位置制御ループゲインおよび速度制御ループゲインにより指定することができるものである。

本発明の柔らかさを指定する作業座標は、ロボットあるいは被駆動体が存在し移動する座標系であり、例えば

X , y , z座標とすることができ、その柔らかさは X , y , z座標方向のバネ定数およびその座標軸の回りのモ一メントのパネ定数とすることができる。また、本発明のサーボモー夕の各軸のサーボゲインは、各軸における柔らかさの程度を表すものである。

図面の箇単な説明

図 1 は、本発明の作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法を実施するサーボ制御系のブロック図、

図 2 は、本発明の一実施例を実施するロボット制御系のブロック図、

図 3 は、作業座標系からモー夕の各軸に組み込まれたサーボ制御系への変換の概略を説明する図、

図 4 は、作業座標系およびサ一ボ制御系での位置フィ一ドバックによる位置ループの状態を説明する図、

図 5 は、従来の柔軟なサーボ制御方法に用いるサーボ系のブロック図である。

発明を実施するための最良の形態図 1 は本発明の作業座標上で柔らかさを指定可能な柔軟なサ一ボ制御方法を実施するサ一ボ制御系のブロック図である。図 5 に示す従来のサーボ制御系と相違する点は、各軸に組まれたサーボ制御系中の位置制御ループのポジションゲイン K p および速度制御ループの比例ゲイン Κ 2 の変更を、作業座標系での柔軟制御の柔らかさの程度の指定に応じて行なう点である。

本発明の柔軟なサ一ボ制御方法においては、作業座標系において柔軟制御の柔らかさの程度を指定すると、この指定された柔らかさの程度はモータの各軸に組み込まれたサ一ボ制御系に対する柔らかさの程度に変換されて、各軸の位置制御ループのポジションゲイン K p および速度制御ループの比例ゲイン K 2 が変更される。

各軸に組まれたサーボ制御系の位置制御ループのポジションゲイン K p および速度制御ループの比例ゲイン K 2 が変更されると、その後は従来の柔軟なサーボ制御方法と同様にして、ポジションゲインが柔らかさの度合いに応じて通常のポジションゲインより小さな値となるため、位置偏差に該ゲイン K p が乗じられて得られる速度指合も通常の制御時よりも小さくなる。そのため、該速度指合とモータ速度との差の速度偏差も通常時よりは小さくなる。そして、速度制御ループの比例ゲイン K 2 も通常より小さな値に変更されているので、比例項 3 からの出力値も通常より小さくなる。さらに、速度制御ル一プの積分器 2 もその出力をクランプ値にリミットされた出力れるため、トルク指合 T c は大きな値となることはなく、人力てによってでも該サーボモー夕で駆動される被駆動体を移動させることができる。

図 2 は本発明の一実施例を実施するロボット制御系のブロック図である。

図 2 中において、 1 0 はロボットを制御するホストコンピュー夕、 1 1 は該ホストコンピュータから出力される移動指合や制御信号を後述するディジタルサーボ回路のプロセッサに引き渡し、逆にディジタルサーボ回路のプロセッサからの各種信号をホストコンピュータに引き渡すための共有メモリである。また、 1 2 は、上述したサ一ボ制御を実行するディジタルサ一ボ（ソフトウェアサーボ）回路で、プロセッサ（ C P U) 、及び R OM、 R AM等のメモリ等で構成されている。 1 3 はロボット 1 4 における各サーボモ一夕の位置のフィードバック値、速度のフィードバック値、電流のフィードバック値等が書き込まれる帰還レジス夕である。

ここで、本発明の作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法において、作業座標系において柔軟制御の柔らかさの程度の指定を、モー夕の各軸に組み込まれたサ一ボ制御系に対する柔らかさの程度に変換して、各軸の位置制御ループの位置ループゲイン（ポジシヨンゲイン） K p および速度ループゲイン（比例ゲイン Κ 2 Χ トルク定数 K t ) K vを求める手順について説明する。

図 3 は、作業座標系からモー夕の各軸に組み込まれたサーボ制御系への変換の概略を説明する図である。図 3 において、 2 1 は通常において作業を行なう例えば X , y , z座標等の作業座標系であり、 2 3 はモー夕の各軸に組み込まれる例えばモ一夕の 3軸とその回りの計 6軸からなるサーボ制御系であり、 2 2 は作業座標系 2 1 での柔らかさの程度を設定するゲインからサーボ制御系 2 3 での柔らかさの程度を設定するゲインへの変換を概念的に示した変換部分である。

サ一ボ制御系 2 3 は、従来の柔軟なサーボ制御において示したように、モータの各軸（ 6軸）に組み込まれたサーボ制御系でのゲインをゲイン K 〜ゲイン K _Jeで表すと、このゲイン K 〜ゲイン K _Jeを柔らかさの程度に応じて設定し、このゲインによりモー夕を制御する。

一方、作業座標系 2 1 は、通常の作業を行なう座標系であり、この座標系を X , y , z座標とすると、柔らかさの程度はゲイン Κ , , Κ，， Κ , , Κ u _t KMV, Κ UZ を指定することによって設定される。なお、このゲイン Κ _χ , Κ，， Κ , はそれぞれ χ軸， y軸， z軸方向のバネ定数であり、ゲイン K_MX, K yv, K_MZはそれぞれ X軸

， y軸， z軸の回りのモーメントのパネ定数である。

そして、通常、作業座標系 2 1上においては、柔らかさの程度はこのゲイン Κ , Κ，， Κ , , K MX, Κ MV , Κ _ΜΖにより指定可能であり、サーボ制御系でのゲイン Κ 〜ゲイン K _Jeを直接に作業座標系 2 1 上で設定することは因難である。そこで、本発明では変換部 2 2 において、作業座標系 2 1 上で設定した柔らかさの程度を表すゲイン，， Κ，， Κ , , K UX , K MV , Κ_ΜΖをサーボ制御系 2 3 でのゲイン〜ゲイン K _Jeに変換し、このゲィン〜ゲイン K J«によつてモ一夕を制御する。

次に、各系におけるゲイン Kを図 4を用いて説明する。図 4 は、作業座標系およびサーボ制御系での位置フィ一ドバックによる位置ループの状態を説明する図であり、図 4の（ a ) は作業座標系を示し、図 4の（ b ) はサーボ制御系を示している。

図 4の（ a ) の作業座標系において、 X軸， y軸， z 軸方向およびその回りについて、位置ループを求めると以下の式となる。

K X ( X co a - X f_b ) = F x ( 1 )

K r ( y c。 d - y ") = F， ( 2 )

K z ( z _e。 4 — z ") = F i ( 3 )

K MX ( 0 "。_d - 0 " _b ) = M x ( 4 )

K MY ( … 1一 _b ) = M， ( 5 ) K uz ( 0 _IC »d - 6> «i b ) = U . ( 6 ) なお、上式において， K， Κ , は作業座標上の各軸方向のパネ定数、 Κ«, Κ «ν K uzは作業座標上の各軸の回りのモーメントのバネ定数であり、 x _c。 <i , y e。 d , Z eo d , x_C od , Θ od , 6» , _e。 <1は作業座標上の指合位置であり、 x _ib, y it , ζ _fb , Θ χί _b , Θ ,χ _b , Θ _{tt b} は作業座標上のフィードバック量であり、 F x , F , , F は作業座標上の各軸方向の力であり、 M x , Μ，， Μ, は作業座標上の各軸方向の回りのモーメントである。

上記式（ 1 ) 〜（ 6 ) において、例えば式（ 1 ) においては、例えば俘止状態（ X _c。《 = 0 ) から X軸方向に x _ibだけ移動させるには F Xの力を要することを意味しており、パネ定数を変えることによってその F xの大きさを変更することができる。

また、図 4 の（ b ) のサーボ制御系において、モー夕の各軸（ 6軸）について、位置ループを求めると以下の式となる。なお、モー夕の各軸を J 1 〜 J 6 によって表すものとする。

K J i ( θ 31 co d 一 Θ J l ί b ) = T J 1 • * · ( 7 )

K J ( θ J2 eo 4 一 θ ja t ) = T ja ( 8 )

K JS ( θ JS eo d 一 θ JS i ) = T JS ( 9 )

K J4 ( θ J4 co d 一 θ tb ) =丁 J4 … ( 1 0 )

K Jfi ( θ J 6 eo d 一 θ J5 ib ) = T J6 … ( 1 1 )

K J6 ( θ J 6 co d 一 θ ja th ) = T J6 … ( 1 2 ) なお、上式において K 〜 K„はサーボ制御系の各軸のモ一夕の柔らかさを表すパネ定数であってゲインを表しており、 0 e。 d はサーボ制御系の各軸への指合位置であり、 6> _{Jl ib}〜 6> _{Je ib}はサーボ制御系のフイードバック量であり、丁 _Jt〜丁 _Jaはサーボ制御系へのモーメンイトである。

上記式（ 7 ) 〜（ 1 2 ) において、例えば式（ 7 ) においては、例えば停止状態（ 6» ^ _e。 _d = 0 ) から各軸方向に 0 _ibだけ回転させるには丁のトルクを要することを意味しており、ゲインを変えることによってそのトルク丁の大きさを変更することができる。上式（ 1 ) 〜（ 6 ) の作業座標上の各軸方向のバネ定数 , Κ，， Κ , および作業座標上の各軸の回りのモ一メントのパネ定数 Κ Κ «ν , Κ _ΜΖを直接設定することが困難であるため、本発明の柔軟なサーボ制御方法においては、上式（ 7 ) 〜（ 1 2 ) 中のサーボ制御系の各軸のモータの柔らかさを表すゲイン Κ 〜Κ _ίβを変化させることによって、上式（ 1 ) 〜（ 6 ) 中の作業座標上の各軸方向のパネ定数 Κ , , Κ , , Κ および作業座標上の各軸の回りのモーメントのパネ定数 K » , K MV , K _MZが指定した値となるように制御する。

そこで次に、上式（ 1 ) 〜（ 6 ) をサーボ制御系の各軸の座標に書き直して、ゲイン K _S , K r , Κ « , Κ MX

, Κ ΜΥ , Κ _ΜΖをゲイン〜ゲイン K J« に変換する関係式を求める。

各軸 0から、 X , y , z軸およびその軸の回りへは、一般に順変換 ί によって変換することができる。この順変換 f によると、式（ 1 ) 〜（ 6 ) の χ。" , y co a , Z e。 d , ,c od , 6» "。ま式（ 7 ) 〜（ 1 2 ) の 6» _c。《ι 〜 _e。 _d を用いて次式

' - ( 1 3 ) によつて表される。

また各軸へのモーメント Τ 〜 Τ と、作業座標上の各軸方向の力 F x , F r ， F z およびと各軸方向の回りのモメント M X ， _y , M , との間には、力に関してヤコビアン J を用いて次式（ 1 4 ) で表されることが一般に知られている。

( 1 4 なお J ^T はヤコビアンの転置を表している。

次に上式（ 1 3 ) , ( 1 4 ) を用いて、式（ 1 ) へ ( 6 ) を書き直すと、

diag 〔 K X. Κ ,. K K MX , K uv . K UZ ♦ 〔 f ( Θ _3i

0 je <s。 il ) — f ( Θ 1 f b 0 J6 ib ) 〕

( J ^τ ) — ¹ {diag 〔 K ,ι K _Je〕

となる。なお、 diag 〔 λ ! ， λ 2 〕は λ t および λ を対角要素に持つ対角行列である。

上式（ 1 5 ) 書き直すと、

J ⁷ ♦ diag 〔 · K _y. Κ ,. Ι"_χ , Κ _Μν . K _MZ〕 ♦ C f (

Θ Ji co ά …， Θ co ά ) 一 f Θ Ji ib ··· , 0 Jfi fb )

- ( 1 6 )

となる。なお、 E rror 6» j は 6> " と S _{Jf b} の誤差（ J_e。_d — 0 _{Jf b} ) を表している。

上式（ 1 6 ) において、 K ,. Κ ,. Κ ζ. Κ _ΜΧ , Κ MV. Κ _M2は作業座標上で柔らかさを指定する際に設定する設定値であり、 ί ( J_{l e}。 <！〜 JO co <, ) は作業座標上で入力する指会値であり、 ί ( J6 f b ) は作業座標上における検出値であり、 E ΤΤΌΤ θ 〜 Ε ΤΤ ΌΤ Θ _Jeはェラーレジス夕に記憶される誤差値であるから、各軸上でのゲイン〜ゲイン K _Je はこれらの既知の値の関数として求めることが.できる。

そこで、式（ 1 6 ) を各軸上でのゲイン〜ゲイン K _Je について解き、作業座標上で指定した柔らかさの度合いをオンライン等で入力し、 C P Uにおいてこのゲイン K 〜ゲイン K _JEの値を各周期毎に変更し、その値に基づいて制御を行なうことにより、柔軟なサ一ボ制御を行なうことができる。

なお、このゲイン K , の設定を、例えばサ一ボ制御系においてあらかじめ基準となる柔らかさを定めておき、その柔らかさに対する度合いにより行なうことができる。また、このゲイン K j は位置ループゲイン（ポジションゲイン） K p と速度ループゲイン K v (比例ゲイン Κ 2 Χ トルク定数 K t ) の積で表されるので、このゲイン K J の設定を位置ループゲイン K p および速度ループゲィン K Vをそれぞれ設定することにより行なうことがでまる _β

例えば、柔らかさが 0 % (充分に固い状態）の場合のゲインを K J ( 0 %) とし、そのときの位置ループゲイン K pおよび速度ループゲイン K vをそれぞれ K p ( 0 %) 、および K v ( 0 %) とすると、ゲインは、位置ループゲイン K p と速度ループゲイン K vの二つのループゲインによりそれぞれ K p = K p ( 0 %) x ( K , /Κ ( 0 % ) ) ^{1/ 2}

- ( 1 7 )

Κ ν = Κ ν ( 0 %) X ( Κ Κ ( 0 %) ) ^{1/ 2}

- ( 1 8 ) となる。

そこで、このロボット制御系におけるディジタルサーボ回路 1 2のプロセッサは、上記のようにして、本発明の作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法において、作業座標系における柔らかさの程度を指定を、モー夕の各軸に組み込まれたサーボ制御系に対する柔らかさの程度に変換して求めれる各軸の位置制御ループの位置ル一プゲイン（ポジションゲイン） Κ ρ、および速度ループゲイン（比例ゲイン Κ 2 X トルク定数 Κ t ) K Vを用いて柔軟なサーボ制御の処理を行なう。なお、柔軟制御措合、および柔らかさの程度は手動によってオペレー夕が入力してもよく、また、あらかじめ柔軟な制御を行なうプログラム領域をプログラムで指定してもよい。

ロボツトの動作を開始すると、ディジタルサーボ回路 1 2のプロセッサは、柔軟制御指合にしたがって、通常の制御から柔軟な制御に切り換え、位置 · 速度ループ処理周期毎に柔軟な制御の処理を行なう。そして、共通メモリ等に記憶しておいた作業座標系における柔らかさの程度を表す K _X. K _Y. K ,, K , K MY . K _MZを読み出し、このデータに基づいて上記した本発明の柔軟な制御方法 P T/JP9 / 1 12

- 1 5 - によりゲイン〜ゲイン K _Jeを例えば位置ループゲイン：^〜、および速度ループゲイン Κ ν 〜Κ V _Je の形態で演算して更新し、再び共通メモリ等に記憶する。ディジタルサーボ回路は、この共通メモリから更新したゲイン〜ゲイン K _Je (位置ループゲイン K p 〜Κ ρ «、および速度ループゲイン Κ ν 〜Κ ν _Ja ) を読み出し、そのデータに基づいてモー夕の制御を行なラ o

その結果、サーボモー夕が出力するトルクは、小さな値となり、この柔軟な制御状憨においては、オペレータがサーボモー夕で制御されるロボットアームを手動によつて任意の位置に移動させることができる。これにより、ワークを工作機械等に取り付ける場合に、ワークの精度が悪く、取付け位置とロボットアームにより搬送されるワークの位置が一致しない場合、手動でロボットァ一ムを移動させて取付け位置にワークを位置決めすることができる。

本発明の実施例によれば、ゲインの設定を位置ル —プゲイン K pおよび速度ループゲイン K vをそれぞれ設定することによりサーボ制御系の安定性が得られるという効果がある。

以上説明したように、本発明によれば、作業座標上での柔らかさの設定により、各軸に組まれたロボットのサーボ系のゲインの変更を可能とする柔軟なサーボ制御方法を提供することができる。

Claims

請求の範囲

1 . 位置制御ループおよび速度制御ループを備える制御系で制御されるサーボモー夕の制御方法において、サ一ボモ一夕が位置する作業座標上において指定した柔らかさを、該サ一ボモ一夕の各軸におけるサーボゲインに変換し、該変換したサーボゲインによりサーボモ一夕を駆動し、該サーボモー夕で駆動される被駆動体を手動で移動可能とした作業座標上で柔らかさを指定可能な柔軟なサ一ボ制御方法。

2 . サーボモータが位置する作業座標上において指定する柔らかさは、作業座標軸方向およびその回りのバネ定数である請求の範囲第 1 項に記載の作業座標上で柔らかさを指定可能な柔軟なサーボ制御方法。

3 . サーボモ一夕の各軸におけるサーボゲインは、該サーボモー夕の各軸の位置制御ループゲインおよび速度制御ループゲインである請求の範囲第】項、又は第〗項記載の作業座標上で柔らかさを指定可能な柔軟なサ一ボ制御方法。