JP2009148894A - 多関節ロボット - Google Patents

Abstract

【課題】 多関節ロボットの冗長関節部を角変位させて特異点を回避することができ、また遊端部の姿勢が制御できる多関節ロボットを提供する。
【解決手段】 第１〜第６アーム体と、基台とを含んで構成される。第１アーム体の一端部は、基台に、第１関節部によって連結され、第２アーム体の一端部は、第２関節部によって、第１アーム体の他端部に連結される。第３アーム体の一端部は、第３関節部によって、第２アーム体の他端部に連結され、第４アーム体の一端部は、第４関節部によって、第３アーム体の他端部に連結される。第５アーム体の一端部は、第５関節部によって、第４アーム体の他端部に連結され、第６アーム体の一端部は、第６関節部によって、第５アーム体の他端部に連結される。また第６アーム体の他端部には、エンドエフェクタに接続可能な第７関節部が設けられる。
【選択図】 図１

Description

本発明は、冗長関節部を含む複数の関節部を有する多関節ロボットに関する。

７つ以上の複数の関節部を有する多関節ロボットの制御については、様々な方式があり、その典型的な従来技術が特許文献１および特許文献２に記載されている。特許文献１の多関節ロボットの制御装置では、多関節ロボットの先端関節が目標位置に距離的に近づくような関節角度を最小２乗法によって１軸ずつ求め、求めた目標角度と現在の関節角度との差に比例した量を１軸ずつ関節を回転させることによって、計算量を少なくして、先端の関節が目標位置に達するようにしている。特許文献２の多自由度マニピュレータの位置決め装置では、手先位置姿勢情報とマニピュレータの全体形状の指定値とから全体形状を指定するための解曲線を求め、全リンクの端点が前記解曲線状に配置されるようにリンク端点位置を算出し、隣接する２リンクのリンク端点と根元側リンクの姿勢とから２リンク間の関節角を算出し、前記関節角から手先側リンクの姿勢を求め、リンク毎に根元から手先まで逐次的に前記関節角とリンク姿勢とを繰返し算出して全関節角を求めることで、位置決めの算出を高速化する。

また前述のような多関節ロボットが障害物を回避するような制御についても様々な方式があり、その典型的な従来技術が特許文献３に記載されている。特許文献３のマニピュレータの制御装置では、始点から終点へ把手を移動するときに、障害物に干渉する状態での各関節の変位を予め離散的に求め、各関節の変位を避けるように各関節の離散的な動作間をマニピュレータの関節補間または単軸動作によって制御する。

また前述のような多関節ロボットを教示するための必要な情報を表示する典型的な従来技術が、特許文献４に記載されている。特許文献４の冗長自由度ロボット装置では、作業に必要な動作を記述する独立変数の現在値に依存する冗長パラメータの取り得る領域の演算を行って、その結果を表示することでロボット教示を行う。

ロボットの空間内の各方向に対する制御仕様を実現する典型的な従来技術が、特許文献５に記載されている。特許文献５のロボットの制御方法では、ロボットに行わせる作業の仕様を作業空間に対して固定された座標系を用いて記述し、各座標軸方向および各座標軸まわりの方向の動作制御目標を明確化し、前記各方向へのロボットの姿勢状態のそれぞれに対応する方向への作業仕様に関する制御性能評価指標を計算し、それらの加重総合評価指標値が最も大きな値となるロボットの姿勢状態を最適姿勢状態とし、この状態で作業を実行させる。

ロボットの動作限界近傍を回避した動作を教示データを作成する典型的な従来技術が、特許文献６に記載されている。特許文献６のロボットの動作軌跡教示装置では、冗長自由度を要素として、ロボットの関節角度空間の余裕を表現する評価関数に基づいて、前記冗長自由度を決定する。

また図５に示すように、従来技術における典型的な形状の関節部を有する多関節ロボットが、特許文献７に開示されている。この多関節ロボットにおいて、基台１７Ａおよび基端部３Ａを除く他の関節部は、いずれの関節部の回転軸も各アームの軸線方向に対して角度を成して形成されている。

特開２０００−１６７７８９号公報 特開平１０−３３３７２３号公報 特開平１１−３３３７８０号公報 特開平５−３３７８６１号公報 特開平５−２３９８２号公報 特開平６−１８７０２０号公報 実開平４−１１５５９２号公報

前述の従来技術では、多関節ロボットの関節部のうち、冗長軸まわりに角変位する冗長関節部を角変位させて、エンドエフェクタなどの遊端部の特定方向への自由度が失われる特異点を回避するという制御は行われていない。また従来技術の多関節ロボットの複数の関節部は、遊端部の姿勢を決定するための関節部として制御しにくいという問題点がある。

本発明の目的は、多関節ロボットの冗長関節部を角変位させて特異点を回避することができ、また遊端部の姿勢が制御できる多関節ロボットを提供することである。

