JPH01316187A - 関節式アームの移動制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は関節式アームの制御に関する。

より具体的には、可変のダンピングファクターを擬似逆
ヤコビアン解法、即ちスートインバースヤコビアン解法
（ｐｓｅｕｄｏｉｎｖｅｒｓｅ　Ｊａｃｏｂｉａｎ　５
ｏｌｕｔｉｏｎ）に応用して関節アームの移動をより滑
らかに行なうための制御装置に関するもので、選択した
目標位置までの距離に応じてダンピングファクターを変
化させ、アームの各ジヨイントのジヨイント角度の変化
を求めるものである。

（発明の背景） 手操作又はその他の方法を用いて関節アームのコントロ
ールを行なう場合、アームを、所定通り、例えば所定の
方向に向けて所定の速度にて所定の距離だけ移動させる
ような命令が、制御コンピュータに対して送られる。こ
れらの命令によって、アーム端部に選択した点は、選択
したターゲットの方に向けて移動する。このように、通
常の場合だと、制御部への入力によって、アームの端部
は所定の距離だけ（所定の時間内に）所定の方向に向け
て移動する。アームが移動するにつれてジヨイントの各
角度を調節し、これらの命令を変更せねばならない。関
節式ジヨイントの各々のジヨイント角度は、アームが所
望通りに移動できるように調節する必要がある。

ジヨイントスペース（ｊｏｉｎｔｓｐａｃｅ）という用
語は、通常の場合、各ジヨイントの角度位置範囲を規定
し、ワークスペース（ｗｏｒｋｓｐａｃｅ）という用語
はアーム端部の空間中における位置範囲を規定するため
に使用される。

ジヨイントスペースからワークスペースへの変換は簡単
に行なうことができる。

これは、−組の閉鎖型数式を誘導することにより、順方
向の運動変換（ｆｏｒｗａｒｄ　ｋｉｎｅｍａｔｉｃｓ
　ｔｒａｎｓｆｏｒｍａｔｉｏｎｓ）を求めることがで
きる。

ｐ＝ｆ（θ）　　　　　、、、（１） ここで、ｐはワークスペースのベクトル、θはジヨイン
トスペースのべ゛クトルである。

順方向の運動変換を誘導する一般的な方法はよく知られ
ている。マニピュレータ即ち関節アームの制御を行なう
には、ワークスペースからジヨイントスペースへの逆方
向の運動変換を正確に行なうことが要求される。

θ＝　ｆ　−”（ｐ　）　　　　、、、（２）単純な閉
鎖型逆運動式は限定されたアーム形状にしか適用できな
い。従って、高いマニビュラビリティ（ｍａｎｉｐｕｌ
ａｂｉｌｉｔｙ）を要求されるワークスペースの領域で
は、特異点（ｓｉｎｇｕｌａｒｉｔｙ）から離れた位置
でしか精度が得られない。特異点は、与えられた目標位
置に対する択一的なジヨイントの解（ｓｏｌｕｔｉｏｎ
ｓ）と解との境界であって、−膜内には特異点の近傍に
幾つかの解がある。

あらゆる適用例において、アーム又はマニピュレータの
制御をリアルタイムで行なうためには、ワークスペース
からジヨイントスペースに効率よく変換せねばならない
。

逆運動の問題を解消する方法の一つとして、ヤコビアン
マトリックスに基づく反復法（ｉｔｅｒａｔｉｖｅｍｅ
ｔｈｏｄ）を用いることが知られている。ワークスペー
ス中のある点（目標）にマニピュレータを位置決めする
のに必要なジヨイントスペースの値を推定するのに反復
計画（ｉｔｅｒａｔｉｏｎ　ｓｃｈｅｍｅ）を適用でき
る。これらの解法では、ジヨイントスペースの初期値、
ジヨイントの修正方向及び大きさを考慮することが重要
である。

ヤコビアン式は、ジヨイントスペースの変分とワークス
ペースの変分と線形的な関係にある。

Ｊ（θ）δθ＝δｐ、　、　、　（３）ここで、Ｊ（θ
）＝δｆ（θ）／δθ　　、　、　、　（４）任意のマ
ニピュレータに対するヤコビアンマトリックスを計算す
る方法は一般的に知られている。

