JPS63276606A - 多関節型ロボットの軌跡制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は宇宙マニプレータ、海洋マニプレータ、原子力
用点検口ゴツト、補修ロボット等の多関節形ロボットの
軌跡制御方式に関する。

〔従来の技術〕

一般にロボットハンドの指令速度札（ｐＤ１　’　ＰＤ
２’・・・飢、）のロビットの関節の指令速度机（δｊ
Ｄ　ｔδ２Ｄ・・・ｂ６Ｄ）に変換する必要がある。そ
れにはハンドの作業変数Ｐと口？ットの関節変数θの間
に成立する。

ｐ＝ｆ（θ）　　　　　　　・・・（１）なる関係式を
利用し、この（１）式を時間微分すると、＝　　、ｙ　
　（ｓ）　　・耐〉　　　　　　　　　　　　　　　・
・・（２ンとなる。

ただしＪ　（ｅ）はｘ　（ｅ）のヤコビ行列が得られ、
ｄｅｔＪ（θ）ＮＯであればθ＝＝　Ｊ−１＜θ）・ｔ と解け、作業座標系Ｘ０−７゜−ｚｏで与えられたハン
ドの位置姿勢速度ｐが関節座標系での速度みに変更でき
る。なお、（２）　、　（３）式において、変数記号の
上のドツトは時間微分を示している。

ここで、第８図の制御装置および第９図のフローチャー
トによυ従来の多関節形ロボットの軌跡制御方式につい
て説明する。

第８図においてオペレータ１は操縦レノ々−２を操作し
、ハンドの作業座標系における指令速度介りと逆変換演
算器１００に入力する。

逆変換演算器１００では、第９図に示すフローチャート
に従って（３）式を解き関節座標系における指令速度へ
に変換しθ、軸〜θ６軸のサーがアンプ２１へ出力して
第４図に示すような多関節形口ぎットを制御する。第４
図に示す口ぎットでは、Ｊ（θ）は具体的には第６図の
ような６×６のヤコビ行列で表現され、ｄｅｔＪ（θ）
は ｄｅｔＪ（θ）　＝　ＤＥＴＯ と求められる。

但し　Ｓ２＝地θ２ Ｓ２３−出（θ２＋０３） Ｓ２３４−地（θ２＋０３＋０４） という略記法を用いている。

第８図において３１はモータ、４Ｉは速度検出器、５１
は関節角検出器であり、１１はこれらを総称したθ、軸
駆動系、１２．１３・・・１５．１６はそれぞれθ２軸
、θ３軸・・・θ６軸駆動系である。

〔発明が解決しようとする問題点〕

ただし、（３）式が解けるためには（４）式のｄｅｔＪ
（θ）ＮＯ である必要がある。ところが、ｄｅｔＪ（＆）　＝　Ｏ
となる特異点、及びその近傍においては例えば第５図（
ａｌの例ではリンク２及びリンク３の方向Ｕというよう
にハンドはある特定の方向へ動けなくなる。

これはロボットのリンク構成の問題で構成を変更例えば
６自由度から７自由度のロボットに変更しないと解決で
きない。しかし、そｎ以外の方向へは機構的には動作可
能であるにも拘わらず、（３）式を利用した制御、方式
では制御できなかった。

そこで、本発明は特異点及びその近傍においても、ある
特定方向以外の残りの方向へ制御でき、軌跡制御の機能
が向上し、操作性が向上する多関節形口？ットの軌跡制
御方式を提供することを目的とする。

〔問題点を解決するための手段〕

本発明は上記目的を達成するため、口ぎットのハンド指
令速度と作業座標の優先順位を入力してヤコビ行列式を
計算し各軸の関節速度を求める方式において、 （１）ヤコビ行列の所定の行と列を取除いた行列式のう
ち１ＤＥＴｉ（縮退ヤコビ行列）　ｌ＞’ｔ（適当な小
さな値）を満す入力した優先順位に従ってｌを求め、 （２）　　（１）で求めたｌに相当するヤコビ行列およ
びヤコビ行列式から関節速度を求める、 ことを特徴とする多関節形ロボットの軌跡制御方式であ
る。

