JPH01171004A - ロボットの制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） この発明は予測制御によりロボットアームの軌道制御を
行なうロボットの制御装置に関するものである。

（従来の技術とその問題点） 第３図は従来のロボット制御装置を示すブロック図であ
る。図において、アーム位置制御系１０はロボットアー
ムを駆動するサーボモータ２０の制御を行なうための速
度・位置フィードバックを備えた位置ＩＩＪ御系であり
、１自由度当りの制御系を示したものである。アーム位
置制御系１０は、位置指令値θ、。ｆと位置帰還値θ「
との差をとる減口器１１と、減算器１１の出力を増幅す
る増幅器１２と、増幅器１２の出力を一定周期毎に保持
するサンプルホールド回路（図中ｒｓ／１−（Ｊと略す
。）１３と、サンプルホールド回路１３の出力値Ｖ　と
速度帰還値Ｖ、との差をとる減算器１４と、減算器１４
の出力を増幅してサーボモータ２０に電流指令値Ｉ、を
与える増幅器１５と、サーボモータ２０の位置現在値θ
　を速度帰還値■。

に変換する変換器１６′と、位置現在値θ。を位置帰還
値θ、に変換する変換器１７とを備えている。なお、図
中の増幅器１２．１５および変換器１６’、１７に示す
記＠Ｋ　　、Ｋｖ、に２．に３は、それぞれの比例ゲイ
ンを示している。また、ロボット制御装置全体は、複数
自由度のそれぞれに対応したサーボモータ２０について
図に示すアーム位置制御系１０をそれぞれ備えており、
個々のアーム位置制御系１０に位置指令値θ　　を与ｅ
ｒ えるため、エンドエフェクタの目標位行父を個々のサー
ボモータ２０の位置（回転角）指令値θ、。

、に変換する座標変換器１′を有している。ただし、目
標位置Ｘは、所定の目標軌道Ｃ上の位置である。

上記のようなアーム位置制御系１０は、ロボットが１自
由度のみの場合には十分精度の良い位置制御が可能であ
る。しかし、ロボットが複数の自由度を有する場合には
各自由度の軸が相互に干渉するので、個々のアーム位置
制御系１０にそれぞれの位置指令値θ　　を与えるだけ
ではエンドエｅｒ フェクタを、目標軌道Ｃに沿って精度良く制御すること
が不可能であった。特に、近年盛んに用いられているダ
イレクトドライブモータは減速ギヤを用いておらず、負
荷側のトルク外乱がそのままモータの外乱となるので、
多軸の相互干渉によるモータのトルク外乱を補償する必
要性が高い。

このような多軸干渉を補償するため、多軸相互間の運動
方程式を考慮した予測制御を行なうロボット制御装置が
従来から知られている。第４図はこのような予測制御を
行なうロボットの制御装置を示すブロック図である。図
に示す予測制御系はエンドエフェクタの目標位置Ｘを各
自由度毎の目標位置θ・、目標速度θｉおよび目標加速
度θ１■ に変換する座標変換器１″と、座標変換器１″の出力を
受けてサーボモータ２０へのトルク指令値τ、。ｆを演
輝するトルク演算器３１と、サーボモータ２０のトルク
を検出してトルク帰還値τ「を得るトルク検出器４２と
、トルク指令値τ　　とｅｆ トルク帰還値τ、との差をとる減算器４３と、減算器４
３の出力を対応するトルク指令電流値Ｉ。

に変換する変換器４４（比例ゲインＫｔ）を備えている
。この制御装置では、前記トルク指令電流値Ｉ、をサー
ボモータ２０に与えることにより行なわれる制御が主要
なものであるが、さらに位置・速度フィードバックを行
うために第３図と同様なアーム位置制御系１０’が設け
られている。トルク指令電流値Ｉｔと、位置・速度フィ
ードバックにより求めた電流指令値Ｉ、とは加算器４５
で加算されてサーボモータ２０に与えられる。このよう
なトルク指令値τ　　に基づく位置制御系でｅｆ は、多軸相互の干渉力、遠心力、コリオリカ、重力等を
考慮した運動方程式に基づいてトルク演算器４１が各サ
ーボモータ２０に与えるべきトルク指令値τ　　を演算
する。

