WO2005063455A1

WO2005063455A1 - ロボットの制御装置

Info

Publication number: WO2005063455A1
Application number: PCT/JP2004/018821
Authority: WO
Inventors: Jun Hagihara
Original assignee: Kabushiki Kaisha Yaskawa Denki
Priority date: 2003-12-26
Filing date: 2004-12-16
Publication date: 2005-07-14
Also published as: US7433758B2; US20070164695A1; JP4192780B2; TW200533485A; TWI325361B; JP2005186235A; DE112004002552T5

Abstract

　計算量を大幅に低減してＣＰＵの負担を軽減することのできるロボットの制御装置を提供する。　互いに干渉する複数軸から構成されるロボットであって、モータ（８），（１８）と、モータ（８），（１８）に減速機等を介して結合されたアーム（９），（１９）と、モータ位置検出器（１０），（２０）とから構成され、各軸を各軸毎の指令通りに動作させるための位置制御部（１），（１１）および速度制御部（２），（１２）を備えたロボットの制御装置において、自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部（７），（１７）と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を求める非干渉トルク信号作成部（６），（１６）と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を求める非干渉位置信号作成部（５），（１５）とを備えた。

Description

明細書

ロボットの制御装置

技術分野

[0001] 本発明は、互いに干渉する複数軸で構成されるロボットを、干渉力が作用しても各軸が指令どおり動作するように、制御を行うロボットの制御装置に関する。

背景技術

[0002] 従来のロボット制御装置は、モデル制御器を有し、モデル制御器内部では、擬似モデルと、擬似モデル用の制御部と、干渉トルクを相殺するためにモデル補償トルクを算出する補正量算出部とを有し、モデル補償トルクが加算されたモデルフィードフォワード指令を、フィードフォワード信号として出力することで、非干渉化を行っている（例えば、特許文献 1参照)。

図 5は特許文献 1に開示された従来の実施の一形態のロボット制御装置の構成を示す制御ブロック図であり、図 6は、図 5のモデル制御器の構成を示す制御ブロック図である。図 6において、 54U、 54Lは補正量演算部、 60U、 60Lはフィードバック制御系への出力信号である。図 5では、各軸ごとに 2慣性系（電動機 +減速機等のパネ +アーム)に近似した 2軸のロボット制御系の基本構成を示している。図 5および図 6では、 2軸を L軸、 U軸とし、 L軸に関する指令や量についてはそのサフィックスに "_L "付力卩し、 U軸に関する指令や量についてはそのサフィックスに" _U "を付加している。

[0003] このロボット制御装置は、各軸ごとの電動機位置指令 X , X に基づいて L軸及 rer_L ref_U

び U軸の制御を行うものであり、 2軸のフィードバック制御系（フィードバック制御器） 5 3L， 53Uと制御対象（電動機、減速機、ロボットアーム）との擬似モデルであるモデル制御器 51を有している。モデル制御器 51は、電動機位置指令 X , X を入力

ref L ref_U とし、ロボットのダイナミクスを考慮して各軸のフィードフォワード指令 U , U を計

FF_L FF_U 算し、各軸の電動機の加速度項へのフィードフォワード補償を行うとともに、規範となるモデルにおける各軸の電動機位置 θ , Θ 、電動機速度

Mm丄 Mm— U

ねじれ角 θ , θ 、ねじれ角速度

Ms丄 Ms— U

[数 2]

を算出して出力する。

以下、モデル制御器 51の出力である各要素には、接頭語「モデル」を付加する。一方、フィードバック制御系（フィードバック制御器） 53L, 53Uにおいて、各軸の位置ゲインは K , K 、速度ゲインは K , K 、トルク定数は K K であり、電動

P丄 P—U 丄 v_U t丄 , t_U 機に与えられる最終的な加速度指令はそれぞれ U , U である。また、各軸の電 ref—し ref— U

動機の慣性モーメントはそれぞれ J ， J 、電流指令は I ， I である。電動機の m_L m_L ref L ref U

実際の位置は Θ ， Θ であり、電動機の実際の速度は

m_L m_U

[数 3]

σ ι„1 ■， ^ ij であり、電動機の実際の加速度は

[数 4]

である。