本発明（１）は、多関節ロボットであって、
複数の関節部によって連結される複数のアーム体および基台を有し、
第１アーム体の一端部は、基台に、第１関節部によって、第１アーム体の軸線と同軸の第１回転軸線まわりに回転自在に連結されており、
第２アーム体の一端部は、第２関節部によって、第１アーム体の軸線および第２アーム体の軸線に対して４５度で傾斜する第２回転軸線まわりに回転自在に、第１アーム体の他端部に連結され、
第３アーム体の一端部は、第３関節部によって、第２アーム体の軸線および第３アーム体の軸線に対して４５度で傾斜する第３回転軸線まわりに回転自在に、第２アーム体の他端部に連結され、
第２回転軸線と第３回転軸線とは平行であり、
第４アーム体の一端部は、第４関節部によって、第３アーム体の軸線および第４アーム体の軸線に対して４５度で傾斜する第４回転軸線まわりに回転自在に、第３アーム体の他端部に連結され、
第３回転軸線と第４回転軸線とは垂直であり、
第５アーム体の一端部は、第５関節部によって、第４アーム体の軸線および第５アーム体の軸線と同軸の第５回転軸線まわりに回転自在に、第４アーム体の他端部に連結され、
第６アーム体の一端部は、第６関節部によって、第５アーム体の軸線および第６アーム体の軸線に対して４５度で傾斜する第６回転軸線まわりに回転自在に、第５アーム体の他端部に連結され、
第６アーム体の他端部に、第６アーム体の軸線と同軸の第７回転軸線まわりに回転自在に設けられ、エンドエフェクタに接続可能な第７関節部を有することを特徴とする。

本発明（１）によれば、第１〜第６アーム体と、基台とを含んで構成される。第１アーム体の一端部は、基台に、第１関節部によって連結され、第２アーム体の一端部は、第２関節部によって、第１アーム体の他端部に連結される。第３アーム体の一端部は、第３関節部によって、第２アーム体の他端部に連結され、第４アーム体の一端部は、第４関節部によって、第３アーム体の他端部に連結される。第５アーム体の一端部は、第５関節部によって、第４アーム体の他端部に連結され、第６アーム体の一端部は、第６関節部によって、第５アーム体の他端部に連結される。また第６アーム体の他端部には、エンドエフェクタに接続可能な第７関節部が設けられる。これによって、遊端部の姿勢が制御できる多関節ロボットを実現することができる。また基端部側に設けられる４つの関節によって、遊端部の位置を決定することができる。

本発明の実施の一形態の多関節ロボット１の制御装置２を示すブロック図である。 多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第１の実施例の手順を示すフローチャートである。 多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第２の実施例の手順を示すフローチャートである。 多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第３の実施例の手順を示すフローチャートである。 従来技術にかかる典型的な多関節ロボットの側面図である。

図１は、本発明の実施の一形態の多関節ロボット１の制御装置２を示すブロック図である。多関節ロボット１は、７自由度を有する冗長自由度のロボットであり、複数の、本実施の形態では６つのアーム体３〜８と、アーム体を回転自在に連結する複数の、本実施の形態では７つの関節部９〜１５とを有する。各アーム体３〜８は、それぞれ連結されて直線状に延びるアーム構成体１６を構成する。

各関節部９〜１５は、同軸関節部と傾斜関節部とが組み合わされて設けられる。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する２つのアーム体のうちの一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。詳細に述べると、第１関節部９、第５関節部１３および第７関節部１５は、同軸関節部であり、第２関節部１０、第３関節部１１、第４関節部１２および第６関節部１４は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの３６０度以上の回転だけでなく、軸線まわりに３６０度以下の角度で角変位する状態も含むものとする。

アーム構成体１６の一端部である第１アーム体３の一端部は、壁面などに固定される基台１７に、第１関節部９によって、第１アーム体３の軸線Ｌ３と同軸の第１回転軸線Ｌ９まわりに回転自在に連結されている。第２アーム体４の一端部は、第２関節部１０によって、第１アーム体３の軸線Ｌ３および第２アーム体４の軸線Ｌ４に対して所定の角度、本実施の形態では４５度で傾斜する第２回転軸線Ｌ１０まわりに回転自在に、第１アーム体３の他端部に連結される。第３アーム体５の一端部は、第３関節部１１によって、第２アーム体４の軸線Ｌ４および第３アーム体５の軸線Ｌ５に対して所定の角度、本実施の形態では４５度で傾斜する第３回転軸線Ｌ１１まわりに回転自在に、第２アーム体４の他端部に連結される。第２回転軸線Ｌ１０と第３回転軸線Ｌ１１とは平行である。

第４アーム体６の一端部は、第４関節部１２によって、第３アーム体５の軸線Ｌ５および第４アーム体６の軸線Ｌ６に対して所定の角度、本実施の形態では４５度で傾斜する第４回転軸線Ｌ１２まわりに回転自在に、第３アーム体５の他端部に連結される。第３回転軸線Ｌ１１と第４回転軸線Ｌ１２とは垂直である。第５アーム体７の一端部は、第５関節部１３によって、第４アーム体６の軸線Ｌ６および第５アーム体７の軸線Ｌ７と同軸の第５回転軸線Ｌ１３まわりに回転自在に、第４アーム体６の他端部に連結される。

