JPH02212085A - マニピュレータの姿勢決定方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［発明の目的］ （産業上の利用分野） 本発明は、人間と同様の肩及び肘の関節４軸及び手首関
節の３軸を備えた７自由度のマニピュレータの姿勢決定
方法に関する。

（従来の技術） 近年、プラントの保守作業などにおいて、高度の精密作
業を実行させるべく、双腕ロボットなど人間と同様の７
自由度のマニピュレータの採用が望まれるようになって
きた。

ここに、これら７自由度のマニピュレータにあっては、
冗長軸、すなわち一義的に姿勢決定できない軸が存在す
るため、何らかの手段によりその冗長性を消す方法が必
要である。

（発明が解決しようとする課題） しかしながら、従来、冗長軸を消す適正な方法が提案さ
れていなかったため、逆運勧学を解くことが出来ず、折
角このようなロボットを作っても、人間の腕のように動
作をさせることができないという問題があった。

そこで、本発明は、２人間の腕と同様の肩及び肘の関節
４軸及び手首関節３軸を備えた７自由度のマニピュレー
タにおいて、その逆運勧学を解くための方法を提供する
ことにより、人間の腕と同様に制御可能とすることを目
的とする。

［発明の構成］ （課題を解決するための手段） 上記課題を解決する本発明のマニピュレータの姿勢決定
方法は、第１図に示すような上腕及び前腕の長さｔ、Ｉ
　ｔ、２１手首から手先の長さし。

で、肩Ｓ及び肘Ｅの関節１〜４の４軸θ１〜θ４及び手
首Ｗの関節５〜７の３軸θ５〜θ７から成る７自由度の
マニピュレータの姿勢決定方法において、１冗長度手先
姿勢では、手首関節の一つを関節原点に固定して求めた
マニピュレータ姿勢から、各関節角がそれぞれの可動範
囲内にあり、かつ冗長軸まわりの仮想手先復元トルクが
最小となる姿勢を選択し、２冗長度手先姿勢では、二つ
の手首関節を関節原点に固定して求めたマニピュレータ
姿勢から各関節角がそれぞれの可動範囲にある姿勢を選
択することを特徴とする。

（作用） 手で物を掴む場合の手先姿勢には次の二つの場合がある
。

一つは第２図に示すように手で何かを握る場合でハンマ
ーやレンチの柄８を握る場合がこれに相当する。この場
合の手先座標系を新たに図のように決めると、ｘｈ軸と
ｙｈ軸まわりの向きθ、及びφは工具の柄８の向きと握
り位置（Ｇ）が決まればおのずと決まるもので、勝手な
角度を採ることは許されない。ところが、ｚｈ軸まわり
の向きψはどの値をとることも許される。このように冗
長度が一つある場合を１冗長度手先姿勢と呼ぶことにす
る。

もう一つの場合は、第３図に示すように親指と人差し指
の先でグラス９を持つような場合である。

この場合は、Ｙｈ軸を水平、に保って、グラスの中身を
こぼさないようにする為にｘｈ軸まわりの向きθはおの
ずと決まるが、Ｙｈ軸及びｚｈ軸まわりの向きφ及びψ
は自由に選択し得る。このように冗長度が二つある場合
を２冗長度手先姿勢と呼ぶことにする。

本発明者の知見によれば、上記のように手先姿勢を分類
した場合、人間の腕の姿勢は手首の筋肉の負担が最少に
なるように機能する以下のごとき簡単な規則によって決
まる。

規則−１：１冗長度手先姿勢の場合、人間の腕は手首関
節の一つが常に関節原点にある姿勢のうち、各関節角が
それぞれの可動範囲内にあり、かつ冗長軸まわりの仮想
手先復元トルクが最小になる腕姿勢をとる。ここで仮想
手先復元トルクとは、各関節において関節変位の関数で
表される関節復元トルクが働くと仮定した時、手先に現
れるトルクを言う。またこの場合、関節原点に固定され
る手首の関節は第４図に示す手を上下に動かす関節に相
当する。

規則−２：２冗長度手先姿勢の場合、人間の腕は手首の
関節の二つが常に関節原点にある姿勢のうち、各関節角
がそれぞれの可動範囲にある姿勢をとる。この場合の手
首関節は第４図及び第５図に示す手を上下に動かす関節
及び左右に動かす関節である。

