JPH05108165A - 制振位置決め制御方式 - Google Patents

Abstract

(57)【要約】 【構成】 １軸または複数軸を有し、各軸が低剛性負荷
（５）をもち、かつ位置フィードバックによって制御さ
れるアクチュエータにより駆動されるロボットマニピュ
レータの位置決め制御方式において、低剛性負荷（５）
の固有の振動周期と同じか２以上の整数倍の周期をもつ
矩形状の波形の信号、または、基本速度指令パターン
と、その基本速度指令パターンを低剛性負荷（５）の固
有振動周期の１／２の奇数倍だけずらしたパターンとを
合成した波形の信号を速度指令として出力するパターン
発生器（１）を設ける。 【効果】 低剛性負荷をもつサーボ系において、位置決
め時間を遅くすることなく、負荷の振動を抑えることが
できる。ロボットマニピュレータの制御のみならず、割
り出しテーブル等の位置決め制御を主として用いられる
自動化装置の位置決め時間を従来法に比し、大幅に高速
化できる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、ロボットのように低剛
性負荷をもつ位置決めサーボ系において、特に高速位置
決めおよび制振を実現できる制御方式に関する。

【０００２】

【従来の技術】低剛性負荷をもつ位置決めサーボ系の制
御において、高速位置決めをさせるため指令時間を短縮
すると、負荷に大きな加速度が加わり、そのために負荷
が振動し、その振動が残留する。その対策として、指令
値をフィルタを通すことも行われるが、位置決め時間が
長くなる。たとえば、フィルタとしては図８に示すよう
に矩形波をいれたら台形波になるようなものが使用され
ていた。このような従来技術では、位置決めを速くしよ
うとすると、負荷が振動し、逆に振動を抑えようとする
と位置決めが遅くなるという問題があった。かかる問題
を解決するものとして、特公昭６０−２９１２１号公報
において提案されたものがある。これは、位置決め制御
対象物を等加速または等減速区間を設けて位置決め制御
する場合、等加速または等減速区間において等加速また
は等減速する時間を制御対象物の固有振動周期Ｔ₀ の整
数倍の時間ｎＴ₀ に等しくするようにした位置決め制御
方法である。

【０００３】

【発明が解決しようとする課題】ところが、前記の特公
昭６０−２９１２１号公報に記載された方法は、制御系
が明確でなく、簡単には実現できないという問題があ
り、また全位置決め時間を周期Ｔの２倍より短くでき
ず、短距離を高速で動かすことに十分効果がないという
問題があった。そこで本発明が解決すべき課題は、低剛
性負荷をもつサーボ系において、位置決めを十分に速
く、かつ振動を効果的に抑えることにある。

【０００４】

【課題を解決するための手段】この課題を解決する第１
の手段は、１軸または複数軸を有し、各軸が低剛性負荷
をもち、かつ位置フィードバックによって制御されるア
クチュエータにより駆動されるロボットマニピュレータ
の位置決め制御方式において、速度指令として、低剛性
負荷の固有の振動周期と同一もしくは２以上の整数倍の
周期をもつ矩形状の波形の信号を出力することである。

【０００５】また第２の手段は、１軸または複数軸を有
し、各軸が低剛性負荷をもち、かつ位置フィードバック
によって制御されるアクチュエータにより駆動されるロ
ボットマニピュレータの位置決め制御方式において、速
度指令として、基本速度指令パターンと、その基本速度
指令パターンを低剛性負荷の固有振動周期の１／２の奇
数倍だけずらしたパターンとを合成した波形の信号を出
力することである。

【０００６】