さらに減速機での減速比が N ， N 、減速機のバネ定数が K ， K 、アームの慣丄 _U c_L c_U 性モーメントが j , j 、アームの実際の位置が θ , Θ 、アームの加速度（負荷

L_L L_U L_L L_U

加速度）が

[数 5]

H i , ⁶i

で表わされている。そして、両軸間の干渉を表わすために、ロボットアームの質量や両軸間の角度により決定される M /M ²の値が L軸の負荷トルクに乗じられて U軸

LU

の負荷加速度に作用し、同様に、 M /M ²の値が U軸の負荷トルクに乗じられて L

UL 軸の負荷加速度に作用する構成となっている。

[0005] このロボット制御系には、 L軸、 U軸のそれぞれごとに、電動機への最終的な加速度指令 U , U と電動機の実際の位置 θ ， θ を入力として、実際のねじれ ref L ref U m_L m_U

角 θ ， θ とその角速度

s丄 s_U

[数 6]

とを推定する状態観測器 52L, 52Uが設けられている。さて、このフィードバック制御器では、モデル制御器 51から出力されるモデル電動機位置 Θ ， Θ と実際の

Mm丄 Mm— U 電動機位置 Θ ， Θ との偏差に対して位置ゲイン κ , K を乗算して、それぞ m丄 m_U p p_U

れの軸の速度指令としてレ、る。

このように得られた各速度指令に、モデル電動機速度

[数 7]

½_mL , 軋と実際の電動機速度

[数 8]

との偏差を加算し、速度ゲイン K ， K を乗算して、それぞれ、加速度指令としてい

L U

る。

[0006] このような加速度指令に対して、（a)モデル制御器 51からのフィードフォワード指令 U , U と、（b)モデルねじれ角 θ , θ と状態観測器 52L, 52Uから出力

FF丄 FF_U Ms丄 Ms— U

されるねじれ角 θ , Θ との偏差にフィードバックゲイン K , K を乗じたものと、 s丄 s_U 1丄 1_U

(C)モデルねじれ角速度

[数 9]

^Θ Ι ， ½— ϋ

と状態観測器 52L, 52Uから出力されるねじれ角速度 [数 10]

との偏差にフィードバックゲイン K , Κ を乗じたものとを加算し、それぞれ、各軸に

2_L 2_U

対する最終的な電動機加速度指令 u ， u としている。

ref L ref U

このロボット制御系では、このように得られた最終的な電動機加速度指令 U , U ref L により各軸の電動機が駆動され、減速比がそれぞれ N ， N である減速機を介し ref U 丄 _U

て、各軸のアームが駆動される。その際、上述したように、軸間の干渉力が各軸の負荷加速度に作用する。

[0007] 次に、モデル制御器 51の構成を説明する。図 6は、フィードバック制御系や電動機、減速機、ロボットアームを表す擬似モデルからなるモデル制御器 51の構成の詳細を示してレ、る。擬似モデルにおける各軸の位置ゲインと速度ゲインがそれぞれモデノレ位置ゲイン K , K とモデル速度ゲイン K , K である。同様に、モデル p _L p _U vM丄 vM_U

電動機慣性モーメント J , J 、モデルアーム慣性モーメント J ， J 、モデル m _L m _U L _L L _U 減速比 N ， N 、モデル減速機バネ定数 K ， Κ 力モデルパラメータとして

_L _U c _L c _U

定められている。

モデル制御器 51は、各軸の電動機位置指令 X ， X を入力とし、この電動機位 ref— L ref— U

置指令 X ， X とモデル電動機位置 Θ ， Θ との偏差にモデル位置ゲイン ref L ref U m_L m_U

K , Κ を乗じて各軸のモデル速度指令としている。そして、これらのモデル速丄 pM— U

度指令から各軸のモデル電動機速度

[数 11]

を減算した値にモデル速度ゲイン κ ， κ を乗算して、それぞれ各軸のモデル v _L v _U

加速度指令としている。

[0008] このモデル加速度指令に、モデル減速機から得られるモデルねじれ角 θ , Θ

Ms— L にモデルフィードバックゲイン K ， K を乗じて得られる値と、モデルねじれ角

Ms— U 1 _L 1 _U