アーム構成体１６の他端部である第６アーム体８の一端部は、第６関節部１４によって、第５アーム体７の軸線Ｌ７および第６アーム体８の軸線Ｌ８に対して所定の角度、本実施の形態では４５度で傾斜する第６回転軸線Ｌ１４まわりに回転自在に、第５アーム体７の他端部に連結される。多関節ロボット１の遊端部となるエンドエフェクタ１８は、第６アーム体８の他端部に、第７関節部１５によって、第６アーム体８の軸線Ｌ８と同軸の第７回転軸線Ｌ１５まわりに回転自在に連結されている。エンドエフェクタ１８は、たとえば溶接トーチおよびハンドリング装置などの手先装置であってもよい。

各アーム体３〜８は、各アーム体３〜８を回転駆動するモータを内蔵する。また各アーム体３〜８は、その軸線を挿通する中空空間が形成される。この中空空間に１または複数の配線が延びて配置される。各配線は、前記モータを駆動させるための電力、前記モータへの回転指令およびエンドエフェクタ１８への電力および圧縮空気などを供給する。

各アーム体３〜８を回転軸線Ｌ９〜Ｌ１５まわりに回転する回転機構は、従来の機構と同様であってもよい。前記回転機構は、一例を挙げると、中空形状の波動歯車機構、たとえばハーモニックドライブ（登録商標）を用いてもよい。波動歯車機構は、入力側部材と出力側部材とを備え、それらが相対的に回転する。入力側部材は、各関節部９〜１５によって連結する２つの部材（基台およびアーム体）の一方に連結され、出力側部材は、前記２つの部材のうち他方に連結される。モータからの回転が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方のアーム体と他方のアーム体とを相対的に回転させることができる。このような回転機構が、各アーム体３〜８に設けられることによって、隣接する２つのアーム体を相対的に回転させることができる。

制御装置２は、中央演算処理部１９および入力部を備える。中央演算処理部１９は、たとえばＣＰＵおよびメモリなどで実現され、多関節ロボット１を制御するために必要な演算を行って、その演算結果を動作指令として多関節ロボット１に与える。入力部は中央演算処理装置１９に各種情報を入力する。

図２は、多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第１の実施例の手順を示すフローチャートである。ステップｓ０で手順が開始されて、ステップｓ１に進む。

絶対値計算段階であるステップｓ１では、絶対値計算手段である中央演算処理部１９は、前記多関節ロボット１の関節部９〜１５のうち予め定める複数の関節部、本実施の形態では第１〜第４関節部９〜１２の角度位置（θ１，θ２，θ３，θ４）のうち、さらに外部から値が与えられる１つの関節部、本実施の形態では第１関節部９を除く第２〜第４関節部１０〜１２の角度位置（θ２，θ３，θ４）を、多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）に変換する擬似的なヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を計算してステップｓ２に進む。前記ヤコビ行列Ｊは、次式（１）で表される。

判定段階であるステップｓ２では、判定手段である中央演算処理部１９は、前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜が、入力部から入力された予め定める設定値｜ｄｅｔ（Ｊｓｅｔ）｜よりも小さいか否かを判定し、前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜以下であると判定するとステップｓ３に進み、前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜を超えると判定するとステップｓ１０に進む。

冗長関節制御段階であるステップｓ３では、冗長関節制御手段である中央演算処理部１９は、エンドエフェクタ１８を予め定める軌道上に保持しながら前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を大きくするように冗長となる関節部である第１関節部９を角変位してステップｓ４に進む。このとき中央演算処理部１９は、エンドエフェクタ１８の位置および姿勢を保持した状態で、第１関節部９を現在位置から、予め定める変位量Δθ１だけプラスの方向に角変位して、残余の関節部である第２〜第７関節部１０〜１５の角度位置（θ２，θ３，θ４，θ５，θ６，θ７）を逆変換によって求め、さらに第１関節部９を現在位置から、予め定める変位量Δθ１だけマイナスの方向に角変位して、残余の関節部である第２〜第７関節部１０〜１５の角度位置（θ２，θ３，θ４，θ５，θ６，θ７）を逆変換によって求めて、ステップｓ４に進む。

ステップｓ４では、中央演算処理部１９は、ステップｓ３において第１関節部９を現在角度位置から、予め定める変位量Δθ１だけ予め定めるプラスの方向に角変位したときのヤコビ行列Ｊ＋の行列式ｄｅｔ（Ｊ＋）と、第１関節部９を現在角度位置から、予め定める変位量Δθ１だけ前記プラス方向とは逆方向のマイナスの方向に角変位したときのヤコビ行列Ｊ−の行列式ｄｅｔ（Ｊ−）とを求めて、ステップｓ５に進む。

ステップｓ５では、中央演算処理部１９は、ステップｓ３において第１関節部９を現在の角度位置から、予め定める変位量Δθ１だけプラスの方向に角変位したときのヤコビ行列Ｊ＋の行列式ｄｅｔ（Ｊ＋）の絶対値｜ｄｅｔ（Ｊ＋）｜が、第１関節部９を現在の角度位置から、予め定める変位量Δθ１だけマイナスの方向に角変位したときのヤコビ行列Ｊ−の行列式ｄｅｔ（Ｊ−）の絶対値｜ｄｅｔ（Ｊ−）｜を超えるか否かを判断し、超えると判断するとステップｓ６に進み、超えないと判断するとステップｓ９に進む。