〔作用〕

本発明は上記のようにしているので、特異点及びその近
傍においても、ある特定方向以外の残りの方向へ制御で
き、軌跡制御の機能が向上し、操作性が向上する。

〔実施例〕

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

第１図は本発明方式を実施するための制御装置の構成図
であり、第８図の従来例を異なるのは、特異点近傍であ
ることをヤコビ行列式の値より判定する判別器２００と
、作業変数の優先順位を設定するとともに、適当な関節
変数と組合わせて、縮退ヤコビ行列を作り、行列式を計
算し、最適な組合せを選択する逆変換演算器１１０でそ
れ以外の要素の構成及び作用は従来例と同じである。

各構成要素の作用を述べる前に一般的な原理を述べる。

従来制御できなかったｄｅｔ　Ｊ（θ）＝０となる特異
点とは第５図に示すような姿勢をロボットがとった時で
あり、この時ノ・ンドはある特定の方向へは動けなくな
る。これは機構的な問題であるため、口２ノドのリンク
構成自体を変更しないと解決できない。

しかしそれ以外の方向へは機構的には動作できる。それ
にも拘わらず制御できないのはひとことで言うと作業座
標系で動けなくなった自由度が関節座標系での冗長自由
度と力ってしまうためである。具体的に述べると、第５
図（ａｔでいえばハンドを九とえばＹ方向へ動かそうと
するとき、θ３軸のリンクｔ３が伸びきっているため、
θ２軸と０３軸のハンドに及ぼす作用が全ったく同じに
なっている。

即ちこの姿勢ではθ２又はθ３軸のいずれを動かせばよ
いかが（３）式からは決められないことを示している。

つまり（３）式は（２）式の６元連立１次方程式を解く
ことと同義であるが、特異点では独立な方程式が５つに
減っている（特異点の条件が重ならない時）。

そこで、み、〜ｂ６のうちどれか１つを定数とし、方程
式を５元連立１次に減らせば（２）式は解ける。

但し作業変数も１つ変数を減らす必要がある。

第５図に示す特異点ではθ２を定数とし後述のように行
列式を求めれば残りの関節変数は、独立であることがわ
かる。なお、θ１，０３〜θ６を冗長自由度に選ぶ場合
についての説明を省略する。

この時、５元連立１次方程式とすることは、第６図ヤコ
ビ行列から第２列と第１行を除くことに相当する。

いま、第１行を除いたヤコビ行列をＪｉと記し、ｄｅｔ
Ｊｌ　＝　ＤＥＴｉとおくと、ＤＥＴ　ｉは第７回に示
すように無効なりＥＴ６を除いて５つ得られる。

即ちＪｉを使用すれば、１番目の作業変数が制御できな
いがＤＥＴｉ）Ｏであれば、残りの作業変数に関して制
御できる。たとえば第５図（ａ）　＋　（ｂ）の場合は
第７図かられかるようにＤＥＴ　１〜ＤＥＴ５４０であ
るから、いずれでも制御できる。第５図（ｃｌの場合は
、ＤＥＴ　３〜ＤＥＴ５のいず九かで制御できる。

以上の考えに基いて判別器２００では現在のロボツトの
関節角θ、と指令速度か。を入力し第２図に示すフロー
チャートに従って演算を行う。第２図の２０１の指令速
度（速度指令値）Ｉ）Ｄはオペレータが入力する（与え
る）ものであり、またロボツトの関節角θ、はロボツト
の各軸の位置検出器から読み取られた実際の角度（位置
）である。

２０２．２０３においてＤＥＴＯ＝　ｄｅｔＪ（θ）よ
り特異点近傍の判定を行い、２０４においてＩＤＥＴＯ
Ｌ＜ε。

ならばハンド指令速度の優先順位（作業変数ｐ。

〜ｐ６のうちどれを制御するかの順位）の高いものから
ＤＥＴ　ｉを計算し、２０５においてｌ　ＤＥＴｉ　（
縮退ヤコビ行列）ｌ＞ｊｌ（適当な小さな値）となるｔ
と選択し、逆変換計算器ＪＪＯに出力する。