θ ， θ を微分して得られるモデルねじれ角速度

Ms— L Ms— U

[数 12]

にモデルフィードバックゲイン K , κ を乗じて得られる値とを減算し、モデル加速度指令 υ L , υ としている。このモデル加速度指令

ref L υ Mref L , υ 力モデ

Mref ref U ル電動機に入力されている。

したがって、モデル加速度指令 U ，

f L u は、

Mre Mref U

[数 13]

と表わすことができる。

し力、し、ここで想定している 2軸ロボットでは、干渉により、 L軸の負荷トルクに、ロボットアームの質量ゃ両軸間の角度により決定される値 M /M ²が乗じられて、 U軸

LU o

の負荷加速度に作用する。同様に、 U軸の負荷トルクに、値 M /M ²が乗じられ

UL oM

て、 L軸の負荷加速度に作用する。

このような干渉をアーム側に作用する外乱としてとらえると、 L軸に関しては、下式に示すモデル補正トルク T をモデル電動機加速度指令 U に加算すれば、干渉

comp_L rer_L

によるアームの振動を軽減することができる。

[数 14]

D は、 U軸から L軸に作用する干渉トルクであって、

[数 15]

と表わすことができる。よって、最終的なモデル電動機加速度指令 U を

FF_L

U = U +T (5)

Fr_L Mref_L comp_L

として、電動機の加速度項へフィードフォワード補償を行う。このモデル制御器 51には、モデルねじれ角 Θ を入力として上述の式（3)，（4)に基づいてモデル補正ト

Ms— U

ルク T を算出する補正量演算部 54Lが設けられており、補正量演算部 54Lから comp— L

のモデル補正トルク T がモデル電動機加速度指令 U に加算される。

comp— L Mref_L

[0010] 同様に、 U軸についても、モデル補正トノレク T を

[数 16]

のように定める。二で D は、 L軸から U軸に作用する干渉；あって、

is丄 U

[数 17] is一 LU— J jr¾«j ' _M g ,馳である。モデルねじれ角 Θ を入力として式（6) , (7)に基づきモデル補正トルク Τ

Ms— L

を求める補正量演算部 54Uが設けられている。その結果、最終的なモデル電動 comp— U

機加速度指令 U は、

FF_U

U =U +T (8)

FF_U ref U comp— U

となる。

[0011] 以上のように、特許文献 1に開示された従来技術においては、このようにモデル制御器 51により干渉トルク補正を行った上で、規範となる各状態量を指令としてフィードバック制御系に入力するので、各ロボットアームは、他軸からの干渉による影響を受けなくなる。

このように、従来のロボット制御装置は、他軸から受ける干渉力をモデル制御器で補正し、それを基に規範となる各状態量をフィードバック制御器に入力することにより、補正トルクにノイズ成分が重畳することがなくなり、ロボットアームは干渉による振動を受けずにツール先端の軌跡精度を向上させるのである。

[0012] 特許文献 1 :特開平 10-329063号公報（第 2-5頁、図 1、図 2)

発明の開示

発明が解決しょうとする課題

[0013] し力ながら、従来のロボット制御装置は、この制御器を実現するために、フィードバック制御部の演算以外に、モデル制御器 51内で、制御周期毎に、ねじれ角の計算など擬似モデルの計算を行う必要があり各軸毎の計算量が膨大になるという問題があった。

また、モデル制御器 51内で、フィードバック制御部の演算以外に、制御周期毎に、擬似モデルを制御するためのモデル用のフィードバック制御の計算を行う必要があり各軸毎の計算量がさらに増えるという問題がある。

また、非干渉化をトルクの補正のみで行うため、モデル補正トルク T の計算にモ comp

デル位置ゲイン K やモデル速度ゲイン K の値まで必要となり、さらに計算が複雑 v

になるという問題もある。

そして、結果として、各軸毎の計算量が膨大になり、制御演算時間が長くなり、本制御装置を実現するために高速な CPUを必要とするという問題がある。

本発明はこのような問題点に鑑みてなされたものであり、互いに干渉する複数軸から構成されるロボットであって、モータに減速機等のばね要素（ばね定数）を介して結合されたアームと、モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸毎の指令通りに動作させるための位置制御部および速度制御部を備えた口ボットの制御装置において、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようにモータを動作させることができ、干渉の影響を受けないで、ロボットァームを動作させることができ、計算量を大幅に低減して CPUの負担を軽減することのできるロボットの制御装置を提供することを目的とする。

