JPS6236243B2

JPS6236243B2 -

Info

Publication number: JPS6236243B2
Application number: JP12597279A
Authority: JP
Inventors: Fumio Noguchi
Original assignee: Shin Meiva Industry Ltd
Current assignee: Shinmaywa Industries Ltd
Priority date: 1979-09-28
Filing date: 1979-09-28
Publication date: 1987-08-06
Also published as: JPS5652408A

Description

【発明の詳細な説明】この発明は自動位置制御装置に関し、より特定
的にはプレイバツク方式を用いる位置制御装置の
改良された追従制御に関する。

従来のプレイバツク方式を用いるロボツト等に
おいては、テイーチングされた通過点を直線また
は円弧補間等を用いて、被制御装置の移動軌跡
（パス）を作成し、それに基づいて被制御装置の
指令位置情報としている。このような従来の方法
では、被制御装置の慣性、摩擦などに基づく応答
特性により、パスの速度や位置精度が悪いという
問題があつた。

そこで、最近では、このようなプレイバツク方
式を用いるロボツトにおいて、制御対象の特性で
ある伝達関数（制御系全体の入力と出力との関
係）を考慮して、テイーチングされた目標点から
負荷ないし駆動系の慣性に対応した新しい目標値
を演算し、これを用いてロボツトを追従制御させ
るようにしたものが提案されている。しかしなが
ら、速度が速くなればなるほど、単に慣性のみを
考慮するだけでは、不十分である。というのは、
制御対象（被制御装置）の総合的な応答特性を十
分考慮しなければ、高速でかつ高精度でパスに追
従させることができないからである。

それゆえに、この発明の主たる目的は、慣性の
みならず総合的な応答特性を考慮した制御が行
え、それによつて高速度かつ高精度でパスに追従
させることができるプレイバツク方式の自動位置
制御装置を提供することである。

この発明は、要約すれば、たとえば前後４点の
テイーチング点より、３次元のパスを直線あるい
は曲線の区別なしに、なめらかに接続すると共
に、駆動系を無駄時間と１次遅れ系で近似するこ
とにより、慣性のみならず総合的な応答特性を考
慮した制御が行えるようにしたものである。

この発明の上述の目的およびその他の目的と特
徴は図面を参照して行う以下の詳細な説明から一
層明らかとなろう。

第１図はこの発明の背景となるかつこの発明が
適用される自動溶断装置の一例を示す図である。
この自動溶断（切断）ロボツトは、水平方向すな
わちＸ軸方向に延長された基台１０１を含み、こ
の基台１０１上には、第１の移動体１０２がこの
Ｘ軸方向に移動自在に搭載されている。移動体１
０２には、一体的に、コラム１０３が立設され、
このコラム１０３によつて垂直方向すなわちＺ軸
方向に移動自在に第２の移動体１０４が嵌装され
る。さらに、この移動体１０４には、Ｘ軸方向お
よびＺ軸方向と直角の方向すなわちＹ軸方向に移
動自在に、ビーム１０５が支承される。このビー
ム１０５の先端には、Ｚ軸方向と同方向の軸１０
６ａ回りに軸１０６が支承され、この軸１０６の
回動角はΦで表わされている。さらに、この軸１
０６の下端には、平衡リンク装置１０７を介し
て、溶断ないし切断トーチ１０９の取付金具１０
８が支持されている。この平衡リンク装置１０７
は、トーチ１０９の姿勢角Ψを可変に、さらにい
かなる角度Ψにおいても、トーチ１０９の作業点
１０９ａの位置が、軸１０６ａ上の一定点に存在
するように構成されている。さらに、取付金具１
０８は、その取付けたトーチ１０９がその軸１０
８ａの回りに回動自在とされていて、その回動角
をΘで表わす。すなわち、この第１図に示す切断
ロボツト１００は、Ｘ，Ｙ，Ｚ，Φ，ΨおよびΘ
の６つの自由度を有し、さらに、その位置制御装
置のための駆動系や制御装置たとえばコンピユー
タなどが付属的に設けられる。そして、トーチ１
０９によつてワークピース２００の切断線２０１
に沿つて、このワークピース２００を溶断ないし
切断する。