第２図の２０４における演算は、実際にロボツトを動作
させて％異点近傍に入ったらＤＥＴ　ｉを優先順位に従
って計算する。

優先順位が上のＤＥＴ　ｉが、１ＤＥＴｉｌ）εをみた
すとは限らないからである。ｌＤＥＴｉｌ）ｇ　　をみ
たすもっとも優先順位の高い１が判別器２００から出力
される。なお、２０４における作業変数間の優先順位は
予じめオ被レータが入力しておく。

逆変換計算器１１０では第３図のフローチャートの３０
１．３０２において判別器２００からの出力信号ｌに基
き、計算すべきＪｌを決め、逆変換の計算を行う。３０
３において１＝０の時は３０４において従来どおυ関節
速度が求められる。ｉ（０の時は３０５においてθ２軸
を冗長と選んでいるの６２Ｄの値）とし、残りの関節速
度ｂｌＤはよシ定める。

３０６において、３０４．３０５で求めた関節速度を１
１〜１６の各軸のサーボアンプへの指令値として出力す
る。

第３図において、ｉ　＝　０とは６×６のヤコビ行列を
表わし、捷たｉ＝１〜５は５×５の縮退ヤコビ行列を表
わしており、特異点は１ＤＥＴｌｌ＝０とな１ｏ− る時のことを示し、さらにＤＥＴｏ＝　ｄｅ　ｔ　Ｊ（
θ）＝０は６軸ア一ム時の特異点を示し、また、ＤＥＴ
ｉ（１＝１〜５）＝Ｏは５軸アームを想定した時の特異
点を示している。

なお、以上述べた実施例は特定の軸構成のものを例に説
明したが、他の軸構成でも可能である。

また以上述べた実施例はロボットのアームの他、ロボッ
トの脚にも適用できるものである。

〔発明の効果〕

以上述べた本発明によれば、特異点及びその近傍におい
ても、ある特定方向へ、は制御できないが、残りの方向
へは従来どおシ制御でき、ロボットの軌跡制御の機能が
向上し、さらにロボットの操作性が向上する多関節形ロ
ボットの軌跡制御方式を提供できる。

【図面の簡単な説明】

第１図は本発明の方式を実施する制御装置の構成図、第
２図は第１図の判別器の動作を説明するためのフローチ
ャート、第３図は第１図の逆変換計算器の動作を説明す
るためのフローチャート、第４図は本発明適用対象の多
関節形口がットを示す図、第５図はロボットの特異点を
説明するための図、第６図はヤコビ行列の例を示す図、
第７図は縮退ヤコビ行列の例を示す図、第８図および第
９図は従来の技術を説明するための図である。 １・・・オペレータ、２・・・操縦レバー（指令速度信
号発生器を含む）、１００・・・従来の逆変換演算器、
１１０・・・本発明の逆変換演算器、２００・・・判別
器、２１・・・サー？アンプ、３１・・・モータ、４１
・・・速度検出器、５１・・・関節角検出器、１１〜１
６・・・θ、軸〜θ６軸の駆動系。 出願人代理人　弁理士　　鈴　江　武　彦（ａ） 第４図 １２　Ｓ２　＋　１３　Ｓ２３＋ム５２３４−０　　　
　（ｅｓ＝＋−２）（ｂ）　　　　　　　　　　　　　
（ｃ）第５図

Claims (1)

1. 【特許請求の範囲】 ロボットのハンド指令速度と作業座標の優先順位を入力
   してヤコビ行列式を計算し各軸の関節速度を求める方式
   において、 （１）ヤコビ行列の所定の行と列を取除いた行列式のう
   ち｜ＤＥＴｉ（縮退ヤコビ行列）｜＞ε＿ｉ（適当な小
   さな値）を満す入力した優先順位に従ってｉを求め、 （２）（１）で求めたｉに相当するヤコビ行列およびヤ
   コビ行列式から関節速度を求める、 ことを特徴とする多関節形ロボットの軌跡制御方式。