JP4647919B2

JP4647919B2 - 制御方法および制御装置

Info

Publication number: JP4647919B2
Application number: JP2004041702A
Authority: JP
Inventors: 栄一八木; 利尚土井; 雅幸掃部
Original assignee: Toyota Motor Corp; Kawasaki Jukogyo KK
Current assignee: Toyota Motor Corp; Kawasaki Motors Ltd
Priority date: 2004-02-18
Filing date: 2004-02-18
Publication date: 2011-03-09
Anticipated expiration: 2024-02-18
Also published as: JP2005230952A

Description

本発明は、冗長関節部を含む複数の関節部を有する多関節ロボットを制御する制御方法および制御装置に関する。

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

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

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

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

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

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

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

本発明の目的は、多関節ロボットの冗長関節部を角変位させて特異点を回避する制御方法および制御装置を提供することである。

本発明（１）は、多関節ロボットの制御方法であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定段階と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ_＋，Ｊ₋）の行列式（ｄｅｔ（Ｊ_＋），ｄｅｔ（Ｊ₋））の絶対値（｜ｄｅｔ（Ｊ_＋）｜，｜ｄｅｔ（Ｊ₋）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御段階とを含むことを特徴とする制御方法である。

また本発明（２）は、前記多関節ロボットは直列に設けられる複数のアーム体（３〜８）と、隣接する２つのアーム体を回転自在に連結する関節部（９〜１５）とを有し、前記関節部（９〜１５）は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線（Ｌ９，Ｌ１３，Ｌ１５）まわりに回転自在に連結する同軸関節部（９，１３，１５）と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線（Ｌ１０，Ｌ１１，Ｌ１２，Ｌ１４）まわりに回転自在に連結する傾斜関節部（１０，１１，１２，１４）とを有し、
前記基本４関節部（９〜１２）は前記予め定める１つの同軸関節部（９）と３つの傾斜関節部（１０，１１，１２）とを含むことを特徴とする。

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

また本発明（４）は、多関節ロボットの制御装置であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で前記多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定手段（１９）と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ_＋，Ｊ₋）の行列式（ｄｅｔ（Ｊ_＋），ｄｅｔ（Ｊ₋））の絶対値（｜ｄｅｔ（Ｊ_＋）｜，｜ｄｅｔ（Ｊ₋）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御手段（１９）とを含むことを特徴とする制御装置である。

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

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

また本発明（２）によれば、前記絶対値計算、前記判定および前記冗長関節制御を繰返して、直列に設けられる複数のアーム体（３〜８）と、隣接する２つのアーム体を回転自在に連結する関節部とを有し、前記関節部（９〜１５）は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部（９，１３，１５）と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部（１０，１１，１２，１４）とを有する多関節ロボットを、特異点を回避するように制御することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

符号の説明

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

Claims

多関節ロボットの制御方法であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定段階と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ_＋，Ｊ₋）の行列式（ｄｅｔ（Ｊ_＋），ｄｅｔ（Ｊ₋））の絶対値（｜ｄｅｔ（Ｊ_＋）｜，｜ｄｅｔ（Ｊ₋）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御段階とを含むことを特徴とする制御方法。
前記多関節ロボットは直列に設けられる複数のアーム体（３〜８）と、隣接する２つのアーム体を回転自在に連結する関節部（９〜１５）とを有し、前記関節部（９〜１５）は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線（Ｌ９，Ｌ１３，Ｌ１５）まわりに回転自在に連結する同軸関節部（９，１３，１５）と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線（Ｌ１０，Ｌ１１，Ｌ１２，Ｌ１４）まわりに回転自在に連結する傾斜関節部（１０，１１，１２，１４）とを有し、
前記基本４関節部（９〜１２）は前記予め定める１つの同軸関節部（９）と３つの傾斜関節部（１０，１１，１２）とを含むことを特徴とする請求項１に記載の制御方法。
少なくとも前記予め定める１つの冗長関節部（９）の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記予め定める１つの冗長関節部（９）の角度位置を前記可動範囲の中央位置に近づけるように前記予め定める１つの冗長関節部（９）を角変位させる補助制御段階をさらに含むことを特徴とする請求項１または２に記載の制御方法。
多関節ロボットの制御装置であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で前記多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定手段（１９）と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ_＋，Ｊ₋）の行列式（ｄｅｔ（Ｊ_＋），ｄｅｔ（Ｊ₋））の絶対値（｜ｄｅｔ（Ｊ_＋）｜，｜ｄｅｔ（Ｊ₋）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御手段（１９）とを含むことを特徴とする制御装置。
少なくとも前記予め定める１つの冗長関節部（９）の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記予め定める１つの冗長関節部（９）の角度位置を前記可動範囲の中央位置に近づけるように前記予め定める１つの冗長関節部（９）を角変位させる補助制御手段（１９）をさらに含むことを特徴とする請求項４に記載の制御装置。