(57)【特許請求の範囲】
【請求項１】
多関節ロボットの制御方法であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定段階と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ＋，Ｊ−）の行列式（ｄｅｔ（Ｊ＋），ｄｅｔ（Ｊ−））の絶対値（｜ｄｅｔ（Ｊ＋）｜，｜ｄｅｔ（Ｊ−）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御段階とを含むことを特徴とする制御方法。
【請求項２】
前記多関節ロボットは直列に設けられる複数のアーム体（３〜８）と、隣接する２つのアーム体を回転自在に連結する関節部（９〜１５）とを有し、前記関節部（９〜１５）は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線（Ｌ９，Ｌ１３，Ｌ１５）まわりに回転自在に連結する同軸関節部（９，１３，１５）と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線（Ｌ１０，Ｌ１１，Ｌ１２，Ｌ１４）まわりに回転自在に連結する傾斜関節部（１０，１１，１２，１４）とを有し、
前記基本４関節部（９〜１２）は前記予め定める１つの同軸関節部（９）と３つの傾斜関節部（１０，１１，１２）とを含むことを特徴とする請求項１に記載の制御方法。
【請求項３】
少なくとも前記予め定める１つの冗長関節部（９）の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記予め定める１つの冗長関節部（９）の角度位置を前記可動範囲の中央位置に近づけるように前記予め定める１つの冗長関節部（９）を角変位させる補助制御段階をさらに含むことを特徴とする請求項１または２に記載の制御方法。
【請求項４】
多関節ロボットの制御装置であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で前記多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定手段（１９）と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ＋，Ｊ−）の行列式（ｄｅｔ（Ｊ＋），ｄｅｔ（Ｊ−））の絶対値（｜ｄｅｔ（Ｊ＋）｜，｜ｄｅｔ（Ｊ−）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御手段（１９）とを含むことを特徴とする制御装置。
【請求項５】
少なくとも前記予め定める１つの冗長関節部（９）の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記予め定める１つの冗長関節部（９）の角度位置を前記可動範囲の中央位置に近づけるように前記予め定める１つの冗長関節部（９）を角変位させる補助制御手段（１９）をさらに含むことを特徴とする請求項４に記載の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、冗長関節部を含む複数の関節部を有する多関節ロボットを制御する制御方法および制御装置に関する。
【背景技術】
【０００２】
７つ以上の複数の関節部を有する多関節ロボットの制御については、様々な方式があり、その典型的な従来技術が特許文献１および特許文献２に記載されている。特許文献１の多関節ロボットの制御装置では、多関節ロボットの先端関節が目標位置に距離的に近づくような関節角度を最小２乗法によって１軸ずつ求め、求めた目標角度と現在の関節角度との差に比例した量を１軸ずつ関節を回転させることによって、計算量を少なくして、先端の関節が目標位置に達するようにしている。特許文献２の多自由度マニピュレータの位置決め装置では、手先位置姿勢情報とマニピュレータの全体形状の指定値とから全体形状を指定するための解曲線を求め、全リンクの端点が前記解曲線状に配置されるようにリンク端点位置を算出し、隣接する２リンクのリンク端点と根元側リンクの姿勢とから２リンク間の関節角を算出し、前記関節角から手先側リンクの姿勢を求め、リンク毎に根元から手先まで逐次的に前記関節角とリンク姿勢とを繰返し算出して全関節角を求めることで、位置決めの算出を高速化する。
【０００３】
また前述のような多関節ロボットが障害物を回避するような制御についても様々な方式があり、その典型的な従来技術が特許文献３に記載されている。特許文献３のマニピュレータの制御装置では、始点から終点へ把手を移動するときに、障害物に干渉する状態での各関節の変位を予め離散的に求め、各関節の変位を避けるように各関節の離散的な動作間をマニピュレータの関節補間または単軸動作によって制御する。
【０００４】
また前述のような多関節ロボットを教示するための必要な情報を表示する典型的な従来技術が、特許文献４に記載されている。特許文献４の冗長自由度ロボット装置では、作業に必要な動作を記述する独立変数の現在値に依存する冗長パラメータの取り得る領域の演算を行って、その結果を表示することでロボット教示を行う。
【０００５】
ロボットの空間内の各方向に対する制御仕様を実現する典型的な従来技術が、特許文献５に記載されている。特許文献５のロボットの制御方法では、ロボットに行わせる作業の仕様を作業空間に対して固定された座標系を用いて記述し、各座標軸方向および各座標軸まわりの方向の動作制御目標を明確化し、前記各方向へのロボットの姿勢状態のそれぞれに対応する方向への作業仕様に関する制御性能評価指標を計算し、それらの加重総合評価指標値が最も大きな値となるロボットの姿勢状態を最適姿勢状態とし、この状態で作業を実行させる。
【０００６】
ロボットの動作限界近傍を回避した動作を教示データを作成する典型的な従来技術が、特許文献６に記載されている。特許文献６のロボットの動作軌跡教示装置では、冗長自由度を要素として、ロボットの関節角度空間の余裕を表現する評価関数に基づいて、前記冗長自由度を決定する。
【０００７】
【特許文献１】特開２０００−１６７７８９号公報
【特許文献２】特開平１０−３３３７２３号公報
【特許文献３】特開平１１−３３３７８０号公報
【特許文献４】特開平５−３３７８６１号公報
【特許文献５】特開平５−２３９８２号公報
【特許文献６】特開平６−１８７０２０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００８】
前述の従来技術では、多関節ロボットの関節部のうち、冗長軸まわりに角変位する冗長関節部を角変位させて、エンドエフェクタなどの遊端部の特定方向への自由度が失われる特異点を回避するという制御は行われていない。
【０００９】
本発明の目的は、多関節ロボットの冗長関節部を角変位させて特異点を回避する制御方法および制御装置を提供することである。
【課題を解決するための手段】
【００１０】
本発明（１）は、多関節ロボットの制御方法であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定段階と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ＋，Ｊ−）の行列式（ｄｅｔ（Ｊ＋），ｄｅｔ（Ｊ−））の絶対値（｜ｄｅｔ（Ｊ＋）｜，｜ｄｅｔ（Ｊ−）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御段階とを含むことを特徴とする制御方法である。
【００１３】
また本発明（２）は、前記多関節ロボットは直列に設けられる複数のアーム体（３〜８）と、隣接する２つのアーム体を回転自在に連結する関節部（９〜１５）とを有し、前記関節部（９〜１５）は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線（Ｌ９，Ｌ１３，Ｌ１５）まわりに回転自在に連結する同軸関節部（９，１３，１５）と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線（Ｌ１０，Ｌ１１，Ｌ１２，Ｌ１４）まわりに回転自在に連結する傾斜関節部（１０，１１，１２，１４）とを有し、
前記基本４関節部（９〜１２）は前記予め定める１つの同軸関節部（９）と３つの傾斜関節部（１０，１１，１２）とを含むことを特徴とする。
【００１４】
また本発明（３）は、少なくとも前記予め定める１つの冗長関節部（９）の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記予め定める１つの冗長関節部（９）の角度位置を前記可動範囲の中央位置に近づけるように前記予め定める１つの冗長関節部（９）を角変位させる補助制御段階をさらに含むことを特徴とする。
【００１５】
また本発明（４）は、多関節ロボットの制御装置であって、
多関節ロボットの各関節部（９〜１５）のうち基本４関節部（９〜１２）で前記多関節ロボットの遊端部（１８）の位置を決定し、１つの関節部（１４）の回転軸線（Ｌ１４）と、その両側の関節部（１３，１５）の各回転軸線（Ｌ１３，Ｌ１５）とが交差する３軸交点を有する残余の関節部（１３〜１５）で前記遊端部（１８）の姿勢を決定するときに、前記基本４関節部（９〜１２）のうちの予め定める１つの冗長関節部（９）に設定する冗長関節部設定手段（１９）と、
前記遊端部（１８）を予め定める軌道上に保持しながら前記予め定める１つの冗長関節部（９）を予め定める変位量（Δθ１）だけプラス方向に角変位させた場合と、マイナス方向に角変位させた場合とのそれぞれについて、前記基本４関節部（９〜１２）のうち前記予め定める１つの冗長関節部（９）を除く関節部（１０〜１２）の角度位置（θ２，θ３，θ４）を前記遊端部（１８）の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列（Ｊ＋，Ｊ−）の行列式（ｄｅｔ（Ｊ＋），ｄｅｔ（Ｊ−））の絶対値（｜ｄｅｔ（Ｊ＋）｜，｜ｄｅｔ（Ｊ−）｜を比較し、値が大きい方向に前記予め定める１つの冗長関節部（９）を前記予め定める変位量（Δθ１）だけ角変位させる冗長関節制御手段（１９）とを含むことを特徴とする制御装置である。
【００１８】
また本発明（５）は、少なくとも前記予め定める１つの冗長関節部（９）の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記予め定める１つの冗長関節部（９）の角度位置を前記可動範囲の中央位置に近づけるように前記予め定める１つの冗長関節部（９）を角変位させる補助制御手段（１９）をさらに含むことを特徴とする。
【発明の効果】
【００１９】
本発明（１），（４）によれば、多関節ロボットの関節部（９〜１５）のうち予め定める複数の関節部（９〜１２）の角度位置を、多関節ロボットの遊端部（１８）の位置に変換するヤコビ行列の行列式の絶対値を計算し、ヤコビ行列の行列式の絶対値が予め定める設定値以下であるか否かを判定する。冗長関節制御では、前記遊端部（１８）を予め定める軌道上に保持しながら前記ヤコビ行列の行列式の絶対値が大きい方向に予め定める１つの冗長関節部（９）を角変位させる。前記ヤコビ行列の行列式が零であるということは、多関節ロボットの遊端部（１８）の特定の方向の自由度が失われる特異点であることを表すので、このような特異点に多関節ロボットの遊端部（１８）が配置されないように、前記ヤコビ行列の行列式の絶対値が前記設定値よりも小さいと判定された場合には、前記予め定める１つの冗長関節部（９）を絶対値が大きい方向に角変位させて、特異点から遠ざけながら前記遊端部（１８）を予め定める軌道上に保持することによって、多関節ロボットの遊端部（１８）が特異点を確実に回避するように動作させることができる。
【００２２】
また本発明（２）によれば、前記絶対値計算、前記判定および前記冗長関節制御を繰返して、直列に設けられる複数のアーム体（３〜８）と、隣接する２つのアーム体を回転自在に連結する関節部とを有し、前記関節部（９〜１５）は、前記２つのアーム体を、各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する同軸関節部（９，１３，１５）と、前記２つのアーム体を、各アーム体の軸線に対して所定の角度を成して傾斜する回転軸線まわりに回転自在に連結する傾斜関節部（１０，１１，１２，１４）とを有する多関節ロボットを、特異点を回避するように制御することができる。
【００２３】
また本発明（３）によれば、補助制御段階では、少なくとも予め定める１つの関節部の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部（１８）を予め定める軌道上に保持しながら、前記関節部の角度位置を前記可動範囲の中央位置に近づけるように前記冗長関節部を角変位させる。これによって特異点を回避できるとともに、各関節部の角度位置が可動範囲を超えることを確実に防止できるので、多関節ロボットの動作領域を広くすることができる。
【００２７】
また本発明（５）によれば、補助制御手段（１９）は、少なくとも予め定める１つの関節部の角度位置が可動範囲の限界位置に近づいた場合、前記遊端部を予め定める軌道上に保持しながら、前記関節部の角度位置を前記可動範囲の中央位置に近づけるように前記冗長関節部を角変位させる。これによって特異点を回避できるとともに、各関節部の角度位置が可動範囲を超えることを確実に防止できるので、多関節ロボットの動作領域を広くすることができる。
【発明を実施するための最良の形態】
【００２８】
図１は、本発明の実施の一形態の多関節ロボット１の制御装置２を示すブロック図である。多関節ロボット１は、７自由度を有する冗長自由度のロボットであり、複数の、本実施の形態では６つのアーム体３〜８と、アーム体を回転自在に連結する複数の、本実施の形態では７つの関節部９〜１５とを有する。各アーム体３〜８は、それぞれ連結されて直線状に延びるアーム構成体１６を構成する。
【００２９】
各関節部９〜１５は、同軸関節部と傾斜関節部とが組み合わされて設けられる。同軸関節部は、隣接する２つのアーム体を、その各アーム体の軸線と同軸の回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する２つのアーム体のうちの一方を、各アーム体の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。詳細に述べると、第１関節部９、第５関節部１３および第７関節部１５は、同軸関節部であり、第２関節部１０、第３関節部１１、第４関節部１２および第６関節部１４は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの３６０度以上の回転だけでなく、軸線まわりに３６０度以下の角度で角変位する状態も含むものとする。
【００３０】
アーム構成体１６の一端部である第１アーム体３の一端部は、壁面などに固定される基台１７に、第１関節部９によって、第１アーム体３の軸線Ｌ３と同軸の第１回転軸線Ｌ９まわりに回転自在に連結されている。第２アーム体４の一端部は、第２関節部１０によって、第１アーム体３の軸線Ｌ３および第２アーム体４の軸線Ｌ４に対して所定の角度、本実施の形態では４５度で傾斜する第２回転軸線Ｌ１０まわりに回転自在に、第１アーム体３の他端部に連結される。第３アーム体５の一端部は、第３関節部１１によって、第２アーム体４の軸線Ｌ４および第３アーム体５の軸線Ｌ５に対して所定の角度、本実施の形態では４５度で傾斜する第３回転軸線Ｌ１１まわりに回転自在に、第２アーム体４の他端部に連結される。第２回転軸線Ｌ１０と第３回転軸線Ｌ１１とは平行である。
【００３１】
第４アーム体６の一端部は、第４関節部１２によって、第３アーム体５の軸線Ｌ５および第４アーム体６の軸線Ｌ６に対して所定の角度、本実施の形態では４５度で傾斜する第４回転軸線Ｌ１２まわりに回転自在に、第３アーム体５の他端部に連結される。第３回転軸線Ｌ１１と第４回転軸線Ｌ１２とは垂直である。第５アーム体７の一端部は、第５関節部１３によって、第４アーム体６の軸線Ｌ６および第５アーム体７の軸線Ｌ７と同軸の第５回転軸線Ｌ１３まわりに回転自在に、第４アーム体６の他端部に連結される。
【００３２】
アーム構成体１６の他端部である第６アーム体８の一端部は、第６関節部１４によって、第５アーム体７の軸線Ｌ７および第６アーム体８の軸線Ｌ８に対して所定の角度、本実施の形態では４５度で傾斜する第６回転軸線Ｌ１４まわりに回転自在に、第５アーム体７の他端部に連結される。多関節ロボット１の遊端部となるエンドエフェクタ１８は、第６アーム体８の他端部に、第７関節部１５によって、第６アーム体８の軸線Ｌ８と同軸の第７回転軸線Ｌ１５まわりに回転自在に連結されている。エンドエフェクタ１８は、たとえば溶接トーチおよびハンドリング装置などの手先装置であってもよい。
【００３３】
各アーム体３〜８は、各アーム体３〜８を回転駆動するモータを内蔵する。また各アーム体３〜８は、その軸線を挿通する中空空間が形成される。この中空空間に１または複数の配線が延びて配置される。各配線は、前記モータを駆動させるための電力、前記モータへの回転指令およびエンドエフェクタ１８への電力および圧縮空気などを供給する。
【００３４】
各アーム体３〜８を回転軸線Ｌ９〜Ｌ１５まわりに回転する回転機構は、従来の機構と同様であってもよい。前記回転機構は、一例を挙げると、中空形状の波動歯車機構、たとえばハーモニックドライブ（登録商標）を用いてもよい。波動歯車機構は、入力側部材と出力側部材とを備え、それらが相対的に回転する。入力側部材は、各関節部９〜１５によって連結する２つの部材（基台およびアーム体）の一方に連結され、出力側部材は、前記２つの部材のうち他方に連結される。モータからの回転が入力側部材に与えられると、入力側部材と出力側部材とが相対的に回転する。これによって一方のアーム体と他方のアーム体とを相対的に回転させることができる。このような回転機構が、各アーム体３〜８に設けられることによって、隣接する２つのアーム体を相対的に回転させることができる。
【００３５】
制御装置２は、中央演算処理部１９および入力部を備える。中央演算処理部１９は、たとえばＣＰＵおよびメモリなどで実現され、多関節ロボット１を制御するために必要な演算を行って、その演算結果を動作指令として多関節ロボット１に与える。入力部は中央演算処理装置１９に各種情報を入力する。
【００３６】
図２は、多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第１の実施例の手順を示すフローチャートである。ステップｓ０で手順が開始されて、ステップｓ１に進む。
【００３７】
絶対値計算段階であるステップｓ１では、絶対値計算手段である中央演算処理部１９は、前記多関節ロボット１の関節部９〜１５のうち予め定める複数の関節部、本実施の形態では第１〜第４関節部９〜１２の角度位置（θ１，θ２，θ３，θ４）のうち、さらに外部から値が与えられる１つの関節部、本実施の形態では第１関節部９を除く第２〜第４関節部１０〜１２の角度位置（θ２，θ３，θ４）を、多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）に変換する擬似的なヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を計算してステップｓ２に進む。前記ヤコビ行列Ｊは、次式（１）で表される。
【００３８】
【数１】
0004647919.tif 000002
【００３９】
判定段階であるステップｓ２では、判定手段である中央演算処理部１９は、前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜が、入力部から入力された予め定める設定値｜ｄｅｔ（Ｊｓｅｔ）｜よりも小さいか否かを判定し、前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜以下であると判定するとステップｓ３に進み、前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜を超えると判定するとステップｓ１０に進む。
【００４０】
冗長関節制御段階であるステップｓ３では、冗長関節制御手段である中央演算処理部１９は、エンドエフェクタ１８を予め定める軌道上に保持しながら前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を大きくするように冗長となる関節部である第１関節部９を角変位してステップｓ４に進む。このとき中央演算処理部１９は、エンドエフェクタ１８の位置および姿勢を保持した状態で、第１関節部９を現在位置から、予め定める変位量Δθ１だけプラスの方向に角変位して、残余の関節部である第２〜第７関節部１０〜１５の角度位置（θ２，θ３，θ４，θ５，θ６，θ７）を逆変換によって求め、さらに第１関節部９を現在位置から、予め定める変位量Δθ１だけマイナスの方向に角変位して、残余の関節部である第２〜第７関節部１０〜１５の角度位置（θ２，θ３，θ４，θ５，θ６，θ７）を逆変換によって求めて、ステップｓ４に進む。
【００４１】
ステップｓ４では、中央演算処理部１９は、ステップｓ３において第１関節部９を現在角度位置から、予め定める変位量Δθ１だけ予め定めるプラスの方向に角変位したときのヤコビ行列Ｊ＋の行列式ｄｅｔ（Ｊ＋）と、第１関節部９を現在角度位置から、予め定める変位量Δθ１だけ前記プラス方向とは逆方向のマイナスの方向に角変位したときのヤコビ行列Ｊ−の行列式ｄｅｔ（Ｊ−）とを求めて、ステップｓ５に進む。
【００４２】
ステップｓ５では、中央演算処理部１９は、ステップｓ３において第１関節部９を現在の角度位置から、予め定める変位量Δθ１だけプラスの方向に角変位したときのヤコビ行列Ｊ＋の行列式ｄｅｔ（Ｊ＋）の絶対値｜ｄｅｔ（Ｊ＋）｜が、第１関節部９を現在の角度位置から、予め定める変位量Δθ１だけマイナスの方向に角変位したときのヤコビ行列Ｊ−の行列式ｄｅｔ（Ｊ−）の絶対値｜ｄｅｔ（Ｊ−）｜を超えるか否かを判断し、超えると判断するとステップｓ６に進み、超えないと判断するとステップｓ９に進む。
【００４３】
ステップｓ６では、中央演算処理部１９は、第１関節部９を現在の角度位置からプラスの方向に角変位して、ステップｓ７に進む。
【００４４】
ステップｓ７では、中央演算処理部１９は、多関節ロボット１の関節部９〜１５のうち冗長関節部である第１関節部９を除く第２〜第７関節部１０〜１５の角度位置（θ２，θ３，θ４，θ５，θ６，θ７）を逆変換によって求めて、ステップｓ８に進み、全ての手順を終了する。
【００４５】
ステップｓ９では、中央演算処理部１９は、第１関節部９を現在の角度位置からマイナスの方向に角変位して、ステップｓ７に進む。
【００４６】
ステップｓ１０では、中央演算処理部１９は、常時最適化を行うか否かを判断し、行うと判断するとステップｓ３に進み、行わないと判断するとステップｓ１１に進む。
【００４７】
ステップｓ１１では、中央演算処理部１９は、多関節ロボット１の関節部９〜１５のうち冗長関節部である第１関節部９を角変位させないで、ステップｓ７に進む。
【００４８】
また中央演算処理部１９は、以上のような絶対値計算段階、判定段階および冗長関節制御段階を繰返して、多関節ロボット１の特異点を回避するように多関節ロボット１を制御してもよい。
【００４９】
本実施例では、第１関節部９を冗長関節部としたけれども、冗長関節部は、第１〜第４関節部９〜１２のいずれか１つであってもよい。本実施例で第１関節部９を冗長関節部としたのは、多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第１関節部９の角度位置θ１から第２〜第４関節部１０〜１２の各角度位置θ２〜θ４を導出することが数学的に最も容易である。一方、たとえば多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第２関節部１０の角度位置θ２から第１、第３および第４関節部９，１１，１２の各角度位置θ１，θ３，θ４を導出することはできないことはないけれども、数学的には煩雑となる。
【００５０】
前述のヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜は、関係する各関節部の各角度位置だけでなく、多関節ロボット１の各アーム体３〜８の長さＬａ，Ｌｂ、Ｌｃ，Ｌｄ，Ｌｅ，Ｌｅ，Ｌｆに依存するので、予め定める設定値｜ｄｅｔ（Ｊｓｅｔ）｜を包括的に最適化することは困難である。このため前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜の決定においては、事前にコンピュータシミュレーション等を用いて、利用者が希望する値を決定することが望ましい。一般的に前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜の値を小さくすると、特異点のごく近傍を除いて、特異点の回避が行われないので、多関節ロボット１の動作を直感的に把握し易くなる。また多関節ロボット１の各アーム体３〜８の移動速度が速い場合には、エンドエフェクタ１８が誤って特異点に陥る可能性が僅かながら存在する。また前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜を充分に大きくした場合には、エンドエフェクタ１８が常に特異点近傍にあるとされるので、特異点回避のための自由度が少なくなる可能性がある。
【００５１】
前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜をより小さくすれば、ロボットの動作は直感と一致するようになるけれども、設定値に応じて特異点に陥る可能性がより高くなる傾向がある。また前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜をより大きくすれば、その大きさに応じて、特異点を回避するための自由度が減少する傾向がある。したがって前記設定値｜ｄｅｔ（Ｊｓｅｔ）｜は、小さすぎても大きすぎても良くなく、その最適値は前述のように多関節ロボット１の各アーム長に依存するので、シミュレーションによって導出することが望ましい。
【００５２】
また前述の制御方法において、少なくとも１つの関節部の所定の位置からの角度位置が可動範囲の限界位置に近づいた場合、エンドエフェクタ１８を予め定める軌道上に保持しながら、当該関節部の角度位置を前記可動範囲の中央位置に近づけるように冗長となる関節部を角変位させる補助制御段階をさらに含むようにしてもよい。具体的には、冗長となる関節部をプラス方向およびマイナス方向に予め定める変位量Δθだけ角変位させて、冗長関節部のそれぞれの角度位置と希望するエンドエフェクタ１８の位置とから逆変換することによって、可動範囲の限界位置に近づいた関節部の角度位置を計算して、前記限界位置から遠ざかるものを採用する。
【００５３】
本実施の形態において、予め定める軌道とは、多関節ロボット１が行う作業に応じて、たとえばアーク溶接時の溶接線およびマテリアルハンドリング時の荷物の通過軌道に応じて、操作者がオンラインまたはオフラインで教示する軌道である。また予め定める変位量Δθ（Δθ１）は、多関節ロボット１において用いられるモータおよび制御器の性能によって決定される。たとえばモータの最高速度が３０度毎秒であり、制御器のサンプリングタイムが０．１秒であった場合には、Δθのとることができる最大値は、３度（＝３０度毎秒×０．１秒）であるけれども、実際には、この値に加速時間および安全係数を見込んだ値が用いられる。
【００５４】
図３は、多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第２の実施例の手順を示すフローチャートである。ステップｔ０で手順が開始されて、ステップｔ１に進む。
【００５５】
冗長関節部設定段階であるステップｔ１では、冗長関節部設定手段である中央演算処理部１９は、多関節ロボット１の各関節部９〜１５のうち予め定める１つの同軸関節部である第１関節部９と３つの傾斜関節部である第２〜第４関節部１０〜１２とを含む基本４関節部９〜１２で多関節ロボット１のエンドエフェクタ１８の位置を決定し、１つの関節部１４の回転軸線Ｌ１４と、その両側の関節部１３，１５の各回転軸線Ｌ１３，Ｌ１５とが交差する３軸交点を有する残余の第５〜第７関節部１３〜１５で前記エンドエフェクタ１８の姿勢を決定するときに、前記基本４関節部９〜１２のうちの１つ、本実施例では入力部によって第１関節部９を冗長関節部に設定入力して、ステップｔ２に進む。
【００５６】
行列生成段階であるステップｔ２では、行列生成手段である中央演算処理部１９は、前記基本４関節部９〜１２のうち冗長関節部となる第１関節部９を除く第２〜第４関節部１０〜１２の角度位置（θ２，θ３，θ４）を、エンドエフェクタの位置（Ｘ，Ｙ，Ｚ）に変換する擬似的なヤコビ行列Ｊを、エンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）と第１関節部９の角度位置θ１で表して、ステップｔ３に進む。このときの前記ヤコビ行列Ｊ＝Ｊ（θ１，Ｘ，Ｙ，Ｚ）は、次式（２）のようにして求められる。
【００５７】
【数２】
0004647919.tif 000003
【００５８】
冗長関節制御段階であるステップｔ３では、冗長関節制御手段である中央演算処理部１９は、エンドエフェクタ１８を予め定める軌道上に保持しながら、前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を最大にするように第１関節部９を角変位させてステップｔ４に進み、全ての手順を終了する。また前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜の代わりに、前記ヤコビ行列Ｊの２乗としてもよく、これは数学的に等価であるからである。
【００５９】
以上のように本実施例では、前記ヤコビ行列Ｊの行列式ｄｅｔ（Ｊ）の絶対値｜ｄｅｔ（Ｊ）｜を最大にするように第１関節部９を角変位させることによって、前記第１関節部９の角度位置θ１が決定し、希望するエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）を実現するうちに、特異点から最も離れたエンドエフェクタ１８の位置を実現することができる。
【００６０】
また前述の制御方法において、少なくとも１つの関節部の所定の位置からの角度位置が可動範囲の限界位置に近づいた場合、エンドエフェクタ１８を予め定める軌道上に保持しながら、当該関節部の角度位置を前記可動範囲の中央位置に近づけるように冗長となる関節部を角変位させる補助制御段階をさらに含むようにしてもよい。具体的には、冗長となる関節部をプラス方向およびマイナス方向に予め定める変位量Δθだけ角変位させて、冗長関節部のそれぞれの角度位置と希望するエンドエフェクタ１８の位置とから逆変換することによって、可動範囲の限界位置に近づいた関節部の角度位置を計算して、前記限界位置から遠ざかるものを採用する。
【００６１】
本実施例では、第１関節部９を冗長関節部としたけれども、冗長関節部は、第１〜第４関節部９〜１２のいずれか１つであってもよい。本実施例で第１関節部９を冗長関節部としたのは、多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第１関節部９の角度位置θ１から第２〜第４関節部１０〜１２の各角度位置θ２〜θ４を導出することが数学的に最も容易である。一方、たとえば多関節ロボット１のエンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）および第２関節部１０の角度位置θ２から第１、第３および第４関節部９，１１，１２の各角度位置θ１，θ３，θ４を導出することはできないことはないけれども、数学的には煩雑となる。
【００６２】
図４は、多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第３の実施例の手順を示すフローチャートである。ステップｕ０で手順が開始されて、ステップｕ１に進む。
【００６３】
条件導出段階であるステップｕ１では、中央演算処理部１９は、多関節ロボット１の関節部９〜１５のうち１つの同軸関節部である第１関節部９と３つの傾斜関節部である第２〜第４関節部１０〜１２とを含む基本４関節部９〜１２で多関節ロボット１のエンドエフェクタ１８の位置を決定し、残余の関節部１３〜１５で前記エンドエフェクタ１８の姿勢を決定するときに、前記基本４関節部９〜１２の角度位置（θ１，θ２，θ３，θ４）を前記エンドエフェクタ１８の位置（Ｘ，Ｙ，Ｚ）に変換するヤコビ行列Ｊの階数（rank）が３未満になる条件を導出して、ステップｕ２に進む。
【００６４】
ステップｕ１で導出される前記ヤコビ行列Ｊの階数が３未満になる条件は、次式（２）〜式（４）である。
【００６５】
【数３】
0004647919.tif 000004
【００６６】
前式（２）〜（４）において、Ｌｂは、第２アーム体４の軸線方向の寸法であり、Ｌｃは、第３アーム体５の軸線方向の寸法であり、Ｌｄは、第４アーム体６の軸線方向の寸法であり、Ｌｅは、第５アーム体７の軸線方向の寸法である。
【００６７】
冗長関節制御段階であるステップｕ２では、中央演算処理部１９は、前記ヤコビ行列Ｊの階数が３以上になるように、多関節ロボットの関節部のうち予め定める冗長関節部の角度を導出して、前記冗長関節部を角変位させてステップｕ３に進み、全ての手順を終了する。
【００６８】
前式（２）〜式（４）のうち、式（２）および式（３）については、θ４を０度および１８０度から遠ざけることによって、前記ヤコビ行列Ｊの回数が３未満になることを回避でき、θ４が９０度および２７０度に近づくと、特異点からかなり遠ざかることになる。したがってθ４を９０度または２７０度にできる限り近づけるように、冗長となる関節部、たとえば第１関節部９の角度位置θ１を決定するという簡便な方法によって、全ての特異点のうちの約３分の２を回避することができる。また式（４）については、前述のような簡便な方法を見出すのは困難ではあるけれども、このような場合は、前述の図２に示す第１の実施例を適用して、冗長となる関節部をプラス方向およびマイナス方向に角変位したときのヤコビ行列Ｊ＋，Ｊ−の行列式ｄｅｔ（Ｊ＋），ｄｅｔ（Ｊ−）の絶対値｜ｄｅｔ（Ｊ＋）｜，｜ｄｅｔ（Ｊ−）｜の大きさを比較して、前記絶対値が大きくなる方向に冗長となる関節部を角変位させればよい。
【００６９】
また前述の制御方法において、少なくとも１つの関節部の所定の位置からの角度位置が可動範囲の限界位置に近づいた場合、エンドエフェクタ１８を予め定める軌道上に保持しながら、当該関節部の角度位置を前記可動範囲の中央位置に近づけるように冗長となる関節部を角変位させる補助制御段階をさらに含むようにしてもよい。具体的には、冗長となる関節部をプラス方向およびマイナス方向に予め定める変位量Δθだけ角変位させて、冗長関節部のそれぞれの角度位置と希望するエンドエフェクタ１８の位置とから逆変換することによって、可動範囲の限界位置に近づいた関節部の角度位置を計算して、前記限界位置から遠ざかるものを採用する。
【図面の簡単な説明】
【００７０】
【図１】本発明の実施の一形態の多関節ロボット１の制御装置２を示すブロック図である。
【図２】多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第１の実施例の手順を示すフローチャートである。
【図３】多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第２の実施例の手順を示すフローチャートである。
【図４】多関節ロボット１の特異点を回避するように、多関節ロボット１を制御する制御方法の第３の実施例の手順を示すフローチャートである。
【符号の説明】
【００７１】
１  多関節ロボット
２  制御装置
９〜１５  関節部
１８  エンドエフェクタ
１９  中央演算処理部
【図１】
0004647919.tif 000005
【図２】
0004647919.tif 000006
【図３】
0004647919.tif 000007
【図４】
0004647919.tif 000008