JPH10329066A - 冗長マニピュレータの特異姿勢検知方法および特異姿勢回避方法 - Google Patents

Abstract

(57)【要約】 【課題】冗長マニピュレータの特異姿勢を検知すること
ができる冗長マニピュレータの特異姿勢検知方法を提供
する。 【解決手段】少なくとも７つの軸を有する冗長マニピュ
レータの特異姿勢検知方法において、冗長マニピュレー
タの各関節角速度θ′と手先位置姿勢速度ｒ′との関係
を示すヤコビ行列の行列式（デターミナント）Ｄを求め
（ステップＳ２１）、さらにデターミナントＤの微分値
Ｄ′を求めて（ステップＳ２２）、この結果により、デ
ターミナントＤが連続α回の減少傾向で（ステップＳ２
３）、かつ関節角速度θ′がリミット速度のβ％以上の
大きい値になっていいるとき（ステップＳ２４）、冗長
マニピュレータの手先位置が特異点近傍空間内にあり、
全体の姿勢が特異姿勢に近づいていると判断する（ステ
ップＳ２５）。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、例えば７軸以上の
多軸リンク構造からなるロボットに適用される冗長マニ
ピュレータの特異姿勢検知方法および特異姿勢回避方法
に関する。

【０００２】

【従来の技術】従来から、マニピュレータを駆動すると
きは手先位置姿勢を指示して各軸角速度を制御すること
が一般的であるが、このとき、マニピュレータ全体の姿
勢を特異姿勢から遠ざけなければならないことが知られ
ている。

【０００３】マニピュレータの特異姿勢とは、各軸角度
が特定の組合せになったとき、ある方向に手先位置姿勢
を動かすことが問題となるような姿勢のことをいい、マ
ニピュレータが特異姿勢になったときは、暴走などを防
ぐためにその特定の方向に手先を動かすことができなく
なってしまう。なお、マニピュレータが特異姿勢になっ
たときの手先位置を特異点といっている。

【０００４】この場合、マニピュレータの姿勢が特異姿
勢に近づくこと、すなわち手先位置が特異点近傍に近づ
くことだけでも問題があり、例えば手先位置姿勢に対応
する各軸角速度を数学的に求めるとき、それらの解が数
学上求められなくなったり、誤って非常に大きな値を算
出するといったことが起こり、後者の場合は軸が高速回
転してマニピュレータが暴走してしまう。

【０００５】ここで、図８に示されるように６つの軸７
１〜７６およびハンド７７を備えたマニピュレータ７０
では、図９に示されるような３つの特異姿勢が例として
考えられる。この場合、（ａ）は手首特異姿勢を示して
おり、軸７４と軸７６とが一直線上で、この姿勢からハ
ンド７７すなわち、手先をＹ方向に移動させるか、手先
をＹ方向に移動させながらこの姿勢を通過しようとする
と、軸７４，７６に逆方向で非常に大きい軸角速度が指
令され、マニピュレータが暴走する。このとき、手先を
Ｚ方向に移動させたとしても暴走は起こらない。同様
に、（ｂ）は軸７２，７５が一直線上にある肩特異姿
勢、（ｃ）は軸７１，７３，７５が一直線上にある肘特
異姿勢を示しており、それぞれ特定の方向に手先を移動
させるたときにマニピュレータが暴走がする。

【０００６】ここで、図１０に示されるように、異なる
特異姿勢をドット８１，８２で表わすならば、各特異姿
勢に対応する特異点近傍はドット８１，８２を含む大き
さの異なる領域８３，８４によって表される。つまり、
あるマニピュレータについて複数の特異姿勢が存在した
とき、特異点近傍の大きさや形は特異姿勢毎にそれぞれ
異なり、手先が特異点へと向かう方向によっても変化す
る。

【０００７】ところで、従来から６軸以下のマニピュレ
ータの特異姿勢は有限個で、手先位置姿勢速度と軸角速
度との関係を示すヤコビ行列Ｊに着目すると、行列式
ｄｅｔＪ＝０という条件で各特異姿勢に対応する各軸
角度が一意に決まり、これらを数学的に算出できること
が知られている。

【０００８】このため、従来は６軸以下のマニピュレー
タを駆動する場合、特異姿勢となる各軸角度の組み合わ
せを予め求めておき、これらの値を駆動中の各軸角度と
比較することにより、マニピュレータが特異姿勢になる
ことを検知している。

