JPS5983209A - ロボツトの制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、ロボットの制御方式に関し、特に多自由度の
ロボットアームの先端を任意の軌跡に沿って精度よく動
かすための制御方式に関するものである。

〔従来技術〕

ロボットの制御方式としては連続軌跡制御方式がよく知
られている。この連続軌跡制御方式は、ロボット先端の
辿るべき軌跡を適当に補間し、補間によ、！ｌ）　ｉ＠
られた軌跡上の各点を目標値として、所定時ｉｄ１間隔
で次々と指示していくことにより、目標軌跡に沿って連
続的にロボット・アームを制御するものである。このと
き指示された目標点にロボットの先端を位置決めするた
めには、ロボットの各軸を協調的に動作させる必要があ
る。

このロボットの各軸の励調動作制却方法の１つを、次に
述べる。

いま、例として、６自由度の多関節形ロボットを、直交
座標系にしたがって制御する場合を考える。

直交座標系で表わしたロボットの先端位置をＸとし、ロ
ボットに固有の座標系で表わしたロボットの関節角度を
θとすると、 ｘ＝（ｘｙｚ）Ｔ　　　　　　　　　・・・・・・・・
・（１）θ＝（θ１θ２θ３θ４θＳθ６）　　　・・
・・・・・・・（２）（ただし、Ｔは転置行列を示す） で表わされる。

このとき、ロボットの先端位置Ｘは、ロボットの関節角
度θを用いて、 Ｘ＝Ｆ　（θ）　　　　　　　　　　・１曲（３）で表
わされる。

すなわち、 Ｆ”　（ｆｔ　ｆ２ｆ３）　　　　　　　　・相開（５
）である。

このロボットの関節角度θがら先端位置Ｘへの変換Ｆを
、座標変換と呼ぶ。

ここで、ロボットの先端位置Ｘを実現するための各関節
角度θを求めるためには、（３）式をθについて解く必
要がある。このとき、未知数６個に対して、方程式が３
個しかないため、一般的には手先姿勢を制御する３軸θ
４．θ５．θ６を所定の値に保持し、すなわち手先姿勢
を手首に対して一定に保持することにして、未知数を３
個に減少し、θｌ・０２・θ３に関して３元連立方程式
を解くことになる。

すなわち、（３）式をθについて解くと、θ＝Ｇ（Ｘ）
　　　　　　　　　　　曲・曲（６）つまり、 （但し、Ｇ＝　（ｇｔ　ｇ２ｇ３））となる、このロボ
ットの先端位置Ｘがら関節角度θへの変換Ｇを逆座標変
換と呼ぶ。

（６）式の逆座標変換関数Ｇを用いて、ロボットの先端
位置Ｘを目標位置Ｘｒに位置決めするロボットの軌跡側
両方式をブロック図にょシ示すと、第１図のようになる
。

第１図において、１ｏ１は逆座標変換手段Ｇであシ、軌
跡に沿って刻々と出力される目標位置ｘｒを協調的に実
現する目標関節角度θ、を算出スル。サーボ増幅器１０
２と関節駆動−Ｅ−−ｚｔｏａと位置決め用フィードバ
ック信号θを発生する工／コーダ１０４から構成される
装置制御サーボ機構は、上記目標関節角度θｒにしたが
って、ロボットの各関節位置をθｒに位置決めする。

ここで、（３）式における関節角度θｒを先端位置Ｘに
変換する座標変換関数Ｆは、一般的には三角関数を含む
非線形関数となるため、ロボットの機構的特徴を利用し
た特殊な解法や、大形計算機によらなければならないニ
ュートンラプソン法等を用いた数値解法以外には、先端
位置Ｘから関節角度θを求める方法がない。なお、先端
立置Ｘから関節角度θを求める方法として、上記のよう
な複雑な解法によらない方式が提案されているし特開昭
５３−１２１３６２号公報参照）。

この制御方式は、（３）式を０１．θ２．θ３で偏微分
して、 ｘ＝Ｊ・θ　　　　　　　　　　　　・・・・・・・・
・（９）但し、 を導き、これから θ＝Ｈ・文　　　　　　　　　　・相開（２）を求め、
この関係式を用いて近似的にロボットを制御しようとす
るものである。

