JPH01267706A - ロボットの制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 【産業上の利用分野】

本発明は、作業空間の直交座標系からロボットのジヨイ
ント座標系への逆変換の解析解が存在しないロボットの
制御方法に関する。

【従来技術】

従来、６軸の多関節ロボットにおいて、第６軸軸線が第
４軸軸線に対して一定量だけオフセットされたオフセッ
ト手首を用いたものが知られている。 このオフセット手首を用いたロボットは手首曲げ軸の動
作範囲が広くとれるという利点があるが、作業空間の直
交座標系からロボットのジヨイント座標系への逆変換を
行うための解析解が存在しないため、ジヨイント座標系
での解を求めるのに数値解析等の複雑な処理を必要とす
る。

【発明が解決しようとする課題】

逆変換の解析解が存在しない場合には、その−例として
、ヤコビ行列を用いて近似解を求める方法がある。ヤコ
ビ行列は、ジヨイント座標系での微小変化量と直交座標
系における微小変化量とを一次近似するための変換行列
であり、逆行列が存在する。従って、現在のジヨイント
座標からヤコビ行列とその逆行列を求め、ヤコビ行列の
逆行列を用いて直交座標系における微小変位に対応する
ジヨイント座標系での微小変位を求めることができる。 そして、現在のジヨイント座標をその微小変位だけ補正
すれば、直交座標系において微小変位した位置及び姿勢
に対応するジヨイント座標を得ることができる。 ところが、ヤコビ行列を用いる方法は、近似解であるた
め、変位が大きくなる程、誤差が大きくなる。従って、
上記の変換を１回行っただけでは、直交座標系における
目標の？ＩｉＭＪ値に制御することができない。このた
め、目標の補間値に対する微小変位がＯに収束するため
には、何度もヤコビ行列による変換を行う必要があった
。又、最初の変位が大きいと、多数回のヤコビ行列によ
る変換を行っても目標とする補間値に収束させることが
できない場合もある。 更に、上記のヤコビ行列を用いる方法は、かなり大変な
計算を必要とするため、上記処理を何度も実行して補間
値へ収束させることは、演算時間を多く要することとな
り、実際には使用し難い。 本発明は、上記の課題を解決するために成されたもので
あり、その目的とするところは、目標とする補間値に対
する変位が大きくても、高速且つ高精度な位置決めを可
能とすることである。

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

上記課題を解決するための発明の構成は、補間値に近接
した位置及び姿勢を実現するジヨイント座標における近
似解を求め、その近似解を順変換して得られる直交座標
系における近似値の前記補間値に対する位置及び姿勢の
偏差を求め、前記近似解からヤコビ行列を求め、そのヤ
コビ行列と前記位置及び姿勢の偏差とからジヨイント座
標における角度偏差を求め、前記近似解を前記角度偏差
で補正して目標角度を求め、その目標角度でロボットの
制御軸を制御するようにしたことである。

【作用】

位置及び姿勢制御の目標となる補間値に近接した位置及
び姿勢を実現するジヨイント座標が補間値の近似解とし
て解析的に演算される。そして、その近似解を順変換し
て直交座標系における近似値が演算され、目標の補間値
との偏差が演算される。又、ジヨイント座標の近似解か
らヤコビ行列が演算され、その逆行列が求められる。そ
して、その逆行列を用いて直交座標系における偏差に対
応したジヨイント座標系における角度偏差が演算される
。その後、近似解がその角度偏差だけ補正されて目標角
度が演算され、その目標角度にロボットの位置及び姿勢
が一致するように制御軸が制御される。 上記のように、本発明は、補間値に近接したジヨイント
座標系での近似解を解析的に求めており、ヤコビ行列に
よる変換を１回行うだけであるので、高速な位置決めが
可能となる。又、ヤコビ行列による変換は、目標の補間
値に近接した値に対して実行されるため、偏差が小さく
、位置決め精度が高い。 一′１９

【実施例】