【０００９】また、マニピュレータを駆動しながらヤコ
ビ行列Ｊの特異値分解を行うことにより、ヤコビ行列
Ｊの行列特性を求め、その結果に基づいてマニピュレ
ータが特異姿勢になることを検知する特異姿勢検知方法
もある。この場合、特異値分解の結果からヤコビ行列
Ｊの行列式ｄｅｔＪを求め、その値が０に近づけば
特異姿勢（行列特性が低下している）に近づいていると
判定する。

【００１０】一方、６軸以下のマニピュレータの特異姿
勢が一意に決まることから、従来の６軸以下のマニピュ
レータの特異姿勢回避方法として、予めマニピュレータ
が特異姿勢とならないように作業軌道を設定しておく方
法や、逆に設定された作業軌道内で特異姿勢とならない
ようにマニピュレータのリンク長およびリンク構成を設
定する方法が知られている。

【００１１】

【発明が解決しようとする課題】上述した従来の特異姿
勢検知方法のうち、予め求めておいた特異姿勢に対応す
る軸角度と実際の軸角度を比較する方法は、マニピュレ
ータの特異姿勢が有限で一意に決定されることを前提と
しているため、７軸以上の軸を有するマニピュレータ
（以下、冗長マニピュレータと称する）には適用するこ
とができないという問題がある。これは、冗長マニピュ
レータでは、手先位置姿勢が決まっても、それを満たす
多くの軸角度の組合せが存在し、それだけでは全体の姿
勢が決定されないため、６軸以下のマニピュレータのよ
うに特異姿勢を数学的に求めることができないことが原
因となっている。

【００１２】また、ヤコビ行列Ｊの特異値分解を行っ
て行列式ｄｅｔＪを求める方法では、特異値分解のた
めに複雑な演算が必要であり、処理に時間がかかるとい
う問題がある。また、行列式ｄｅｔＪが０に近づいて
いるか否かを判定するための判定値は、特異姿勢毎にか
なりのバラつきがあり一意に決めることはできない。仮
に、適切な判定値が得られたとしても、行列式ｄｅｔ
Ｊの値のみに着目し、特異点近傍の大きさの違いや手
先が特異点に向かう方向などを考慮していないため、検
知が遅れてマニピュレータが暴走してしまったり、暴走
の起こらない方向から手先が近づいた場合を異常と検知
することもある。

【００１３】一方、従来の特異姿勢回避方法も、マニピ
ュレータの特異姿勢が有限で一意に求められることを前
提としているため、冗長マニピュレータに適用すること
ができないという問題がある。また、複雑な特異姿勢の
データに基づいて作業軌道やリンク構成を予め設定しな
ければならないため、経験の豊かな者でないと設定を行
えず、設定自体にも多くの時間を必要とする。さらに、
作業の一つ一つに対応しなければならないため、汎用的
にマニピュレータを使用するには不向きで、しかも最終
的に設定された作業軌道には、所望とする作業本来の軌
道に比べて余分な動き、例えば軌道の変更などが含まれ
ているため、その分だけ余計な作業時間がかかるという
問題がある。

【００１４】本発明は、このような問題を解決するため
になされたもので、７軸以上の冗長マニピュレータの特
異姿勢を検知することができる冗長マニピュレータの特
異姿勢検知方法を提供することを目的とする。

【００１５】また、本発明の他の目的は７軸以上の冗長
マニピュレータを特異姿勢を回避させながら駆動するこ
とができる冗長マニピュレータの特異姿勢回避方法を提
供することにある。

【００１６】

【課題を解決するための手段】本発明の冗長マニピュレ
ータの特異姿勢検知方法は、少なくとも７つの軸を有す
る冗長マニピュレータの特異姿勢検知方法において、冗
長マニピュレータの各軸角速度と手先位置姿勢速度との
関係を示すヤコビ行列式Ｊとその転置行列式Ｊ^T と
の積ＪＪ^T の次式に示すデターミナントＤ

【００１７】

【数３】 の減少傾向および冗長マニピュレータの各軸角速度に基
づいて、冗長マニピュレータの姿勢が特異姿勢に近づい
ていることを検知する。このとき、行列式の値の減少傾
向は、行列式の微分値の変化に基づいて判断することが
望ましい。

【００１８】また、本発明の冗長マニピュレータの特異
姿勢回避方法は、少なくとも７つの軸を有する冗長マニ
ピュレータの特異姿勢回避方法において、冗長マニピュ
レータの各軸角速度と手先位置姿勢速度との関係を示す
ヤコビ行列式Ｊとその転置行列式Ｊ^T との積Ｊ
Ｊ^T の次式に示すデターミナントＤ

【００１９】