このヤコビアンマトリックスを逆転すると、ワークスペ
ースの変分（ｄｉｆｆｅｒｅｎｔｉａｌ　ｃｈａｎｇｅ
ｓ）はジヨイントスペースの変分に変換される。

（３）式はガウスの消去法を用いて解を求め、ジヨイン
ト修正することができる。

ワークスペース中のエラーを最小化するのに必要なジヨ
イント修正推定値のかなり正確な値をヤコビアン式から
得ることができる。しかしながら、マニピュレータが特
異点の近くに位置するとき、又はヤコビアンマトリック
スが正方行列（ｓｑｕａｒｅ）でないとき、正確な推定
値を求めることは困難である。

ヤコビアンの行列式（ｄｅｔｅｒｍｉｎａｎｔ）は特異
点ではゼロである。従って、特異点でヤコビアンマトリ
ックスの反転はできない。特異点の近傍では、ワークス
ペース内で僅かに変化させるのにジヨイントを物理的に
大きく変化させねばならない。このため、逆ヤコビアン
から求めたジヨイント修正は非常に大きくなり、大きな
エラーを含むことになる。換言すれば、特異点の近くで
ヤコビアン演算を用いると、アームは極めて不安定な結
果となる。

この問題を解消するために、ヤコビアンマトリックスを
擬似的に逆転させるスートインバース法（ｐｓｅｕｄｏ
ｉｎｖｅｒｓｅ）が用いられている。スートインバース
法はヤコビアンマトリックスが正方行列でないときに有
効である。スートインバース法は一般的に次の式によっ
て与えられる。

Ｊ”（θ）＝　（Ｊ　”（θ）Ｊ（θ）・）−’　Ｊ　
”（θ）　　、、、（５）ヤコビアンの横列（ｒａｎｋ
）が、操作すべきアームのエフェクターの自由度の数よ
りも小さい場合、（３）式の解は、最小二乗法的に最小
の誤差になる。

ＩＩ　Ｊ　（θ）δθ−δｐ　Ｉｔ　　　　　、、、（
６）スートインバースは明白に得られなくともよい。

次の式によって、ガウスの消去法を用いてより少ない作
業で解を求めることができる。

（ＪＴ（θ）Ｊ（θ））δθ＝　Ｊ　”（θ）δｐ、、
、（７）ヤコビアンマトリックスが対称であるとき、計
算に必要な演算数を少なくし、例えばｎＸｎヤコビアン
にすることができる。例えば、ＪＴ（θ）Ｊ（θ）（下
記のワンブラー（Ｗａｍｐｌｅｒ）のもの）を用いると
（ｎ　”＋　ｎ　’）／２を掛け、（ｎ８−ｎ”）／２
を加える必要がある。ヤコビアンの変換によって位置エ
ラーは増加する。このため、ｎ２を掛け、（ｎ　２−ｎ
）を加える必要がある。マニピュレータの自由度が６度
の場合、１６２を掛けて１２０を加えた数の式を作成す
る必要がある。

二重（ｒｅｄｕｎｄａｎｔ）マニピュレータについて、
マトリックスＪ（θ）のスートインバースＪ”（θ）の
計算数を減らした他の数式を用いることも提案されてい
る。

Ｊ’（θ）＝　Ｊ　Ｔ（θ）（Ｊ　”（θ）Ｊ（θ））
−”　　、、、（８）上記に示されるように、ＪＴ（θ
）Ｊ（θ）の値（ｄｅｔｅｒｍｉｎａｔｅ）がゼロのと
き、スートインバースは存在しない。従って、システム
は特異点では効果がないことになる。

二重マニピュレータ及び非二重マニピュレータに逆擬似
技術を用いて演算することにより、マルチジヨイント式
マニピュレータ（ジヨイント数が最大７個）の逆運動を
解析したものがある。しかし、これらの解法では、特異
点の位置又はその近傍での問題をどのように取り扱うか
については明らかにしていない。