このような規則を第１図に示すマニピュレータに適用す
れば冗長度は消去され、人間の腕と同じ関節配置の７自
由度の冗長マニピュレータの逆運勧学を数値的に解くこ
とが可能になり、マニピュレータを無理なく円滑に制御
できる。

（実施例） 以下、第１図に示すマニピュレータにつき１冗長度と２
冗長度の具体的な例を挙げ、逆運勧学によりそれぞれの
姿勢を求める実施例を説明する。

第６図は１冗長度姿勢をボルト締結作業の例で示す説明
図である。本例では、レンチ１０を握る人の腕の姿勢は
、「人間の腕の姿勢は決定できる」という第１の規則を
用い、この場合の冗長軸は第２図に示したと同様にｚｈ
軸となるので、第６図において基準座標系に於けるレン
チの柄８の向き及び握り点Ｇの位置ベクトル（Ｘａ　、
　Ｙｃ　、　Ｚａ　）とｚｈ軸回りの回転角ψが決まれ
ば、手先座標系を基準座標系に変換する４×４の座標変
換行列Ｔは容易に求まり０）式で表わすことができる。

それゆえ基準座標糸における手首Ｗの位置ベクトル（Ｘ
ｗ　　Ｙｗ　　Ｚｗ）は、（２）式で求まる。

ここでＣ，−ｃｏｓθｂ　ｅ　　５ｂ−ｓ１ｎθ６であ
る。

それゆえ基準座標糸における肘Ｅの位置ベクトル（Ｘｉ
　　Ｙｌ！　　ＺＥ）は、 ここでＬ３は手首から握り点までの長さである。

次に、第５図に示す手を左右に動かす関節の角度θ６を
仮定すれば基準座標に対する前腕の向きを示す方向余弦
は、（３）式の第１行の３ｘ　１ベクトルで与えられる
。

のように表すことが出来る。ここでＬ２は前腕の長さで
ある。また第１図の第６関節は第５図の手首を左右に動
かす関節に相当する。

Ｇ、Ｗ、Ｅの関係を図に示すと第７図のようになる。Ｅ
点は手首関節角度θ６の値に応じてＬ２を半径とする円
弧上の一点を占める。そこでθ６を変えながら肩Ｓから
肘Ｅまでの距離ＬＳＥを計算し、 ＬＳＥ　−Ｌｌ　　　　　　　　　　　　　　・・・（
５）が成立する点を探索する。ここでＬｌは上腕の長さ
である。

（５）式を満たすＥ点が存在したら、それを肘の位置Ｅ
とし、この時のθ６を第６関節の関節角とする。（５）
式を満たすＥ点が存在しないときはψを変えて（５）式
を満たすＥ点を探す。

この様にして、手首と肘の座標が求まれば、第１関節か
ら第５関節までの各々の関節角は容易に求めることが出
来る。

一般式で示すと、いま座標系ｉに於ける位置と方向を座
標系ｉ−１の位置と方向に変換するＡ行列をＡ１で表す
ことにすれば、第１図のマニピュレータについて求めた
Ａ行列を使って計算したＡ１−Ａ２　・Ａ、の最後の３
ｘ１行ベクトルが肘Ｅの位置ベクトル（ＸＥ　、ＹＥ　
、　Ｚｗ　）　”となる。

Ａ４　・Ａ２　ΦＡ、を次のように表せばＴ３−Ａｌ　
−Ａ２・Ａ３ θ０．θ２は次の（７）式を解くことによって得られる
。

θ４はＳ、Ｅ、Ｗのつくる三角形の三辺の長さが既知で
あることから、第２余弦法則を用いて（８）式で求める
ことが出来る。

θ４　＝ｃｏｓ　−’　（（Ｌ：ｗ−Ｌ”、　−ｔ、：
　）／（２ＬＬ　　・Ｌ２　））・・・（８）ここでＬ
ｓｗは肩から手首までの距離である。

いまＡ４　・Ａ２・Ａ３・Ａ４・Ａ、を、・・・（６） ・・・（９） と置けば、第３関節の角度θ、は次式を解いて求まる。

（９）式のＡ５は未知変数θ、を含むので奇妙に感じる
かもしれないが、■）の左辺にはθ、は含まれないので
問題は無い。

残るθ、は次式から求めることが出来る。