【数４】 の減少傾向および冗長マニピュレータの各軸角速度に基
づいて、冗長マニピュレータの姿勢が特異姿勢に近づい
ているか否かを検知し、冗長マニピュレータの姿勢が特
異姿勢に近づいていると検知されたとき、行列式の値が
増加するように冗長マニピュレータを制御する第１の特
異姿勢回避方法と、冗長マニピュレータの各軸角速度を
減少させる第２の特異姿勢回避方法とを組み合わせて行
うことにより、冗長マニピュレータの姿勢を特異姿勢か
ら回避する。

【００２０】

【発明の実施の形態】図１は、本発明の一実施形態に係
る冗長マニピュレータの特異姿勢検知方法および特異姿
勢回避方法を適用した冗長マニピュレータの制御方法を
示すフローチャートであり、図２はこのフローチャート
に従って制御される冗長マニピュレータの構成を示して
いる。

【００２１】図２に示されるとおり、この冗長マニピュ
レータは７つの軸（関節）２，４，６，８，１０，１
２，１４を備えた７自由度のマニピュレータである。ベ
ース面１６にアーム１が設置され、このアーム１から軸
２を介してアーム３が設けられ、アーム３から軸４を介
してアーム５が設けられている。アーム５からは、軸６
を介してアーム７が設けられており、アーム７からは軸
８を介してアーム９が設けられている。アーム９から
は、軸１０を介してアーム１１が設けられており、アー
ム１１からは軸１２を介してアーム１３が設けられてい
る。アーム１３からは軸１４を介してハンド１５が設け
られている。ハンド１５は、この冗長マニピュレータの
手先として設けられており、塗装作業や溶接作業を行う
ときはハンド１５の代わりに塗装ガンや溶接トーチを設
置してもよい。

【００２２】７つの軸２，４，６，８，１０，１２，１
４のなかで軸６は、この冗長マニピュレータにおいて姿
勢を決定するキーとなる軸、すなわち冗長軸である。冗
長マニピュレータ全体の冗長性は、冗長軸である軸６の
軌跡となって現れ、軸６を制御することによって手先位
置姿勢を変えずに全体の姿勢を変えることができるよう
になっている。ただし、マニピュレータのリンク構成、
軸数によって冗長軸となる軸は変化する。

【００２３】このような冗長マニピュレータを駆動する
場合、手先位置および手先姿勢をそれぞれ３要素のデー
タで示した計６要素のデータからなる手先位置姿勢値を
定義し、所望とする手先位置姿勢の軌道に従った幾つか
のポイントの手先位置姿勢値を予め冗長マニピュレータ
に与えておく。

【００２４】この場合、冗長マニピュレータは一定のサ
ンプリング周期で（ここでは１０ｍｓとする）、その時
の手先位置姿勢値と与えられた手先位置姿勢値とを比較
して、手先位置姿勢に対応した６要素の手先位置姿勢速
度ｒ′を求める（ステップＳ１）。

【００２５】次に、手先位置姿勢速度ｒ′と各軸２，
４，６，８，１０，１２，１４の軸角速度（以下、関節
角速度と称する）θ′との関係を示すヤコビ行列Ｊを
求める（ステップＳ２）。このヤコビ行列Ｊを用いる
ことで、手先位置姿勢速度ｒ′と関節角速度θ′とは関
係式ｒ′＝Ｊθ′によって表される。ただし、手先位
置姿勢速度ｒ′は６要素で、関節角速度θ′は７要素で
あるため、ヤコビ行列Ｊは６×７行列となる。

【００２６】次に、この関係式ｒ′＝Ｊθ′を変形し
て得られる次式によって、手先位置姿勢速度ｒ′から関
節角速度θ′を求める（ステップＳ３）。 θ′＝Ｊ^# ・ｒ′＋（Ι−Ｊ^# ・Ｊ）・ξ（θ）・Ｋ ……（１） ただし、Ｊ^# は７×６疑似ヤコビ行列、ξ（θ）は評
価関数と呼ばれる７×１方向ベクトル、Ｋは任意のスカ
ラー値である。また、右辺第１項目のＪ^# ・ｒ′は特
殊解と呼ばれ、冗長マニピュレータを目標とする手先位
置姿勢に移動させるための最も移動量が小さい解であ
り、手先位置姿勢速度ｒ′によって一意に決まる。ま
た、右辺第２項の（Ι−Ｊ^# ・Ｊ）・ξ（θ）・Ｋ
は同次項と呼ばれ、この値によって冗長マニピュレータ
の手先位置姿勢は変わらずに、冗長マニピュレータ全体
の姿勢（肘の位置）のみが変化する。同次項のうち評価
関数ξ（θ）によって冗長マニピュレータ全体の姿勢が
定まる。すなわち、この評価関数ξ（θ）の値を調節す
ることにより、冗長マニピュレータの冗長性を制御する
ことができ、同じ手先位置姿勢のままで無限個の姿勢を
とることができる。