但し、ＨばＪの逆行列であυ、 Ｈ（θ）＝Ｊ（θ）−１・曲面α養 なる関係がある。

すなわち、ａＪ式を用いて、 θ≧００　（初期定数）　　　　　　・・聞・・・θ９
と近似し、あらかじめＨ（θ。）を計算しておき、実際
にロボットを動作させるときに、 θ≧Ｈ（θｏ）１文　　　　　　　　・四囲側なる近似
式を用いて、目標とするロボットの移動方向、つまりロ
ボットの目標位置速度ベクトル文・から、目標関節速度
ベクトルｏｒを求め、この速度ベクトルを操作量として
、ロボットの軌跡を制御しようとするものである。

第２図に、上記の制御方式のブロック図を示す。

第２図において、２０１はロボットの目標位置Ｘ・から
目標位置速度ベクトル文Ｔを算出する手段であり、２０
２はこの目標位置速度ベクトル文ｒを（ＩＦ５式にもと
づいて目標関節速度ベクトルθｒに逆座標変換する手段
である。サーボ増幅器２０３と関節駆動モータ２０４お
よび速度制御用フィードバック信号θを発生するタコジ
ェネレータ２０５から構成される速度制御サーボ機構に
より、上記目標関節速度ベクトルθｒにしたがってロボ
ットの各関節速度θが追従することになる。

上記制御方式は、きわめて簡単な計算にもとづくもので
あるため、ロボットのリアルタイム制御に滴してはいる
が、ロボットの先端位１σＸに対する各関節角度θを近
似式α６）にもとづいて計算しているため、先端位置Ｘ
を初期位置Ｘ、＝Ｆ（θ０）から犬きく移動させると、
この近似誤差が増大し、目標とする軌跡から大きく外れ
てしまうという欠点がある。

この入点を改善するため第３図に示すような軌跡制御方
式が考えられる。この制御方式は、近似式（１６）では
なく、０３）式そのものを用いてロボットを制御するも
のである。すなわち、ロボットにおける時々刻々の関節
角度θを検出し、この値を用いてＧ（１１式および０Ｌ
１）式からロボットのその時の姿勢に応じた逆座漂変換
行列Ｈ（θ）を計３１．することにより、ロボットの制
御精度を高めるものである。

第３図において、３０１はロボットの目標位置Ｘ・から
目標位置速度ベクトル文、を算出する手段であｐ、３０
２はロボットの関節位置θを検出するエンコーダ３０６
の出力信号にもとづき、この目標位置速度ベクトルＸｒ
をθ〔式、（１４式および（１３）式にしたがって目標
関節速度ベクトルｄｒに逆座標変換する手段である。サ
ーボ増幅器３０３と関節駆動モータ３０４および速度制
御用フィードバック信号θを発生するタコジェネレータ
３０５から構成される速度制御−ナーボ機構によシ、上
記目標関節速度ベクトルθＴにしたがって、ロボットの
各関節速度θが追従することになる。

ここで、ポイントとなる逆座標変換関数Ｈ（θ）は、０
１式で示される座標変換関数Ｊ（θ）の逆行列であるた
め、機械的な演算操作によシ比較的簡単に求めることが
できる。したがって、この制御方式は、ロボットの機構
に左右されない一般的な方法として取扱うことができる
。

第３図に示す制御ｎ方式によって、ロボットの軌跡精度
はかなシ改善されたが、実際のサーボ機構には種々の損
失が存在し、またマイクロ・コンピュータ等を用いてこ
の制御則を実施する場合、計算時間に起因する時間遅れ
によって操作速度ベクトルに誤差が生じるため、ロボッ
トが指示された軌跡どおりに動作しないという現実的な
問題がある。

〔発明の目的〕

本発明の目的は、これら従来の制御方式における欠点を
改善するため、ロボットの機構に左右されず、サーボ機
構系に存在する種々の損失を補償し、かつｍ算時間によ
る遅れに起因する位置誤差等も補償して、ロボットの高
精度の連続軌跡制御を行うことができるロボットの制御
方式を提供することにある。

〔発明の概安〕