第２図は第１図に示すロボツト１００の位置制
御のための概略ブロツク図を示す。この第２図に
おいて、共通バス５００には、操作卓３００、コ
ンピユータ４００および各軸の位置決め装置６０
０_x，６００_y，６００_z，６００〓，６００〓，
６００〓が連結されている。

操作卓３００は、第３図に詳細に示すが、モー
ド切換スイツチ３０１が設けられる。このモード
切換スイツチ３０１は、「Ｍ」の位置でマニユア
ルモードを設定し、「MT」の位置でマニユアル
操作によるテイーチングモード、「ST」の位置で
第１図に示すワークピース２００の切断線２０１
を検出する光学手段等を作動させてするテイーチ
ングモード、「TE」の位置でテストモード、
「Ａ」の位置でオートモードを、それぞれ設定し
得る。そして、このようなモード切換スイツチ３
０１からは、バス５００に各モードに相当する信
号が導出される。この操作卓３００は、さらに、
溶断ないし切断速さ指令スイツチ３０２、スター
トボタンスイツチ３０３を含む。さらに、操作卓
３００は、各軸のマニユアル操作スイツチ３０４
_x，３０４_y，３４_z，３０４〓，３０４〓，およ
び３０４〓を有する。これらスイツチ３０４_x，
３０４_y，および３０４_zは、それぞれ、「Ｕ」側
に倒すことにより各制御軸に沿つて原点より遠ざ
かる方向にトーチ１０９を移動させ、「Ｄ」側へ
倒すことによりトーチ１０９を原点に接近させる
方向に移動させる。さらに、スイツチ３０４〓，
３０４〓，および３０４〓は、「Ｃ」側へ倒すこ
とにより、各軸回りに時計方向に回動制御させ、
「CC」側へ倒すことにより反時計方向に回動制御
させるものである。

なお、コンピユータ４００は、周知のように、
入出力インタフエース回路４０１、算術論理ユニ
ツト（ALU）４０２、リードオンリメモリ
（ROM）４０３およびランダムアクセスメモリ
（RAM）４０４を含む。

そして、第３図に示す操作卓３００の操作にし
たがつて、第２図に示すコンピユータ４００から
各軸位置決め装置６００_xないし６００〓に、指
令位置情報を与える。それと共に、これら位置決
め装置６００_xないし６００〓から指令位置に達
したことを表わす信号が、フイードバツクされ
る。

第４図には、従来の各軸の位置決め装置６００
_xないし６００〓のうち、代表的にＸ軸の位置決
め装置６００_xのブロツク図が示される。残余の
位置決め装置６００_yないし６００〓は、この第
４図に示す構成とほぼ同様であるので、ここでは
Ｘ軸の位置決め装置６００_xについて説明し、そ
の他の軸については説明を省略する。位置決め装
置６００_xには、バス５００すなわちコンピユー
タ４００からの指令位置情報を受取るためのバツ
フアレジスタ６０１_xが設けられる。このバツフ
ア６０１_xには、コンピユータ４００から時間間
隔ｔで送り出される指令位置情報がロードされ、
その出力はＤ／Ａ変換器６０２_xに与えられる。
したがつて、６０２_xの出力はサーボアンプ６０
３_xの一方入力に与えられる。このサーボアンプ
６０３_xの他方入力には、たとえばインクリメン
タルエンコーダなどを含む位置検出器６０５_xか
らの検出出力を受ける。そして、サーボアンプ６
０３_xの出力は油圧回路など（図示せず）を作動
させるモータ６０４_xの駆動入力として与えられ
る。したがつて、これら各コンポーネント６０３
_x，６０４_xおよび６０５_xによつて、駆動系が構
成される。位置検出器６０５_xからは、さらに、
この駆動系によつて、指令位置に達したことを表
わす信号が、バス５００を介してコンピユータ４
００に与えられる。