以下、本発明を具体的な実施例に基づいて説明する。 第１図は６ＩＩＩＩ多関節ロボットの機構を示した機構
図である。■がロボット本体であり、フロアに本体１を
固定するベース２が配設され、ベース２上にはコラム３
が固設されており、コラム３はボディ４を回転自在に配
設している。ボディ４はアッパーアーム５を回動自在に
軸支し、アッパーアーム５は、フォアアーム６を回動自
在に軸支している。ボディ４、アッパーアーム５、フォ
アアーム６は、それぞれ、サーボモータＭｌ、Ｍ２．Ｍ
３（第２図参照）によって、第１軸−１，第２軸１１２
゜第３軸Ｗ、の回りに回転駆動される。この回転角はエ
ンコーダＥｌ、Ｅ２．Ｅ３によって検出される。 又、フォアアーム６は、第４釉Ｗ４の回りにも回転可能
であり、フォアアーム６の先端部にはりストアが第５軸
Ｗ、の回りに回転可能に軸支され、さらに、リストアの
先端部８は第６軸Ｗ、の周りに回転自在に構成されてい
る。そして、この先端部８に工具（図示路）が取り付け
られる。尚、第４軸ｗ４、第５軸Ｗ１、第６軸Ｗ、１Ｌ
ｔ−ホモ−９Ｍ４、Ｍ５、Ｍ６によって駆動される。 第２図はロボットの姿勢制御装置の電気的構成を示した
ブロックダイヤグラムである。２０はマイクロコンピュ
ータ等から成る中央処理装置である。この中央処理装置
２０には、メモリ２５、サーボモータを駆動するための
サーボＣＰＵ２２　ａ〜２２ｆ、ジョグ運転の指令、教
示点の指示等を行う操作盤２６が接続されている。ロボ
ットに取付けられた各軸Ｗ、〜１１６駆動用のサーボモ
ータＭ１〜Ｍ６は、それぞれサーボＣＰＵ２２ａ　〜２
２ｆによって駆動される。 前記サーボＣＰＵ２２ａ〜２２ｆのそれぞれは、中央処
理装置２０から出力される出力角度データθ１〜θ６と
、サーボモータＭ１〜Ｍ６に連結されたエンコーダＥ１
〜Ｅ６の出力α１〜α６との間の偏差を演算し、この演
算された偏差の大きさに応じた速度で各サーボモータＭ
１〜Ｍ６を回転させるように作動する。 前記メモリ２５にはロボットを教示点データに従って動
作させるためのプログラムが記憶されたＰＤＡ領域２５
１とロボットの位置と姿勢を表す教示点データを記憶す
るＰＡ領域２５２が設けられており、教示モードにおい
て、複数の教示点における位置データと姿勢データが記
憶される。 次に、本装置の作用を説明する。 第６図に示すように、教示点Ｔ１．　Ｔ２．−において
、ロボット１の工具先端の位置と姿勢が４×４の行列（
以下、姿勢と位置とを示す行列を「位置行列」という）
により記憶されている。そして、教示点間は、補間によ
り得られた位置及び姿勢に制御される。例えば、教示点
ＴＩと教示点Ｔ２の間には、補間点Ｎ１．　Ｎ２．　Ｎ
３等が存在する。そして、その各補間点での位置は教示
点Ｔ１と教示点Ｔ２を例えば直線で結んで等分すれば、
直線補間による補間点Ｎ１の位置座標が求められる。又
、補間点Ｎ１での姿勢行列は教示点Ｔｌから教示点Ｔ２
への姿勢変化に対して回転主軸と回転角とを求め、その
回転角を補間点の数だけ等分して補間回転角を求め、教
示点Ｔ１における姿勢行列にその補間回転角だけ回転さ
せる姿勢変換行列を掛けて、補間点での姿勢行列を求め
ることができる。 このように、現先端の経路にふける補間点での位置と姿
勢とを表した位置行列が既に演算され記↑１＠されてい
るとして、第３図に示すフローチャートに従って処理が
実行される。ステップ１００では、その補間目標点に近
接した位置及び姿勢を実現するためのジヨイント座標が
演算される。その近接点（近似解、以下近接点という）
を３１とする。その演算方法は、第４図〜第８図を参照
して説明される。 第４図に示すように、各座標系の原点０．〜０゜が図示
するようにとられており、座標系○、−ｘ１Ｙ、（以下
単に、「座標系０１Ｊと記す。他の座標系についても同
様）は、第１軸りが回転した時に姿勢が変化し、同様に
座標系０２〜０６は、それぞれ、第２軸ｖ４２〜第６軸
Ｗ６の回転に伴って姿勢を変化するようにとられている
。又、ロボットを作業空間に固定された固定座標系０−
ＸＹＺ　（以下、「固定座標系０」と記す）のＺ軸方向
に直立させた状態で、フォアアーム６とリストアとの関
係は、第８図に示すように、第４軸Ｗ、と第６軸Ｗ６と
が平行になり、第４軸Ｗ４は原点０からＹ軸方向にＣだ
けオフセットし、第６軸Ｗ、は原点０から−Ｘ軸方向に
Ｂだけオフセットした状態になっている。 先ず、現在位置のジヨイント座標は各エンコーダＥ１〜
Ｅ６の出力を読み取ることにより求めることが出来る。 今、現在位置を教示点Ｔ１とする。 近接点Ｓ１における各軸を解析的に求めるのであるが、
近接点Ｓ１における第６軸１ｌＩ６の角度θ６が現在位
置Ｔ１における第６軸Ｗ６の角度θ６に等しい、即ち、
第６軸１１６は移動中回転しないと仮定する。すると、
座標系０６から座標系０．への逆変換行列Ａｓ−’は、
次式で与えられる。 又、座標系０６の固定座標系Ｏを基準とする位置行列Ｈ
は、次式で定義される。 の位置ベクトルである。 すると、座標系０．の位置行列には次式で与えられる。 Ｋ＝Ｈ−Ａ、−’ 従って、第４軸イ、と第５軸Ｗ、との交点をリスト点り
、（Ｌ、、　Ｌ、、Ｌ、）とすれば、Ｌ１＝−Ａ１ａ１
＋　ｐ＋”（Ｌ　　−ＣＯ８θ　ｇ−０＋　　−１Ｓｌ
ｎ　　θ　＠）　ＩＢ＋（ｒ＋、−５ｉｎθ、＋Ｏ１’
ＣＯ８θ、）−ＣＬｘ’　Ａφｔｌｚ＋Ｐ２＋（ｎｚ・
ＣＯ９０ｇ−０２・Ｓ：ｎθＧ）ＩＢ＋（ｎｚＴｓｉｎ
θａ＋０２’ｃＯ８θｇ）’ＣＬｓ＝−Ａ−ａｓ＋Ｐ３
＋（ｒ＋３＋ｃｏｓθ＊−０ｆｆ’９Ｉｎθ、）・Ｂ＋
（ｎ３・ｓｉｎθｇ＋０３’ｃＯ３θｇ）　−にこで、
リスト点Ｌ０から固定座標系ＯのＸＹ平面への投影を考
えると、第５図のようになる。図から理解されるように
、第１軸角度θ１が０度のとき、リスト点Ｌ０はＹ＝Ｃ
を通りＸ軸に平行な直線上を動く。又、逆に、θ２〜θ
６を固定して、θ１のみを変化させると、リスト点Ｌ０
は、半径「の円上を動く。 従って、ｊａｎ　（θ１＋α）＝− Ｌ＋ 、、θ＋　＝−ａ　＋ａｔａｎ２（Ｌｌ、　Ｌ＋）とな
る。 一方、ｒｌｃｏｓ（−−α＞＝Ｃ 従って・ となり、θ１がリスト点Ｌ０の座！（Ｌｚ、Ｌｌ）を用
いて求められたことになる。 又、各座標原点の位置関係を示した第７図において、 ；＝ｆｆ＝（−Ｃ−ｓｉｎθ＋、　　Ｃ−ｃｏｓθ８，
０）６＝［が＝　（０，Ｏ，Ｄ） Ｌ、＝（Ｌ、、Ｌｌ、Ｌ、） イ・届、；・部＜　ｒＴ＋　＝　Ｅ、　ｌテトＦ）ご・
Ｇ己、Ｔ−；−ｉ：・（Ｌｌ・Ｃ・５．。θＩ＋　Ｌ、
−Ｃ０゜。５θ１゜し、−〇） ｌＵｚＯｚＬｏ−ｙ　、　ｌｕ＋Ｕ２Ｌｏ＝α、　１Ｏ
ｉＵｔＬｏ＝βとする。 三角形Ｕ、ＬＬ、に対して余弦定理を用いれば、１　０
、Ｌ、　Ｉ　　’＝　ｌ　　ｕ、ｏ、　ｌ　　ｔ＋ｌ　
　Ｕ、Ｌ、　＋　　２−２１ＬＤ２１　１Ｌ１２Ｌ０１
　　　ｃｏＳＢＩ　ＵａＬｏ　ｌ　２・Ｉ　１１，０．
１　”　＋　ｏ−ｉ、−１’４１　　ＬＯｉ　ｌ　　ｌ
　　ＯｚＬ＋　ｌ　　ｃｏｓｒ又、ＩＩ　＋　Ｌｏ＝Ｌ
−０１１＋ ＝（Ｌ＋＋Ｃ−５ｉｎθ＋、Ｌｌ−Ｃ−ｃｏｓθ、、　
Ｌ、）であるから、三角形Ｕ、Ｕ、Ｌ、に余弦定理を用
いて、１富１′＝１℃−１′＋１不キｌ′ −２ｌ　Ｕ、Ｏ２ｌ　１１１Ｊｏ　ｌ　ｃｏｓα従って
、 （Ｌ＋＋Ｃ−５ｉｎθ＋）”＋（ｔ、＊−（：　・ｃｏ
ｓθ　）　２　＋　１．　、２＝０２＋（Ｌ＋＋Ｃ−５
ｉｎθ　＋）’（Ｌｌ−Ｃ−ｃｏｓθ　ｌ）２＋　（Ｌ
＝−Ｄ）’−２０１ロスｌ、ｌ　　ｃｏｓα−Ｌｓ ここで、 ｌ　ＵｚＬｏ　Ｉ　＝　（（Ｌｒ＋Ｃ−５ｉｎθ＋）’
＋（Ｌ−Ｃ−ｃｏｓθｌ）２”（Ｌ−−Ｄ）’）　　１
／フ よって、上記のα、β、Ｔとθ２＝π−（α＋β）。 θフ；π−ｒから、θ１．θ、が求められる。 次に、θ１．θ７．θ１．θ６より、θ４．θ１．を求
める。固定座標系Ｏから座標系０１への変換行列をＡ＋
、座標系０、から座標０２への変換行列をＡ２゜座標系
０．から座標系０６への変換行列をＡ６とする。 Ａ＋Ａ２Ａ３Ａ＜Ａｓ＝に＝ｔｌＡｉ−’とすると、 より、θ４＝ａｔａｎ２（−ｍ＋＋ｍ２）θ、＝ａｔａ
ｎ２（Ｊｓ、　＋＋ａ） このようにして、近接点Ｓ１におけるジヨイント座標で
の近似解（θ９．θ２．θ３．θ４．θ１．θ６）が得
られる。 次に、ステップ１０２へ移行して、順変換を行い、近接
点Ｓ１での位置行列が演算され、ステップ１０４におい
て、直交座標での近接点Ｓ１と補間点Ｎ１との位置及び
姿勢の偏差Δｅ、が演算される。 そして、ステップ１０６にふいて近似角θ、〜θ６を用
いてヤコビ行列Ｊとその逆行列Ｊ−’が計算され、次式
によりジヨイント座標系における各軸の角度偏差Δθ１
が演算される。 Δθ、＝Ｊ−１・Δｅ。 そして、ステップ１０８へ移行して、近接点Ｓ１におけ
る各軸角度（θ１．θ２．θ５．θ１．θ２．θ６）を
偏差角度Δθ、だけ補正し、補正後の角度が目標の補間
値Ｎ１におけるジヨイント座標での値とされる。 そして、そのジヨイント座標での角度に各制御軸が制御
されることになる。 上記実施例では、近似解を求めるのに、第６軸を固定し
たが、第１軸を固定したり、他の複数の軸を固定したり
しても良い。