ａｆ ところが、サーボモータ２ｏは回転リップル。

モータ鉄損、サーボモータ２０内に組込まれているＰＷ
Ｍ制御回路のトルク不感帯等に基づく非線型性が大きい
。従って、主要な指令値であるトルク指令電流値■１を
非線型性の大きなサーボモータ２０に入力する制御系で
は精度良い制御が困難であるという問題があった。また
、正確なトルク帰還値τ「を得るために必要な精度良い
トルクセンサを得ることが難しく、それによって制御精
度に限界があるという問題もあった。

（発明の目的） この発明は上記のような問題点を解決するためになされ
たもので、ロボットアームの複数自由度の相互干渉を補
償できるとともに、トルクセンサが不要でかつ制御精度
の良いロボット制御装置を得ることを目的とする。

（目的を達成するための手段） この発明のロボット制御装置は、複数の自由度のそれぞ
れについて駆動手段が設けられるとともに、前記駆動手
段に対応して設けられた駆動制御手段に制御入力を与え
ることによってロボットのエンドエフェクタの軌道制御
を行なう装置として構成されており、この装置には、前
記アームの相互干渉を無視した場合に前記制御入力とし
て与えるべき基準制御入力値を、前記エンドエフェクタ
の所望の軌道から算出された前記アームの制御目標値に
基いて求める基準制御入力値導出手段と、前記干渉によ
って前記駆動手段の駆動状態に生ずる影響を補償するた
めに前記駆動手段が出力すべき一般化補償力の値を前記
制御目標値に基づいて求める一般化補償力導出手段と、
前記駆動手段に前記一般化補償力を出力させるために前
記制御入力として与えるべき制御入力補償値の値を求め
る制御入力補償値導出手段と、前記基準制御入力値と前
記制御入力補償値とを合成して前記駆動制御手段へ前記
制御入力として与える合成手段とが設けられている。

（実施例） Ａ、ロボットの制御装置の構成 第１図は、本発明の一実施例によるロボットの制御装置
を示すブロック図である。図においてアーム位置制御系
１０は第３図に示した従来のロボット制御装置における
アーム位置制御系１０と同じものであるが、サーボモー
タ２０を詳細なブロックで示している。すなわち、サー
ボモータ２０は電流指令値Ｉ　と電流帰還値Ｉ、との差
をとる減算器２１と、減算器２１の出力を増幅して電圧
□指令値■、とする増幅器２２と、電圧指令値■、と電
圧帰還値ｖｆとの差をとる差分器２３と、サーボモータ
２０の巻線２４とを備えている。そして、サーボモータ
２０の電流現在値■。はブロック２５においてトルク現
在値τ。に変換される。

また、電流現在値Ｉ。により発生するトルクτ。

はブロック２６においてトルク外乱Δτが減算され、減
惇されたトルク（τ。−Δτ）は、ブロック２７で速度
現在値θ。に変換される。速度現在値θ。はブロック２
８で位置現在値θ。へと変換される。また、サーボモー
タ２０は、電流現在値１ｃを電流帰還値■、に変換する
変換器２９と、速度現在値θ　に基づく起電力を電圧帰
還値Ｖ。

とする変換器３０とを備えている。また、以上の増幅器
２２．ブロック２５．変換器２９．３０はそれぞれ比例
ゲインＫ・、に、、に１．ＫＢを有している。また、図
における巻線２４の記号は巻線抵抗Ｒと巻線インダクタ
ンスＬを示し、ブロック２７の記号はモータのイナーシ
ャＪを示している。

アーム位置制御系１０の他の構成は前述の通りであり、
減算器１１．増幅器１２．サンプルホールド回路１３．
減算器１４．増幅器１５．変換器１６．１７を備えてい
るとともに、増幅器１２゜１５および変換器１７．１８
はそれぞれ比例ゲインＫ　　、Ｋ　　、Ｋ　　、に３を
有している。

Ｖ２ さらに、この実施例によるロボットの制御ＩＩ装置では
、ロボットのエンドエフェクタの目標位置Ｘを各自由度
の局所座標系における目標位ｎθｉ。

らをまとめて「制御目標値」と呼ぶ。）に変換する座標
変換器１を備えている。この制御目標値θ。

