(57)【要約】【課題】目標位置および目標姿勢に到達するように移動可能な多関節ロボットの提供。【解決手段】関節アームは、複数の関節により直列に連結された複数のリンクを有している。順運動学演算部２２は、パルスエンコーダ１６ａ〜１６ｆにより検出された各リンクの回転角度に基づき、関節アームの先端部の現在位置および現在姿勢を算出する。回転角更新部２５は、算出された関節アームの先端部の現在位置および現在姿勢の、関節アームの先端部の目標位置および目標姿勢に対する偏差が所定値以下となるまで、繰り返してリンクの回転角度を更新し、偏差が所定値以下となった場合、更新された回転角度に基づきリンクは回転される。偏差判定部２３は、偏差が所定値以下でなく、かつ、修正角度の算出回数が所定回数を超えた場合には、次回に算出される偏差を減少させ、減少された偏差が所定値以下であるか否かを判断する。【選択図】図２
2010036293.tif 000002
【特許請求の範囲】
【請求項１】
関節により複数のリンクが直列に連結された関節アームと、
各々の前記リンクを、隣接する前記リンクに対して前記関節の回転軸を中心に旋回させる駆動モーターと、
電力を供給して前記駆動モーターを作動させるモーター駆動手段と、
各々の前記リンクの隣接する前記リンクに対する回転角度に基づき、前記関節アームの先端部の現在位置および現在姿勢のうちの少なくともいずれかを算出する先端部演算手段と、
前記先端部演算手段により算出された前記関節アームの先端部の現在位置または現在姿勢の、前記関節アームの先端部の目標位置または目標姿勢に対する偏差を算出し、算出した前記偏差が所定条件を満足するか否かを判断する偏差判定手段と、
算出された前記偏差に基づき、各々の前記リンクの隣接する前記リンクに対する修正角度を算出し、算出された前記修正角度に基づき前記回転角度を更新する回転角度更新手段と、
を備え、
前記モーター駆動手段は、
前記偏差判定手段により前記偏差が前記所定条件を満足していると判断された場合、前記回転角度更新手段により更新された前記回転角度に基づき前記駆動モーターを作動させ、各々の前記リンクを隣接する前記リンクに対して旋回させて、前記関節アームの先端部の現在位置または現在姿勢を修正し、
前記偏差判定手段により前記偏差が前記所定条件を満足していないと判断された場合、
前記回転角度更新手段は、
前記偏差が前記所定条件を満足すると判断されるまで、前記偏差に基づき、各々の前記リンクの隣接する前記リンクに対する前記修正角度を算出し、前記修正角度に基づき、繰り返して前記回転角度を更新する多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、次回に算出される前記偏差に対する前記所定条件を緩和し、前記偏差が緩和された前記所定条件を満足するか否かを判断することを特徴とする多関節ロボット。
【請求項２】
前記偏差判定手段は、
前記関節アームの先端部の現在位置と目標位置とにより算出された前記偏差に対する前記所定条件は緩和せず、前記関節アームの先端部の現在姿勢と目標姿勢とにより算出された前記偏差に対する前記所定条件のみを緩和することを特徴とする請求項１記載の多関節ロボット。
【請求項３】
前記偏差判定手段は、
前記偏差が所定値以下であるか否かを判断し、
前記偏差判定手段により前記偏差が前記所定値以下でないと判断された場合、前記偏差が前記所定値以下であると判断されるまで、
前記回転角度更新手段は、繰り返して前記回転角度を更新し、
前記偏差判定手段は
前記偏差が前記所定値以下でないと判断した場合には、次回に算出される前記偏差を所定量だけ減少させ、所定量だけ減少された前記偏差が前記所定値以下であるか否かを判断することを特徴とする請求項１または２に記載の多関節ロボット。
【請求項４】
前記偏差判定手段は、
所定量だけ減少された前記偏差が前記所定値以下でないと判断した場合、次回に算出される前記偏差をさらに所定量だけ減少させ、さらに所定量だけ減少された前記偏差が前記所定値以下であるか否かを判断することを特徴とする請求項３記載の多関節ロボット。
【請求項５】
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、前記関節アームの先端部の現在姿勢と目標姿勢とにより算出された前記偏差が、前記所定条件を満足するか否かを判断せず、前記関節アームの先端部の現在位置と目標位置とにより算出された前記偏差が、前記所定条件を満足するか否かのみを判断することを特徴とする請求項２記載の多関節ロボット。
【請求項６】
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、前記関節アームの先端部の現在姿勢と目標姿勢とによる前記偏差を算出せず、前記関節アームの先端部の現在位置と目標位置とによる偏差のみを算出することを特徴とする請求項５記載の多関節ロボット。
【請求項７】
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、前記関節アームの先端部の現在姿勢と目標姿勢とにより算出された前記偏差をゼロとした後、前記所定条件を満足するか否かを判断することを特徴とする請求項２記載の多関節ロボット。
【請求項８】
前記偏差判定手段は、
前記偏差がある比較値以下であるか否かを判断し、
前記偏差判定手段により前記偏差が前記比較値以下でないと判断された場合、前記偏差が前記比較値以下であると判断されるまで、
前記回転角度更新手段は、繰り返して前記回転角度を更新し、
前記偏差判定手段は
前記偏差が前記比較値以下でないと判断した場合には、前記比較値を所定量だけ増大させ、次回に算出される前記偏差が所定量だけ増大された前記比較値以下であるか否かを判断することを特徴とする請求項１または２に記載の多関節ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の関節を有する関節アームを備えた多関節ロボットに関する。
【背景技術】
【０００２】
これまで、多関節ロボットに関する多くの従来技術があった。多関節ロボットは、複数のリンクが関節において直列に連結されて関節アームが形成され、各リンクには、隣接したリンクに対して関節を中心に回転させる駆動モーターが設けられている（例えば、特許文献１参照）。
【０００３】
この多関節ロボットの制御装置は、その作業内容に応じて、関節アームの先端にあるエンドエフェクタの位置および姿勢を目標位置および目標姿勢と一致させるように、駆動モーターの作動を制御している。このために、駆動モーターに備えられた角度センサにより、関節アームを構成する各リンクの回転角度を検出し、検出された各リンクの回転角度を用いて、順運動学による演算により、エンドエフェクタの現在位置および現在姿勢を求めている。
【０００４】
算出されたエンドエフェクタの現在位置および現在姿勢は、エンドエフェクタの目標位置および目標姿勢と比較されて、双方の間の差が算出される。当該差とヤコビ行列の疑似逆行列から各リンクの修正角度が演算され、算出された修正角度に基づき各リンクの回転角度が更新される。上述した差が一定の値以下の場合、更新された回転角度に基づき、駆動モーターを作動させエンドエフェクタを移動させている。ここで、上述したヤコビ行列は、各リンクの回転角度をエンドエフェクタの現在位置および現在姿勢に変換するための関数を行列化したものである。
【０００５】
前述の差が一定の値を超えている場合、更新された各リンクの回転角度から、再びエンドエフェクタの現在位置および現在姿勢を求め、上述した場合と同様に、エンドエフェクタの目標位置および目標姿勢と比較されて、双方の間の差が算出される。以下、エンドエフェクタの現在位置および現在姿勢と目標位置および目標姿勢との間の差が一定の値以下になるまで収束演算を行い、エンドエフェクタを目標位置および目標姿勢とするための各リンクの回転角度を求めている。
【特許文献１】特開２００５−２３０９５２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、エンドエフェクタの目標位置および目標姿勢に対して、関節アームの形状が成立不可能な特異姿勢にある場合、上述した収束演算においてリンクの回転角度の解が求まらず、関節アームを目標位置および目標姿勢に到達させることができない。
通常、多関節ロボットにおいては、エンドエフェクタを現在位置および現在姿勢から最終目標とする位置および姿勢まで移動させる軌跡上に、複数の補間点を形成して、各補間点ごとに上述した収束演算をしている。このため、仮に、最終目標位置および最終目標姿勢を満足するリンクの回転角度の解が求められても、いずれかの補間点において収束解が求まらなければ、その補間点において多関節ロボットは停止してしまう。
本発明は上記事情に鑑みてなされたものであり、その目的は、目標位置および目標姿勢に到達するように移動可能な多関節ロボットを提供することにある。
【課題を解決するための手段】
【０００７】
以下、上記課題を解決するのに適した各手段につき、必要に応じて作用効果等を付記しつつ説明する。
（手段１）手段１に係る多関節ロボットは、
関節により複数のリンクが直列に連結された関節アームと、
各々の前記リンクを、隣接する前記リンクに対して前記関節の回転軸を中心に旋回させる駆動モーターと、
電力を供給して前記駆動モーターを作動させるモーター駆動手段と、
各々の前記リンクの隣接する前記リンクに対する回転角度に基づき、前記関節アームの先端部の現在位置および現在姿勢のうちの少なくともいずれかを算出する先端部演算手段と、
前記先端部演算手段により算出された前記関節アームの先端部の現在位置または現在姿勢の、前記関節アームの先端部の目標位置または目標姿勢に対する偏差を算出し、算出した前記偏差が所定条件を満足するか否かを判断する偏差判定手段と、
算出された前記偏差に基づき、各々の前記リンクの隣接する前記リンクに対する修正角度を算出し、算出された前記修正角度に基づき前記回転角度を更新する回転角度更新手段と、
を備え、
前記モーター駆動手段は、
前記偏差判定手段により前記偏差が前記所定条件を満足していると判断された場合、前記回転角度更新手段により更新された前記回転角度に基づき前記駆動モーターを作動させ、各々の前記リンクを隣接する前記リンクに対して旋回させて、前記関節アームの先端部の現在位置または現在姿勢を修正し、
前記偏差判定手段により前記偏差が前記所定条件を満足していないと判断された場合、
前記回転角度更新手段は、
前記偏差が前記所定条件を満足すると判断されるまで、前記偏差に基づき、各々の前記リンクの隣接する前記リンクに対する前記修正角度を算出し、前記修正角度に基づき、繰り返して前記回転角度を更新する多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、次回に算出される前記偏差に対する前記所定条件を緩和し、前記偏差が緩和された前記所定条件を満足するか否かを判断する。
【０００８】
したがって、手段１に係る多関節ロボットによれば、収束解が求まらない場合には、所定条件を緩和することにより、リンクの回転角度が求まりやすくなるため、関節アームが移動軌跡の途中で停止することがなく、目標位置および目標姿勢に到達するように移動することができる。
【０００９】
また、収束解が求まりにくい場合にも、短時間で各リンクの回転角度を求めて関節アームを移動させることができ、作動効率を向上させることが可能になる。なお、偏差判定手段により偏差が所定条件を満足していないと判定された場合に、先端部演算手段が新たに算出する関節アームの先端部の現在位置および現在姿勢は、実際の関節アームの先端部の位置および姿勢ではなく、演算に用いるための関節アームの先端部の位置および姿勢である。
【００１０】
（手段２）手段１に記載の多関節ロボットにおいて、
前記偏差判定手段は、
前記関節アームの先端部の現在位置と目標位置とにより算出された前記偏差に対する前記所定条件は緩和せず、前記関節アームの先端部の現在姿勢と目標姿勢とにより算出された前記偏差に対する前記所定条件のみを緩和する。
したがって、手段２に係る多関節ロボットによれば、関節アームの先端部の現在位置と目標位置とにより算出された偏差に対する所定条件を緩和することがないため、先端部が移動軌跡上の位置を外れることがなく、先端部の他部材に対する干渉等を避けることができる。
【００１１】
（手段３）手段１または２に記載の多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差が所定値以下であるか否かを判断し、
前記偏差判定手段により前記偏差が前記所定値以下でないと判断された場合、前記偏差が前記所定値以下であると判断されるまで、
前記回転角度更新手段は、繰り返して前記回転角度を更新し、
前記偏差判定手段は
前記偏差が前記所定値以下でないと判断した場合には、次回に算出される前記偏差を所定量だけ減少させ、所定量だけ減少された前記偏差が前記所定値以下であるか否かを判断する。
【００１２】
手段３は、次回に算出される偏差を所定量だけ減少させることで、減少された偏差と所定値との比較条件を緩和している。つまり、所定値そのものを大きくすることにより比較条件を緩和するのではなく、偏差を実際の偏差よりも小さな値として算出することで、上記手段における「次回に算出される偏差に対する所定条件」を緩和している。なお、手段３における「偏差が所定値以下であること」が、上記手段における「所定条件」である。
【００１３】
このように、手段３に係る多関節ロボットによれば、偏差が所定値以下でないと判断された場合には、偏差を所定量だけ減少させるため、簡単な演算によりリンクの回転角度の収束解を求まりやすくすることができる。
ここで所定量だけ減少させるとは、偏差を一定量減少させる場合も、偏差を一定割合減少させる場合も含んでいる。
【００１４】
（手段４）手段３の多関節ロボットにおいて、
前記偏差判定手段は、
所定量だけ減少された前記偏差が前記所定値以下でないと判断した場合、次回に算出される前記偏差をさらに所定量だけ減少させ、さらに所定量だけ減少された前記偏差が前記所定値以下であるか否かを判断する。
【００１５】
したがって、手段４に係る多関節ロボットによれば、所定量だけ減少された偏差が未だ所定値以下にないと判断した場合には、偏差をさらに所定量だけ減少させることにより、さらにリンクの回転角度が求まりやすくなるため、いっそう短時間で各リンクの回転角度を求めることができる。
【００１６】
（手段５）手段２に記載の多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、前記関節アームの先端部の現在姿勢と目標姿勢とにより算出された前記偏差が、前記所定条件を満足するか否かを判断せず、前記関節アームの先端部の現在位置と目標位置とにより算出された前記偏差が、前記所定条件を満足するか否かのみを判断する。
したがって、手段５に係る多関節ロボットによれば、関節アームの先端部の現在位置と目標位置とにより算出された偏差が、所定条件を満足するか否かのみを判断するため、偏差判定手段による演算時間を大幅に短縮できる。
【００１７】
（手段６）手段５の多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、前記関節アームの先端部の現在姿勢と目標姿勢とによる前記偏差を算出せず、前記関節アームの先端部の現在位置と目標位置とによる偏差のみを算出する。
したがって、手段６に係る多関節ロボットによれば、関節アームの先端部の現在姿勢と目標姿勢とによる偏差を算出しないため、偏差判定手段による演算時間を短縮できるとともに、演算に要するメモリ容量を低減できる。
【００１８】
（手段７）手段２に記載の多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差が前記所定条件を満足していないと判断した場合には、前記関節アームの先端部の現在姿勢と目標姿勢とにより算出された前記偏差をゼロとした後、前記所定条件を満足するか否かを判断する。
したがって、手段７に係る多関節ロボットによれば、複雑な演算を行うことなく、関節アームの先端部の現在姿勢と目標姿勢とにより算出された偏差に対する所定条件を緩和することができる。
【００１９】
（手段８）手段１または２の多関節ロボットにおいて、
前記偏差判定手段は、
前記偏差がある比較値以下であるか否かを判断し、
前記偏差判定手段により前記偏差が前記比較値以下でないと判断された場合、前記偏差が前記比較値以下であると判断されるまで、
前記回転角度更新手段は、繰り返して前記回転角度を更新し、
前記偏差判定手段は
前記偏差が前記比較値以下でないと判断した場合には、前記比較値を所定量だけ増大させ、次回に算出される前記偏差が所定量だけ増大された前記比較値以下であるか否かを判断する。
【００２０】
したがって、手段８に係る多関節ロボットによれば、収束解が求まらない場合には、偏差が所定量だけ増大された比較値以下であるか判断されるため、簡単な演算によりリンクの回転角度の収束解を求まりやすくすることができる。
ここで所定量だけ増大させるとは、比較値を一定量増大させる場合も、比較値を一定割合増大させる場合も含んでいる。
【発明を実施するための最良の形態】
【００２１】
＜実施形態１＞
図１乃至図６に基づき、本発明の実施形態１による多関節ロボットについて説明する。図１は、本実施形態による多関節ロボットを形成する関節アーム１（本発明の関節アームに該当する）を、スケルトン図により概略的に表している。関節アーム１は、７個のリンク１１ａ〜１１ｇ（本発明のリンクに該当する）が６個の関節１２ａ〜１２ｆ（本発明の関節に該当する）により直列に連結されて形成されている。本実施形態による多関節ロボットの関節アーム１は、６個の関節１２ａ〜１２ｆにおいてリンク１１ａ〜１１ｇが旋回することのできる、６自由度を有するロボットであるが、これに限られるものではない。
【００２２】
第１リンク１１ａは一端が床面ＦＬに固定され、他端が第１関節１２ａの一側に接続されている。第１関節１２ａの他側には、第２リンク１１ｂの一端が接続され、第２リンク１１ｂの他端には第２関節１２ｂの一側が接続されている。以下、第３リンク１１ｃ、第４リンク１１ｄ、第５リンク１１ｅ、第６リンク１１ｆ、第７リンク１１ｇが、それぞれ第３関節１２ｃ、第４関節１２ｄ、第５関節１２ｅ、第６関節１６ｆを介して順に連結されている。
【００２３】
第１関節１２ａの他側は一側に対して、矢印１５ａに示すように、図１において上下方向に延びる軸を中心に回転可能とされており、これにより、第２リンク１１ｂは隣接する第１リンク１１ａに対して、第１関節１２ａの回転軸を中心に矢印１５ａ方向に旋回可能である。
【００２４】
また、第２関節１２ｂの他側は一側に対して、矢印１５ｂに示すように、図１において紙面に垂直な方向に延びる軸を中心に回転可能とされており、これにより、第３リンク１１ｃは隣接する第２リンク１１ｂに対して、第２関節１２ｂの回転軸を中心に矢印１５ｂ方向に旋回可能である。
【００２５】
以下、第３関節１２ｃ、第４関節１２ｄ、第５関節１２ｅ、第６関節１６ｆについても回転可能とされており、第４リンク１１ｄ、第５リンク１１ｅ、第６リンク１１ｆ、第７リンク１１ｇも、それぞれ関節１２ｃ、１２ｄ、１２ｅ、１２ｆの回転軸を中心に、矢印１５ｃ、１５ｄ、１５ｅ、１５ｆ方向に旋回可能である。尚、本願の全体にわたって、関節１２ａ〜１２ｆを介して連結されているリンク１１ａ〜１１ｇ同士を、互いに隣接するリンク１１ａ〜１１ｇということにする。
【００２６】
第１関節１２ａには第１ステッピングモーター１３ａが取り付けられており、電力が供給されることにより、第２リンク１１ｂを第１リンク１１ａに対して旋回させる。また、第２関節１２ｂには第２ステッピングモーター１３ｂが取り付けられており、電力が供給されることにより、第３リンク１１ｃを第２リンク１１ｂに対して旋回させる。
以下、同様に、第３関節１２ｃ、第４関節１２ｄ、第５関節１２ｅ、第６関節１６ｆにはそれぞれステッピングモーター１３ｃ、１３ｄ、１３ｅ、１３ｆ（ステッピングモーター１３ａ〜１３ｆは、本発明の駆動モーターに該当する）が取り付けられており、電力が供給されることにより、各々リンク１１ｄ、１１ｅ、１１ｆ、１１ｇを旋回させる。
【００２７】
第７リンク１１ｇの先端には、ツール１４が取り付けられている。ツール１４は第７リンク１１ｇとともに、第６関節１２ｆの回転軸を中心に矢印１５ｆ方向に旋回可能とされている。また、ツール１４は互いに対向するように延びた一対の狭持片１４ａ、１４ｂを具備している。狭持片１４ａ、１４ｂは、それぞれ枢支軸１４ｃ、１４ｄを中心に、矢印１４ｅ、１４ｆ方向に揺動可能とされており、狭持片１４ａ、１４ｂの先端（本発明の関節アームの先端部に該当する）が互いに接近することにより、ワーク等を把持することが可能とされている。
【００２８】
上述したように関節アーム１は、ステッピングモーター１３ａ〜１３ｆを駆動してリンク１１ａ〜１１ｆを旋回させることにより、リンク１１ａ〜１１ｆの回転角度が累積して先端部にあるツール１４に働くため、ツール１４の先端の位置および姿勢を、その作業内容に応じた目標位置および目標姿勢に一致させることができる。
【００２９】
次に、図２に基づいて、コントローラー２を中心とした多関節ロボットの電気的な構成を説明する。コントローラー２には、それぞれモーター駆動部３ａ〜３ｆ（本発明のモーター駆動手段に該当する）を介して、上述したステッピングモーター１３ａ〜１３ｆが電気的に接続されている。モーター駆動部３ａ〜３ｆは、パワートランジスタまたはリレー等により形成されたモータードライバーで、コントローラー２からの駆動信号に従い電力を供給し、ステッピングモーター１３ａ〜１３ｆを必要な角度だけ駆動させる。
【００３０】
パルスエンコーダ１６ａ〜１６ｆは、それぞれステッピングモーター１３ａ〜１３ｆに内蔵されており、コントローラー２と接続されている。パルスエンコーダ１６ａ〜１６ｆは、各々のステッピングモーター１３ａ〜１３ｆの回動角度を検出することにより、延いてはリンク１１ａ〜１１ｇのそれぞれが隣接するリンク１１ａ〜１１ｇに対する回転角度を検出して、検出信号をコントローラー２に送信することができる。ステッピングモーター１３ａ〜１３ｆにパルスエンコーダ１６ａ〜１６ｆを設ける代わりに、リンク１１ａ〜１１ｇまたは関節１２ａ〜１２ｆに、リンク１１ａ〜１１ｇの回転角度を直接に検出可能なセンサを取り付けてもよい。
【００３１】
コントローラー２と接続された入力装置４は、モニター画面を有した操作盤であり、ユーザーが操作可能とされている。入力装置４には、多関節ロボットの電源スイッチが設けられるとともに、関節アーム１の先端部にあるツール１４の先端の最終目標位置および最終目標姿勢、ツール１４の先端の補間点における位置および姿勢等を、コントローラー２に対し入力可能とされている。
【００３２】
メモリ５は揮発性メモリおよび不揮発性メモリにより形成され、コントローラー２と接続されており、記憶された各種の情報をコントローラー２に供給する。メモリ５のプログラム記憶部（ＰＭ）５１には、関節アーム１を動作させる作動プログラム、順運動学演算実行プログラム、ヤコビ行列演算実行プログラムおよび疑似逆行列演算実行プログラム、回転角度演算実行プログラム等を記憶している。
【００３３】
また、データ記憶部（ＤＭ）５２には、関節アーム１を動作させるための作動データ、ツール１４の先端の現在位置と、最終目標位置との間を直線補間するためのデータ、パルスエンコーダ１６ａ〜１６ｆの検出信号から各リンク１１ａ〜１１ｇの回転角度を検出するためのデータ等が記憶されている。
さらに、係数記憶部（ＣＭ）５３には、偏差が所定条件を満足する否かを判断するための係数データ等が記憶されている。
【００３４】
ツールドライバ６は、コントローラー２およびツール１４の狭持片１４ａ、１４ｂを作動させるための図示しない電動モーターに接続されている。ツールドライバ６は、モーターリレーまたはトランジスタモジュール等により形成されており、コントローラー２からの作動信号に基づき電動モーターを駆動させ、ツール１４の狭持片１４ａ、１４ｂを作動させる。
コントローラー２の制御部２１はＣＰＵにより形成され、入力装置４に対する操作に基づいて各演算部を制御するとともに、モーター駆動部３ａ〜３ｆに対し発信し、関節アーム１を動作させる。
【００３５】
また、順運動学演算部２２（本発明の先端部演算手段に該当する）は、制御部２１により制御され、関節アーム１の仕様に応じ、パルスエンコーダ１６ａ〜１６ｆにより検出された各リンク１１ａ〜１１ｇの回転角度に基づき、順運動学によりツール１４の先端の現在位置および現在姿勢を算出する。
また、偏差判定部２３（本発明の偏差判定手段に該当する）は制御部２１により制御され、順運動学演算部２２により算出されたツール１４の先端の現在位置および現在姿勢の、ツール１４の目標位置および目標姿勢に対する偏差を算出し、算出された偏差が所定条件を満足するか否かを判断する。
【００３６】
また、行列演算部２４は制御部２１により制御され、検出された各リンク１１ａ〜１１ｇの回転角度と、順運動学演算部２２により算出されたツール１４の先端の現在位置および現在姿勢とから、ヤコビ行列およびその疑似逆行列を算出する。
また、回転角更新部２５（本発明の回転角度更新手段に該当する）は制御部２１により制御され、偏差判定部２３により算出された偏差に基づき、各々のリンク１１ａ〜１１ｇの隣接するリンク１１ａ〜１１ｇに対する修正角度を算出し、これらの回転角度を更新する。
【００３７】
次に、図３乃至図６に基づき、本実施形態による多関節ロボットのコントローラー２による作動制御方法について説明する。
最初に、入力装置４に対するユーザーの操作により、３次元上においてツール１４の先端の最終目標位置（ゴールポイント）Ｆ、および最終目標位置Ｆでのツール１４の姿勢がコントローラー２に対して入力される。制御部２１は関節アーム１の周辺の障害物との干渉の回避を考慮して、図３に示すように、ツール１４の先端の現在位置（スタートポイント）Ｓと、ツール１４の先端の最終目標位置Ｆとを結ぶように、３次元上においてツール１４の先端の移動軌跡Ｈを形成する。
【００３８】
次に、制御部２１が形成された移動軌跡Ｈを直線補間することにより、移動軌跡Ｈ上には複数の補間点Ｐ1〜Ｐnが設けられる。あるいは、ユーザーが入力装置４を操作して、移動軌跡Ｈを直線補間することにより、補間点Ｐ1〜Ｐnを形成してもよい。各々の補間点Ｐ1〜Ｐnにおいては、周辺の障害物との干渉の回避等を考慮して、ツール１４の先端の位置および姿勢（目標位置および目標姿勢：Ｒwref）が定められている。
【００３９】
次に、ツール１４の先端を現在位置から直近の補間点Ｐ1〜Ｐnに移動させるために、各リンク１１ａ〜１１ｇを隣接するリンク１１ａ〜１１ｇに対して回転させる角度：θnowを求める収束演算を行う。この回転角度：θnowは、ツール１４の先端が補間点Ｐ1〜Ｐn上の目標位置および目標姿勢：Ｒwrefを充足することが可能な値である必要がある。
例えば、ツール１４の先端が現在スタートポイントＳにある場合において、ツール１４の先端を補間点Ｐ1に移動させる場合を考える。図５に示すように、最初に入力装置４により、補間点Ｐ1上における目標位置および目標姿勢：Ｒwrefが入力される。また、パルスエンコーダ１６ａ〜１６ｆにより、スタートポイントＳ上の、各々のリンク１１ａ〜１１ｇの隣接するリンク１１ａ〜１１ｇに対する、現在の回転角度：θnowがコントローラー２に入力される（ステップＳ５０１）。
【００４０】
次に、制御部２１のメモリカウンター中のｋを０とし（ステップＳ５０２）、その後、ｋが１だけインクリメントされる（ステップＳ５０３）。次に、順運動学演算部２２により、各リンク１１ａ〜１１ｇの現在の回転角度：θnowに基づき、ツール１４の先端の現在位置および現在姿勢：Ｒwnowを算出する（ステップＳ５０４、図４において符号７５に示す）。
【００４１】
次に、ツール１４の先端の目標位置および目標姿勢：Ｒwrefとツール１４の先端の現在位置および現在姿勢：Ｒwnowとから、ツール１４の先端の現在位置および現在姿勢：Ｒwnowの、ツール１４の先端の目標位置および目標姿勢：Ｒwrefに対する偏差：ΔＲw＝Ｒwref−Ｒwnowが算出される（ステップＳ５０５、図４において符号７１に示す）。具体的にいえば、図４において符号７２に示すように、偏差：ΔＲwは位置偏差パラメータ：ΔＰwと姿勢偏差パラメータ：ΔＱw（以下、それぞれ位置偏差：ΔＰw、姿勢偏差：ΔＱwという）とにより構成されている（ΔＲw＝（ΔＰw，ΔＱw））。また、ツール１４の先端の現在位置：Ｐwnow、目標位置：Ｐwref、現在姿勢：Ｑwnow、目標姿勢：Ｑwrefとすると、ΔＰw＝Ｐwref−Ｐwnow、ΔＱw＝Ｑwref−Ｑwnowとなる。
【００４２】
次に、偏差：ΔＲwが所定の比較値：εと比較され、偏差：ΔＲwが比較値：ε以下（本発明の所定条件に該当する）の場合、収束解が求まったとされる（ステップＳ５０６）。具体的にいえば、偏差：ΔＲwを構成する位置偏差：ΔＰwと姿勢偏差：ΔＱwが、それぞれ所定の比較値：εp、εQと比較される。位置偏差：ΔＰw（＝Ｐwref−Ｐwnow）が比較値：εp以下で、かつ、姿勢偏差：ΔＱw（＝Ｑwref−Ｑwnow）が比較値：εQ以下の場合、収束解が求まったとされる。
【００４３】
これによりモーター駆動部３ａ〜３ｆは、回転角更新部２５により更新された回転角度：θnowに基づき、ステッピングモーター１３ａ〜１３ｆを作動させ、各々のリンク１１ａ〜１１ｇを隣接するリンク１１ａ〜１１ｇに対して旋回させて、ツール１４の先端の現在位置および現在姿勢：Ｒwnowを修正し、補間点Ｐ1への移動を完了する（ステップＳ５０８）。ツール１４の先端の現在位置および現在姿勢：Ｒwnowが修正された後、メインルーチンによる制御は終了する。
尚、メインルーチンの１巡目において収束解が求まった場合は、ステップＳ５０８において、入力された現在の回転角度：θnowに基づき、ステッピングモーター１３ａ〜１３ｆを作動させるため、リンク１１ａ〜１１ｇは実質的には旋回しない。
【００４４】
一方、偏差：ΔＲwが比較値：εよりも大きい場合（位置偏差：ΔＰwが比較値：εpよりも大きい、または、姿勢偏差：ΔＱwが比較値：εQよりも大きい場合）、収束解は求まっておらず、リンク１１ａ〜１１ｇの修正角度：Δθの算出回数がＮ回を超えたか否かが判定される（ステップＳ５０７）。リンク１１ａ〜１１ｇの修正角度：Δθの算出回数がＮ回以下の場合、ステップＳ５０９へと進む。
【００４５】
次に、関係式：Ｊ・θnow＝Ｒwnowに基づき、各リンク１１ａ〜１１ｇの現在の回転角度：θnow、および順運動学演算部２２により求められたツール１４の先端の現在位置および現在姿勢：Ｒwnowを用いて、ヤコビ行列：Ｊを算出する（ステップＳ５０９）。前述したように、ヤコビ行列：Ｊとは、各リンク１１ａ〜１１ｇの回転角度：θnowをツール１４の先端の現在位置および現在姿勢：Ｒwnowに変換するための関数を行列化したものである。本実施形態の場合、ヤコビ行列：Ｊは６行６列（６×６）の行列となる。
【００４６】
次に、求められたヤコビ行列：Ｊから、関係式：Ｊ♯＝ＪT・（Ｊ・ＪT）-1に基づき、ヤコビ行列：Ｊの疑似逆行列：Ｊ♯が求められる（ステップＳ５１０）。尚、上式において、例えば、ＡTは行列Ａの転置行列を表し、Ａ-1は行列Ａの逆行列を表している。
次に、疑似逆行列：Ｊ♯と偏差：ΔＲwとから、関係式：Δθ＝Ｊ♯・ΔＲwに基づき、リンク１１ａ〜１１ｇの修正角度：Δθが求められ（ステップＳ５１１、図４において符号７３に示す）、求められた修正角度：Δθにより、リンク１１ａ〜１１ｇの回転角度：θnowが更新される（ステップＳ５１２、図４において符号７４に示す）。
【００４７】
次に、制御部２１のメモリカウンター中のｋを１だけインクリメントし（ステップＳ５０３）、再び、ステップＳ５０４以降の演算処理を実行する。すなわち、順運動学演算部２２が前回更新されたリンク１１ａ〜１１ｇの回転角度：θnowに基づき、ツール１４の先端の現在位置および現在姿勢：Ｒwnowを新たに算出する。また、回転角更新部２５は、新たに算出された偏差：ΔＲwが比較値：ε以下であると判断されるまで（収束解が求まるまで）、繰り返して回転角度：θnowを更新する。
【００４８】
偏差：ΔＲwが比較値：εよりも大きいと判断された後、リンク１１ａ〜１１ｇの修正角度：Δθの算出回数がＮ回を超えたと判定された場合、図６に示したサブルーチンによる制御が開始される。
図６に示したサブルーチンにおいては、最初に、制御部２１のメモリカウンター中のＵを０とし（ステップＳ６０１）、その後、Ｕが１だけインクリメントされて、１巡目のルーチンが開始する（ステップＳ６０２）。さらに、制御部２１のメモリカウンター中のＭを０とし（ステップＳ６０３）、その後、Ｍが１だけインクリメントされる（ステップＳ６０４）。
次に、上述したメインルーチンの場合と同様に、ヤコビ行列：Ｊを算出し（ステップＳ６０５）、求められたヤコビ行列：Ｊから、ヤコビ行列：Ｊの疑似逆行列：Ｊ♯が求められる（ステップＳ６０６）。
【００４９】
次に、疑似逆行列：Ｊ♯とメインルーチンにおいて算出された偏差：ΔＲwとから、関係式：Δθ＝Ｊ♯・ΔＲwに基づき、リンク１１ａ〜１１ｇの修正角度：Δθが求められ（ステップＳ６０７、図４において符号７８に示す）、求められた修正角度：Δθにより、リンク１１ａ〜１１ｇの回転角度：θnowが更新される（ステップＳ６０８、図４において符号７９に示す）。
次に、順運動学演算部２２により、新たに更新されたリンク１１ａ〜１１ｇの回転角度：θnowに基づき、ツール１４の先端の現在位置および現在姿勢：Ｒwnowを算出する（ステップＳ６０９、図４において符号８０に示す）。
【００５０】
次に、ツール１４の先端の目標位置および目標姿勢：Ｒwrefと、ツール１４の先端の現在位置および現在姿勢：Ｒwnowとから、ツール１４の先端の現在位置および現在姿勢：Ｒwnowの、ツール１４の先端の目標位置および目標姿勢：Ｒwrefに対する偏差：ΔＲwが算出される（ステップＳ６１０、図４において符号７６に示す）。
この場合、偏差：ΔＲwは位置偏差：ΔＰwと姿勢偏差：ΔＱｗ´とにより構成されている。また、ツール１４の先端の現在位置：Ｐwnow、目標位置：Ｐwref、とすると、位置偏差：ΔＰw＝Ｐwref−Ｐwnowとなる。
【００５１】
しかしながら、ツール１４の先端の現在姿勢：Ｑwnow、目標姿勢：Ｑwrefとすると、姿勢偏差：ΔＱw´は、関係式：ΔＱw´＝０．９・（Ｑwref−Ｑwnow）により算出される（図４において符号７７に示す）。すなわち、メインルーチンの場合と同様にΔＱw＝Ｑwref−Ｑwnowとすれば、ΔＱw´＝０．９・ΔＱwとなる。ここで、０．９は重み係数：Ｗqであり、サブルーチンにおける１巡目のルーチンにおいては０．９と定められている。
【００５２】
次に、偏差：ΔＲwを構成する位置偏差：ΔＰwと姿勢偏差：ΔＱw´が、それぞれ所定の比較値：εp、εQと比較される。位置偏差：ΔＰw（＝Ｐwref−Ｐwnow）が比較値：εp以下で、かつ、姿勢偏差：ΔＱw´（＝０．９・（Ｑwref−Ｑwnow））が比較値：εQ以下の場合、収束解が求まったとされる（ステップＳ６１１）。これから分かるように、サブルーチンによる制御においてはメインルーチンによる制御に対し、位置偏差：ΔＰwの比較値：εpに対する比較条件は緩和されていないが、姿勢偏差：ΔＱw´は減少されているため、姿勢偏差：ΔＱw´の比較値：εQに対する比較条件は緩和されている。
【００５３】
収束解が求まると、モーター駆動部３ａ〜３ｆは、回転角更新部２５により更新された回転角度：θnowに基づき、ステッピングモーター１３ａ〜１３ｆを作動させ、各々のリンク１１ａ〜１１ｇを隣接する前記リンク１１ａ〜１１ｇに対して旋回させて、ツール１４の先端の現在位置および現在姿勢：Ｒwnowを修正する（ステップＳ６１４）。ツール１４先端の現在位置および現在姿勢：Ｒwnowが修正された後、サブルーチンによる制御は終了する。
【００５４】
一方、位置偏差：ΔＰwが比較値：εpよりも大きい、または、姿勢偏差：ΔＱw´が比較値：εQよりも大きい場合、収束解は求まっておらず、リンク１１ａ〜１１ｇの修正角度：Δθの算出回数がＬ回を超えたか否かが判定される（ステップＳ６１２）。リンク１１ａ〜１１ｇの修正角度：Δθの算出回数がＬ回以下の場合、Ｍがさらに１つずつインクリメントされ、収束解が求まるまで上述した演算が再度繰り返される（ステップＳ６０４〜ステップＳ６１１、図４において符号７６〜８０に示す）。
【００５５】
収束解が求まらずにＭがＬを超えた場合（ステップＳ６１２）、ＵがＳを超えているか否かが判定され（ステップＳ６１３）、メモリカウンター中のＵがＳ以下である場合、Ｕが１つだけインクリメントされた上で、サブルーチンの２巡目のルーチンが開始され、再び上述した演算が繰り返される（ステップＳ６０４〜ステップＳ６１２）。
【００５６】
サブルーチンの２巡目のルーチンにおいては、位置偏差：ΔＰwはイニシャルルーチンと同様に関係式：ΔＰw＝Ｐwref−Ｐwnowにより算出されるが、姿勢偏差：ΔＱw´は、関係式：ΔＱw´＝０．９2・（Ｑwref−Ｑwnow）により算出される（ステップＳ６１０、図４において符号８２に示す）。すなわち、重み係数：Ｗq＝０．９2と定められる。
【００５７】
また、サブルーチンの２巡目のルーチンにおいても、位置偏差：ΔＰwと姿勢偏差：ΔＱw´が、それぞれ比較値：εp、εQと比較される。位置偏差：ΔＰw（＝Ｐwref−Ｐwnow）が比較値：εp以下で、かつ、姿勢偏差：ΔＱw´（＝０．９2・（Ｑwref−Ｑwnow））が比較値：εQ以下の場合、収束解が求まったとされる（ステップＳ６１１）。これから分かるように、サブルーチンの２巡目のルーチンによる制御においては、メインルーチンによる制御に対し、姿勢偏差：ΔＱw´の比較値：εQに対する比較条件がさらに緩和された状態で、収束演算が行われる。
【００５８】
これ以降サブルーチンにおいて、Ｕ巡目のルーチンにおける姿勢偏差：ΔＱw´を、関係式：ΔＱw´＝０．９U・（Ｑwref−Ｑwnow）で求めた上で（図４において符号８７に示す）、位置偏差：ΔＰwが比較値：εp以下で、かつ、姿勢偏差：ΔＱw´が比較値：εQ以下であると判断されるまで（収束解が求まるまで）、回転角更新部２５は繰り返して修正角度：Δθを算出する。
【００５９】
位置偏差：ΔＰwが比較値：εpよりも大きい、または、姿勢偏差：ΔＱw´が比較値：εQよりも大きいと判断され（ステップＳ６１１）、リンク１１ａ〜１１ｇの修正角度：Δθの算出回数がＬ回を超えたと判定された（ステップＳ６１２）後、さらに、メモリカウンター中のＵがＳを超えたと判定された場合、収束解は求まらずにサブルーチンは終了する（ステップＳ６１３）。
【００６０】
本実施形態によれば、偏差判定部２３は、偏差：ΔＲwが比較値：ε以上であると判断し、かつ、回転角更新部２５による修正角度：Δθの算出回数がＮ回を超えた場合には、次回（サブルーチンにおけるイニシャルルーチン）に算出される偏差：ΔＲwの比較値：εに対する比較条件を緩和し、偏差：ΔＲwが緩和された比較条件を満足するか否かを判断する。
したがって、収束解が求まらずに、修正角度：Δθの算出回数がＮ回を超えた場合には、比較条件を緩和することにより、リンク１１ａ〜１１ｇの回転角度：θnowが求まりやすくなるため、関節アーム１が移動軌跡Ｈの途中で停止することがなく、目標位置および目標姿勢に到達するように移動することができる。
【００６１】
また、収束解が求まりにくい場合にも、短時間で各リンク１１ａ〜１１ｇの回転角度：θnowを求めて関節アーム１を移動させることができ、作動効率を向上させることが可能になる。
また、ツール１４の先端の現在姿勢：Ｑwnowと目標姿勢：Ｑwrefとにより算出された姿勢偏差：ΔＱwに対する比較条件のみを緩和し、ツール１４の先端の現在位置：Ｐwnowと目標位置：Ｐwrefとにより算出された位置偏差：ΔＰwに対する所定条件を緩和することがないため、ツール１４の先端が移動軌跡Ｈ上の位置を外れることがなく、先端の他部材に対する干渉等を避けることができる。
【００６２】
また、偏差：ΔＲwが比較値：ε以下でないと判断し、かつ、修正角度：Δθの算出回数がＮ回を超えた場合には、次回に算出される姿勢偏差：ΔＱw´を所定量だけ減少させて収束解を求める演算を行い、所定量だけ減少された姿勢偏差：ΔＱw´が比較値：εQ以下であるか否かを判断するため、簡単な演算によりリンク１１ａ〜１１ｇの回転角度：θnowの収束解を求まりやすくすることができる。
【００６３】
また、所定量だけ減少された姿勢偏差：ΔＱw´により構成された偏差：ΔＲwを用いて収束演算を行っても、偏差：ΔＲwが比較値：ε以下でないと判断した場合、次回、算出される姿勢偏差：ΔＱw´をさらに所定量だけ減少させる。そして、さらに所定量だけ減少された姿勢偏差：ΔＱw´を用いて収束演算を行い、姿勢偏差：ΔＱw´が比較値：εQ以下であるか否かを判断することにより、さらにリンク１１ａ〜１１ｇの回転角度：θnowが求まりやすくなるため、いっそう短時間で各リンク１１ａ〜１１ｇの回転角度：θnowを求めることができる。
【００６４】
＜実施形態２＞
図７に基づき、本発明の実施形態２による多関節ロボットについて説明する。以下、実施形態１と異なる構成についてのみ説明を行う。本実施形態のサブルーチンによる制御においては、ツール１４の先端の目標位置：Ｐwrefとツール１４の先端の現在位置：Ｐwnowとから、ツール１４の先端の位置偏差：ΔＰwが算出され、位置偏差：ΔＰwをツール１４の先端の偏差：ΔＲwとしている（ステップＳ７０８）。
【００６５】
すなわち本実施形態においては、偏差：ΔＲwを算出する際に、ツール１４の先端の目標姿勢：Ｑwrefとツール１４の先端の現在姿勢：Ｑwnowとによるツール１４の先端の姿勢偏差：ΔＱwは算出されていない。
したがって、位置偏差：ΔＰwのみが所定の比較値：εpと比較され、位置偏差：ΔＰwが比較値：εp以下の場合、収束解が求まったとされる（ステップＳ７０９）。一方、ツール１４の先端の姿勢偏差：ΔＱwと比較値：εQとの比較は行われない。
【００６６】
本実施形態によれば、姿勢偏差ΔＱwが比較値：εQ以下であるか否かを判断せず、ツール１４の先端の現在位置Ｐwnowと目標位置Ｐwrefにより算出された位置偏差ΔＰwが、比較値：εp以下であるか否かのみを判断するため、偏差判定部２３による演算時間を大幅に短縮できる。
また、ツール１４の先端の現在姿勢Ｑwnowと目標姿勢Ｑwrefとによる姿勢偏差ΔＱwを算出せず、現在位置Ｐwnowと目標位置Ｐwrefとによる位置偏差ΔＰwのみを算出するため、偏差判定部２３による演算時間を短縮できるとともに、演算に要する回転角更新部２５のメモリ容量を低減できる。
【００６７】
尚、本実施形態の場合、それぞれステップＳ７０４、ステップＳ７０５にて算出されるヤコビ行列：Ｊおよびその疑似逆行列Ｊ♯は、３行６列（３×６）の行列となる。
また、本実施形態においては、偏差判定部２３において位置偏差ΔＰwの算出とともに、ツール１４の先端の現在姿勢Ｑwnowと目標姿勢Ｑwrefとによる姿勢偏差ΔＱwの算出を行い、姿勢偏差ΔＱwと比較値εQとの比較は行わずに、位置偏差ΔＰwと比較値εpとの比較のみを行ってもよい。
【００６８】
＜実施形態３＞
図８に基づき、本発明の実施形態３による多関節ロボットについて説明する。以下、実施形態１と異なる構成についてのみ説明を行う。本実施形態のサブルーチンによる制御においては実施形態１と同様に、ツール１４の先端の偏差：ΔＲwが、ツール１４の先端の位置偏差：ΔＰwおよび姿勢偏差：ΔＱwとにより構成されている。
ツール１４の先端の位置偏差：ΔＰwは、ツール１４の先端の目標位置：Ｐwrefとツール１４の先端の現在位置：Ｐwnowとから算出されている。一方、ツール１４の先端の姿勢偏差：ΔＱwは常時０とされている（ステップＳ８０８）。
【００６９】
したがって、ツール１４の先端の目標位置：Ｐwrefとツール１４の先端の現在位置：Ｐwnowとから算出された位置偏差：ΔＰwと、常に０とされた姿勢偏差：ΔＱwとにより形成された偏差：ΔＲwが、所定の比較値：εと比較され、偏差：ΔＲwが比較値：ε以下の場合、収束解が求まったとされる（ステップＳ８０９）。したがって、実質的には位置偏差：ΔＰwが比較値：εp以下であれば、収束解が求まったとされる。
本実施形態によれば、位置偏差：ΔＰwと、常に０とされた姿勢偏差：ΔＱwとにより形成された偏差：ΔＲwが比較値：ε以下であるか否かを判断するため、複雑な演算を行うことなく、ツール１４の先端の姿勢偏差：ΔＱwに対する比較条件を緩和することができる。
【００７０】
＜実施形態４＞
図９に基づき、本発明の実施形態４による多関節ロボットについて説明する。以下、実施形態１と異なる構成についてのみ説明を行う。本実施形態のサブルーチンによる制御においては実施形態１と同様に、ツール１４の先端の偏差：ΔＲwが、ツール１４の先端の位置偏差：ΔＰwおよび姿勢偏差：ΔＱwとにより構成されている。ツール１４の先端の偏差：ΔＲwは、ツール１４の先端の現在位置および現在姿勢：Ｒwnowと、目標位置および目標姿勢：Ｒwrefとから、関係式：ΔＲw＝Ｒwref−Ｒwnowにより求められる（ステップＳ９１０）。
【００７１】
メモリカウンター中のＵを１としたサブルーチンの１巡目のルーチンにおいては、偏差：ΔＲwが所定の比較値：１．１×εと比較され、偏差：ΔＲwが比較値：１．１×ε以下の場合、収束解が求まったとされる（ステップＳ９１１）。具体的にいえば、偏差：ΔＲwを構成する位置偏差：ΔＰwと姿勢偏差：ΔＱwが、それぞれ所定の比較値：１．１×εp、１．１×εQと比較される。位置偏差：ΔＰw（＝Ｐwref−Ｐwnow）が比較値：１．１×εp以下で、かつ、姿勢偏差：ΔＱw（＝Ｑwref−Ｑwnow）が比較値：１．１×εQ以下の場合、収束解が求まったとされる。
【００７２】
また、サブルーチンの２巡目以降のＵ巡目のルーチンにおいては、比較値を１．１U×εとすることにより比較値をさらに増大させた状態で、収束演算を行う。尚、本実施形態のサブルーチンのＵ巡目のルーチンにおいて、位置偏差：ΔＰw（＝Ｐwref−Ｐwnow）が比較値：εp以下で、かつ、姿勢偏差：ΔＱw（＝Ｑwref−Ｑwnow）が比較値：１．１U×εQ以下の場合、収束解が求まったとしてもよい。
【００７３】
本実施形態によれば、偏差：ΔＲwが比較値ε以下でないと判断し、かつ、修正角度Δθの算出回数がＮ回を超えた場合には、比較値εを所定量だけ増大させ、次回、算出される偏差：ΔＲwが所定量だけ増大された比較値：１．１×ε以下であるか否かを判断するため、簡単な演算によりリンク１１ａ〜１１ｇの回転角度：θnowの収束解を求まりやすくすることができる。
【００７４】
＜他の実施形態＞
本発明は、上述した実施形態に限定されるものではなく、次のように変形または拡張することができる。
本発明による多関節ロボットは、関節アーム１の先端が３次元位置および２次元姿勢をとることの可能な５軸以上のロボットであれば、適用可能である。
上述したメインルーチンにおいて、修正角度：Δθの算出回数が所定回数を超えた場合には、サブルーチンにおいて、ツール１４の先端の姿勢偏差：ΔＱwに対する所定条件は緩和せず、ツール１４の先端の位置偏差：ΔＰwに対する所定条件のみを緩和してもよい。
【図面の簡単な説明】
【００７５】
【図１】本発明の多関節ロボットの関節アームの概略図
【図２】多関節ロボットの電気的構成を示したブロック図
【図３】移動軌跡上における補間点の形成方法を示す概略図
【図４】リンクの修正角度の収束解を求める方法を示すブロック図
【図５】実施形態１による多関節ロボットの制御方法のメインルーチンを表したフローチャート
【図６】制御方法のサブルーチンを表したフローチャート
【図７】実施形態２による制御方法のサブルーチンを表したフローチャート
【図８】実施形態３による制御方法のサブルーチンを表したフローチャート
【図９】実施形態４による制御方法のサブルーチンを表したフローチャート
【符号の説明】
【００７６】
図面中、１は関節アーム、３ａ〜３ｆはモーター駆動部（モーター駆動手段）、１１ａ〜１１ｇはリンク、１２ａ〜１２ｆは関節、１３ａ〜１３ｆはステッピングモーター（駆動モーター）、１４はツール（関節アームの先端部）、２２は順運動学演算部（先端部演算手段）、２３は偏差判定部（偏差判定手段）、２５は回転角更新部（回転角度更新手段）を示している。
【図１】
2010036293.tif 000003
【図２】
2010036293.tif 000004
【図３】
2010036293.tif 000005
【図４】
2010036293.tif 000006
【図５】
2010036293.tif 000007
【図６】
2010036293.tif 000008
【図７】
2010036293.tif 000009
【図８】
2010036293.tif 000010
【図９】
2010036293.tif 000011