【００２７】ここでは、予め設定された条件に従って評
価関数ξ（θ）を適当に決定し、それに対応する関節角
速度θ′で冗長マニピュレータを駆動して、後述するよ
うにして手先位置が特異点近傍に近づいているか否かを
検知する（ステップＳ４）。

【００２８】このとき、冗長マニピュレータの手先位置
が特異点近傍に近づいていなければ（ステップＳ５でＮ
ｏ）、そのまま通常通りに冗長マニピュレータを駆動
し、手先位置が特異点近傍に近づいていれば（ステップ
Ｓ５でＹｅｓ）、後述するように冗長マニピュレータの
冗長性を利用して、手先位置姿勢の軌道そのものは変化
させることなく、冗長マニピュレータの姿勢のみを変化
させて冗長マニピュレータを特異姿勢から回避させる
（ステップＳ６）。

【００２９】このようなステップＳ１〜ステップＳ６の
処理をサンプリング周期毎に繰り返すことによって、冗
長マニピュレータが特異姿勢に近づいているか否かを自
動的に検知し、特異姿勢に近づいているときはそれを回
避しながら冗長マニピュレータを駆動して、最終的な目
標まで手先位置姿勢を変化させる。

【００３０】次に、ステップＳ４で示される特異姿勢検
知処理について、図３のフローチャートを参照して詳細
に説明する。この場合、まず特異点近傍を検知するため
のデータの一つとして、図１中のステップＳ２で得られ
たヤコビ行列Ｊの行列式（以下、デターミナントと称
する）Ｄを求める（ステップＳ２１）。このとき、冗長
性のない６軸以下のマニピュレータであればヤコビ行列
Ｊは６×６の正方行列となるが、７軸の冗長マニピュ
レータののときはヤコビ行列Ｊは６×７であり正方行
列ではないので、ヤコビ行列Ｊとその転置行列Ｊ^T
とによりデターミナントＤは次式のように定義される。

【００３１】

【数５】

【００３２】この定義に従ってデターミナントＤを算出
する方法の一つとして、特異値分解を行って固有値を求
め、求められた固有値からデターミナントＤを得る方法
がある。しかし、この方法では処理に非常に時間がかか
り、サンプリング周期の１０ｍｓ毎にデターミナントＤ
を求めることは難しい。そこで、以下に述べるようにガ
ウス・ザイデル法を用い、最小限の計算量でデターミナ
ントＤを求める。具体的には、まずヤコビ行列Ｊとそ
の転置行列Ｊ^T とを掛け合わせて、次式に示されるよ
うな６×６行列Ｊ×Ｊ^T を求める。

【００３３】

【数６】

【００３４】次に、行列Ｊ×Ｊ^T の１列目要素（Ａ
₁₁，Ａ₂₁，Ａ₃₁，Ａ₄₁，Ａ₅₁，Ａ₆₁）のなかから絶対値
の最も大きい要素（ピボットと呼ぶ）を探し、ピボット
の存在する行を１行目と交換する。例えば、要素Ａ₃₁が
ピボットであったとすると、次式に示されるように３行
目と１行目とが交換される。

【００３５】

【数７】

【００３６】次に、１列目のピボット以下の要素の値が
すべて０になるように行列演算を行う。先の例と同様
に、要素Ａ₃₁がピボットであったとすると、２行目の各
要素に対しては次式のような行列演算が行われる。 ２行目１列：Ａ₂₁−Ａ₃₁×Ａ₂₁／Ａ₃₁＝０ ２行目２列：Ａ₂₂−Ａ₃₂×Ａ₂₁／Ａ₃₁＝Ｂ₁₁ ２行目３列：Ａ₂₃−Ａ₃₃×Ａ₂₁／Ａ₃₁＝Ｂ₁₂ ２行目４列：Ａ₂₄−Ａ₃₄×Ａ₂₁／Ａ₃₁＝Ｂ₁₃ ２行目５列：Ａ₂₅−Ａ₃₅×Ａ₂₁／Ａ₃₁＝Ｂ₁₄ ２行目６列：Ａ₂₆−Ａ₃₆×Ａ₂₁／Ａ₃₁＝Ｂ₁₅ 以下同様に、３行目以下の各要素についても演算を行
い、行列Ｊ×Ｊ^T を次式に示されるように変形す
る。