【作用】本願の第１の発明においては、図７のブロック
図で表されるように、低剛性負荷５をもつ位置決めサー
ボ系において、入力として負荷の固有振動周期の整数倍
で終えるような速度一定指令を入れる。つまり、図７で
入力Ｒ(t) は Ｒ(t) ＝ ｖ＝一定 ０≦ｔ≦Ｔｎ (ｎ＝１，２，・・・） ０ ｎＴ＜ｔ ・・・(1) を指令として入れる。（Ｔ＝２π／ω_n ） 低剛性負荷５の振動をみるためにθ≡ｙ−ｘとすると θ”＋２ζω_n θ’＋ω_n ² θ＝−ｘ”(t) ・・・(2) となる。また制御系を１次形、つまり ｘ”＋Ｋ_p ｘ’＝Ｋ_p Ｒ(t) ・・・(3) と表現できるとしている。ここでζ＝０とすると、 θ”＋ω_n ² θ＝−ｘ”(t) ・・・(4) また、制御系への入力が（１）式で表されるものである
ため、ｘ”(t) は ｘ”(t) ＝ Ａ₁ exp(-K_pt) ０≦ｔ≦Ｔｎ Ａ₂ exp(-K_p(t-nT)) ｎＴ＜ｔ ・・・(5) Ａ₁ ≡ｖＫ_p ，Ａ₂ ≡ｖＫ_p（−１＋exp(-K_pnT)) ・・・(6) となる。

【０００７】一般に、ｘ”＝Ａ_i exp(-K_p t)のとき
（２）式の解θ(t) ，θ’(t) は、初期値（θ(0) ，
θ’(0) ＝（θ₀ ，θ’₀ ) とすると、 θ(t) ＝｛A_i／(K_p ²＋ω_n ²)｝・｛R_i cos(ω_nt＋φ)−exp(-K_pt)｝ θ’(t)＝−｛A_i／(K_p ²＋ω_n ²)｝・｛R_iω_nsin(ω_nt＋φ)−K_p exp(-K_pt)｝ ・・・(7) ここで Ｒ_i ＝√〔｛((K_p ²＋ω_n ²)／A_i)θ₀＋１｝²＋(1／ω_n ²) ｛((K_p ²＋ω_n ²)／A_i) θ’₀ −K_p｝²〕 ・・・(8) φ＝ tan^-1〔｛(1／ω_n)θ’−(K_p／ω_n)(A_i／(K_p ²＋ω_n ²))｝ ／｛θ₀(K_p ²＋ω_n ²)＋A_i｝〕 ・・・(9) cosφ＝(1／R_i)｛θ₀(K_p ²＋ω_n ²)＋A_i｝， sinφ＝−(1／R_i)｛(1／ω_n)θ’−(K_p／ω_n)(A_i／(K_p ²＋ω_n ²))｝ ・・・(10)