ジヨイント修正によってマニピュレータが目標に接近し
なかった場合、スートインバースを用い、得られたジヨ
イント修正の大きさを反復して変動させ、他の反復値を
用いてジヨイント修正量を修正することも提案されてい
る。しかしながら、これらの解法は、リアルタイムで効
率よく行なうことができないし、方向が正しいと仮定す
ると、ジヨイントスペースの僅かな変化でヤコビアンは
大きく変化するため、特異点に対しては有効でないかも
しれない。

シー　ダブリュ　ワンブラー（Ｃ，Ｗ、Ｗａｍｐｌｅｒ
）は、Ｔｒａｎｓａｃｔｉｏｎｓ　ｏｎ　Ｓｙｓｔｅｍ
ｓ　ａｎｄ　Ｄａｍｐｅｄ　Ｌｅａｓｔ　５ｑｕａｒｅ
ｓ　！Ｊｅｔｈｏｄ、　Ｖｏｌ、ＳＭＣ−１６、ＮＯ６
１，１９８６年１月７２月、ｐ９３−１０１に他の文献
を発表している。これは、「ベクトル形成及びダンプ最
小二乗法によるマニピュレータの逆運動解決」というも
ので、特異点近傍におけるジヨイントスペースの計算を
、次の式で示されるジヨイントの変分をダンプすること
によって安定化させる。

ここで、■は恒等行列（ｉｄｅｎｔｉｔｙ　ｍａｔｒｉ
ｘ）、Ｋはダンピングファクターにの平方根である。

上記式のスートインバースは一定のファクターｋを正方
形マトリックス（ｓｑｕａｒｅ　ｐｒｏｄｕｃｔ　ｍａ
ｔｒｉｘ）の対角線要素に加えたものである。

Ｊ”（θ）＝（Ｊ”（θ）Ｊ（θ）＋　ｋ　Ｉ　）−”
Ｊ　”（θ）、、、（１０） このスートインバースの変形によって正符号のマトリッ
クスが得られ、これは反転できないことが保証される。

Ｗａｍｐｌｅｒは更に又、固定ダンピングファクターを
用いて、このダンピング定数を選択し、特異点又はその
近傍における安定性（ｓｔａｂｉｌｉｔｙ）を確実なも
のとし、目標に到達しないようにすることを提案してい
る。このダンピングポーズはエラーの大きさだけでなく
、解（ｓｏｌｕｔｉｏｎ）の大きさも制限する。ダンピ
ングは次の関数を最小化するのと同じである。

１１Ｊ（θ）δθ−δｐ１Ｍ＋ｋｌｌδθＩ＋　２．、
：（１１）特異点の近傍では、ダンピングファクターに
よってスートインバースの特異値は、無限の方ではなく
ゼロの方に向けて移動する。ダンピングと共にジヨイン
ト修正は有限値に維持されるが、式（３）のジヨイント
修正の解を求める際、安定性と誤差を折衷（ｃｏｍｐｒ
ｏｍｉｓｅ）　している。安定性のために、ダンプ値は
特異点近傍にてマニピュレータの操作のために選択され
る。しかし、特異点から離れた位置では、このダンピン
グファクターは有効でない。

Ｗａｍｐｌｅｒ氏は、安定のためのダンピングファクタ
ーを選択し、ジヨイント修正をゼロにして目標に到達し
ないようにしている。しかし、ダンピング定数が大きい
と、マニピュレータは特異点の近傍位置に到達できない
。ジヨイント修正は非常に速く目標位置に向かってゼロ
に進む。目標位置は、マニピュレータが到達可能であっ
て、特異点に接近している。固定されたダンピング定数
は、特異点近傍の目標に対しても有効ではない。それは
、ジヨイント修正が非常に小さく、解への収斂（ｃｏｎ
ｖｅｒｇｅｎｃｅ）が非常にゆっくりとしているからで
ある。

この問題を解消するために、可変のダンピング技術を用
いて式（６）の誤差の大きさの中に折衷点を求め、ジヨ
イント修正を行なうことが提案されている（Ｙ、Ｎａｋ
ａｍｕｒａ、　１９８６年、博士論文、京都大学、オー
トメーションリサーチ研究所「ロボットマニピュレータ
の軌道制御における運動研究」）。