【００３７】

【数８】

【００３８】次に、行列Ｊ×Ｊ^T のうち０でない値
を有する要素Ｂ_ij（ｉ＝１〜５，ｊ＝１〜５）の部分を
対象として、同様にピボット探索、行交換、ピボット列
の要素消去を行って要素Ｃ_ij（ｉ＝１〜４，ｊ＝１〜
４）を求める。そして、以下同じように処理を繰り返す
ことにより、行列Ｊ×Ｊ^T を最終的に次式に示され
るような右上三角行列に変形する。

【００３９】

【数９】

【００４０】この場合、対角要素Ａ₁ ，Ｂ₁ ，Ｃ₁ ，Ｄ
₁ ，Ｅ₁ ，Ｆ₁ を互いに掛け合わせた値がデターミナン
トＤとなる。つまり、デターミナントＤは次式のように
示される。

【００４１】Ｄ＝Ａ₁ ×Ｂ₁ ×Ｃ₁ ×Ｄ₁ ×Ｅ₁ ×Ｆ₁ このデターミナントＤが０のとき冗長マニピュレータが
特異姿勢、つまり手先位置が特異点となることから、理
論上は冗長マニピュレータの暴走が起こり得る特異点近
傍とそれ以外の領域とを区別するためにデターミナント
Ｄの境界値を適当に設定しておき、その境界値と実際の
デターミナントＤの値を比較して、冗長マニピュレータ
の手先位置が特異点近傍にあるか否かを検知すればよ
い。

【００４２】しかし、特異点近傍の大きさは特異姿勢毎
に異なり、手先が特異点に向かう向きによっても変化す
ることから、通常はそのような境界値を設定することが
難しく、またデターミナントＤの値が１２ケタの正数で
あっても、冗長マニピュレータが特異姿勢となって暴走
することもある。

【００４３】そこで、さらにターミナントＤの微分値
Ｄ′を求め（ステップＳ２２）、この微分値Ｄ′に基づ
いてデターミナントＤの時間的変化、特に減少傾向を調
べ、その結果から以下のようにして冗長マニピュレータ
の手先位置が特異点近傍に近づいているか否かを判断す
る。

【００４４】ここで、この冗長マニピュレータが駆動中
に特異姿勢となり暴走したときのデターミナントＤとそ
の微分値Ｄ′を調べておき、その結果が図４に示される
ようになったとする。（ａ）はデターミナントＤ、
（ｂ）はその微分値Ｄ′の時間的変化を示し、それぞれ
横軸は時間ｔ、縦軸はデターミナントＤもしくは微分値
Ｄ′の値を表しており、（ａ）において線２１がデター
ミナントＤを示し、線２２がエラーを示している。

【００４５】この場合、時間ｔ＝ｔ１において暴走が起
こっており、それまでのデターミナントＤの時間的変化
を調べると、連続してデターミナントＤが減少し、微分
値Ｄ′（減少率）は増大しながら徐々に特異姿勢に近づ
いている。また、暴走する直前の時間ｔ＝ｔ２において
微分値Ｄ′が最小値に達している。このとき、関節角速
度θ′は冗長マニピュレータのモータが有するリミット
速度の約３０％といった非常に大きな値になっている。
すなわち、冗長マニピュレータの手先位置が特異点近傍
に近づくと、デターミナントＤが連続して減少し、関節
角速度θ′が異常に増大することがわかる。

【００４６】従って、これらデターミナントＤの増減
や、関節角速度θ′の大きさに着目すれば、デターミナ
ントＤの値のみに着目したときに比べて、より適切に特
異姿勢検知を行うことができると考えられる。しかし、
上述したように特異点近傍の大きさは特異姿勢によって
それぞれ異なり、特異点に向かう方向によっても変化す
るので、これらの値に着目したとしても、どの時点を特
異点近傍とするかは後述する特異姿勢回避法との関わり
もあって一意には決められない。一方、一般的に考えて
特異姿勢回避が容易にできるような、暴走の危険の小さ
い段階を特異点近傍と判断しておいた方がよいと思われ
る。

【００４７】そこで、本実施形態では図５に示されるよ
うにポイントＡからＢまでの間に特異点Ｓが存在したと
き、その特異点Ｓから外に広がる５つの特異点近傍空
間、すなわち暴走する危険の高い順に第１の特異点近傍
空間３１、第２の特異点近傍空間３２、第３の特異点近
傍空間３３、第４の特異点近傍空間３４および第５の特
異点近傍空間３５を設定する。そして、第１から第５の
特異点近傍空間をそれぞれ特異点近傍段階１〜５に対応
させ、各特異点近傍段階に対して図６に示されるような
特異点近傍検知判断条件を定めて、手先位置が特異点近
傍に近づいているか否かを段階的に検知する。なお、図
６には後述する各特異点近傍段階に対応する特異姿勢回
避方法も併せて示されている。