ステップｓ６では、中央演算処理部１９は、第１関節部９を現在の角度位置からプラスの方向に角変位して、ステップｓ７に進む。

ステップｓ７では、中央演算処理部１９は、多関節ロボット１の関節部９〜１５のうち冗長関節部である第１関節部９を除く第２〜第７関節部１０〜１５の角度位置（θ２，θ３，θ４，θ５，θ６，θ７）を逆変換によって求めて、ステップｓ８に進み、全ての手順を終了する。

ステップｓ９では、中央演算処理部１９は、第１関節部９を現在の角度位置からマイナスの方向に角変位して、ステップｓ７に進む。

ステップｓ１０では、中央演算処理部１９は、常時最適化を行うか否かを判断し、行うと判断するとステップｓ３に進み、行わないと判断するとステップｓ１１に進む。

ステップｓ１１では、中央演算処理部１９は、多関節ロボット１の関節部９〜１５のうち冗長関節部である第１関節部９を角変位させないで、ステップｓ７に進む。

また中央演算処理部１９は、以上のような絶対値計算段階、判定段階および冗長関節制御段階を繰返して、多関節ロボット１の特異点を回避するように多関節ロボット１を制御してもよい。

本実施例では、第１関節部９を冗長関節部としたけれども、冗長関節部は、第１〜第４関節部９〜１２のいずれか１つであってもよい。本実施例で第１関節部９を冗長関節部としたのは、多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第１関節部９の角度位置θ１から第２〜第４関節部１０〜１２の各角度位置θ２〜θ４を導出することが数学的に最も容易である。一方、たとえば多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第２関節部１０の角度位置θ２から第１、第３および第４関節部９，１１，１２の各角度位置θ１，θ３，θ４を導出することはできないことはないけれども、数学的には煩雑となる。

前述のヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜は、関係する各関節部の各角度位置だけでなく、多関節ロボット１の各アーム体３〜８の長さＬａ，Ｌｂ、Ｌｃ，Ｌｄ，Ｌｅ，Ｌｅ，Ｌｆに依存するので、予め定める設定値｜ｄｅｔ（Ｊｓｅｔ）｜を包括的に最適化することは困難である。このため前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜の決定においては、事前にコンピュータシミュレーション等を用いて、利用者が希望する値を決定することが望ましい。一般的に前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜の値を小さくすると、特異点のごく近傍を除いて、特異点の回避が行われないので、多関節ロボット１の動作を直感的に把握し易くなる。また多関節ロボット１の各アーム体３〜８の移動速度が速い場合には、エンドエフェクタ１８が誤って特異点に陥る可能性が僅かながら存在する。また前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜を充分に大きくした場合には、エンドエフェクタ１８が常に特異点近傍にあるとされるので、特異点回避のための自由度が少なくなる可能性がある。

前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜をより小さくすれば、ロボットの動作は直感と一致するようになるけれども、設定値に応じて特異点に陥る可能性がより高くなる傾向がある。また前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜をより大きくすれば、その大きさに応じて、特異点を回避するための自由度が減少する傾向がある。したがって前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜は、小さすぎても大きすぎても良くなく、その最適値は前述のように多関節ロボット１の各アーム長に依存するので、シミュレーションによって導出することが望ましい。

また前述の制御方法において、少なくとも１つの関節部の所定の位置からの角度位置が可動範囲の限界位置に近づいた場合、エンドエフェクタ１８を予め定める軌道上に保持しながら、当該関節部の角度位置を前記可動範囲の中央位置に近づけるように冗長となる関節部を角変位させる補助制御段階をさらに含むようにしてもよい。具体的には、冗長となる関節部をプラス方向およびマイナス方向に予め定める変位量Δθだけ角変位させて、冗長関節部のそれぞれの角度位置と希望するエンドエフェクタ１８の位置とから逆変換することによって、可動範囲の限界位置に近づいた関節部の角度位置を計算して、前記限界位置から遠ざかるものを採用する。

本実施の形態において、予め定める軌道とは、多関節ロボット１が行う作業に応じて、たとえばアーク溶接時の溶接線およびマテリアルハンドリング時の荷物の通過軌道に応じて、操作者がオンラインまたはオフラインで教示する軌道である。また予め定める変位量Δθ（Δθ１）は、多関節ロボット１において用いられるモータおよび制御器の性能によって決定される。たとえばモータの最高速度が３０度毎秒であり、制御器のサンプリングタイムが０．１秒であった場合には、Δθのとることができる最大値は、３度（＝３０度毎秒×０．１秒）であるけれども、実際には、この値に加速時間および安全係数を見込んだ値が用いられる。

図３は、多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第２の実施例の手順を示すフローチャートである。ステップｔ０で手順が開始されて、ステップｔ１に進む。