この修正された擬似逆ヤコビアンは、当該技術分野では
、特異点の強いインバース、即ちＳＲインバースとして
説明されている。特異点から離れた位置では、ダンピン
グは必要でなく、マニピュレータが特異点に接近したと
きにより多くのダンピングが加えられる。マニピュレー
タのマニピュラビリティを用いて特異点への接近度を求
めることができる。特異点への接近度を求めるのに必要
な計算量は非常に多くなり得る。マニビュラビリティの
推定を用いる方法では　ｎ８のオーダの演算（ｏｐｅｒ
ａｔｉｏｎｓ）を必要とし、ここでｎはマニピュレータ
の自由度の数を表わす。

固定されたダンピング法の場合、ＳＲインバースは特異
点に近くて比較的大きなダンピング値であるから、特異
点近傍におけるこのような運動は制限される。

特異点の近傍では、エラーはなお非常に大きい。

可変のダンピング計画（ｄａｍｐｉｎｇ　ｓｃｈｅｍｅ
）の場合、未到達位置は固定ダンピング技術を用いるこ
とによって安定可能であるが、通常の場合、ダンピング
値は低いため安定度は損なわれる。可変のダンピングフ
ァクターを用いることによって、スートインバースを確
実に逆転させることができる。しかし、特異点から離れ
た所での作業を抑制できない。特異点に近い位置では、
正確な値を得ることができない。

（発明が解決しようとする課題） 本発明は、関節式アームの制御装置において、特異点及
びマニピュラビリテイの高い領域の両方の位置制御をう
まく行なうことができる装置を提供することを目的とす
る。

（課題を解決するための手段） 広い意味において、本発明は関節式アームの動きを制御
する方法に関し、アームの端部点（ｅｎｄ　ｐｏｉｎｔ
）の目標地点を定めるための命令を制御コンピュータに
供給し、アニムの各ジヨイントの変化量を決めるための
ダンピングファクターを有するスートインバースのヤコ
ビアンを反復して供給し、アーム端部点から目標までの
距離に応じて反復毎にダンピングファクターの大きさを
調節し、１乃至２０の反復回数によって角度を求め、そ
の角度に基づいてアームのジヨイント角度を調節する工
程から構成される。

ジヨイント角度は反復する毎に調節することが望ましい
。

ダンピングファクターは次の式に基づいて調節すること
が望ましい。

ｋｘ＝ｋｌｌδＩ）　ＩＩ　”　　　　　　、、、（１
２）ここで、ｋ３は調節されたダンピングファクター、
ｋは関節式アームの全長（メートル）の０００１乃至１
倍の範囲内で選択した定数、１１δｐ　ＩＩは反復後の
アーム端部と目標との間の距離、Ｘは１乃至３の範囲内
で予め選択された数である。

より具体的には、本発明は関節式アームの操作を制御す
る方法に関し、特異点の近傍又はマニビュラビリティの
高い領域におけるアームの操作を改善したもので、該方
法は、関節式アーム上の端部点を、選択された方向に向
けて選択された距離だけ時間内に目標に向けて移動させ
る命令を制御コンピュータに発し、アームの調節可能な
ジヨイント角度を変えることにより所定の運動を行なう
ことができるようにしており、端部点を目標に向けて移
動させるためのジヨイント角度の決定は下記式に基づく
スートインバースのヤコビアンマトリックスの反復性解
法から求めるものであって、鎖式は Ｊ責θ）＝　（Ｊ　”（θ）Ｊ（θ）十ｋ　ｓ　Ｉ　）
−”Ｊ　”（θ）、　、　（１３） で表わされ、■は恒等行列、ｋ３は前述した（１２）式
に３＝ｋｌｌδｐ　ＩＩ　”　　から求めた可変のダン
ピングファクターであり、ｋは関節式アームの全長（メ
ートル）の０．００１乃至１倍の範囲内で選択した定数
、１１δｐ　ＩＩは反復後のアーム端部と目標との間の
距離、Ｘは１乃至３の範囲内で予め選択された数として
おり、反復毎にに３の値を調節し、１乃至２０回の範囲
内で反復させた後、ジヨイント角度を調節して目標に向
けてアームを移動できるようにしている。