【００４８】この場合、ステップＳ２３においてデター
ミナントＤの時間的な変化を調べ、デターミナントＤが
連続１００回（α＝１００）以上減少し、かつ関節角速
度θ′の要素の一つでもリミット速度のβ％（β＝１，
５，８，１２，１５）を越えていれば（ステップＳ２４
でＹｅｓ）、手先位置が上述した第１から第５のいずれ
かの特異点近傍空間内であると判断し（ステップＳ２
５）、そのときの関節角速度θ′の値に従って特異点近
傍段階（１〜５）を決める（ステップＳ２６）。

【００４９】この際、最初にデターミナントＤが連続１
００回以上減少し、かつ関節角速度θ′の要素がリミッ
ト速度の１％を越えたとき、手先位置が特異点近傍空間
外から特異点近傍空間３５に入ったものとして特異点近
傍段階を５に設定する。さらに、デターミナントＤが連
続１００回以上減少し、関節角速度θ′の要素が１つで
もリミット速度の５％を越えたとき、特異点近傍段階を
４に設定するといったように、特異点近傍段階を５から
１まで段階的に一つづつ変化させる。

【００５０】なお、αおよびβの値は、冗長マニピュレ
ータやその移動データに依存しており、冗長マニピュレ
ータの特性、作業内容の特性などによって調整可能なパ
ラメータである。例えば、もうすこし早い時点で特異点
近傍に入ったことを検知したい場合は、これらα，βの
値を小さくすればよい。

【００５１】一方、ステップＳ２３でデターミナントＤ
の時間的な変化を調べた結果、デターミナントＤが連続
１００回（α＝１００）以上増大していたときは、逆に
手先位置が特異点近傍空間の外側に向かっていると判断
し（このときは関節角速度θ′に関する条件はない）、
特異点近傍段階を一つ戻す（ステップＳ２８）。

【００５２】次に、上述したようにして冗長マニピュレ
ータの手先位置が特異点近傍にあると検知されたときの
特異姿勢回避処理について詳細に説明する。この場合、
デターミナントＤが連続して減少しており、関節角速度
θ′が大きくなっている状態である。

【００５３】このとき、デターミナントＤが増加するよ
うに冗長マニピュレータを制御する第１の特異姿勢回避
方法と、冗長マニピュレータの手先位置姿勢速度ｒ′を
減少させる第２の特異姿勢回避方法とを組み合わせて用
いることを考える。

【００５４】第１の特異姿勢回避方法の場合、冗長マニ
ピュレータの冗長性を利用して、（１）式に含まれる評
価関数ξ（θ）を適当に調節することにより、デターミ
ナントＤが増加し、全体の姿勢が向かいつつある特異姿
勢とは逆の姿勢になるように、すなわち手先位置を特異
点に向かう方法とは逆方向に移動させる。このとき、手
先位置姿勢の変化を一旦停止させ、冗長マニピュレータ
全体の姿勢を特異姿勢から遠ざけるようにすることで、
手先位置姿勢の軌道を変えることなくデターミナントＤ
を増加方向に向かわせる。

【００５５】一方、第２の特異姿勢回避方法の場合、冗
長マニピュレータを通常状態で駆動していたとき、サン
プリング周期毎に現在位置から次の目標値まで一定速度
で到達できるように分割して得られた１サンプリング毎
の手先位置姿勢速度ｒ′を、さらに細かく分割すること
により手先位置姿勢速度ｒ′を減少させて、対応する関
節角速度θ′を小さくする。ただし、速度を減少させる
ので、作業にかかる時間そのものは少し増大する。

【００５６】ここで、これらの第１および第２の特異姿
勢回避方法は、図６に示されるように手先位置の特異点
近傍段階に応じて組み合わされる。つまり、暴走の危険
性が最も小さい特異点近傍段階５では、まず第２の特異
姿勢回避方法を用いて通常速度の０．１倍に手先位置姿
勢速度ｒ′を落とす。次の、特異点近傍段階４では一定
時間（例えば１ｓ）の間だけ第１の特異姿勢回避方法を
用いて、冗長マニピュレータ全体の姿勢を特異姿勢から
遠ざける。特異点近傍段階３，２，１では、第１および
第２の特異点回避方法を共に用いることにより、冗長マ
ニピュレータ全体の姿勢を特異姿勢から遠ざけたまま、
手先位置姿勢速度ｒ′をさらに落とす。