【発明の効果】

本発、明は、補間値に近接した値の近似解を解析的に求
め、その近似解から決定されるヤコビ行列の逆行列によ
り、近似解に対応した直交座標系での値と目標の補間値
との偏差からジヨイント座標系での角度偏差を求めて、
近似解を補正した値を補間値に対応するジヨイント座標
での解としている。従って、近似解を解析的に求めるこ
ととヤコビ行列を使用するヤコビ法を１回使用すること
がらヤコビ法を複数回実行するよりも演算速度が速いと
いう効果がある。又、ヤコビ変換は目標の補間値に接近
した近似解に対して行われるので、微小変位が小さく補
間値に対応した正確な角度が演算されるので、位置決め
精度が向上する。

【図面の簡単な説明】

第１図は本発明の実施例方法を使用したロボットを示し
た斜視図。第２図はロボットの制御装置の構成を示した
ブロック図。第３図はロボットの制御装置のＣＰＵの処
理手順を示したフローチャート。第４図、第５図、第６
図、第７図及び第８図は、実施例方法を説明するための
説明図である。 １　ロボット　２−ベース　３−コラム４　ボディ　５
°゛アツパーアーム ６゛フオアアーム　？−リスト ８　゛先端部　Ｌｏ　　リスト点 特許出願人　　豊田工機株式会社 代　理　人　　弁理士　藤谷　修 第１　図 第３図 第５図 第６図

Claims (1)

1. 【特許請求の範囲】 直交座標系で表現されたロボットのハンド先端位置と姿
   勢の補間値によりロボットを制御し、直交座標系からジ
   ョイント座標系への逆変換の解析解が存在しないロボッ
   トの制御方法において、前記補間値に近接した位置及び
   姿勢を実現するジョイント座標における近似解を求め、 上記近似解を順変換して得られる直交座標系における近
   似値の前記補間値に対する位置及び姿勢の偏差を求め、 前記近似解からヤコビ行列を求め、そのヤコビ行列と前
   記位置及び姿勢の偏差とからジョイント座標における角
   度偏差を求め、 前記近似解を前記角度偏差で補正して目標角度を求め、 その目標角度にロボットの制御軸を制御するロボットの
   制御方法。