冗長関節部設定段階であるステップｔ１では、冗長関節部設定手段である中央演算処理部１９は、多関節ロボット１の各関節部３〜８のうち１つの同軸関節部である第１関節部９と３つの傾斜関節部である第２〜第４関節部１０〜１２とを含む基本４関節部９〜１２で多関節ロボット１のエンドエフェクタ１８の位置を決定し、残余の第５〜第７関節部１３〜１５で前記エンドエフェクタ１８の姿勢を決定するときに、前記基本４関節部９〜１２のうちの１つ、本実施例では入力部によって第１関節部９を冗長関節部に設定入力して、ステップｔ２に進む。

行列生成段階であるステップｔ２では、行列生成手段である中央演算処理部１９は、前記基本４関節部９〜１２のうち冗長関節部となる第１関節部９を除く第２〜第４関節部１０〜１２の角度位置（θ２，θ３，θ４）を、エンドエフェクタの位置（Ｘ，Ｙ，Ｚ）に変換する擬似的なヤコビ行列Ｊを、エンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）と第１関節部９の角度位置θ１で表して、ステップｔ３に進む。このときの前記ヤコビ行列Ｊ＝Ｊ（θ１，Ｘ，Ｙ，Ｚ）は、次式（２）のようにして求められる。

冗長関節制御段階であるステップｔ３では、冗長関節制御手段である中央演算処理部１９は、エンドエフェクタ１８を予め定める軌道上に保持しながら、前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を最大にするように第１関節部９を角変位させてステップｔ４に進み、全ての手順を終了する。また前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜の代わりに、前記ヤコビ行列Ｊの２乗としてもよく、これは数学的に等価であるからである。

以上のように本実施例では、前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を最大にするように第１関節部９を角変位させることによって、前記第１関節部９の角度位置θ１が決定し、希望するエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）を実現するうちに、特異点から最も離れたエンドエフェクタ１８の位置を実現することができる。

また前述の制御方法において、少なくとも１つの関節部の所定の位置からの角度位置が可動範囲の限界位置に近づいた場合、エンドエフェクタ１８を予め定める軌道上に保持しながら、当該関節部の角度位置を前記可動範囲の中央位置に近づけるように冗長となる関節部を角変位させる補助制御段階をさらに含むようにしてもよい。具体的には、冗長となる関節部をプラス方向およびマイナス方向に予め定める変位量Δθだけ角変位させて、冗長関節部のそれぞれの角度位置と希望するエンドエフェクタ１８の位置とから逆変換することによって、可動範囲の限界位置に近づいた関節部の角度位置を計算して、前記限界位置から遠ざかるものを採用する。

本実施例では、第１関節部９を冗長関節部としたけれども、冗長関節部は、第１〜第４関節部９〜１２のいずれか１つであってもよい。本実施例で第１関節部９を冗長関節部としたのは、多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第１関節部９の角度位置θ１から第２〜第４関節部１０〜１２の各角度位置θ２〜θ４を導出することが数学的に最も容易である。一方、たとえば多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第２関節部１０の角度位置θ２から第１、第３および第４関節部９，１１，１２の各角度位置θ１，θ３，θ４を導出することはできないことはないけれども、数学的には煩雑となる。

図４は、多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第３の実施例の手順を示すフローチャートである。ステップｕ０で手順が開始されて、ステップｕ１に進む。

条件導出段階であるステップｕ１では、中央演算処理部１９は、多関節ロボット１の関節部９〜１５のうち１つの同軸関節部である第１関節部９と３つの傾斜関節部である第２〜第４関節部１０〜１２とを含む基本４関節部９〜１２で多関節ロボット１のエンドエフェクタ１８の位置を決定し、残余の関節部１３〜１５で前記エンドエフェクタ１８の姿勢を決定するときに、前記基本４関節部９〜１２の角度位置（θ１，θ２，θ３，θ４）を前記エンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列Ｊの階数（rank）が３未満になる条件を導出して、ステップｕ２に進む。

ステップｕ１で導出される前記ヤコビ行列Ｊの階数が３未満になる条件は、次式（２）〜式（４）である。

前式（２）〜（４）において、Ｌｂは、第２アーム体４の軸線方向の寸法であり、Ｌｃは、第３アーム体５の軸線方向の寸法であり、Ｌｄは、第４アーム体６の軸線方向の寸法であり、Ｌｅは、第５アーム体７の軸線方向の寸法である。

冗長関節制御段階であるステップｕ２では、中央演算処理部１９は、前記ヤコビ行列Ｊの階数が３以上になるように、多関節ロボットの関節部のうち予め定める冗長関節部の角度を導出して、前記冗長関節部を角変位させてステップｕ３に進み、全ての手順を終了する。

前式（２）〜式（４）のうち、式（２）および式（３）については、θ４を０度および１８０度から遠ざけることによって、前記ヤコビ行列Ｊの回数が３未満になることを回避でき、θ４が９０度および２７０度に近づくと、特異点からかなり遠ざかることになる。したがってθ４を９０度または２７０度にできる限り近づけるように、冗長となる関節部、たとえば第１関節部９の角度位置θ１を決定するという簡便な方法によって、全ての特異点のうちの約３分の２を回避することができる。また式（４）については、前述のような簡便な方法を見出すのは困難ではあるけれども、このような場合は、前述の図２に示す第１の実施例を適用して、冗長となる関節部をプラス方向およびマイナス方向に角変位したときのヤコビ行列Ｊ＋，Ｊ−の行列式ｄｅｔ（Ｊ＋），ｄｅｔ（Ｊ−）の絶対値｜ｄｅｔ（Ｊ＋）｜，｜ｄｅｔ（Ｊ−）｜の大きさを比較して、前記絶対値が大きくなる方向に冗長となる関節部を角変位させればよい。