【００５７】このようにして、第１および第２の特異姿
勢回避方法を組み合わせて用いながら冗長マニピュレー
タを駆動した場合、デターミナントＤおよびその微分値
Ｄ′の変化は図７に示されるようになる。ここで、
（ａ）はデターミナントＤ、（ｂ）はその微分値Ｄ′の
時間的変化を示し、それぞれ横軸は時間ｔ、縦軸はデタ
ーミナントＤもしくは微分値Ｄ′の値を表している。

【００５８】同図に示されるように、上述した特異姿勢
回避方法を用いなかったときは、点線４２に示されるよ
うにデターミナントＤが急激に減少して暴走が起こって
いるのに対し、特異姿勢回避方法を用いたときは、実線
４２で示されるようにｔ＝ｔ１において特異点近傍段階
が５になり、ｔ＝ｔ２において特異点近傍段階が４にな
り、ｔ＝３において特異点近傍段階が３になり、ｔ＝４
において特異点近傍段階が４になり、ｔ＝５において特
異点近傍段階が５になるといったように、手先位置が徐
々に特異点に近づいていき、それ以後はｔ＝ｔ６におい
て特異点近傍段階が２に戻り、さらにｔ＝７において特
異点近傍段階が３に戻って、以下特異点から徐々に遠ざ
かっている。

【００５９】このように、各特異点近傍段階において第
１および第２の特異姿勢回避方法を組み合わせて用いた
ことにより、冗長マニピュレータを暴走させることな
く、手先位置姿勢が軌道を変えずに目標地点まで到達す
る。

【００６０】以上のように、本実施形態の特異姿勢検知
方法によれば、ヤコビ行列ＪのデターミナントＤとそ
の微分値Ｄ′とを算出して、デターミナントＤの時間的
な増減傾向を調べ、デターミナントＤがα回連続に減少
し、かつ関節角速度θ′がリミット速度のβ％といった
大きな値になっているとき、冗長マニピュレータが特異
姿勢に近づいているものと検知する。

【００６１】すなわち、デターミナントＤの値だけで特
異姿勢検知を行うのではなく、その微分値Ｄ′と各関節
各速度θ′との関係を調べることによって、特異姿勢が
一意に求められないような７軸以上の冗長マニピュレー
タでも特異姿勢検知を行うことができる。しかも、マニ
ピュレータの移動方向によって異なる特異点近傍の境界
を考慮しているので、手先位置が暴走の起こらない方向
から特異点に近づいた場合などは無視され、常に適切に
特異姿勢検知を行うことができる。

【００６２】一方、本実施形態の特異姿勢回避方法によ
れば、冗長マニピュレータの冗長性を利用して特異姿勢
を回避しているので、予め特異姿勢を求めておくことが
できない冗長マニピュレータでも特異姿勢を回避するこ
とができる。しかも、冗長軸を用いることによって、手
先位置姿勢を変化させずに全体の姿勢を変化させている
ので、従来のように手先位置姿勢の無駄な軌道変更が必
要なくなる。

【００６３】また、冗長マニピュレータが特異姿勢に近
づいたことを自動的に検知して特異姿勢回避を行い、特
異姿勢の近傍を通過した後は自動的に特異姿勢回避を終
了して通常通りの制御に戻るようにして、リアルタイム
で冗長マニピュレータの特異姿勢回避を行っている。つ
まり、従来６軸以下のマニピュレータに対して行ってい
たように、マニピュレータのリンク構成やリンク長の調
整、作業内容の調整などを予め行っておく必要がなくな
り、作業者は特異姿勢回避に関わる処理を全てマニピュ
レータに任せ、特異姿勢のことは全く考慮せずに作業軌
道を決めることができる。従って、経験の浅い作業者で
も冗長マニピュレータのティーチング作業を簡単に行え
るようになり、作業時間が短縮され、作業員の数も軽減
する。

【００６４】この結果、冗長マニピュレータによる作業
全般、すなわち溶接作業や塗装作業、さらに複雑な障害
物を回避する作業など汎用マニピュレータに必要とされ
る様々な作業を従来よりも容易、かつ適切に行うことが
できる。

【００６５】