θ、θのうち、目標位置θの値は指令位置変換器（アー
ムモータアンプ逆特性フィルタ）４に与えられる。この
指令位置変換器４は、複数の自由度（アーム）の相互の
干渉を無視して、目標位置θの値から、アームの駆動υ
制御のための基準制御入力値（具体的には、基準位置指
令値θ　　、）をｒｅｆ、＋ 求める基準制御入力値導出手段を形成している。

また、第１図の装置には、干渉補償のための補償トルク
演算器２と補償トルク変換器７とが設けられている。こ
れらのうち、補償トルク演算器２は、上記干渉によって
サーボモータ２０の駆動状態に生ずる影響を補償するた
めの補償トルクΔτ（一般化補償力）の値を、制御目標
値θ、θ、θに基づいて求める一般化補償力導出手段を
形成している。

さらに、補償トルク変換器３は、サーボモータ２０が補
償トルクΔτに相当するトルクを付加的に出すための位
置補償値Δθ　　・　（制御入力補ｒｅｆ、＋ １値）を求める制御人力補ｒ！ｉ値導出手段を形成して
いる。そして、基準位置指令値θ　　、と位置ｒｅｆ、
＋ 補償値へ〇　　、とは、加算器５（合成手段）にｒｅｆ
、＋ おいて加締（合成）され、アーム位置制御系１０に制御
入力信号として与えられる。

Ｂ、ロボットアームの運動方程式 次に、上記のロボットの制御装置において、制御目標値
θ、θ、θから補償トルクΔτｉを求めるための方程式
について説明する。

一般に、多自由度を有するロボットアームの方程式は、
ラグランシュの運動方程式により以下のように表わされ
る。

・・・（１） ここで、τｉは自由１ｉｉの一般化力としてのトルク、
θ・、θにはそれぞれ自由度ｊ、にの一般化変位として
の関節角を示す。また、ロボットはｎ個の自由度を有し
、１≦ｊ≦ｎかつ１≦に≦ｎとする。また、記号上の点
（ドツト）は時間微分を示す。さらにＨｉｊ、ｈｉｊｋ
はそれぞれ自由度ｉ。

」又はｉ、ｊ、にの組合せごとに定まる係数、０、は重
力項を示す。

なお、（１）式の右辺第１項は慣性力、第２項は遠心力
とコリオリカ、第３項は重力を表わす項である。

この運動方程式によるトルクτｉを、当該自由度ｉの自
己のみの慣性力τ１、とその他の干渉力（以下「補償ト
ルクＪと呼ぶ。）Δτ・とに分割する。

＝τ・・＋Δτ、　　　　　　　　　　・・・（２）従
って、 τ・・＝Ｈ・・θ・　　　　　　　　　　　・・・（３
）＋１１１１ 以上の自己の慣性力τ・・と補償トルクΔτ１とのうら
、自己の慣性力τｉｉは、アーム位置制御系１０に基準
位置指令値θ　　・を与えることによｒｅｆ、　＋ り実現される。但し、基準位置指令値θ　　、とｒｅ（
＋ して与えるべき値は、アーム位置制御系１０の伝達関数
Ｇ（Ｓ）を考慮して決定する必要がある。

すなわち、アーム位置制御系１０への入力が基準位置指
令値θ　　、のみであり、出力が位置現在ｒａｆ、＋ 値θ。どなるとき、両者に次式の関係が成立つ。

θ　−Ｇ（Ｓ）θ、。ｆｊ　　　　　　　　・・・（５
）に れを変形して θ　　、＝　　　　００　　　　　　　　・・・（６）
ｒｅｆ・’　　Ｇ（Ｓ） このため、干渉を無視した状態で位置現在値θ　を目標
位置θ・に一致させるには、θ０＝θ、とじて（６）式
により求めたθ　　、をアーム＋　　　　　　　　　　
　　　　　ｒｅｆ、＋位置制御系１０に与える必要があ
る。そこで、第１図に示す実施例では座標変換器１によ
って求められた目標位置θ、を、アーム位Ｅ　ｆｉｉ制
御系１０の伝達関数Ｇ（Ｓ）の逆特性を有する変換器（
アームモータアンプ逆特性フィルタ）４に入力すること
によって、位置指令値θ　　・を得ている。