本発明のロボットの制御方式は、複数の動作軸から構成
されるロボットの制御方式において、所定の空間座標系
で指示された移動目標位置と、ロボット固有の座標系で
の各軸の位置変位量を前記空間座標系に変換した現在位
置との偏差量を、速度ベクトルに変換して偏差速度ベク
トルとし、空間座標系で表わされた該偏差速度ベクトル
をロボット固有の座標系に逆変換し、逆変換されたもの
を操作（ベクトルとしてロボットを所望の軌跡に沿って
動作させることに特徴を有する。

〔発明の実施例〕

以下、本発明の動作原理と実施例を、図面にょシ説明す
る。

本発明のロボット制御方式では、Ｑ３）式で示される速
度制御則を用い、かつその目標位置速度ベクトルをロボ
ットの実際の現在位置Ｘと目標位置ｘｒとの偏差ベクト
ルとすることにょシ、サーボ機構に発生する種々の損失
に起因するロボットの目標軌跡からのずれを補償し、軌
跡精度を向上させることができる。すなわち、いま、所
望の移動軌跡に沿って、所定の時間間隔ΔＴどとに刻々
と目標位置Ｘｒ（ｔ）が指令されるものとすると、次式
％式％ （） ング周期） また、ロボット固有の座標系において検出した現在位置
θを、（３）式に示す座標変換式Ｆで所定の空間座標系
に座標変換したロボットの現在位置をＸ　（ｔ）とする
。すなわち、 Ｘ（ｔ）＝Ｆ（θ（ｔ））　　　　　　　　　・・・・
・曲りηと表すことができる。

このとき、（１３）式における所定の空間座標系での目
標速度ベクトル火ｒ（ｔ）トシテ、 文、　（ｔ）＝Ｋ　・（Ｘ　、（ｔ）−Ｘ（ｔ）　）　
　　　　　−・・−・−（１母が与えられる。

（但し、Ｋはに１　＋　ｋ２＊　ｋ３を要素とする対角
行列である。） （旧式は、目標速度ベクトル文・（１）として、所定の
空間座標系におけるロボットの目標位置とロボットの実
際の現在位置との偏差ベクトルを与えることｋ　；（＋
Ｌ：味している。すなわち、所定の空間座標系でのロボ
ットの目標位置Ｘ　ｒ　（ｔ）からの位置ずれを補正す
るように目標速度ベクトルＸｒ（ｔ）’を決定している
わけである。

次に、（１８式で決定された所定の空間座標系での目標
速度大・（１）をα１式にしたがって、ｊ、（ｔ）＝Ｈ
（θ（ｔ）　）　・Ｘ　ｒ（ｔ）　　　　　−・−曲−
ａ９のように、ロボット固有の座標系における目標速度
θｒに逆座標変換する。このとき、変換係数行列Ｈ（θ
（ｔ））の値は、ロボット固有の座標系で、検出された
ロボットの現在位置θを用いてα〔式および０４式にし
たがって計算する。

その次に、この目標速度θｒを操作量として、速度制御
サーボ機構を駆動する。この速度制御サーボ機構は、一
般的には、速度指令値ｂｒとタコジェネレータ等を用い
て検出されたロボットの現在速度θとの偏差をサーボ増
幅器で増幅し、この増幅出力によってロボットの各軸を
動かすモータを駆動することによシ、ロボットの各軸を
指令速度δ・で動作するものである。

第４図は本発明の実施例を示すロボット制御方式のブロ
ック図である。

第４図において、４００はサーボ機構のサンプリング周
期ごとの目標位置ｘｒを発生する手段でろって、最終的
目標値ＸＴとそこに至る速度Ｖを入力することにより、
例えばソフトウェアによる線形補間を行って、時々刻々
の目標位置ｘｒを発生する。

４０７は、ロボットの各軸に取付けられたエン：ｆｆ−
ダ４０６によって検出されるロボット固有）座標系での
位置変位θから、α９式にしたがって所定の空間座標系
での現在位ｔｉｘに座標変換する手段である。４０１は
、動作軌跡上の目標位置ｘｒと現在位置Ｘの偏差ベクト
ルから、α神式にしたがって目標速度ベクトル文ｒを算
出する手段である。