ジヨイント角度は反復毎に調節を行ない、目標に向けて
アームを移動させることが望ましく、望ましいＸの値は
２である。

（望ましい実施例の説明） 関節式アームの如きマニピュレータを操作するとき、第
１図に符号（１０）で示すように、ジョイスティックを
操作して制御信号をコンピュータ（１２）に送り、アー
ムのジヨイント部の角度変位を制御するものである。図
示の構成では、コンピュータ（１２）によってアクチュ
エータ部材（１４）（１６）を制御し、角度Ａを及び角
度Ｂの調節を夫々行なうものである。角度Ａは第１のア
ームセグメント（１８）と基準線（２０）との間の角度
であり、角度Ｂはアームセグメント（１８）の長軸とア
ームセグメント（２２）の長軸との延長線が形成する角
度である。

アームセグメント（２２）の自由端部の端部点（２４）
が移動すべき目標の位置はジョイスティック（１０）の
動きによって決まる。

ワークスペースの位置制御信号を、スティック制御によ
って発生させるよりも、ロボットの軌道（ｐａｔｈ）ブ
ラニングシステムによってコンピュータから発生させる
ことができることは勿論である。

本発明によれば、アームの自由度を更に大きくする必要
がある場合、第２図に示すジヨイント（２６）、アーム
セグメント（２８）、ジヨイント（３０）及びアームセ
グメント（３２）の如き、ジヨイント及び端部エフェク
ター又はアームセグメントを追加すればよい。この構成
の場合、関節アームの端部点は符号（３４）で示され、
伸張したアームセグメント（３２）の自由端部がその位
置となる。この場合、角度Ｃ及び角度りをコントロール
することによって、アームセグメント（２２）（２ｇ）
間の角度調節を行ない、ジヨイント（３０）の軸（３６
）の回りにおけるアームセグメント（３２）の回転を調
節することができる。第２図には、これら角度の調節を
行なうアクチュエータを示していないが、適当なアクチ
ュエータを用いればよい。

本発明を例示的に説明するために、より単純化した構成
例を第１図に示している。

ジョイスティック（１０）によって設定された初期位置
と目標位置との間の差（ｅｒｒｏｒ）、即ちエラーは、
ジヨイントスペース又はワークスペースの中で特定でき
る。ワークスペース中のエラーはジョイスティックによ
って設定され、このエラーはジヨイントスペース中のエ
ラーの大きさと略一致している。エラーは、ジョイステ
ィックによって設定された目標に達するために、アーム
が移動せねばならない距離又はジヨイントが角度を変化
させねばならない値を表わしている。

本発明の場合、各ジヨイントのジヨイント角度θは、下
記式に基づき、スートインバースを用いて求める。

Ｊ”（θ）＝　（Ｊ　”（θ）Ｊ（θ）＋　ｋ　８　Ｉ
　）−”　Ｊ　’（θ）、、、（１３） δθ＝Ｊ箕θ）δｐ 本発明の場合、ダンピングファクターは、スートインバ
ースの反復毎に、端部位置と目標位置とのワークスペー
スの差の大きさに基づいて修正される。従って、誤差を
ダンプしたスートインバース（ＦＤＰ）のダンピングフ
ァクターは、次の式に基づいて変動する。

ｋ、　ｓ＝　ｋＩＩδＩ）　１１　”　　　　　　、、
、（１２）ここで、ｋ３は調節されたダンピングファク
ター、ｋは関節式アームの全長（メートル）の０．００
１乃至１倍の範囲内で選択した定数、１１δｐ　Ｉｆは
反復後のアーム端部と目標との間の距離、Ｘは１乃至３
の範囲内で予め選択された数である。

−膜内に、ｋとＸの値は、最も望ましいアウトプットが
得られるように、任意に選択したサンプルの計算によっ
て求める。アームセグメントが２つの単純な構成の場合
、長さ２ｍのアームに対して、ｋ＝１及びｘ＝２のとき
に非常に満足の得られる結果が得られた。ｋの値はアー
ムが複雑になればなるほど小さくなる。アームが２又は
３本のアームセグメントから構成する場合、Ｘの値は例
えば１のように低くするが、アームが複雑になれば通常
の場合、Ｘはかなり大きくなる。例えば、Ｘの値を３の
ように高くした場合、アームが行き過ぎたり、アームの
オシレーションが生じることもあるため、Ｘの値は約２
にすることが望ましい。