ｒｅｆ、＋ 一方、補償トルクΔτｉは、補償トルク演算器２によっ
て上記（４）式に基づいて求められる。また、これをア
ーム位置制御系１０内部の増幅器１２からブロック２５
までの間の伝達特性に基づいて位置補償値Δθ　　・に
変換し、アーム位置制ｒｅｆ、　＋ 御系１０に与えている。すなわち、サーボモータ２０の
軸トルクに対する外乱としての補償トルクΔτｉは、ア
ーム位置制御系１０における位置指令値θ　　、の外乱
としての位置補償値Δθｒｅｆ。

ｒｅｆ、＋ 、に単純換算でき、以下の式で表わされる。

上記（７）式に基づいて、補償トルクΔτｉを変換器３
により位置補這値へ〇　　・に変換する。

ｒｅ（＋ 以上のようにして求められた基準位置指令値θ、。７．
１と位置補償値Δθ　　・を加剪器５によｒｅｆ、＋ り加算してアーム位置制御系１０に与えることにより、
上記（１）式で示した運動方程式を考慮した予測制御を
行うことができる。

Ｃ０水平２自由度ロボットにおける　　１次に、上記の
予測制御を水平２自由度を有するロボットに適用した場
合について説明する。

第２図は水平２自由度ロボットのロボットアームの機構
を示す図である。図において、第１と第２のアームＡ１
．Ａ２はそれぞれモータＭ１．Ｍ２により駆動され、そ
の位置は回転角θ　、θによって規定される。また、第
２のアームＡ２の先端にはエンドエフェクタＥが設けら
れている。

なお、第１と第２のアームＡ１．Ａ２はそれぞれ質ｆｆ
ｉ　ｍ　１　、　ｍ　２および有効長ｌ　、１２を有す
るとともに、モータＭ、Ｍ、２のそれぞれの中心から各
アームＡ、Ａ２の重心までのそれぞれの距離がＳ　、Ｓ
２であるものと仮定する。

第１と第２のモータＭ、Ｍ２でそれぞれ駆動される部分
についてのラグランシュの運動方程式は（１）式を用い
て次のように表わせる。

”１”Ｈ１ｔθ１＋Ｈ１２θ２ 一２ｍ　　Ｉ　　Ｓ　　ｓｉｎθ　・θ　θ・θ　　・
・・（８） 第２　ｊ！１８２　　Ｓｉｎθ２２ τ　＝Ｈθ　＋Ｈ２２θ２ ４ｍ　　Ｉ　　Ｓ　　　ｓｉｎθ　ＣＯ１−（９）２１
２　　　ま ただし Ｈ１１＝ｍ１８″１ ４ｍ　　＜１′″＋Ｓ＋２１１Ｓ２ＣＯ５θ２）＋　１
１＋　ＩＨ１＋　１２　　　　　　　　　・・・（１０
）Ｈ＝ｍ　　（Ｓ　　＋ｊ！１８２　　ＣＯ３θ、２）
＋−１２・・・（１１） Ｈ２２＝　ｍ　２　Ｓ　２　＋　Ｉ　２　　　　　　　
　　・＝　（１２）なお、■　、■　はそれぞれ第１と
第２の７−ムＡ１．Ａ２のそれらの重心に関する慣性モ
ーメント、Ｉ　はモータＭ１のイナーシャであり、他旧 方のモータＭ２のイナーシャは第２およびＩ２に内に含
めている。なお、このロボットは水平２自由度であるた
め（１）式第３項の重力項は存在しない。

（８）式、（９）式で示されるトルクτ　、τ　をそれ
ぞれ自己のみの慣性力τ　、τ　と補償トルりΔτ１．
Δτ２に分割する。まず、第１のモータＭ１て駆動する
部分については次のようになる。