また、自軸に対し複数の軸からの干渉力が作用する場合も、複数軸の関係を考慮して複雑な計算をすることなぐ各軸から自軸へ作用する干渉力の合計値を計算する処理を行うだけで、後は 2軸間の非干渉化と同様の処理を行うだけで簡単に複数軸間の非干渉化することを目的とする。また、考慮してレ、なかった軸間の干渉を追加する場合もプログラム修正はほとんどなぐ各軸から作用する干渉力の合計値を計算する処理を追加するだけで簡単に全ての軸の非干渉化することを目的とする。

課題を解決するための手段

[0014] 上記問題を解決するため、本発明の第 1の構成は、互いに干渉する複数軸から構成されるロボットであって、モータと、前記モータに減速機等のばね要素を介して結合されたアームと、前記モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸毎の指令通りに動作させるための位置制御部および速度制御部を備えたロボットの制御装置において、自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトルク指令信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉トルク信号作成部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を、自軸の指令と他軸から作用する干渉力の計算値力求める非干渉位置信号作成部とを備えたことを特徴とするものであるこの第 1の構成においては、干渉力計算部と非干渉トルク信号作成部と非干渉位置信号作成部を有し、擬似モデルの計算と擬似モデル用の制御の計算は行わず、且つ、モータトノレク信号だけではなぐモータ位置信号を干渉を考慮して作成することで、計算量が大幅に低減する。

[0015] 本発明の第 2の構成は、軸が 2軸である場合の干渉について、前記干渉力計算部における干渉力は、次式

d =B*x * s

isl refl

d =A*x *s²

is2 ref2

によって計算し、

前記非干渉トルク信号作成部における非干渉トルク信号は、次式

t =ひ *J /Kl*s⁴+ l +J )s²) *x

refffl ml LI ml LI refl

-(] /Kl*s²+1) *d

ml is2

t = l *J /K2*s⁴+ l +J )s²) *x _ϋ /K2*s²+1) *d

m2 isl

によって計算し、

前記非干渉位置信号作成部における非干渉位置信号は、次式

X = i /Kl*s² + 1) *x -1/Kl*d

refffl LI refl is2

x = Ci /K2*s² + 1) *x -1/K2*d

ref— ffi L2 ref2 isl

によって計算することを特徴とする。

但し、 J :1軸目モータ慣性モーメント

ml

J :1軸目アーム慣性モーメント

1

Κ1:1軸目減速機ばね定数

J :2軸目モータ慣性モーメント

m2

J :2軸目アーム慣性モーメント

2

K2: 2軸目減速機ばね定数

A( = B) :2軸の構成、成す角度、幾何学的関係から求まる係数

s:ラプラス演算子

d :1軸目から 2軸目に作用する干渉力

isl

d :2軸目から 1軸目に作用する干渉力

is2

X :1軸目位置指令

refl

X :2軸目位置指令

ref2

この第 2の構成においては、干渉力、非干渉トルク信号、および非干渉位置信号を計算する数式が特定される。

また、本発明の第 3の構成は、前記、非干渉トルク信号作成部および非干渉位置信号作成部の処理で使用する、他軸から作用する干渉力の計算値は、軸が 2軸を超える場合、各軸から自軸へ作用する干渉力計算値の総和であることを特徴とするものである。

この第 3の構成においては、複数軸間に干渉力が作用する場合に、自軸に対しても複数の軸から干渉力を受ける。その場合、 2軸間の干渉力の総和を計算することにより、自軸に作用する干渉力を求めることができる。

発明の効果 [0017] 本発明の第 1の構成によれば、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようにモータを動作させることができ、結果として干渉の影響を受けないで、指令どおりロボットアームを動作させることができる。また、非干渉化を実現するための計算を非常に少なくできる。その結果、 CPUの計算量を大幅に低減することができるため、安価な CPUを用いることができる。