また前述の制御方法において、少なくとも１つの関節部の所定の位置からの角度位置が可動範囲の限界位置に近づいた場合、エンドエフェクタ１８を予め定める軌道上に保持しながら、当該関節部の角度位置を前記可動範囲の中央位置に近づけるように冗長となる関節部を角変位させる補助制御段階をさらに含むようにしてもよい。具体的には、冗長となる関節部をプラス方向およびマイナス方向に予め定める変位量Δθだけ角変位させて、冗長関節部のそれぞれの角度位置と希望するエンドエフェクタ１８の位置とから逆変換することによって、可動範囲の限界位置に近づいた関節部の角度位置を計算して、前記限界位置から遠ざかるものを採用する。

本発明は、以下の実施の形態が可能である。
（１）冗長関節部を含む複数の関節部を有する多関節ロボットの特異点を回避するように前記多関節ロボットを制御する制御方法であって、
前記多関節ロボットの関節部のうち予め定める複数の関節部の角度位置を、多関節ロボットの遊端部の位置に変換するヤコビ行列の行列式の絶対値を計算する絶対値計算段階と、
前記ヤコビ行列の行列式の絶対値が予め定める設定値以下であるか否かを判定する判定段階と、
前記判定段階において前記ヤコビ行列の行列式の絶対値が前記設定値よりも小さいと判定されると、前記遊端部を予め定める軌道上に保持しながら前記ヤコビ行列の行列式の絶対値を大きくするように前記冗長関節部を角変位する冗長関節制御段階とを含むことを特徴とする制御方法。

絶対値計算段階では、多関節ロボットの関節部のうち予め定める複数の関節部の角度位置を、多関節ロボットの遊端部の位置に変換するヤコビ行列の行列式の絶対値を計算する。判定段階では、前記ヤコビ行列の行列式の絶対値が予め定める設定値以下であるか否かを判定する。冗長関節制御段階では、前記判定段階において前記ヤコビ行列の行列式の絶対値が前記設定値以下であると判定されると、前記遊端部を予め定める軌道上に保持しながら前記ヤコビ行列の行列式の絶対値を大きくするように冗長関節部を角変位する。前記ヤコビ行列の行列式が零であるということは、多関節ロボットの遊端部の特定の方向の自由度が失われる特異点であることを表す。このような特異点に多関節ロボットの遊端部が配置されないように、前記ヤコビ行列の行列式の絶対値が前記設定値よりも小さいと判定された場合には、前記冗長関節部を角変位させて、常に前記ヤコビ行列の行列式の絶対値が前記設定値よりも大きくなる、すなわち特異点から遠ざけながら前記遊端部を予め定める軌道上に保持することによって、多関節ロボットの遊端部が特異点を確実に回避するように動作することができる。

（２）前記絶対値計算段階、前記判定段階および前記冗長関節制御段階を繰返して、前記多関節ロボットの特異点を回避するように前記多関節ロボットを制御することを特徴とする制御方法。

前記絶対値計算段階、前記判定段階および前記冗長関節制御段階を繰返して、前記多関節ロボットの特異点を回避するように前記多関節ロボットを制御するので、常に多関節ロボットの遊端部が特異点を確実に回避するように多関節ロボットを制御することができる。

（３）直列に設けられる複数のアーム体と、隣接する２つのアーム体を回転自在に連結する関節部とを有し、前記関節部は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有する多関節ロボットの特異点を回避するように制御する制御方法であって、
多関節ロボットの各関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部のうちの１つを冗長関節部に設定する冗長関節部設定段階と、
前記基本４関節部のうち前記冗長関節部を除く関節部の角度位置を、前記遊端部の位置に変換するヤコビ行列を、前記遊端部の位置と前記冗長関節部の角度位置で表すようにする行列生成段階と、
前記遊端部を予め定める軌道上に保持しながら、前記ヤコビ行列の行列式の絶対値を最大にするように前記冗長関節部を角変位させる冗長関節制御段階とを含むことを特徴とする制御方法。

冗長関節部設定段階では、多関節ロボットの各関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部のうちの１つを冗長関節部に設定する。行列生成段階では、前記基本４関節部のうち前記冗長関節部を除く関節部の角度位置を、前記遊端部の位置に変換するヤコビ行列を、前記遊端部の位置と前記冗長関節部の角度位置で表すようにする。冗長関節制御段階では、前記遊端部を予め定める軌道上に保持しながら、前記ヤコビ行列の行列式の絶対値を最大にするように前記冗長関節部を角変位させる。前記ヤコビ行列の行列式が零であるということは、多関節ロボットの遊端部の特定の方向の自由度が失われる特異点であることを表す。このような特異点に多関節ロボットの遊端部が配置されないように、前記冗長関節部を角変位させて、前記ヤコビ行列の行列式の絶対値が前記設定値よりも大きくなる、すなわち特異点から遠ざけながら前記遊端部を予め定める軌道上に保持することによって、多関節ロボットの遊端部が特異点を確実に回避するように動作することができる。