４０２は前記目標速度ベクトル文ｒを、１式、（１４式
および１１式にしたがって、位置変位θをもとにロボッ
ト固有の座標系における目標速度ベクトルθｒに逆座標
変換する手段である。サーボ増幅器４０３と、各関節駆
動モータ４０４と一１速度制御用フィードバック信号θ
を発生するタコジェネレータ４０５とから構成される速
度制御サーボ機構は上記目標速度ベクトルθｒにロボッ
トの速度θを追従させる。

なお、各手段４００，４０１．・・・４０７の機能は、
周知のマイクロコンピュータにより、それぞれ実現する
ことができる。

なお、ここでは、所定の空間座標系におけるロボットの
現在位置Ｘを、ロボット固有の座標系における位置変位
θから座標変換Ｆを用いて算出したが、例えばテレビジ
ョン・カメラ等の視覚装置を用いて、直接空間座標系で
のロボットの現在位置Ｘを測定することも可能である。

第５図は、本発明の実施例を示すロボット機構の説明図
である。

このロボットは、３つの回転形関節（几１）５０１、（
Ｒ２）５０２．（几３）５０３と、これらを連接する３
つのリンク（Ｌｓ）５１１゜（Ｌ２）５１２．（Ｌｓ）
５１３から構成されている。

リンク５１１の基部は直交座標系ｘｙｚの原点に位置し
、その方向はＺ軸と一致している。また、リンク５１１
，５１２，５１３の長さはそれぞれ１１　、　ｔ２　、
　ｔｓであり、各回転軸几！　、几２゜Ｒｓの回転角は
それぞれＸ軸からθ！、リンク５１１からθ冨、リンク
５１２からＣ３である。

このとき、ロボットの先端位置Ｘ（ｘ、ｙ、ｚ）は＼ロ
ボットの各関節角度θ１．θ２．θ３によって、次式で
表わされる。

ｘ　＝：　（１４ｉｎθ２−１ｔ３ｓｉｎ（θ２＋θ３
））ｃｏｓθｒ　　　　−−・Ｑ’Ｉ　”　（ｔ２ｓｉ
ｎθ２　＋ｔｓｓｉｎ　（θ２＋θ３））ｓｉｎθｒ　
　　・＋・＋・＋４′Ｄｚ”　Ａｔ　＋　１２　ｃｏｓ
θ２　＋　Ａｓ　Ｃ０５（θ２＋θ３）・・・・・・・
・・＠ここで、 ｆｘ（θｌ、θ２．θ３）＝（ｔ２５１ｎθ２＋ｔ３＋
ｉｎ（θ２＋θ、））ｃｏｓθ１°°°°゛°°°・の ｆｚ（θ２．θ２．θ３　）　＝　（Ｌ２ｓｉｎθ２　
＋ｔ３ｓｉｎ（θ２＋６３））ｓｉｎθ１・・・・・・
・・・Ｃ４ ｈ（θ１．θ８．θ３）　＝ｔｌ＋ｔ、ｃｏｓθｚ＋ｔ
３ｃｏｓ（θ２＋θ３）・・・・・・・・・（２） とおく。次にｆ２１式、　Ｑ、１）式、３３式をそれぞ
れθ、。

θ２．θ３で偏微分すると、次のようになる。

但し、 となる。

ここで、次のように置くと、 但し、Ｄｓ　、Ｄ２　、Ｄ３　Ｄは行列式であって、次
のように表わされる。

ここで、次のように置くと、 ここで、ロボットの軌跡上の目標位置をｘｒとし、Ｘｙ
＝　（ｘｒ　ｙｒ　Ｚｒ）Ｔ で表わしたとき、目標速度ベクトル文ｒは火ｙ：（Ｘｙ
　　ｙｒ　　Ｚ・） で表わされるが、これを次のように定める。

次に、目標速度ベクトルＸｒを（３４）式より・・・・
・・（３６） のようにして逆座標変換し、目標速度ベクトルθｒとし
て、速度制御サーボ機構に入力する。

第６図は、第５図のロボットの制御方式を示すブロック
図である。