本発明の第 2の構成によれば、干渉力、非干渉トルク信号、および非干渉位置信号を計算する数式を特定することにより、ロボット制御を具体化することができる。また、第 3の構成によれば、自軸に対し複数の軸からの干渉力が作用する場合も、複数軸の関係を考慮して複雑な計算をすることなぐ各軸から自軸へ作用する干渉力の合計値を計算する処理を行うだけで、後は 2軸間の非干渉化と同様の処理を行うのみでよぐ簡単に複数軸間の非干渉化が可能になる。

また、考慮してレ、なかった軸間の干渉を追加する場合などもプログラム修正はほとんどなぐ各軸から作用する干渉力の合計値を計算する処理を追加するだけで簡単に全ての軸の非干渉化を実現することができる。

図面の簡単な説明

[0018] [図 1]本発明の第 1実施例の構成を示すブロック図である。

[図 2]本発明の第 2実施例の構成を示すブロック図である。

[図 3]干渉力の計算に必要な係数 A， Bを求める際の力学モデルである。

[図 4]単軸を 2慣性系とし、互いの干渉力を考慮した制御対象モデルである。

[図 5]従来のロボット制御装置の構成を示すブロック図である。

[図 6]従来のロボット制御装置のモデル制御器内部の処理を示すブロック図である。符号の説明

[0019] 1 位置制御部

2 速度制御部

3 微分器

4 微分器

5 非干渉位置信号作成部

6 非干渉トルク信号作成部 7 干渉力計算部

8 モータ

9 アーム

10 位置検出器

11 位置制御部

12 速度制御部

13 微分器

14 微分器

15 非干渉位置信号作成部

16 非干渉トルク信号作成部

17 干渉力計算部

18 モータ

19 アーム

20 位置検出器

38 モータ

39 アーム

30 位置検出器

51 モデル制御器

52L、 52U 状態観測器

53L、 53U フィードバック制御系（フィードバック制御器） 54L、 54U 補正量演算部

60L、 60U フィードバック制御系への出力される信号 100 1軸目ロボットの制御装置

200 2軸目ロボットの制御装置

300 n軸目ロボットの制御装置

500 干渉力加算器

発明を実施するための最良の形態

以下、本発明の実施の形態について図を参照して説明する実施例 1

[0021] 図 1は、本発明のロボットの制御装置を説明する全体構成図である。図において、 1 00は 1軸目ロボットの制御装置、 200は 2軸目ロボットの制御装置を表している。

1軸目ロボットの制御装置 100は、 1軸目位置指令 X と 1軸目モータ位置検出値 X

refl

と 2軸目から 1軸目に作用する干渉力計算値 d を入力し、制御対象が指令通り動 fbl is2

作するよう制御演算を行い、 1軸目トルク指令 t を出力する。 8は 1軸目モータを表し

refl

1軸目ロボットの制御装置 100から出力される 1軸目トルク指令 t により動作する。 9 refl

は 1軸目アームであり、モータ 8と減速機等を介して結合されている。 10は 1軸目モータ 8の位置 X を検出する 1軸目モータ位置検出器である。

i l

2軸目ロボットの制御装置 200は、 2軸目位置指令 X と 2軸目モータ位置検出値 X

ref2

と 1軸目から 2軸目に作用する干渉力計算値 d を入力し、制御対象が指令通り動 b2 isl

作するよう制御演算を行い、 2軸目トルク指令 t を出力する。 18は 2軸目モータを表

ref2

し 2軸目ロボットの制御装置 200から出力される 2軸目トルク指令 t により動作する。

ref2

19は 2軸目アームであり、 2軸目モータ 18と減速機等を介して結合されている。 20は 2軸目モータ 18の位置 X を検出する 2軸目モータ位置検出器である。

ί¾2

また、両軸とも、通常、出力されたトルク指令値 t は電流指令値に変換され電流ァ ref

ンプを経てモータが動作するが、その応答は無視できるほど十分に速ぐまた本発明では特に関係がないため省略している。

[0022] ここで、モータ 8, 18とアーム 9, 19を結合する減速機は、ばね要素を有するため、ロボットの制御装置 100およびロボットの制御装置 200の制御対象は、どちらも、モータ慣性モーメントとアーム慣性モーメントが、ばね要素で結合された 2慣性系と考えられる。