Ａ９＝Ａ４　　・Ａ３　φＡ２　・Ａ１　ΦＴ・Ａ７　
φＡ６・・・（１１） 仮想手先力ベクトルＦは次式によって計算する。

Ｆ＝（ＪＴ）＋・τＲ・・・Ｏ ここでＪはマニピュレータヤコビアン、＋は疑似逆行列
を表す。τＲは仮想関節復元トルクベクトルである。Ｆ
とτ８は次のように表される。

””　［ｆｘｈ、　　ｆＹｋ＋　　ｆＺｌ　　ｒＸｈ＋
　　τＹｌ　　τＺｈｌ　”・・・■ τ、＝［τＲ１＋　　τＲ２＋　　τＲ３ｒ　　τＲ４
・　τＲ５＋　　τＲ６＋τ　Ｒ７コ　７　　　　　　
　　　　　　　　　　　　　　　　　　　・・・昏９■
式の終わりの３項が仮想手先復元トルクである。よって
、これにより、仮想復元トルクを最小とする手先姿勢が
求まる。

次に、第２図に示す２冗長度姿勢において、手先座標糸
のｙｈ及びｚｈ軸回りの角度は、障害物が無い限りある
程度自由に選ぶことが出来、一意的に決まらないので、
「２冗長度手先姿勢の場合、人間の腕は、手首の関節の
二つが常に中立位置（関節角−０度）となる姿勢をとる
」という規則２を適用する。

もし掴み点Ｇが決まれば、手先のｙｈ軸回りの角度φ及
びｚｈ軸回りの角度ψを仮定して、１冗長度手先姿勢の
場合と同様にして、手首Ｗと肘Ｅの位置が計算出来る。

φとψを少しずつ変えながらＥ点から肩Ｓまでの距離が
Ｌｌに等しくなるようなＥ点を求めれば、この時のＷ、
Ｅ、Ｓの位置のベクトルとマニピュレータのＡ行列を使
って、１冗長度手先姿勢の場合と同様にして、θ１から
θ５までの関節角を求めることが出来る。最後にミこの
様にして求まった複数のマニピュレータの姿勢の中から
各関節角の可動範囲を超えないものを選ぶ。

以上のように本例では、人間の腕の姿勢決定に対し２つ
の規則があるという知見に基いて、この規則に従って７
自由度のマニピュレータを制御するので、冗長軸を有効
に消すことができ、マニピュレータを人間の腕と同様に
制御できる。

本発明は、上記実施例に限定されるものではなく、適宜
の設計的変更を行うことにより、適宜の態様で実施し得
るものである。

［発明の効果］ 以上の通り、本発明は特許請求の範囲に記載の通りのマ
ニピュレータの姿勢決定方法であるので、人間の腕と同
じ関節配置の７自由度マニピュレータを人間の腕の動き
と同じように制御可能になる。

【図面の簡単な説明】

第１図は本発明に適用される７自由度のマニピュレータ
を示す斜視説明図、第２図は１冗長度姿勢の例を人間の
手で示す説明図、第３図は２冗長度姿勢の例を示す説明
図、第４図及び第５図は冗長軸の説明図、第６図は１冗
長度姿勢の他の例を示す平面説明図、第７図は掴み点及
び手首並びに肘の位置関係を示す説明図である。 １〜７・・・関節 θ１〜θ７・・・各関節に対応する軸

Claims (2)

【特許請求の範囲】
1. （１）肩及び肘の関節４軸及び手首関節３軸を備えた７
   自由度のマニピュレータの姿勢決定方法において、１冗
   長度手先姿勢では、手首関節の一つを関節原点に固定し
   て求めたマニピュレータ姿勢から、各関節角がそれぞれ
   の可動範囲内にあり、かつ冗長軸まわりの仮想手先復元
   トルクが最小となる姿勢を選択することを特徴とするマ
   ニピュレータの姿勢決定方法。
2. （２）肩及び肘の関節４軸及び手首関節３軸を備えた７
   自由度のマニピュレータの姿勢決定法において、２冗長
   度手先姿勢では、二つの手首関節を関節原点に固定して
   求めたマニピュレータ姿勢から各関節角がそれぞれの可
   動範囲にある姿勢を選択することを特徴とするマニピュ
   レータの姿勢決定方法。