（４）直列に設けられる複数のアーム体と、隣接する２つのアーム体を回転自在に連結する関節部とを有し、前記関節部は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有する多関節ロボットの特異点を回避するように制御する制御方法であって、
多関節ロボットの関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部の角度位置を前記遊端部の位置に変換するヤコビ行列の階数が３未満になる条件を導出する条件導出段階と、
前記ヤコビ行列の階数が３以上になるように、多関節ロボットの関節部のうち予め定める冗長関節部の角度を導出して、前記冗長関節部を角変位させる冗長関節制御段階とを含むことを特徴とする制御方法。

条件導出段階では、多関節ロボットの関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部の角度位置を前記遊端部の位置に変換するヤコビ行列の階数が３未満になる条件を導出する。冗長関節制御段階では、前記ヤコビ行列の階数が３以上になるように、多関節ロボットの関節部のうち予め定める冗長関節部の角度を導出して、前記冗長関節部を角変位させる。このように前記冗長関節部を角変位させて、前記ヤコビ行列の階数が３以上になるように前記冗長関節部を角変位させることによって、多関節ロボットの遊端部が特異点を確実に回避するように動作することができる。ここで求められるヤコビ行列は４行３列の行列であり、正方行列とはならないために、通常用いられているような行列式は定義できない。この場合、前記４行３列のヤコビ行列がフルランクすなわち階数が３であれば、多関節ロボットの遊端部は全ての方向への移動自由度を持つことが保証されるため、特異点とはならない。一方、階数が３未満であれば、多関節ロボットの遊端部の位置の座標のいずれかが１次従属となり、縮退された方向が存在することとなるために、その特定の方向の自由度が失われて、特異点が生じてしまう。

（５）少なくとも１つの関節部の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部を予め定める軌道上に保持しながら、前記関節部の角度位置を前記可動範囲の中央位置に近づけるように前記冗長関節部を角変位させる補助制御段階をさらに含むことを特徴とする制御方法。

補助制御段階では、少なくとも１つの関節部の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部を予め定める軌道上に保持しながら、前記関節部の角度位置を前記可動範囲の中央位置に近づけるように前記冗長関節部を角変位させる。これによって特異点を回避できるとともに、各関節部の角度位置が可動範囲を超えることを確実に防止できるので、多関節ロボットの動作領域を広くすることができる。

（６）冗長関節部を含む複数の関節部を有する多関節ロボットの特異点を回避するように前記多関節ロボットを制御する制御装置であって、
前記多関節ロボットの関節部のうち予め定める複数の関節部の角度位置を、多関節ロボットの遊端部の位置に変換するヤコビ行列の行列式の絶対値を計算する絶対値計算手段と、
前記ヤコビ行列の行列式の絶対値が予め定める設定値以下であるか否かを判定する判定手段と、
前記判定手段によって前記ヤコビ行列の行列式の絶対値が前記設定値よりも小さいと判定されると、前記遊端部を予め定める軌道上に保持しながら前記ヤコビ行列の行列式の絶対値を大きくするように前記冗長関節部を角変位する冗長関節制御手段とを含むことを特徴とする制御装置。

絶対値計算手段は、多関節ロボットの関節部のうち予め定める複数の関節部の角度位置を、多関節ロボットの遊端部の位置に変換するヤコビ行列の行列式の絶対値を計算する。判定手段は、前記ヤコビ行列の行列式の絶対値が予め定める設定値以下であるか否かを判定する。冗長関節制御手段は、前記判定手段によって前記ヤコビ行列の行列式の絶対値が前記設定値以下であると判定されると、前記遊端部を予め定める軌道上に保持しながら前記ヤコビ行列の行列式の絶対値を大きくするように冗長関節部を角変位する。前記ヤコビ行列の行列式が零であるということは、多関節ロボットの遊端部の特定の方向の自由度が失われる特異点であることを表す。このような特異点に多関節ロボットの遊端部が配置されないように、前記ヤコビ行列の行列式の絶対値が前記設定値よりも小さいと判定された場合には、前記冗長関節部を角変位させて、常に前記ヤコビ行列の行列式の絶対値が前記設定値よりも大きくなる、すなわち特異点から遠ざけながら前記遊端部を予め定める軌道上に保持することによって、多関節ロボットの遊端部が特異点を確実に回避するように動作することができる。

（７）直列に設けられる複数のアーム体と、隣接する２つのアーム体を回転自在に連結する関節部とを有し、前記関節部は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有する多関節ロボットの特異点を回避するように制御する制御装置であって、
多関節ロボットの各関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部のうちの１つを冗長関節部に設定する冗長関節部設定手段と、
前記基本４関節部のうち前記冗長関節部を除く関節部の角度位置を、前記遊端部の位置に変換するヤコビ行列を、前記遊端部の位置と前記冗長関節部の角度位置で表すようにする行列生成手段と、
前記遊端部を予め定める軌道上に保持しながら、前記ヤコビ行列の行列式の絶対値を最大にするように前記冗長関節部を角変位させる冗長関節制御手段とを含むことを特徴とする制御装置。