本実施例では、単軸ごとの制御対象を 2慣性系とし、互いの軸間の干渉を考えた図 4に示す伝達関数で表される構成を考える。

図 4中、各記号の意味は以下の通りである。

J : 1軸目モータ慣性モーメント

ml

J : 1軸目アーム慣性モーメント

1

Kl : l軸目減速機ばね定数 J :2軸目モータ慣性モーメント

m2

J :2軸目アーム慣性モーメント

2

K2： 2軸目減速機ばね定数

A, B: 2軸の構成、成す角度、幾何学的関係から求まる係数

s:ラプラス演算子

d :1軸目から 2軸目に作用する干渉力

isl

d :2軸目から 1軸目に作用する干渉力

is2

xl:アーム 1の位置

x2:アーム 2の位置

また、上記 A, Bは力学計算で求めることが可能である。

例えば 2つの軸が図 3に示すような関係の場合を考える。 1軸目と 2軸目の成す角度を Θとし、 1軸目と 2軸目の軸間の距離を Lで表し、各軸、回転中心から質量中心までの距離を Ll、 L2で表し、それぞれの質量を Ml, M2とすると、 A, Bは式（9)のようになる。

A = B = M2*L2*L*cos( Θ ) +M2*L2²'.' (9)

[0023] 以下、実際に 1軸目ロボットの制御装置 100内部の構成を説明する。

図 1において、 1は 1軸目の位置制御部であり、 1軸目非干渉位置信号 X と 1軸目

refffl 位置検出値 X がー致するよう、それらの差である 1軸目位置偏差 errlを入力し、制

ί¾1

御演算を行い、 1軸目速度指令 V を出力する。

refl

位置制御部 1内の処理はどのように行っても良レ、が、例えば式（10)に示すように 1 軸目位置ループ比例ゲイン K を用いて比例制御を行えばょレ、。

i

V =K *errl …（10)

refl 1

2は 1軸目速度制御部であり、 1軸目非干渉速度信号 V と 1軸目の速度検出値 V refffl

がー致するように、式（11)で計算される 1軸目速度偏差 V を入力し、制御演算を fbl errl

行い、 1軸目トノレク指令値 t を出力する。

reffbl

V =V +V — V · · · (11)

errl ref— ffl refl fbl

[0024] 速度制御部 2内の処理はどのように行っても良レ、が、例えば式（12)に示すように 1 軸目速度ループ比例ゲイン K および 1軸目速度ループ積分ゲイン Kを用いて比例積分制御を行えばよい。

t =K * (1+K /s) *v ··· (12)

ref_fb 1 vl ll errl

3と 4は微分器を表す。

5は 1軸目非干渉位置信号作成部であり、 1軸目位置指令 X と 2軸目力、ら 1軸目へ

refl

作用する干渉力計算値 d を入力し、干渉力 d が作用しても、 1軸目位置指令 X 通

is2 is2 refl りにアーム 9先端 xlが動作するための 1軸目モータ位置信号を図 4に示す 2軸の干渉モデルの関係を用いて作成し、 1軸目非干渉位置信号 X として出力する。 X

reff l ref ffl の計算式を式（13)に示す。

X = i /Kl*s²+1) *x _1/Kl*d ··· (13)

refffl Ll refl is2

[0025] 6は 1軸目非干渉トルク信号作成部であり、 1軸目位置指令 x と 2軸目から 1軸目

refl

へ作用する干渉力計算値 d を入力し、干渉力 d の影響が作用しても、 1軸目位置

is2 is2

指令 χ 通りにアーム 9先端 xlが動作するための 1軸目モータトルク指令信号を図 4 refl

に示す 2軸の干渉モデルの関係を用いて作成し、 1軸目非干渉トルク信号 t として

refffl 出力する。 t の計算式を式（14)に示す。

refffl

t =Q *J /Kl*s

refffl ml Ll +Cl +J )s ) *x

ml Ll refl

-Q /Kl*s²+1) *d ··· (14)

ml is2

7は 1軸目干渉力計算部であり、 1軸目位置指令 X を入力し、 1軸目から 2軸目へ

refl

作用する干渉力 d を計算し出力する。図 4から分かるように、 d は式（15)で計算さ

isl isl