前述したように、ヤコビアン式によれば、ジヨイントの
変化が少ないときでも、修正必要量を非常に正確に推定
できる。ジヨイント変化が極めて少ない場合でも特異点
の近傍で適切な修正量を求めることができる。しかしな
がら、ジヨイント変化が大きいとき、目標位置に達する
際にアームの行過ぎ及びオシレーションを避けるために
ジヨイント修正は控え目（ｃｏｎｓｅｒｖａｔｉｖｅ）
にせねばならない。このように、ジヨイント変化が大変
小さい場合、これまでのヤコビアン式でも満足のいく結
果が得られる。しかし、ジョイスティックを操作すると
ワークスペースはかなり大きく変化するため、多くの場
合、ジヨイントスペースを大きく変化させる必要がある
。従って、ジヨイントスペースを大きくする場合、逆ヤ
コビアン法の単独使用ではアームに行過ぎ及びオシレー
ションが生じるが、これを防ぐため、ジヨイント変化を
控え目にせねばならない。これは特異点の近傍では更に
重要なことである。なぜなら、逆ヤコビアン法の単独使
用の場合、特異点の近傍ではジヨイント又はワークスペ
ースを僅かに変化させるとジヨイントを大きく修正せね
ばならなくなるからである。この問題はダンピングを用
いることによって緩和される。

距離の関数として、即ち端部位置と目標位置との距離に
応じてダンピングファクターを修正し、ワークスペース
のエラーに基づいてダンピングファクターを変化させる
。ダンピングファクターはワークスペースに比例して変
動し、目標位置がマニビュラビリティの高い領域又は特
異点の近傍にあるか否かとは無関係に、ワークスペース
のエラーが大きくなるにつれてダンピングファクターも
大きくなる。

端部位置から目標までの距離に応じて特異点の近傍でダ
ンピング値を変化させると、ダンピング値が小さくなる
とワークスペースのエラーも小さくなり、収斂率は大き
くなる。これによって、特異点にて最小のダンピング値
を維持しつつ、端部点は最小のオシレーションで目標に
到達し、確実に逆転できる。

マニピュラビリティの高い領域において、前述したよう
にジヨイント変化が小さい場合でも、擬似的な逆転を行
なうことによってジヨイント変化の必要量を正確に求め
ることができる。ジヨイント変化又はワークスペースの
変化が大きい場合、スートインバースによってジヨイン
ト修正を非常に大きくなり、目標を行き過ぎたり、マニ
ピュレータはオシレーションを生じたりする。本発明の
場合、行過ぎを小さくし、マニピュラビリティの高い領
域でワークスペースの大きな変化を生じないようにする
ものである。これは、ワークスペースの必要変化量、即
ち端部点とジョイスティックが設定した目標との間の距
離に応じてダンピング値を変化することにより行なうも
のである。

特異点近傍の目標点を反復するため、その目標は所定の
許容範囲内に到達可能とし、ジヨイント修正は目標とす
る特異点の近傍ではあまりに速くゼロとならないように
している。これは、マニピュレータが目標に接近するに
つれて、ダンピングファクターが小さくなるためである
。特異点に接近しすぎると、ジヨイントの解と目標の解
との境界が近づき、特異点の近くでは幾つかの解が存在
することとなって、マニピュレータはある解から他の解
へと移動するからである。

特異点近傍での操作性を改善するため、特異点から離れ
たジヨイントスペースの開始位置、即ち初期値を、現在
の解が他の解よりもジヨイントスペースに近づくように
人為的に移動させることが望ましい。

もう一つの方法は、限界に到達したジヨイントを単に動
かないようにするものである。ジヨイントを動かないよ
うにするため、そのジヨイントに対応するヤコビアンマ
トリックスの列（ｃｏｌｕｍｎ）カゼ口に設定される場
合、ジヨイント修正はゼロに等しくなる。そのジヨイン
トが位置エラーをなくすのに寄与することにより、ゼロ
となる。