’ｒ：　１＝　ｒ　１すΔτ１　　　　　　・・・（１
３ａ）”　１１＝Ｈ１１θ１　　　　　　　　　　・・
・（１３ｂ）Δτ　＝Ｈ１２θ２ 一２ｍ　　　Ｉ　　　Ｓ　　　ｓｉｎθ　　・θ　　θ
−ｍ　　１　　３　　　ｓｔｎθ　　・θ・・・（１３
ｃ） なお、補償トルクΔτ１としては（１３ｃ）式で与えら
れるものに（１０）式第２項の変動分を含めたものとし
てもよい。すなわち、 Δτ′＝Δτ＋２ｍ　　Ｉ　　Ｓ　　ｃｏｓθ　・θ・
・・（１３Ｃ’） を補償トルクとしてもよく、この場合には、（１３ｂ）
式の係数Ｈ１１から上記変動分が取除かれる。つまり、
このときには、静的な干渉（すなわち、第２のアームＡ
２が存在すること自体による第１のモータＭ１への影Ｙ
１）と、動的な干渉（第１と第２のアームＡ、Ａ２が相
対運動することによる第１のモータＭ１への影響）の双
方が、補償トルクΔτ′１の中に含まれている。これに
対して、（１３ｃ）式の補償トルクΔτ１は、動的な干
渉のみを含んでいる。換言すれば、この発明において叫
、すべての干渉を分離する必要はない。

第２のモータＭ２で駆動される部分については次のよう
になる。

τ２＝τ２２＋Δτ２　　　　　　　　　・・・（１４
ａ）・・・（１４ｃ） 上記（１３ｂ）式と（ｉ４ｂ）式に示す各々のアームＡ
　　、Ａ　　の自己のみの慣性力で　、τ　についでは
、前記（６）式に従ってそれぞれの一面角θ１゜θ２を
アーム位置制御系１０の逆特性で変換して基準位置指令
値θ　　、を求め、これをアーム位ｒｅｆ、＋ 置制御系１０に与えればよい。すなわち、θ　　“＝□
θ・　　　　　　　・・・（１５）ゝ””　　Ｇ（Ｓ）
　　’ ただし、ｉ＝１．２である。

また、アーム位置制御系１０の伝達関数Ｇ（Ｓ）は次式
で表わされる。

Ｇ　（Ｓ）−に、ＫｖＫ、に、／ （Ｋ、ＫｖＫ、に、に３ ＋　（ＫＶＫｉ　Ｋ、に２　＋に８に、）Ｓ＋（Ｒ＋に
、に１）ＪＳ　　＋ＪＬＳ　　）・・・（１６） 特にディジタル制御を行なう場合には、（１１）式をＺ
変換した伝達関数Ｇ（Ｚ、）を次のように表わすことが
できる。

・・・（１１） ここで、ｍｏ、ｄｏ−ｄ３は定数である。従って、と表
わすことができる。ここでり、は定数である。

（１８）式を（１５）式に代入すれば次式となる。

これを時刻ｎにおけるサンプル値θ・　（ｎ）で表■ わせば次式のようになる。

θｒｅｆ、ｉ　（ｎ）＝　、；ｏｏ、ｅ、　（ｎ−１＞
・・・（２０） 従って、（２０）式に基づいて変換器４により位置目標
値θ・を基準位置指令値θ　　、に変換すれば、ｌ　　
　　　　　　　　ｒｅｆ、　＋ それぞれの軸における自己のみの慣性力に対応した基準
位置指令値θ　　・を得ることができる。

ｒｅｆ、　＋ 一方、補償トルクΔτ１に対応した位置補償値Δθ、。

（ｉを求めるには、まず補償トルク演算器２により（１
３ｃ）式又は（１４ｃ）式に基づいてそれぞれのモータ
Ｍ、Ｍ２の補償トルクΔτｉを演算する。次に、これを
（７）式に代入して位置補償値Δθ、。ｆ、ｔを求めれ
ばよい。

特に、ディジタル制御を行なう場合には、（７）式をＺ
変換して次式を得る。

＝１／Ｋ　（１＋ＮＺ−１）・Δθ ｒｅｆ、ｉ ・・・（２１） ここで、■はサンプリング周期、Ｋ、Ｎは定数である。

これを時刻ｎのサンプル値Δτ、（ｎ）で表わせば、次
式を得る。

Δθ　　・　（ｎ） ｒｅｆ、＋ ＝に一Δｒ−（ｎ）十ＫＮΔｒ・　（ｎ−１）・・・（
２２） 従って、（２２）式に基づいて変換器３により補償トル
クΔτ、を位置補償値Δθ　　・に変換すれ＋　　　　
　　　　　　ｒｅｆ、　＋ ば、それぞれの軸における干渉力に対応した位置補償値
Δθ　　・を得ることができる。