冗長関節部設定手段は、多関節ロボットの各関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部のうちの１つを冗長関節部に設定する。行列生成手段は、前記基本４関節部のうち前記冗長関節部を除く関節部の角度位置を、前記遊端部の位置に変換するヤコビ行列を、前記遊端部の位置と前記冗長関節部の角度位置で表すようにする。冗長関節制御手段は、前記遊端部を予め定める軌道上に保持しながら、前記ヤコビ行列の行列式の絶対値を最大にするように前記冗長関節部を角変位させる。前記ヤコビ行列の行列式が零であるということは、多関節ロボットの遊端部の特定の方向の自由度が失われる特異点であることを表す。このような特異点に多関節ロボットの遊端部が配置されないように、前記冗長関節部を角変位させて、前記ヤコビ行列の行列式の絶対値が前記設定値よりも大きくなる、すなわち特異点から遠ざけながら前記遊端部を予め定める軌道上に保持することによって、多関節ロボットの遊端部が特異点を確実に回避するように動作することができる。

（８）直列に設けられる複数のアーム体と、隣接する２つのアーム体を回転自在に連結する関節部とを有し、前記関節部は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部とを有する多関節ロボットの特異点を回避するように制御する制御装置であって、
多関節ロボットの関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部の角度位置を前記遊端部の位置に変換するヤコビ行列の階数が３未満になる条件を導出する条件導出手段と、
前記ヤコビ行列の階数が３以上になるように、多関節ロボットの関節部のうち予め定める冗長関節部の角度を導出して、前記冗長関節部を角変位させる冗長関節制御手段とを含むことを特徴とする制御装置。

条件導出手段は、多関節ロボットの関節部のうち１つの同軸関節部と３つの傾斜関節部とを含む基本４関節部で多関節ロボットの遊端部の位置を決定し、残余の関節部で前記遊端部の姿勢を決定するときに、前記基本４関節部の角度位置を前記遊端部の位置に変換するヤコビ行列の階数が３未満になる条件を導出する。冗長関節制御手段は、前記ヤコビ行列の階数が３以上になるように、多関節ロボットの関節部のうち予め定める冗長関節部の角度を導出して、前記冗長関節部を角変位させる。このように前記冗長関節部を角変位させて、前記ヤコビ行列の階数が３以上になるように前記冗長関節部を角変位させることによって、多関節ロボットの遊端部が特異点を確実に回避するように動作することができる。

（９）少なくとも１つの関節部の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部を予め定める軌道上に保持しながら、前記関節部の角度位置を前記可動範囲の中央位置に近づけるように前記冗長関節部を角変位させる補助制御手段をさらに含むことを特徴とする制御装置。

補助制御手段は、少なくとも１つの関節部の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部を予め定める軌道上に保持しながら、前記関節部の角度位置を前記可動範囲の中央位置に近づけるように前記冗長関節部を角変位させる。これによって特異点を回避できるとともに、各関節部の角度位置が可動範囲を超えることを確実に防止できるので、多関節ロボットの動作領域を広くすることができる。

１ 多関節ロボット
２ 制御装置
９〜１５ 関節部
１８ エンドエフェクタ
１９ 中央演算処理部

Claims (1)

1. 複数の関節部によって連結される複数のアーム体および基台を有し、
   第１アーム体の一端部は、基台に、第１関節部によって、第１アーム体の軸線と同軸の第１回転軸線まわりに回転自在に連結されており、
   第２アーム体の一端部は、第２関節部によって、第１アーム体の軸線および第２アーム体の軸線に対して４５度で傾斜する第２回転軸線まわりに回転自在に、第１アーム体の他端部に連結され、
   第３アーム体の一端部は、第３関節部によって、第２アーム体の軸線および第３アーム体の軸線に対して４５度で傾斜する第３回転軸線まわりに回転自在に、第２アーム体の他端部に連結され、
   第２回転軸線と第３回転軸線とは平行であり、
   第４アーム体の一端部は、第４関節部によって、第３アーム体の軸線および第４アーム体の軸線に対して４５度で傾斜する第４回転軸線まわりに回転自在に、第３アーム体の他端部に連結され、
   第３回転軸線と第４回転軸線とは垂直であり、
   第５アーム体の一端部は、第５関節部によって、第４アーム体の軸線および第５アーム体の軸線と同軸の第５回転軸線まわりに回転自在に、第４アーム体の他端部に連結され、
   第６アーム体の一端部は、第６関節部によって、第５アーム体の軸線および第６アーム体の軸線に対して４５度で傾斜する第６回転軸線まわりに回転自在に、第５アーム体の他端部に連結され、
   第６アーム体の他端部に、第６アーム体の軸線と同軸の第７回転軸線まわりに回転自在に設けられ、エンドエフェクタに接続可能な第７関節部を有することを特徴とする多関節ロボット。

Images