【０００８】故に、（７）式よりθ₀ ＝θ’₀ ＝０のと
きｔ＝ｎＴでは（７），（８）式よりsin(ω_nnT)＝０，
cos(ω_nnT)＝１、また（１０）式よりcosφ＝Ａ_i／
Ｒ_i，sinφ＝−(K_p／ω_n)(Ａ_i／Ｒ_i) なので θ(nT)＝｛A_i／(K_p ²＋ω_n ²)｝・〔１−exp(-K_pt)〕 θ’(nT)＝−｛K_p A_i／(K_p ²＋ω_n ²)｝・〔１−exp(-K_pt)〕 ・・・(11) この結果を利用すると、ｔ＞ｎＴでは、（８）式より
振動項の半径Ｒは Ｒ² ＝｛((K_p ²＋ω_n ²)／A₂) θ(nT)＋１｝² ＋(1／ω_n ²)｛((K_p ²＋ω_n ²)／A₂) θ’(nT)−K_p｝² ＝｛(A₁／A₂)(1−exp(-K_pnT)＋1｝² ＋(1／ω_n ²)｛−K_p(A₁／A₂)(1−exp(-K_pnT)−K_p｝² ・・・(12) ここでexp(-K_pnT)＝０と仮定すると、Ａ₂ ＝−Ａ₁ な
ので、（１２）式より Ｒ＝０ ・・・(13) となる。したがって、ｔ＞ｎＴ以降は（７），（８），
（９），（１３）式より θ(t) ＝｛A₁／(K_p ²＋ω_n ²)｝−exp(-K_p(t-nT)) θ’(t)＝−｛K_p A₁／(K_p ²＋ω_n ²)｝exp(-K_p(t-nT))＝−K_p θ(t) ・・・(14) （１４）式よりθ(t) ，θ’(t) はｔ＞ｎＴにおいては
振動成分がなくなり、ｔとともに指数的にθ(t) →０，
θ’(t)→０となる。以上のように、図７の入力とし
て周期Ｔの整数値で終わるようなステップ入力を入れる
と、指令修了後、振動成分がなくなり、速やかに位置決
めが行われる。

【０００９】次に、本願の第２の発明においては、第７
図のブロック図で表されるように、低剛性負荷をもつ位
置決めサーボ系において、入力として負荷の固有振動周
期の１／２の奇数倍だけずらした２つのパターンを合成
して入れる。つまり、入力 R(t)＝v(t)＋v(t−m・T ／2) (m＝2n＋1， n＝0， 1， 2， ・・・） (v(t)は任意の関数、Ｔは負荷の固有振動周期＝２π／ω_n) ・・・(21) を指令として入れる。低剛性負荷の変位ｙ(t)と制御系
からの出力ｘ(t)の差をθ(t)＝ｙ(t)−ｘ(t)とすると、 θ”(t)＋２ζω_nθ’(t) ＋ω_n ²θ(t) ＝ω_n ²Ｆ(t) ・・・(22) Ｆ(t)＝−(1／ω_n ²)ｘ”(t) ・・・(23) ζ：減衰係数、ω_n ：固有振動数 を考えると振動の様子が調べられる。いま、ζ＝０とお
けるとし、式を簡単にするためにｔ＝ｔ′／ ω_n とす
ると、（２２）式は θ”(t′) ＋θ(t′) ＝Ｆ(t′) ・・・(24) となる。なお、以降ｔ′＝ｔと置く。ただし、時間スケ
ールを変えることによりＴ／２はπとなる。指令Ｒ(t)
においてｖをステップ入力の集まりと考えると（図６参
照）、次の式が導かれる。

【００１０】

【数１】

【００１１】考えている制御系（図７）は線形であるの
で、 Ｒ_i(t)＝ｖ_i(ｕ_i(t)＋ｕ_i(t・π・ｍ)) ・・・(26) なる入力（図４ｂ）に対するアームの変位をｙ_i(t)，制
御系からの出力をｘ_i(t)とすると、Ｒ(t) を入力したと
きのθ(t) ＝ｙ(t) −ｘ(t) は、

【００１２】

【数２】

【００１３】となる。すると、θ_i(t) ，θ_i’(t)は、
θ_i(0)＝θ_i’(0) ＝０とすると、 (i) ０≦ｔ＜ｔ_i のとき θ_i(t)＝０，θ_i’(t)＝０ ・・・(28) (ii) ｔ_i ≦ｔ≦ｔ_i＋π・ｍのとき θ_i(t)＝Ｒ₀ cos(ｔ−ｔ_i＋φ₀)＋Ｆ₀(ｔ−ｔ_i) θ_i’(t)＝−Ｒ₀ sin(ｔ−ｔ_i＋φ₀)＋Ｆ₀(ｔ−ｔ_i) ・・・(29) Ｒ₀ ＝√（Ｆ₀(0)²＋Ｆ₀ ’(0)²) ・・・(30) cosφ₀ ＝Ｆ₀(0)／Ｒ₀ ，sin φ₀ ＝−Ｆ₀(0)／Ｒ₀ ・・・(31) Ｆ₀(t)＝−｛ｖ_iK_p／(K_p ²＋ω_n ²)｝exp((-K_p／ω_n)t) ・・・(32) (iii) ｔ_i ＋π・ｍ＜ｔのとき θ_i(t)＝Ｒ₁ cos(ｔ−mπ−ｔ_i＋φ₁)＋Ｆ₁(ｔ−ｔ_i−π・m) θ_i’(t)＝−Ｒ₁ sin(ｔ−mπ−ｔ_i＋φ₁)＋Ｆ₁(ｔ−ｔ_i−π・m) ・・・(33) Ｒ₁＝√((θ_i(t_i−mπ)−Ｆ₁(0))²＋(θ_i’(t_i−mπ)−Ｆ₁’(0)²) ・・・(34) cosφ₁＝｛θ₁(ｔ_i＋πm)−Ｆ₁(t_i＋πm＋0)｝／Ｒ₁ ， sinφ₁＝−｛θ₁(t_i＋πm)−Ｆ₁(t_i＋πm＋0)｝／Ｒ₁ ・・・(35) Ｆ₁(t)＝−｛ｖ_iK_p／(K_p ²＋ω_n ²)｝ ・(1−exp(-K_p／ω_n)πm exp((-K_p／ω_n)t)) ・・・(36) ここでθ_i(t)，θ_i’(t)はｔ＝ｔ_i＋π・ｍで連続で
あり、（２９）式より θ_i(t_i＋mπ)＝Ｒ₀ cos(mπ＋φ₀)＋Ｆ₀(mπ) ＝−Ｒ₀ cos(φ₀)＋Ｆ₀(mπ) ＝−Ｆ₀(0)＋Ｆ₀(mπ) ・・・(37) θ_i’(t_i＋mπ)＝−Ｒ₀ sin(mπ＋φ₀)＋Ｆ₀ ’(mπ) ＝Ｒ₀ sin(φ₀)＋Ｆ₀ ’(mπ) ＝−Ｆ₀ ’(0)＋Ｆ₀ ’(mπ) ・・・(38) （３６）式より Ｆ₁’(t)＝Ｆ₀’(t)−Ｆ’(m π)exp((-K_p／ω_n)t) ・・・(39) （３６），（３７），（３８），（３９）式を（３４）
式に代入して、 Ｒ₁ ＝√｛(2Ｆ₀(mπ))²＋(2Ｆ’(m π))²｝ ・・・(40) ここでＦ₀(mπ)≒０，Ｆ₀’(mπ)≒０ならば ・・・(41) Ｒ₁ ≒０ ・・・(42) となる。したがって（３３）式より θ_i(t) ＝Ｆ₁(t−t_i−mπ) θ_i’(t) ＝Ｆ₁ ’(t−t_i−mπ) となり、Ｆ₁(t)は（３６）式で表されるから、θ_i(t)，
θ_i’(t)は振動しなくなる。したがって、θ_i(t)の重ね
合わせとして（２７）式で表されるθ(t)も、ｔ_M+1＜ｔ
においては、振動成分がなくなる。つまり制振されたこ
とになる。またこの方法は、フィルタに通さないので、
位置決め時間が長くなることもない。なお（４１）式の
条件は exp(-(K_p／ω_n)πm)＝exp(-K_p(T／2)m)≒０ ・・・(43) であり、この範囲において制振作用がある。

【００１４】

【実施例】以下、本発明を実施例に基づいて具体的に説
明する。図１は本願の第１の発明を実施するためのブロ
ック図（第１実施例）である。図において１はパターン
作成器、２，４は積分器、３は増幅器、５は低剛性負荷
を示している。このブロック図において、位置指令が加
えられると、パターン発生器１によって長さがｎＴのス
テップ速度指令が作られる。図２はそのパターンを表し
ている（ｎ＝１の場合）。パターン作成器１より出力さ
れた速度指令は、積分器２，増幅器３，積分器４を通っ
たあと、位置指令として出力され、位置フィードバック
系へ入力される。その結果、負荷とサーボ系出力との差
θ(t) とθ’(t) は図３に示したような位相空間上の動
きとなり、ｔ＞Ｔ以降は振動がなくなり、（θ，θ’)
＝（０，０）へ向かう。パターン作成器１は、ソフトウ
エアサーボでは位置指令値をｘ₀ とするとｖ＝ｘ₀ ／Ｔ
ｎ（ｎ＝１，２，・・・）とするｖを計算するプログラ
ムを作るだけで簡単に実現可能である。

【００１５】次に本願の第２の発明の実施例（第２実施
例）について説明する。前記の第１実施例では、指令の
形が矩形状に限定されるため加減速が急激になり、十分
な効果が得られない場合があるが、第２の発明の実施例
ではこの点を改善している。図１において、位置指令が
与えられると、パターン作成器１によって１／２・Ｔ・
ｍ（ｍ＝１，３，５・・・）ずらしたパターン２つを合
成したパターンが速度指令として作成される。そのパタ
ーンの例として図４（ａ），（ｂ），（ｃ）などがあ
る。それぞれ図４（ｄ），（ｅ），（ｆ）の基本パター
ンをＴ／２ずらしたパターンを合成したものである。パ
ターン作成器１より出力された速度指令は、積分器２，
増幅器３，積分器４を通った後、位置指令として出力さ
れ、位置制御系へ入力される。その結果、低剛性負荷５
は指令が終わった時間で振動しなくなる。たとえば図４
（ａ）の波形を入力したときは図５のような出力とな
る。パターン作成器１はハード的に作ってもソフトウエ
アで計算してもよい。

【００１６】

【発明の効果】以上説明したように、本発明によれば、
低剛性負荷をもつサーボ系において、位置決め時間を遅
くすることなく、負荷の振動を抑えることができる。ロ
ボットマニピュレータの制御のみならず、割り出しテー
ブル等の位置決め制御に主として用いられる自動化装置
の位置決め時間を従来法に比し、大幅に高速化できる。

【図面の簡単な説明】

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

【図２】 図１のパターン作成器より出力されるパター
ンの例を示す波形図である。

【図３】 図２のパターンに対する出力の動きを示す位
相面図である。

【図４】 図１のパターン作成器より出力されるパター
ンの各例を示す波形図である。

【図５】 図４（ａ）のパターンに対する出力θの図で
ある。

【図６】 入力をステップ信号の集まりと考えた場合の
例を示す波形図である。

【図７】 低剛性負荷をもつ位置決めサーボ系のブロッ
ク図である。

【図８】 従来の制振制御でよく使われるフィルタの入
力と出力の関係を示す図である。

【符号の説明】

１ パターン作成器、２，４ 積分器、３ 増幅器、５
低剛性負荷

フロントページの続き (72)発明者 大薗 久男 福岡県北九州市八幡西区黒崎城石２番１号 株式会社安川電機内

Claims (2)

【特許請求の範囲】
1. 【請求項１】 １軸または複数軸を有し、各軸が低剛性
   負荷をもち、かつ位置フィードバックによって制御され
   るアクチュエータにより駆動されるロボットマニピュレ
   ータの位置決め制御方式において、速度指令として、低
   剛性負荷の固有の振動周期と同一もしくは２以上の整数
   倍の周期をもつ矩形状の波形の信号を出力することを特
   徴とする制振位置決め制御方式。
2. 【請求項２】 １軸または複数軸を有し、各軸が低剛性
   負荷をもち、かつ位置フィードバックによって制御され
   るアクチュエータにより駆動されるロボットマニピュレ
   ータの位置決め制御方式において、速度指令として、基
   本速度指令パターンと、その基本速度指令パターンを低
   剛性負荷の固有振動周期の１／２の奇数倍だけずらした
   パターンとを合成した波形の信号を出力することを特徴
   とする制振位置決め制御方式。