本発明はエラーをダンプしたスートインバース（ｅｒｒ
ｏｒ　ｄａｍｐｅｄ　ｐｓｅｕｄｏｉｎｖｅｒｓｅ）を
利用するがら、アームは、より速やかに、かつより少な
いオシレーションにて所定の目的地に到達する。

アームのリーチ長さを超える領域では、マニピュレータ
は単に位置エラーを最小にするだけである。本発明は、
エラーをダンプしたスートインバースを用いるから、ア
ームは到達不能な位置に向かい、操作は著しく改善され
る。

本発明を実施する際、アーム位置の実際の修正、即ちア
クチュエータ（１４）（１６）は反復毎に直ちに作動す
るのが望ましい。本発明の場合、修正量が大きくても、
修正されたダンピングファクターによってエラーは制限
され、アームは目標に対してより直接的に導かれる。幾
つかの反復結果を、アクチュエータがトリガーする前に
最大２０回まで必要に応じて蓄積することができる。

以下に、曲線ｋ　ｓ　（Ｗａｍｐｌｅｒ）によって示さ
れる固定ダンピング係数又は曲線ｋ　２（Ｎａｋａｍｕ
ｒａ）によって示される可変ダンピング係数を組み入れ
たスートインバースと、曲線に、によって示される本発
明のものとを比較した実施例を添付の図面に示す。

ｋ、は実線、ｋ、は破線、ｋ３は点線で示している。

固定ダンピング値は０．２に設定した。２本のアームを
リンクさせ、各アームの長さは１ｍのものを用いた。可
変ダンピングファクターについては、ダンピング係数を
０．０１に設定した。ダンピングファクターには、下記
式によって求めた位置にて、マニピュレータのマニピュ
ラビリティに基づいて求めたものである。

ｍ　＝　　５ｑｒｔ［ｄｅｔ（Ｊ　”（θ）Ｊ（θ）］
　　、、、（１３）ｋ２は次の式に基づいて計算した。

ｋ　ｔ＝　ｋ　４　（１ｍ／ｍ、）２−（１４）ここで
に４は一定のダンピングファクターであり、マニピュラ
ビリティが１よりも小さいときにだけダンピングされる
。

本発明の実施例では、ダンピング係数を１に設定した。

第３図乃至第６図において、図番に付したアルファベッ
トＡ乃至Ｃは、八が距離誤差、Ｂが第１ジヨイントの修
正及びＣが第２ジヨイントの修正を表わしている。

第３図及び第６図は、マニピュレータの基部における特
異点の回りの大きなジヨイント変化と小さなジヨイント
変化の例である。

第４図はマニピュラビリティの高い領域における大きな
ジヨイント変化の例を示しており、第５図は伸張したエ
ルボ−の特異点近傍の大きなジヨイント変化の例を示し
ている。

第３図から明らかなように、本発明の場合、ジヨイント
変化が大きいと距離エラーは速やかに小さくなるが、他
のシステムはかなり良く作動する。

一方、すべての条件において、本発明の位置エラーをみ
ると、ｋ８は常に最も低い値にまで小さくなる。同じよ
うに、すべての場合において、本発明のものは、ジヨイ
ント変化の位置や大きさとは関係なく、各角度の反復数
は少ない。

本発明はマニピュレータの端部点を特異点近傍の目標に
向けて移動させる手段を提供し、簡単なスートインバー
スに必要な計算以外に追加の計算は殆んど必要としない
方法を提供するものである。

概していえば、本発明は特異点近傍又はマニピュラビリ
ティの高い領域のすべての位置に、従来技術よりも少な
い繰返し数で収斂させることができる。前述したように
、エラーをダンプしたスートインバースによって、特異
点の極く近傍まで移動できるから、マニピュレータは択
一的な解に移動することができる。これら形式のマニピ
ュレータにおいて、スートインバースが１つの解に向が
うように適当な予防措置を講じるべきである。

本発明は第１の反復に良好な近似値を与えるもので、エ
ラーをダンプしたスートインバースによってダンピング
ファクターを変化させることにより、変化量を制限し、
反復毎にアクチュエータを作動させるのに適している。

本発明の詳細な説明したが、特許請求の範囲に規定され
た本発明の精神から逸脱することなく変形をなすことは
できる。