【発明の効果】以上述べたように、本発明にかかる冗長
マニピュレータの特異姿勢検知方法によれば、冗長マニ
ピュレータの各軸角速度と手先位置姿勢速度との関係を
示すヤコビ行列の行列式の値の減少傾向および冗長マニ
ピュレータの各軸角速度に基づいて、冗長マニピュレー
タの姿勢が特異姿勢に近づいていることを検知している
ので、７軸以上の冗長マニピュレータでも特異姿勢検知
を行うことができ、しかも冗長マニピュレータの移動方
向によって異なる特異点近傍を考慮できるので、従来の
６軸マニピュレータに適用されていた特異姿勢検知方法
に比べて、適切に特異姿勢検知を行うことが可能にな
る。

【００６６】また、本発明にかかる冗長マニピュレータ
の特異姿勢回避方法によれば、上述した特異姿勢検知方
法によってマニピュレータの姿勢が特異姿勢に近づいて
いると検知されたとき、行列式の値が増加するように冗
長マニピュレータを制御する第１の特異姿勢回避方法
と、冗長マニピュレータの各軸角速度を減少させる第２
の特異姿勢回避方法とを組み合わせて行うことにより、
冗長マニピュレータでも特異姿勢回避を行うことができ
るようになる。しかも、冗長マニピュレータの冗長性を
利用して、手先位置姿勢を保ったまま全体の姿勢を変化
させて、リアルタイムで冗長マニピュレータを特異姿勢
から回避させているので、従来のように予めマニピュレ
ータの特異姿勢を調べてリンク構成や作業経路の設定を
行う必要がなくなり、冗長マニピュレータを用いた作業
を容易かつ短時間で行うことができる。

【図面の簡単な説明】

【図１】本発明の一実施形態に係る冗長マニピュレータ
の特異姿勢検知方法および特異姿勢回避方法を適用した
冗長マニピュレータの制御を示すフローチャート

【図２】図１に示されるフローチャートにしたがって制
御される冗長マニピュレータの構成を示す図

【図３】図１中の特徴点近傍検知処理を説明するための
フローチャート

【図４】図２に示される冗長マニピュレータが暴走した
ときのデターミナントとその微分値の変化の例を示す図

【図５】同実施形態における特異点近傍段階について説
明するための図

【図６】同実施形態における特異点近傍段階の各段階の
条件データおよび特異姿勢回避

【図７】同実施形態の制御方法を適用したときの冗長マ
ニピュレータのデターミナントとその微分値を示す図

【図８】従来の６軸マニピュレータのリンク構成の例を
示す図

【図９】図８に示したマニピュレータの特異姿勢の例を
示す図

【図１０】従来のマニピュレータにおいて特異姿勢と特
異点近傍の大きさとの関係を説明するための図

【符号の説明】

１，３，５，７，９，１１，１３…アーム ２，４，６，８，１０，１２，１４…軸 １５…ハンド １６…ベース面

Claims (3)

【特許請求の範囲】
1. 【請求項１】少なくとも７つの軸を有する冗長マニピュ
   レータの特異姿勢検知方法において、 前記冗長マニピュレータの各軸角速度と手先位置姿勢速
   度との関係を示すヤコビ行列式Ｊとその転置行列式
   Ｊ^T との積ＪＪ^T の次式に示すデターミナントＤ 【数１】 の減少傾向および前記冗長マニピュレータの各軸角速度
   に基づいて、前記冗長マニピュレータの姿勢が特異姿勢
   に近づいていることを検知することを特徴とする冗長マ
   ニピュレータの特異姿勢検知方法。
2. 【請求項２】前記デターミナントＤの減少傾向は、前記
   デターミナントＤの微分値の変化に基づいて判断するこ
   とを特徴とする請求項１に記載の冗長マニピュレータの
   特異姿勢検知方法。
3. 【請求項３】少なくとも７つの軸を有する冗長マニピュ
   レータの特異姿勢回避方法において、 前記冗長マニピュレータの各軸角速度と手先位置姿勢速
   度との関係を示すヤコビ行列式Ｊとその転置行列式
   Ｊ^T との積ＪＪ^T の次式に示すデターミナントＤ 【数２】 の減少傾向および前記冗長マニピュレータの各軸角速度
   に基づいて前記冗長マニピュレータの姿勢が特異姿勢に
   近づいているか否かを検知し、 前記冗長マニピュレータの姿勢が特異姿勢に近づいてい
   ると検知されたとき、前記行列式の値が増加するように
   前記冗長マニピュレータを制御する第１の特異姿勢回避
   方法と、前記冗長マニピュレータの各軸角速度を減少さ
   せる第２の特異姿勢回避方法とを組み合わせて行うこと
   により、前記冗長マニピュレータを特異姿勢から回避す
   ることを特徴とする冗長マニピュレータの特異姿勢回避
   方法。