第６図において、６００はロボットの軌跡上の目標位置
ＸｒをＸｒ、　ｙｒ、Ｚｒに分けて発生する手段であシ
、最終的目標位置（Ｘ＋　ｙ＋　ｚ）ＸＴとそこに至る
までの速度Ｖを入力することにより、サーボ（幾構のサ
ンプリング周期で線形補間等の処理全行い、時々刻々と
変化する目標位置（Ｘｒ、ｙｒ、Ｚｒ）を発生する。６
７０はロボットの各関節５０１，５０２，５０３にそれ
ぞれ設置されたエンコーダ６６１，６６２，６６３によ
って検出されるロボットの関節角度θ１．θ２゜θ３か
ら式＋２（ｆ）、１２υ、＠にしたがってロボットの先
端位＋ｉｔＸ　（ｘ　、　ｙ　、　ｚ　）　を算出スフ
：、座標変換手段である。６１０は、軌跡上の目標位置
Ｘｒ（ｘｒ。

ｙｒ、Ｚｒ）と、ロボットの現在位置Ｘ（ｘ＋　ｙ＋２
）から、（３５）式にしたがって目標速度ベクトル文・
（ｘ・、ｙ・、ｔ・）を算出する手段である。

６２０は、前記目標速度ベクトル文・（ｘ・。

ｙｒ、Ｚｒ）を前記ロボットの関節角度θ（θ１゜θ２
．θ３　）をもとに、（３６）式にしたがって、ロボッ
トの目標関節速度θ（θ’ｌ＋θｒ２　、θｒａ　）に
逆座標変換する手段である。これらの各手段６００゜６
１０．６２０，６７０の機能は、マイクロコンピュータ
によって簡単に実現することができる。

サーボ増幅器６３１，６３２，６３３と、各関節駆動モ
ータ６４１，６４２，６４３と、速度制御用フィードバ
ック信号δ１．δ２＋ｊ３を発生するタコジェネレータ
６５１，６５２，６５３から構成される速度サーボ機構
は、ロボットの各関節速度ｊ（δｌ、δ２．δ３　）を
目標関節速度結果、ロボットの先端Ｘは、目標軌跡ｘｒ
に沿ってきわめて精度よく追従制御されることになる。

このように、本発明の制御方式では、ロボットの各関節
を直交座標系での先端位置の目標速度ベクトル文・によ
り指令する方式であるため、目標関節速度ベクトルｂｒ
への逆座標変換が比較的簡単で、かつ−膜化が可能であ
る。さらに、目標速度ベクトル文の決定に際しては、目
標位置Ｘｒとロボットの現在位置Ｘの偏差ベクトルを用
いて、実際のロボットの移動位置Ｘをフィードバックす
るので、サーボ機構で発生する種々の損失および各種演
算時間による遅れが原因となる位置誤差も補償すること
ができるようになシ、きわめて高精度で連続軌跡制御を
行うこ七ができる。

〔発明の効果〕

以上説明したように、本発明によれば、ロボットの機構
に左右されず、比較的簡単で、−膜化が可能な演算方法
を適用できるとともに、サーボ機構に存在する種々の損
失や演算時間による遅れに起因した位置誤差等も補償で
きるので、ロボットを高精度で連続軌跡制御篩すること
が可能である。

【図面の簡単な説明】

第１図、第２図および第３図はそれぞれ従来のロボット
制御方式を示すブロック図、第４図は本発明のロボット
制御方式の原理を示すブロック図、第５図は本発明の実
施例を示すロボットの機構斜視図、第６図は第５図にお
けるロボット制御方式を示すブロック図である。 ４００．６００・・・目標位置発生手段、４０１゜６１
０・・・速度ベクトル発生手段、４０２，６２０・・・
速度ベクトル逆座標変換手段、４０７，６７０・・・位
置変位の座標変換手段。

Claims (1)

【特許請求の範囲】
1. 複数の回転軸から構成されるロボットの制御方式におい
   て、所定の空間座標系で指示された移動目標位置と、ロ
   ボット固有の座標系での各軸の位置変位量を上記空間座
   標系に変換した現在位置との偏差量を、速度ベクトルに
   変換して偏差速度ベクトルとし、さらに空間座標系で表
   わされた上記偏差速度ベクトルをロボット固有の座標系
   に逆変換し、逆変換された上記偏差速度ベクトルを操作
   ベクトルとしてサーボ機構に入力することを特徴とする
   ロボットの制御方式。