れる。

d =B*x *s²''.(15)

isl refl

最終的に 1軸目ロボットの制御装置 100からモータ 8へ出力される 1軸目トルク指令値 t は以下の式（16)で計算される。

refl

t =t +t ·'·(16)

refl refifl ref ft 1

[0026] 次に、 2軸目ロボットの制御装置 200内部の構成を説明する。

図において、 11は 2軸目の位置制御部であり、 2軸目非干渉位置信号 X と 2軸目

ref_ff2 位置検出値 X がー致するよう、それらの差である 2軸目位置偏差 eir2を入力し、制

fb2

御演算を行い、 2軸目速度指令 V を出力する。

ref2

位置制御部 11内の処理はどのように行っても良レ、が、例えば式（17)に示すように 2軸目位置ループ比例ゲイン K を用いて比例制御を行えばょレ、。

2

V =K *err2 ··· (17)

ref2 p2

12は 2軸目速度制御部であり、 2軸目非干渉速度信号 V と 2軸目の速度検出値

ref ff2

v がー致するように、式（18)で計算される 2軸目速度偏差 v を入力し、制御演算 fb2 err2

を行い、 2軸目トルク指令値 t を出力する。

ref_fb2

V =v +v v ··· (18)

err2 ref ff2 ref2 ί¾2

[0027] 速度制御部 12内の処理はどのように行っても良レ、が、例えば式（19)に示すように 2軸目速度ループ比例ゲイン Κ および 2軸目速度ループ積分ゲイン Κを用いて比

v2 i2

例積分制御を行えばよい。

t =K * (1+K /s) *v ··· (19)

ref fb2 v2 i2 err2

13と 14は微分器を表す。

15は 2軸目非干渉位置信号作成部であり、 2軸目位置指令 X と 1軸目から 2軸目

ref2

へ作用する干渉力計算値 d を入力し、干渉力 d が作用しても、 2軸目位置指令 X

isl isl ref2 通りにアーム 19先端 x2が動作するための 2軸目モータ位置信号を図 4に示す 2軸の干渉モデルの関係を用いて作成し、 2軸目非干渉位置信号 X として出力する。 X

ref_ff2

の計算式を式（20)に示す。

ref_ff2

X = ϋ /K2*s²+1) *x _1/K2*d ·'·(20)

ref_ff2 L2 ref2 isl

[0028] 16は 2軸目非干渉トルク信号作成部であり、 2軸目位置指令 x と 1軸目から 2軸目

ref2

へ作用する干渉力計算値 d を入力し、干渉力 d の影響が作用しても、 2軸目位置

isl isl

指令 χ 通りにアーム 19先端 x2が動作するための 2軸目モータトルク指令信号を図 ref2

4に示す 2軸の干渉モデルの関係を用いて作成し、 2軸目非干渉トルク信号 t とし

ref_ff2 て出力する。 t の計算式を式（21)に示す。

-( /K2*s²+1) *d · · · (21)

m2 isl

17は 2軸目干渉力計算部であり、 2軸目位置指令 X を入力し、 2軸目から 1軸目

ref2

へ作用する干渉力 d を計算し出力する。図 4から分力、るように、 d は式（22)で計算

is2 is2

される。

d =A*x *s ··· (22) 最終的に 2軸目ロボットの制御装置 200からモータ 18へ出力される 2軸目トルク指令値 t は以下の式（23)で計算される。

ref2

t =t +t · · · (23)

ref2 ref_ff2 ref_i¾2

[0029] このように、干渉力が作用しても、各軸アーム 9, 19が各軸指令通りに動作するように、モータ位置信号と、モータトノレク指令信号を作成し、それらの通りモータ 8, 18が動作するようにフィードバック制御を行う構成にしているので、軸間干渉があった場合も、各軸アーム 9， 19は自軸の指令通りに動作することができる。

本発明が特許文献 1と異なる部分は、擬似モデルやモデル用の制御器を有さず、非干渉位置信号作成部と非干渉トルク信号作成部とを備え、モータトルク補正信号だけではなく、モータ位置信号も干渉を考慮して作成する部分である。

実施例 2