ｒｅｆ、　＋ 以上のようにして求められた位置指令値θ、。、。

、と位置補償値へ〇　　、を加算器５により加算＋　　
　　　　　　　ｒｅｆ、＋ してアーム位置制御系１０に与えれば、ロボットアーム
の運動方程式（（１）式）に基いた予測制御を行なえる
。従って、複数自由度の相互干渉を補償しつつ各サーボ
モータ２０の位置制御を行なえるので、エンドエフェク
タＥの軌道制御を精度良く行なうことができる。また、
このとき用いるアーム位置制御系１０は、通常の位置制
御系を用いることができるので、精密なトルクセンサが
不要であるという利点がある。

Ｄ、変形例 この発明は上記実施例に限定されるものではなく、次の
ような変形も可能である。

■上記実施例ではアーム位置制御系１０には基準位置指
令値θ　　、と位置補償値Δθ　　・をｒｅｆ、　＋　
　　　　　　　　ｒｅｆ、　＋加算して入力することと
しているが、位置補償値Δθ、。（ｉの寄与は基準位置
指令値θ　　・に比ｒａｆ、＋ べて小さいので、基準位置指令値θ　　・を入力ｒｅｆ
、＋ する頻度（例えば１μｓに１回）に比べて位置補償値Δ
θ　　・を入力する頻度は少なくてもよいｒｅｒ、　＋ （例えば３〜４μｓに１回）。

■サーボモータ２０としてダイレクトドライブモータを
用いたロボットでは、複数自由度相互の干渉力がモータ
のトルクに影響しやすいので、この発明の効果が特に大
きいが、ダイレクトドライブモータに限らず、他のタイ
プのモータを使用している場合や、直動アクチュエータ
を駆動手段として含むロボットにも適用可能である。

■上記実施例では水平２自由度を有するロボットへの適
用する例を示したが、これに限らず、−般の複数自由度
を有するロボットに適用できることはいうまでもない。

（発明の効果） 以上説明したように、この発明によればロボットアーム
の複数自由度間の相互干渉によるトルクを補償する補償
トルクを演算し、当該補償トルクに応じた位置補償値を
加味した制御入力値を駆動制御系に与えてエンドエフェ
クタの軌道を補償するようにしたので、複数自由度の相
互干渉を補償できるとともに、トルクセンサが不要でか
つ制御精度が良いという効果がある。

【図面の簡単な説明】

第１図は本発明の一実施例を示すブロック図、第２図は
本発明の一実施例を適用するロボットアームの構成を示
す図、 第３図及び第４図は従来のロボットの制御ｌｌ装置を示
す図である。 １・・・座標変換器、　　　２・・・補償トルク演算器
、３・・・補償トルク変換器、４・・・指令位置変換器
５・・・加算器、　　　　１０・・・アーム位置制御系
、２０・・・サーボモータ

Claims (1)

【特許請求の範囲】
1. （１）複数の自由度を有するロボットのアームの各自由
   度毎に設けられた駆動手段を、前記駆動手段に対応して
   設けられた駆動制御手段に制御入力を与えることによつ
   て駆動制御し、それによって前記ロボットのエンドエフ
   ェクタの軌道制御を行なうロボットの制御装置であって
   、 前記アームの相互の干渉を無視した場合に前記制御入力
   として与えるべき基準制御入力値を、前記エンドエフェ
   クタの所望の軌道から算出された前記アームの制御目標
   値に基いて求める基準制御入力値導出手段と、 前記干渉によつて前記駆動手段の駆動状態に生ずる影響
   を補償するために前記駆動手段が出力すべき一般化補償
   力の値を前記制御目標値に基づいて求める一般化補償力
   導出手段と、 前記駆動手段に前記一般化補償力を出力させるために前
   記制御入力として与えるべき制御入力補償値の値を求め
   る制御入力補償値導出手段と、前記基準制御入力値と前
   記制御入力補償値とを合成して前記駆動制御手段へ前記
   制御入力として与える合成手段とを備えることを特徴と
   するロボットの制御装置。