このように、従来の位置決め装置は、フイード
バツクループを用いるのが一般的である。このよ
うな、駆動系の応答特性は、たとえば第５図の実
線で示されるような特性を持つたものとなる。こ
のような特性を、以下に述べるような目的で、簡
単化するために、この第５図の破線で示すような
１次遅れＴと無駄時間τで近似する。これらの値
は実際の装置での測定やシユミレートによつて最
も誤差が少なくなるように、たとえば第５図に示
す斜線部分の面積が最小となるように、決定する
か、あるいは最小２乗法によつて決定する。

この近似された駆動系の伝達関数Ｇ_(s)は、入
力をＸ_(s)とし、出力をＹ_(s)とすれば、Ｇ_(s)＝Ｙ_（ｓ）／Ｘ_（ｓ）＝ｅ⁻〓^ｓ／１＋Ｔｓ…
…(1) で示される。この第(1)式より、入力Ｘ_(s)を求め
ると、Ｘ_(s)・ｅ^-〓^s＝Ｙ_(s)（１＋Ts）となる。
これを実時間系に直すと、ｘ_(t-〓₎＝ｙ_(t)＋Ｔ・（ｙ_（ｔ）／ｄｔ） ……(2) となる。そして、望ましい出力ｙ_(t)がなんらか
の式で示されるようならば、逆にｙ_(t)＋Ｔ×
（ｙ_（ｔ）／ｄｔ）を、無駄時間τだけ時間を進めて入
力すればよいということになる。詳細は後述する。

一方、テイーチングされた各点をなめらかに結
ぶ式を求める必要がある。

第６図はバスをなめらかに接続する方法につい
て説明するための図である。そして、この第６図
において、各テイーチング点の位置座標はベクト
ル表示Ｒ_i（ｉ；１ないしｎ）とする。そして、
今、曲線分をパラメータによるベクトル表示とし
てＤ_i,_o(u)と表現する。ただし、第１の添字ｉ
は、ｉ番目の接続要素であることを示し、第２番
目の添字ｎは、ｎ段階目に作られた曲線分である
ことを示し、ｕはパラメータで０から１の間を動
くものとする。

こうしておいて、Ｄ_i,_o(u)をＤ_i,_o-1(u)とＤ_i+1,_o-
_１（ｕ）の曲線分を、ウエイトω（τ）なる配分で、
ｕを移動させたときの擬似重心点の軌跡とすれ
ば、次式(3)のように表わされる。

Ｄ_i,_o(u)＝｛∫^１ _ｕω（τ）・Ｄ_i,_o-1(〓₎ｄτ ＋∫^ｕ _０ω（τ）・Ｄ_i+1,_o-1(〓₎ｄτ｝ ÷｛∫^１ _ｕω（τ）ｄτ＋∫^ｕ _ｐω（τ）ｄτ｝
……(3) ここで、ω（ｕ）は∫^１ _０ω（τ）ｄτ＝１を満
たすものとし、また最初のテイーチング点をＲ_i
とおけば、第０段目の曲線分はＤ_i,_0(u)＝Ｒ_i・ｆ₀,_0(u) ｆ₀,_0(u)＝１ ……(4) であるので、第１段階目の曲線分は次式(5)で表わ
される。

Ｄ_i,_1(u)＝Ｒ_i・ｆ₀,_1(u) ＋Ｒ_i+1・1f_i,_1(u) ……(5) ここで、ｆ₀,_1(u) ＝∫^１ _ｕω（τ）ｄτ＝１−ｕｆ₁,_1(u) ＝∫^ｕ _０ω（τ）ｄτ＝ｕである。

以下同様にして、第ｎ段階目の曲線分はで表わされる。

いま、着目しているのが第３段階目の曲線分の
方程式であるので、これをｕをパラメータとする
式で表わせば次式(7)で表わされる。

Ｄ_i,_3(u)＝Ｒ_i・ｆ₀,_3(u)＋Ｒ_i+1 ・ｆ₁,_3(u)＋Ｒ_i+2・ｆ₂,_3(u) ＋Ｒ_i+3・ｆ₃,_3(u) ……(7) ただし、ｆ₀,₃＝（１−３ｕ×３ｕ^２−ｕ^３）／６ｆ₁,₃＝（４−６ｕ^２＋３ｕ^３）／６ｆ₂,₃＝（１＋３ｕ＋３ｕ^２−３ｕ^３）／６ｆ₃,₃＝ｕ^３／６ここで、０≦ｕ≦１とする。

さて、前記(7)式においては、Ｒ_i点より曲線分
を作り出しているが、この曲線は第６図に示すよ
うに、指示されたＲ_i点を通つていない。実際の
問題としては、必要なのはテイーチングされた点
を通り、なおかつ十分になめらかであるような曲
線の方程式である。この処理を行うために、逆に
テイーチング点Ｒ_i点より、計算上の基準点Ｐ_i点
を求め、この基準点Ｐ_i点の値を前(7)式に代入す
れば良いことになる。

基準点Ｐ_iを求めるために、前(7)式のｕ＝０の
ときを考えれば、６・Ｐ_i＝Ｒ_i＋４・Ｒ_i+1＋Ｒ_i+2 ……(8) となり、ｉに関する連立方程式を解けば｛Ｒ_i｝
より｛Ｐ_i｝が求められるが、実際問題として
は、遠くの｛Ｒ_i｝がＰ_iにおよぼす誤差は微少で
あるので、基準点Ｐ_iに関して前後９点程度のテ
イーチング点Ｒ_iを考えればよい。このときの連
立方程式の近似解はＰ_i＝√３Ｒ_i，Ｐ_i+j＝Ｒ_i・α^j ただしα＝√３−２これらをまとめればと表わされる。

そして、この(9)式において、Ｐ_iを順次ｉ＝１
ないしｎまで計算して求める。ただし、テイーチ
ングされていない境界外のベクトルＲ_i±jに対し
ては、たとえばテイーチング点の延長した点を入
れるなどして、適当な値を代入する。さて、前(9)
式で求めた点Ｐ_iから点Ｒ_iを通り、各区間の接続
が十分なめらかであるようなパスの方程式Ｄ_i(u)
は、次式(10)で表わされる。

Ｄ_i(u)＝f₀×Ｐ_i＋f₁×Ｐ_i+1 ＋f₂×Ｐ_i+2＋f₃×Ｐ_i+3 ……(10) ただし、f₀＝１−３ｕ＋３ｕ^２−ｕ^３／６ f₁＝４−６ｕ^２＋３ｕ^３／６ f₂＝１＋３ｕ＋３ｕ^２−３ｕ^３／６ f₃＝ｕ^３／６０≦ｕ≦１なお、このようなパスの方程式は、たとえば
1976年12月発行の「情報処理」のVol.17No.12など
ですでに知られている。

ところが、前(10)式において、ｕを区間０≦ｕ≦
１の間で、一定間隔に代入してＤ_i(u)の値を求め
ても、パス上の移動距離は等間隔になる保証はな
いので、前(2)式のように、位置指令のみで速度な
らびに位置のコントロールを行おうとするときは
きわめて不便である。したがつて、こ発明ではｕ
にかわる新しい変数ｔを用いて、ｕ＝at³＋bt²＋ct ……(11) の変換を行う。そして、変数ｔを等間隔に取れ
ば、パス上の指令位置も等間隔となる。そのよう
に前(10)式を変形する。このために、ｕの期間内
で、０，１／２，１の３点で、ｄＤ／ｄｔが望ましい値
Ｖ_cとなるように、ａ，ｂ，およびｃを決定する。ｕが
それぞれ０，１／２，１のときの変数ｔの値を、それぞれt₀，t₁，およびt₂とし、ｄＤ／ｄｕの値をそれぞれ V₀，V₁，およびV₂とすれば、前(11)式より、となる。

一方、前(10)式から、次式（13）式が得られる。

ｄＤ／ｄｕ＝１／２｛（Ｐ_i+3−3P_i+2＋3P_i+1−Ｐ_i）×u² ＋（2P_i+2−4P_i+1＋2P_i） ×ｕ＋（Ｐ_i+2−Ｐ_i）｝ ……(13) そして、この第（13）式にｕの値を代入するこ
とにより、が得られる。この（14）式と次式（15）式とによ
り、第（16）式が得られる。

そして、前(12)式と（16）式とを満足するａ，
ｂ，およびｃの係数を、たとえば収束演算等の方
法で決定すれば、望ましい形でのパスの方程式が
次式（17）で得られる。

Ｄ_(t)＝１／６．０｛（Ｐ_i+3−3P_i+2＋3P_i+1 −Ｐ_i）（at³＋bt²＋ct）³ ＋（3P_i+2−6P_i+1＋3P_i）（at³＋bt²＋ct）² ＋（3P_i+2−3P_i）（at³＋bt²＋ct）＋（Ｐ_i+2＋４・Ｐ_i+1＋Ｐ_i）｝ｄＤ_（ｔ）／ｄｔ＝１／２．０｛（Ｐ_i+3−3P_i+2 ＋3P_i+1−Ｐ_i）（at³＋bt²＋ct）² ＋（2P_i+2−4P_i+1＋2P_i）（at³＋bt²＋ct）＋（Ｐ_i+2−Ｐ_i）｝（3at²＋2bt＋ｃ） ……(17) この第（17）式からもわかるように、指令値を
更新する時間間隔と、要求されるパス上の速度Ｖ
_cとにより、適当な時間ｔの刻みを設けて、前
（17）式の計算を行い、そして前(2)式に代入すれ
ば、駆動系の１次遅れを見込んだ指令値が計算で
きる。

第７図は無駄時間の補正を行なう回路の一例を
示す図である。なお、この回路はＸ軸について記
しているが、他の軸についても同様である。この
第７図の回路において、前述の第４図の従来の回
路との違いは、バツフア６０１_xとＤ／Ａ変換器
６０２_xとの間にクロツクパルス発生器６０８_xと
メモリレジスタ６０７_xで構成される出力遅延回
路が付加されている点である。メモリレジスタ６
０７_xは、遅延時間（τ_０−τ_x）をクロツクパル
ス発生器６０８_xのクロツク周期で除した数だけ
のメモリ領域を持つており、さらにメモリの最上
位はバツフア６０１_xに接続され、またメモリの
最下位はＤ／Ａ変換器６０２_xに接続されてい
る。なお、τ_０は各軸の無駄時間の中で最も大き
いものを表わし、τ_xはＸ軸の無駄時間を表わし
ている。

上記のような構成において、クロツクパルス発
生器６０８_xからメモリレジスタ６０７_xにクロツ
クパルスが与えられると、メモリレジスタ６０７
_xの内容は順次１段ずつ下段にシフトされ、最下
位のメモリの内容がＤ／Ａ変換器６０２_xに与え
られる。また、メモリの最上位にはバツフア６０
１_xの内容が書込まれる。したがつて、バツフア
６０１_xに書込まれた指令位置情報は遅延時間
（τ_０−τ_x）に応じたクロツクパルスがメモリレ
ジスタ６０７_xに入力された後に、Ｄ／Ａ変換器
６０２_xに転送される。すなわち、メモリレジス
タ６０７_xとクロツクパルス発生器６０８_xとで構
成される出力遅延回路は、バツフア６０１_xに書
込まれた指令位置情報を遅延時間（τ_０−τ_x）
だけ遅延させてＤ／Ａ変換器６０２_xに転送す
る。

ところで、第４図に示す従来の回路では、各軸
に無駄時間τ_x，τ_Y，……τ〓が存在したとき、
コンピユータ４００により求められた指令値が各
軸同時にバツフアに書込まれても実際に各軸が動
き出すまで、それぞれ、τ_x，τ_Y，……τ〓なる
時間が費され、軸間の同期がとれずに、各軸がば
らばらに動くため、パスの追従精度が悪くなると
いつた問題があつた。そこで、第７図に示したよ
うな出力遅延回路を設け、たとえば軸の中で最も
大きな無駄時間を持つものを基準値τ_０として、
Ｘ軸にはτ_０−τ_x，Ｙ軸にはτ_０−τ_Y，……の
値を持つように遅延時間を設定しておけば、バツ
フアに書込まれた指令位置情報は、たとえばＸ軸
の場合（τ_０−τ_x）時間だけ遅れてＤ／Ａ変換
器６０２_xに転送され、さらに軸が動くまでτ_xの
時間がかかり、合わせてτ_０の時間がかかること
になる。したがつて、全軸ともバツフアに指令位
置情報が書込まれた時刻からτ_０時間後に動き出
すことになり、パスの追従精度は大幅に向上され
る。

第８図は、このような操作ないし動作を示すフ
ロー図であり、この第８図を参照することによつ
て、上述の操作ないし動作が一層よく理解されよ
う。

以上のように、この発明によれば、関連する複
数のテイーチング点をなめらかに接続すると共
に、各軸の駆動系を無駄時間と１次遅れ系で近似
することにより、各軸の総合的な応答特性を考慮
した制御が行える。そのために、より高速度でか
つ高精度を有して、パスに追従させることがで
き、特に被制御体を高速で位置制御する装置にお
いては非常に有効である。

【図面の簡単な説明】

第１図はこの発明の背景となるかつこの発明が
実施され得る自動位置決め装置の一例としての自
動溶断ないし切断装置を示す。第２図は第１図の
ような位置決め装置の概略ブロツク図である。第
３図は操作卓３００の一例を示す図解図である。
第４図はＸ軸の従来の位置決め装置６００_xを示
すブロツク図である。第５図は応答特性の一例を
示すグラフである。第６図はパスをなめらかに接
続する方程式を説明するための図解図である。第
７図は無駄時間を処理するように修正されたＸ軸
位置決め装置６００_xの一例を示すブロツク図で
ある。第８図はこの発明の一実施例の操作ないし
動作を説明するフロー図である。図において、１００は自動溶断ないし切断装
置、２００はワークピース、３００は操作卓、４
００はコンピユータ、５００は共通パス、６００
_xないし６００〓は各軸位置決め装置、６０７_xは
メモリ、６０８_xはクロツクパルス発生器を示
す。

Claims

【特許請求の範囲】１プレイバツク方式で通過点をテイーチング
し、これらの点間を補間して指令位置情報を考慮
し、被制御装置の各軸が前記指令位置情報に基づ
いて制御される自動位置制御装置において、関連の複数の点位置を滑らかに結ぶ曲線の方程
式を求める手段、前記曲線方程式を求める手段によつて求められ
た曲線の方程式に基づいて適宜の時間間隔ごとの
指令位置を求める手段、前記指令位置を求める手段によつて求められた
各指令位置を前記被制御装置の１次遅れを考慮し
て補正する１次遅れ補正手段、および前記１次遅れ補正手段によつて補正された各指
令位置を前記被制御装置の各軸に与える際、各軸
が有する無駄時間に応じて各軸ごとに前記指令位
置の出力タイミングを制御し、それによつて無駄
時間の補正を行なう無駄時間補正手段を備える、
自動位置制御装置。２前記無駄時間補正手段は、前記各指令位置の情報を受けるかつ複数の記憶
領域を有する記憶手段と、前記記憶手段にロードされた指令位置の情報
を、前記各軸の無駄時間に相当する時間前記各記
憶領域に順次ロードさせるためのクロツクパルス
発生手段とを含む、特許請求の範囲第１項記載の
自動位置制御装置。