[0030] 図 2は第 2実施例の構成を示す図である。本実施例が実施例 1と異なる点は、実施例 1では 2軸の干渉を考慮していたのに対し、本実施例では複数軸間に干渉力が作用する場合を考慮している点である。

図 2に示すように、 n軸で構成されたロボットに関して考える。ここでは、 2軸目力 n 軸目それぞれから 1軸目へ干渉が作用している場合の 1軸目の非干渉化を例に取り説明するが、他の軸間の干渉に関しても全く同様に実現できる。

図中、 38は n軸目モータを表し n軸目ロボットの制御装置 300から出力される n軸目トルク指令 t により動作する。 39は n軸目アームであり、モータ 38と減速機等を介し

refn

て結合されている。 30は n軸目モータ 38の位置 X を検出する n軸目モータ位置検 fbn

出器である。

500は干渉力加算器であり、 2軸目から 1軸目に作用する干渉力計算値 d 力 n軸

is2 目から 1軸目に作用する干渉力計算値 d までの干渉力の総和 d を求める処理を isn is— ail

式（24)に示すように行う。

d =d + d + - - - +d - - - (24)

is— all is2 is3 isn

ここで、 d 力 d までは、実施例 1で図 3を用いて説明したのと同様に、 1軸目から

is2 isn

n軸目までの各軸の構成、成す角度、幾何学的関係から求まる。

[0031] このように、複数軸間に干渉が作用する場合でも、それぞれの軸から自軸へ作用する干渉力計算値の合計値を入力とし、後は、実施例 1と同様の処理を行うだけで、簡単な構成で計算量も少なくすべての軸間の非干渉化を行うことが可能である。また、軸が増えた場合や、考慮していな力た干渉を新たに考慮する場合なども、各軸の処理は全く変更することなぐ各軸間の干渉力計算値の和を計算する部分を追加修正するだけなので、簡単に対応できる。

産業上の利用可能性

本発明は、複数軸間で干渉力が作用しても、計算量は少なぐロボットの非干渉化が実現できるため、軸数の多いロボットの非干渉化制御や、安価な CPUを用いた口ボットの制御装置などの用途にも使用できる。

Claims

請求の範囲

[1] 互いに干渉する複数軸から構成されるロボットであって、モータと、前記モータに減速機等のばね要素を介して結合されたアームと、前記モータの位置を検出するモータ位置検出器とから構成された各軸を、各軸毎の指令通りに動作させるための位置制御部および速度制御部を備えたロボットの制御装置において、

自軸の指令から他軸に作用する干渉力を計算で求める干渉力計算部と、他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータトノレク指令信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉トルク信号作成部と、

他軸から作用する干渉力がある場合も自軸が指令どおり動作するようなモータ位置信号を、自軸の指令と他軸から作用する干渉力の計算値から求める非干渉位置信号作成部と

を備えたことを特徴とするロボットの制御装置。

[2] 軸が 2軸である場合の干渉について、

前記干渉力計算部における干渉力は、次式

d =B*x * s

isl refl

is2 ref2

によって計算し、

efl

-( /Kl*s²+1) *d

-( /K2*s²+1) *d

m2 isl

によって計算し、

X = (J /Kl*s² + 1) *x _1/Kl*d

refffl LI refl is2

x = (J /K2*s² + 1) *x _1/K2*d

ref— ffi L2 ref2 isl

によって計算することを特徴とする請求項 1記載のロボットの制御装置。但し、 J :1軸目モータ慣性モーメント

ml

J :1軸目アーム慣性モーメント

1

κι:ι軸目減速機ばね定数

J :2軸目モータ慣性モーメント

m2

J :2軸目アーム慣性モーメント

2

K2: 2軸目減速機ばね定数

A( = B) :2軸の構成、成す角度、幾何学的関係から求まる係数

s:ラプラス演算子

d :1軸目から 2軸目に作用する干渉力

isl

d :2軸目から 1軸目に作用する干渉力

is2

X :1軸目位置指令

refl

X :2軸目位置指令

ref2

前記非干渉トルク信号作成部および非干渉位置信号作成部の処理で使用する、他軸から作用する干渉力の計算値は、軸が 2軸を超える場合、各軸から自軸へ作用する干渉力計算値の総和であることを特徴とする請求項 1記載のロボットの制御装置