【図面の簡単な説明】

第１図は本発明によって制御することのできる簡単な関
節式アームの説明図、第２図は第１図よりも複雑で自由
度を多くした関節式アームの説明図、第３Ａ図乃至第３
Ｃ図、第４Ａ図乃至第４Ｃ図、第５Ａ図乃至第５Ｃ図、
及び第６Ａ図乃至第６Ｃ図は本発明例とダンピングファ
クターを組み入れた他の反復性擬似逆ヤコビアン技術の
例との比較を示すグラフである。 （１０）、、、ジョイスティック （１２）、、、コンピュータ （１４）（１６）　、　、　、アクチュエータ（１８）
（２２）　（（２８）（３２）　、　、　、アームセグ
メント（２４）（３４）、、、端部点 反復数 反復数 ＦＩ　Ｇ、　３　Ｃ。 反復数

Claims (7)

【特許請求の範囲】
1. （１）アーム端部の目標位置を定めるための命令を制御
   コンピュータに供給し、アームの各ジョイントの各ジョ
   イント角度の変化量を規定するためのダンピングファク
   ターを有する逆擬似ヤコビアンを反復して適用し、アー
   ム端部から目標までのワークスペース距離の計算値に基
   づいて反復毎にダンピングファクターの大きさを調節し
   、１乃至２０の反復数によって角度を求め、その角度に
   基づいてアームのジョイント角度を調節する工程から構
   成されることを特徴とする関節式アームの移動制御方法
   。
2. （２）ジョイント角度は反復毎に調節される特許請求の
   範囲第１項に記載の方法。
3. （３）ダンピングファクターは次の式に基づいて調節さ
   れる特許請求の範囲第１項に記載の方法。 ｋ＿３＝ｋ‖δｐ‖＾ｘ 但し、 ｋ＿３は調節されたダンピングファクター、ｋは関節式
   アームの全長（メートル）の０．００１乃至１倍の範囲
   内で選択した定数、 ‖δｐ‖は反復後のアーム端部と目標との間の距離、 ｘは１乃至３の範囲内で予め選択された数である。
4. （４）ｘは２である特許請求の範囲第３項に記載の方法
   。
5. （５）関節式アームの操作制御方法であって、特異点の
   近傍又はマニピュラビリティの高い領域におけるアーム
   の操作を改善するものであり、関節式アームはアームセ
   グメントがアームに回動可能に連結され、アームを移動
   させるアクチュエータとアームセグメントとの間にジョ
   イント角度を調節可能に形成し、アクチュエータはジョ
   イント角度を変えてアームを移動できるようにしており
   、該方法は、アーム上の端部点の目標位置を規定するた
   めの命令をコンピュータ制御部に送り、該コンピュータ
   は下記式の逆擬似ヤコビアンマトリックスによる反復解
   を用いて端部点を目標位置に向けて移動させるのに必要
   なジョイント角度を決定し、目標位置に対する端部点の
   計算位置に基づいて反復毎にｋ＿３の値を調節し、アク
   チュエータを作動し、１乃至２０回の反復後、ジョイン
   ト角度を調節して目標に向けてアームの端部が移動でき
   るようにしていることを特徴とする関節式アームの操作
   制御方法。 Ｊ＾＋（θ）＝（Ｊ＾Ｔ（θ）Ｊ（θ）＋ｋ＿３Ｉ）＾
   −＾１Ｊ＾Ｔ（θ）但し、 Ｉは恒等行列、 ｋ＿３は式ｋ＿ａ＝ｋ‖δｐ‖＾ｘから求めた可変のダ
   ンピングファクターであり、 ｋは関節式アームの全長（メートル）の０．００１乃至
   １倍の範囲内で選択した定数、 ‖δｐ‖は反復後のアームの端部点と目標との間の距離
   、 ｘは１乃至３の範囲内で予め選択された数としている。
6. （６）ｘの値は２である特許請求の範囲第５項に記載の
   方法。
7. （７）アクチュエータはコンピュータによって作動し、
   反復後、ジョイント角度を調節して目標に向けてアーム
   の端部が移動できるようにしている特許請求の範囲第５
   項に記載の方法。