JP2013215839A - 冗長自由度を持つロボットの制御方法及びロボット制御装置、並びにロボット制御システム - Google Patents

Abstract

【課題】
ハード構成が複雑にならず、エンドエフェクタの軌跡精度、及び速度変動の制御性能を向上することができる。
【解決手段】
手先位置姿勢を目標値とした拘束条件の下で、冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における、関節軸の回転方向を判定する（Ｓ５０）。回転方向を判定した関節軸のうち、反転する関節軸が最も少ないときのリンク位置姿勢を選択する（Ｓ７０）。そして、選択したリンク位置姿勢で、手先位置姿勢を目標値として、関節軸毎に設けられた回転系アクチュエータを制御する。
【選択図】図３

Description

本発明は、冗長自由度を持つロボットの制御方法及びロボット制御装置、並びにロボット制御システムに関する。

従来から、多関節ロボットでは、各関節軸に設けられた回転系アクチュエータが減速機を介して各リンクを駆動することが行われている。前記減速機は歯車機構により構成されており、該歯車機構を構成する歯車間にはバックラッシが設けられている（特許文献１、特許文献２）。ところで、多関節ロボットを動作させる場合、関節軸が正転・逆転を行うと、前記バックラッシによる無制限領域が発生し、エンドエフェクタの軌跡精度、及び速度変動等の制御性能が著しく劣化する。例えば、ある軸の方向が反転するときに前記バックラッシのためマニピュレータが即座に反転することができずに、その個所において指令軌跡からの誤差が生じる。

特許文献１では、冗長自由度を持たない多関節ロボットにおいて、ツール先端の現在位置と、目標位置における各関節軸（すなわち、全関節軸）の重力トルクを演算し、前記現在位置と、目標位置のそれぞれの重力トルクの向きにより反転状態か非反転状態かを判定すると共に、重力トルクの絶対値の大きさに応じて、バックラッシを補正するようにしている。

特許文献２では、バネを設けて歯車間に生じるバックラッシを除去する技術が開示されている。

特開平７-２００１８号公報 特開２００８-１９９０号公報

ところが、特許文献１の技術を、冗長自由度を持つ多関節ロボットにそのまま応用できない問題がある。すなわち、冗長自由度を持つ多関節ロボットでは、ツールの先端の現在位置から目標位置まで、各関節軸を駆動する場合、前記冗長自由度によって、関節軸によっては、正転または逆転のいずれでも良い場合があるからであり、一義的に正転逆転を決められない場合がある。

また、特許文献２で開示されたバックラッシを除去する機構を、冗長自由度を持つ多関節ロボットに設けるとハード構成において部品点数が増えて、冗長自由度を持たないロボットに比してコストが高くなる問題がある。

本発明の目的は、ハード構成が複雑にならず、エンドエフェクタの軌跡精度、及び速度変動の制御性能を向上することができる冗長自由度を持つロボットの制御方法及びロボット制御装置並びにロボット制御システムを提供することにある。

上記問題点を解決するために、請求項１の発明は、作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法において、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定し、回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢を選択し、前記選択したリンク位置姿勢で、前記手先位置姿勢を目標値として、前記関節軸毎に設けられた回転系アクチュエータを制御することを特徴とする冗長自由度を持つロボットの制御方法を要旨としている。

請求項２の発明は、請求項１において、前記各リンク位置姿勢における複数の所定の関節軸の回転方向の判定には、重力加速度の影響を受ける方向に回転する関節軸の回転方向の判定を含み、前記回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢の選択では、前記重力加速度の影響を受ける方向に回転する関節軸の合計反転軸数が最も少ないときのリンク位置姿勢を選択することを特徴とする。

請求項３の発明は、請求項１において、前記各リンク位置姿勢における複数の所定の関節軸の回転方向の判定は、全関節軸の回転方向の判定であることを特徴とする。
請求項４の発明は、作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御装置において、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定する判定部と、回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢を選択する選択部と、前記選択したリンク位置姿勢で、前記手先位置姿勢を目標値として、前記関節軸毎に設けられた回転系アクチュエータを制御する制御部を備えることを特徴とするロボット制御装置を要旨としている。

請求項５の発明は、請求項４において、前記判定部の判定には、重力加速度の影響を受ける方向に回転する関節軸の回転方向の判定を含み、前記選択部が行う選択では、前記重力加速度の影響を受ける方向に回転する関節軸の合計反転軸数が最も少ないときのリンク位置姿勢を選択すること含むことを特徴とする。

請求項６の発明は、請求項４において、前記判定部は、全関節軸の回転方向の判定を行うことを特徴とする。
請求項７の発明は、作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御システムにおいて、シミュレーションにより、作業プログラムに記述された手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定する判定部と、回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢を選択する選択部と、前記選択部で選択したリンク位置姿勢を、前記作業プログラムに記述された手先位置姿勢と関連付けて記憶する記憶部と、前記作業プログラムの実行処理時に、前記手先位置姿勢に関連付けされた前記リンク位置姿勢で、前記手先位置姿勢を目標値として、前記関節軸毎に設けられた回転系アクチュエータを制御する制御部を備えることを特徴とするロボット制御システムを要旨としている。

請求項１乃至請求項３の発明によれば、ハード構成が複雑にならず、エンドエフェクタの軌跡精度、及び速度変動の制御性能を向上することができる冗長自由度を持つロボットの制御方法を提供できる。

請求項４乃至請求項６の発明によれば、ハード構成が複雑にならず、エンドエフェクタの軌跡精度、及び速度変動の制御性能を向上することができるロボット制御装置を提供できる。

請求項６の発明によれば、ハード構成が複雑にならず、エンドエフェクタの軌跡精度、及び速度変動の制御性能を向上することができるロボット制御システムを提供できる。
請求項７の発明によれば、ハード構成が複雑にならず、エンドエフェクタの軌跡精度、及び速度変動の制御性能を向上することができる冗長自由度を持つロボット制御システムを提供できる。

一実施形態の冗長自由度を有するマニピュレータのスケルトン図。 一実施形態のロボット制御装置の概略構成図。 一実施形態のロボット制御装置が実行するプログラムのフローチャート。 一実施形態の姿勢パラメータの説明図。 回転方向の判定条件の説明図。 他の実施形態のロボット制御装置が実行するプログラムのフローチャート。 他の実施形態のロボット制御システム。

以下、本発明を具体化した一実施形態の作業自由度に対して冗長自由度を有するロボット制御装置及びロボット制御方法を図１〜図４を参照して説明する。
まず、本実施形態の作業自由度に対して冗長自由度を有するマニピュレータについて説明する。

図１に示すように、マニピュレータ１０は、８個のリンク１１〜１８が７個の関節２１〜２７により直列に連結されて形成されている。多関節ロボットであるマニピュレータ１０は、７個の関節２１〜２７においてリンク１２〜１８が旋回することのできる７自由度（自由度ｎ＝７）を有するロボットであり、その作業空間の次元数（次元数ｍ）は６であって、１（＝ｎ−ｍ）の冗長性を有する。すなわち、本実施形態のマニピュレータは、作業自由度６に対して、１つの冗長自由度を有する。

第１リンク１１は一端が床面ＦＬに固定され、他端が第１関節２１の一側に接続されている。第１関節２１の他側には、第２リンク１２の一端が接続され、第２リンク１２の他端には第２関節２２の一側が接続されている。以下同様に、第３リンク１３、第４リンク１４、第５リンク１５、第６リンク１６、第７リンク１７及び第８リンク１８が、それぞれ第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７を介して順に連結されている。

第１関節２１の他側は一側に対して、矢印３１に示すように、図１において上下方向に延びる軸を中心に回転可能とされており、これにより、第２リンク１２は隣接する第１リンク１１に対して、第１関節２１の回転軸（Ｊ１軸）を中心に矢印３１方向に旋回可能である。

また、第２関節２２の他側は一側に対して、矢印３２に示すように、図１において紙面に垂直な方向に延びる軸（Ｊ２軸）を中心に回転可能とされている。これにより、第３リンク１３は隣接する第２リンク１２に対して、第２関節２２の回転軸を中心に矢印３２方向に回転可能である。

以下、第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７についてもそれぞれ、回転可能とされており、第４リンク１４、第５リンク１５、第６リンク１６、第７リンク１７及び第８リンク１８も、それぞれ関節２３〜２７の回転軸（Ｊ３軸〜Ｊ７軸）を中心に、矢印３３〜３７方向に旋回可能である。尚、本願の全体にわたって、第１関節２１〜２７を介して連結されているリンク１１〜１８同士を、互いに隣接するリンク１１〜１８という。また、Ｊ１軸〜Ｊ７軸は、関節軸に相当する。

なお、Ｊ２軸、Ｊ４軸及びＪ６軸の回転方向は、図１に示すように、重力加速度が働く方向と一致している。
図１に示すように、第１関節２１には第１サーボモータ４１が取り付けられており、電力が供給されることにより、第２リンク１２を図示しない減速機を介して第１リンク１１に対して旋回させる。

また、第２関節２２には第２サーボモータ４２が取り付けられており、電力が供給されることにより、第３リンク１３を図示しない減速機を介して第２リンク１２に対して旋回させる。以下、同様に、第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７にはそれぞれサーボモータ４３〜４７が取り付けられており、電力が供給されることにより、各々リンク１４〜１８を図示しない減速機を介して旋回させる。

なお、各モータは、各関節内に設けられるが、図１では、説明の便宜上、関節とは分離して図示している。また、本実施形態では回転系アクチュエータとしてサーボモータであるＡＣモータが使用されているが、限定されるものではない。

第８リンク１８の先端には、エンドエフェクタとしてのツール４９が取り付けられている。ツール４９は第８リンク１８とともに、第７関節２７の回転軸（Ｊ７軸）を中心に図１に示すように矢印３７方向に旋回可能とされている。ツール４９は、例えば、ワーク等を把持可能なハンドである。なお、ツール４９の種類は、本発明とは関係しないため、限定されるものではない。

上述したようにマニピュレータ１０は、第１サーボモータ４１〜第７サーボモータ４７を駆動して第２リンク１２〜第８リンク１８を回転させることにより、第２リンク１２〜第８リンク１８の回転角度が累積して先端部にあるツール４９に働くため、ツール４９の先端の位置および姿勢を、その作業内容に応じた目標位置および目標姿勢に一致させることが可能である。

次に、図２を参照して、前記マニピュレータ１０を制御するロボット制御装置としてのコントローラＲＣを中心とした多関節ロボットの電気的な構成を説明する。
コントローラＲＣは、コンピュータ９０と、コンピュータ９０に電気的に接続されたＰＷＭジェネレータ５１〜５７と、ＰＷＭジェネレータ５１〜５７に電気的に接続されたサーボアンプ６１〜６７を有する。各サーボアンプ６１〜６７はそれぞれ第１サーボモータ４１〜第７サーボモータ４７に電気的に接続されている。

コンピュータ９０は、制御指令をＰＷＭジェネレータ５１〜５７に出力し、ＰＷＭジェネレータ５１〜５７は、当該制御指令に基づいてＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じてサーボモータ４１〜４７を作動させることにより、各リンク１２〜１８を回転させる。

前記サーボモータ４１〜４７にはロータリエンコーダ７１〜７７が内蔵されており、インターフェイス８０を介してコンピュータ９０と接続されている。ロータリエンコーダ７１〜７７は、各々のサーボモータ４１〜４７の回動角度を検出することにより、すなわち、リンク１２〜１８のそれぞれが隣接するリンク１１〜１７に対する回転角度（関節角度）を検出して、その検出信号をコントローラＲＣに送信する。ロータリエンコーダ７１〜７７は、回転角度検出器に相当する。なお、回転角度検出器としては、ロータリエンコーダに限定するものではなく、レゾルバ、或いは、ポテンショメータであってもよい。

なお、第１サーボモータ４１〜第７サーボモータ４７に対してロータリエンコーダ７１〜７７を設ける代わりに、リンク１１〜１８または第１関節２１〜第７関節２７に、リンク１１〜１８の回転角度（関節角度）を直接に検出可能なセンサを取り付けてもよい。

前記コンピュータ９０は、ＣＰＵ９１、ＲＯＭ９２、ＲＡＭ９３、及びハードディスク等の不揮発性の記憶部９４、及びインターフェイス９５等を備え、バス９６を介して電気的に接続されている。

記憶部９４には、各種データ、ロボットに各種作業を行わせるための作業プログラム、各種パラメータ等が記憶されている。すなわち、本実施形態のロボットは、ティーチングプレイバック方式で作動するロボットであり、前記作業プログラムが実行されることにより、前記マニピュレータ１０が動作する。ＲＯＭ９２は、システム全体のシステムプログラムが記憶されている。ＲＡＭ９３は、ＣＰＵ９１の作業用のメモリであって、各種演算等が実行されるときに一時的にデータが格納される。ＣＰＵ９１は、判定部、選択部、及び制御部に相当する。

コントローラＲＣには入力装置８２が前記インターフェイス９５を介して接続されている。入力装置８２は、図示しないモニター画面及び各種入力キー等を有する操作盤であり、ユーザーが各種のデータを入力操作可能とされている。入力装置８２は、多関節ロボットの電源スイッチが設けられるとともに、コンピュータ９０に対して、マニピュレータ１０の先端部にあるツール４９の先端（以下、手先という）の最終目標位置および最終目標姿勢、ツール４９の先端の補間点における位置および姿勢の入力、並びに、冗長性を利用したマニピュレータ１０の姿勢変更のためのジョグ操作等による入力が可能となっている。

（実施形態の作用）
次に、本実施形態による多関節ロボットのコントローラＲＣの作用を図３及び図４を参照して説明する。

図３は、コントローラＲＣのＣＰＵ９１が実行する作業プログラムに書き込まれている教示点（最終目標位置、並びにツール４９の先端の補間点における位置）に手先を位置させるときの制御フローチャートである。前記ロータリエンコーダ７１〜７７は下記各ステップが実行される制御周期よりも、十分に短い検出周期で回転角度（すなわち、関節角度）を検出している。

（Ｓ１０）
まず、Ｓ１０では、ＣＰＵ９１は、記憶部９４の作業プログラムに書き込まれている教示点（手先位置）、手先姿勢及び速度データをＲＡＭ９３の所定領域にセットする。なお、手先位置及び手先姿勢を合わせて手先位置姿勢という。

（Ｓ２０）
Ｓ２０では、ＣＰＵ９１は、姿勢パラメータΦを初期化する、本実施形態ではΦ＝０にして初期化するが、初期値は０に限定するものではない。

ここで、姿勢パラメータΦについて説明する。
図４に示すように姿勢パラメータΦは、冗長自由度を有する前記マニピュレータ１０が、手先位置を固定した場合、すなわち、手先位置姿勢を拘束した場合において、その冗長自由度により許容されるリンク位置姿勢を示すものである。具体的には、図４に示すようにマニピュレータ１０の第４関節２４は、第２関節２２（以下、第１基準点Ｗという）を中心とし、第３リンク１３〜第４リンク１４のリンク長の合計を半径とした球Ａ１と、第６関節２６（以下、第２基準点Ｋという）を中心とし、第５リンク１５〜第６リンク１６のリンク長の合計を半径とする球Ａ２とが形成する交差円Ｅ上の移動が可能である。従って、本実施形態では、この交差円Ｅ上に第４関節２４が位置するようにして、リンク位置姿勢が変わる。

図４示すように、前記交差円Ｅの中心を通る中心軸Ｏは、第１基準点Ｗ（第２関節２２中心）と第２基準点Ｋ（第６関節２６中心）を通過する軸である。第４関節２４はこの交差円Ｅ上に位置するため、姿勢パラメータΦは、交差円Ｅ上のリンク位置姿勢を示すことができる。そこで、交差円Ｅ上の適宜の位置Ｒから、変更された位置までの角度を、ここでは、姿勢パラメータΦとして定義されている。本実施形態では位置Ｒは、第４関節２４の現在位置とする。

（Ｓ３０）
Ｓ３０では、ＣＰＵ９１は姿勢パラメータΦとして所定値を加算して更新する。すなわち、仮想的に姿勢パラメータΦを増加させてリンクの位置姿勢を変えたものとするのである。増加させる姿勢パラメータΦは、１度等の数値であるが、数値は限定するものではなく、複数のリンク位置姿勢を取らせるための大きさの角度が好ましい。

（Ｓ４０）
Ｓ４０では、手先位置姿勢から関節角度を求めるために逆変換演算を行う。
ここで、第１関節２１〜第７関節２７の関節角度θ１，θ２，θ３，…，θ７とし、手先座標（ｘ，ｙ，ｚ）及び手先姿勢（ａ，ｂ，ｃ）とすると、ベクトルｑ及び手先位置姿勢Ｘは下記のように表される。

各関節角度θ１，θ２，θ３，…，θ７は、式（２）に示すように表すことができ、これらの式は、逆変換式である。本ステップでは、式（２）により、手先位置姿勢Ｘと姿勢パラメータΦにより、各関節角度θ１，θ２，θ３，…，θ７を算出する。

（Ｓ５０）
Ｓ５０では、ＣＰＵ９１は、前記各関節角度θ１，θ２，θ３，…，θ７に到達するための各関節の現在位置からの各軸（各関節軸）の回転方向を判定する。

回転方向の方法について説明する。
ＣＰＵ９１は、関節軸毎に現在位置からＳ４０で算出した各関節角度までの角速度ωを演算し、角速度ω＞０の時は正方向、角速度ω＜０の時は負方向の回転方向と判定する。すなわち、正回転及び負回転の判定をゼロクロスで判定する。

なお、回転方向の判定は、上記の角速度ω＞０の時は正方向、角速度ω＜０の時は負（逆）方向の回転方向と判定する以外に下記のようにしてもよい。
図５に示すように、角速度ω（＞０）が正の関節軸においては、角速度ω＞ω０かつ角加速度ｄω/ｄｔ＞α（＞０）のときのみに、正方向と判定し、角速度ω（＜０）が負の関節軸においては、角速度ω＜-ω０かつ、角加速度ｄω/ｄｔ＜β（＜０）のときに負方向に回転していると判定してもよい。

ここで、ω０＝０であれば、既に説明したゼロクロスで回転方向を判別することになるが、ω０≠０であって、０に近い数値の場合、ゼロ付近の不安定部分であるところでは、正回転または負（逆）回転の判定を禁止し、停止と判定することになる。そして、角加速度ｄω/ｄｔを設定することで、角速度を０に近づきにくくできる。

従って、ω０＝０とした場合、各関節軸の回転方向の判定は、正回転（ω＞０）、負回転、（ω＜０）或いは停止（ω＝０）のいずれかとなる。
前述のω０≠０とした場合、各関節軸の回転方向の判定は、正回転（角速度ω＞ω０かつ角加速度ｄω/ｄｔ＞α（＞０））、負回転（角速度ω＜-ω０かつ、角加速度ｄω/ｄｔ＜β（＜０））、停止（角速度-ω０≦ω≦ω０）のいずれかとなる。

ＣＰＵ９１は、上記のように判定して得られた当該姿勢パラメータΦにおける各関節軸の回転方向を記憶部９４に記憶する。
（Ｓ６０）
Ｓ６０では、ＣＰＵ９１は、姿勢パラメータΦが、予め設定された上限値Φｍａｘ以下であれば、Ｓ３０に戻り、姿勢パラメータΦが予め設定された上限値Φｍを超えた場合には、Ｓ７０に移行する。この上限値Φｍは、予め試験等により設定されたものである。

（Ｓ７０）
Ｓ７０では、ＣＰＵ９１は、合計反転軸数の最少である姿勢パラメータ判定処理を行う。

具体的には、まず、ＣＰＵ９１は、記憶部９４に記憶した姿勢パラメータ毎の各関節軸のうち、特に、回転時に重力加速度の影響を受ける軸、すなわち、第２関節２２のＪ２軸、第４関節２４のＪ４軸、及び第６関節２６のＪ６軸についての回転方向が、前回更新した姿勢パラメータで判定したときの回転方向と同じか否かで、反転しているか否かを判定する。そして、ＣＰＵ９１は、反転している関節軸数を合計する。すなわち、合計反転軸数を算出する。

ここでの反転とは、前回更新した姿勢パラメータのときは正回転で、今回の場合は負（逆）回転である場合と、前回更新した姿勢パラメータのときは負（逆）回転で、今回の場合は正回転である場合を含む。なお、姿勢パラメータの更新が初回の場合における反転の判定は、サーボモータの正逆いずれか一方の回転方向に設定されており、その回転方向と同じか否かで判定する。

そして、ＣＰＵ９１は、第２関節２２のＪ２軸、第４関節２４のＪ４軸、及び第６関節２６のＪ６軸の合計した反転軸数（すなわち、合計反転軸数）が最も少ない姿勢パラメータΦを選択する。

なお、各サーボモータに作動連結されている減速機（歯車機構）は、サーボモータが同じ方向に回転を継続する場合は、当該歯車機構の歯車同士は相互に当接して回転しているため、バックラッシによる無制限領域はなく、作動遅れが生じることはない。しかし、サーボモータが逆転すると、前記歯車機構では歯車のバックラッシによる無制限領域があるため、その分の作動遅れがあるとともに歯当たり音が生ずることになる。また、前記Ｊ２軸、Ｊ４軸及びＪ６軸は、その回転方向が、重力加速度が働く方向と一致するため重力加速度の影響による外乱トルクの変動を受けやすいとともに、反転した際にその外乱トルクの変動量が大きくなる場合が発生し、さらに前記反転したときのバックラッシによる作動遅れ等の影響も受けることになる。

そこで、本実施形態では、Ｊ２軸、Ｊ４軸及びＪ６軸の合計した反転軸数が最も少ない姿勢パラメータΦを選択するのである。
なお、合計反転軸数が最も少ない姿勢パラメータΦが複数ある場合には、ＣＰＵ９１は、姿勢パラメータΦの値が、現在位置Ｒからの変位が最も小さいものを選択する。現在位置Ｒからの変位が最も小さいものを選択すれば、その姿勢パラメータΦで示すリンク位置姿勢を早期に取ることができるからである。

（Ｓ８０）
Ｓ８０では、ＣＰＵ９１は、Ｓ７０で選択した姿勢パラメータΦの各関節軸の関節角度となるように今回の制御周期分の位置指令（制御指令）としてＰＷＭジェネレータ５１〜５７にそれぞれ出力する。ＰＷＭジェネレータ５１〜５７は、その位置指令に基づくＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じて各関節軸の第１サーボモータ４１〜第７サーボモータ４７を作動させることにより、各リンク１２〜１８を回転させる。なお、速度データの使用は、ＣＰＵ９１は、速度フィードバック制御を行う際に使用しているが、本発明とは関係しないため、説明を省略する。

（Ｓ９０）
ＣＰＵ９１は、Ｓ９０において、各ロータリエンコーダ７１〜７７からの検出信号に基づいて、最終目標位置、及び最終目標姿勢（すなわち、最終の目標位置姿勢）に手先が到達したか否かを判定し、達していないと判定した場合はＳ１０に戻り、反対に作業プログラムの最終の目標位置姿勢に達していると判定した場合は、このフローチャートを終了する。なお、Ｓ１０に戻った場合、作業プログラムに記述されている次の手先位置姿勢、速度データをセットする。

本実施形態では、下記の特徴を有する。
（１） 本実施形態の冗長自由度を持つロボットの制御方法は、手先位置姿勢Ｘを目標値とした拘束条件の下で、冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における、Ｊ２軸、Ｊ４軸及びＪ６軸（関節軸）の回転方向を判定する（Ｓ５０）。そして、回転方向を判定したＪ２軸、Ｊ４軸及びＪ６軸（関節軸）のうち、反転する関節軸が最も少ないときのリンク位置姿勢を選択する（Ｓ７０）。そして、選択したリンク位置姿勢で、手先位置姿勢を目標値として、関節軸毎に設けられたサーボモータ４１〜４７（回転系アクチュエータ）を制御する。

このため、冗長自由度を利用して、バックラッシの影響を受けにくい反転しない関節軸を優先的に作動させ、或いは反転する関節軸を少なくしたリンク位置姿勢で手先位置姿勢を目標値として作動することができる。この結果、ハード構成が複雑にならず、ツール（エンドエフェクタ）の軌跡精度、及び速度変動の制御性能を向上することができる冗長自由度を持つロボットの制御方法を提供できる。

（２） 本実施形態の冗長自由度を持つロボットの制御方法では、Ｓ５０における、姿勢パラメータΦで表される各リンク位置姿勢における複数の所定の関節軸の回転方向の判定には、重力加速度の影響を受ける方向に回転するＪ２軸、Ｊ４軸、及びＪ６軸（関節軸）の回転方向の判定を含む。また、Ｓ７０における、回転方向を判定した関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢の選択では、Ｊ２軸、Ｊ４軸、及びＪ６軸（関節軸）の合計反転軸数が最も少ないときのリンク位置姿勢を選択する。

ここで、Ｊ２軸、Ｊ４軸及びＪ６軸は、その回転方向が、重力加速度が働く方向と一致するため重力加速度の影響による外乱トルクの変動を受けやすいとともに、反転した際にその外乱トルクの変動量が大きくなる場合が発生し、さらに前記反転したときのバックラッシによる作動遅れ等の影響も受けることになる。本実施形態では、Ｊ２軸、Ｊ４軸、及びＪ６軸（関節軸）の合計反転軸数が最も少ないときのリンク位置姿勢を選択するため、上記した、重力加速度の影響による外乱トルクの変動や、反転した際にその外乱トルクの変動量が大きくなることが抑制でき、さらに前記反転したときのバックラッシによる作動遅れ等の影響も軽減できる冗長自由度を持つロボットの制御方法を提供できる。

（３） 本実施形態のロボット制御装置（コントローラーＲＣ）のＣＰＵ９１は、判定部として、手先位置姿勢を目標値とした拘束条件の下で、冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢におけるＪ２軸、Ｊ４軸及びＪ６軸（関節軸）の回転方向を判定する。また、ＣＰＵ９１は、選択部として、回転方向を判定したＪ２軸、Ｊ４軸及びＪ６軸（関節軸）のうち、反転する関節軸が最も少ないときのリンク位置姿勢を選択する。また、ＣＰＵ９１は、制御部として、選択したリンク位置姿勢で、手先位置姿勢を目標値として、関節軸毎に設けられたサーボモータ４１〜４７（回転系アクチュエータ）を制御する。

この結果、本実施形態によれば、冗長自由度を利用して、バックラッシの影響を受けにくい反転しない関節軸を優先的に作動させ、或いは反転する関節軸を少なくしたリンク位置姿勢で手先位置姿勢を目標値として作動することができる。この結果、ハード構成が複雑にならず、ツール（エンドエフェクタ）の軌跡精度、及び速度変動の制御性能を向上することができるロボット制御装置を提供できる。

なお、本実施形態では、重力加速度の影響がない関節軸、すなわち、Ｊ１軸、Ｊ３軸、Ｊ５軸、Ｊ７軸は、合計反転軸数には含まないようにしているが、この場合は、ツール（エンドエフェクタ）の軌跡精度、及び速度変動の影響が少ないからである。

（４） 本実施形態のロボット制御装置では、ＣＰＵ９１（判定部）の判定には、重力加速度の影響を受ける方向に回転する関節軸の回転方向の判定を含む。また、ＣＰＵ９１（選択部）が行う選択では、重力加速度の影響を受ける方向に回転するＪ２軸、Ｊ４軸及びＪ６軸（関節軸）の合計反転軸数が最も少ないときのリンク位置姿勢を選択する。

このため、Ｊ２軸、Ｊ４軸及びＪ６軸は、その回転方向が、重力加速度が働く方向と一致するため重力加速度の影響による外乱トルクの変動を受けやすいとともに、反転した際にその外乱トルクの変動量が大きくなる場合が発生し、さらに前記反転したときのバックラッシによる作動遅れ等の影響も受けることになる。本実施形態では、Ｊ２軸、Ｊ４軸、及びＪ６軸（関節軸）の合計反転軸数が最も少ないときのリンク位置姿勢を選択するため、上記した、重力加速度の影響による外乱トルクの変動や、反転した際にその外乱トルクの変動量が大きくなることが抑制でき、さらに前記反転したときのバックラッシによる作動遅れ等の影響も軽減できるロボット制御装置を提供できる。

なお、本発明の実施形態は前記実施形態に限定されるものではなく、下記のように変更しても良い。
・ 前記実施形態では、図３のＳ７０では、ＣＰＵ９１は、記憶部９４に記憶した姿勢パラメータ毎の各関節軸のうち、特に、回転時に重力加速度の影響を受ける軸、すなわち、第２関節２２のＪ２軸、第４関節２４のＪ４軸、及び第６関節２６のＪ６軸についての回転方向が、前回更新した姿勢パラメータで判定したときの回転方向と同じか否かで、反転しているか否かを判定するようにした。そして、ＣＰＵ９１は、反転している関節軸数を合計する。すなわち、合計反転軸数を算出するようにした。そして、ＣＰＵ９１は、第２関節２２のＪ２軸、第４関節２４のＪ４軸、及び第６関節２６のＪ６軸の合計反転軸数が最も少ない姿勢パラメータΦを選択するようにした。

この代わりに、図６に示すＳ７０Ａの全関節軸において、合計反転軸数を算出するようにしてもよい。そして、ＣＰＵ９１は、全関節軸の合計反転軸数が最も少ない姿勢パラメータΦを選択する。

このようにしても、前記実施形態の（１）及び（３）の効果を容易に実現できる。
・ 前記実施形態では、Ｓ２０〜Ｓ７０の処理は、作業プログラムの実行時にリアルタイムに行うようにしている。この制御方法及びロボット制御装置に代えて、Ｓ２０〜Ｓ７０の処理を、下記のようにロボット制御システム及びロボット制御方法で行っても良い。

図７に示すように、ティーチングプレイバック方式で作成された作業プログラムを、コントローラＲＣと通信が可能に接続された外部コンピュータ１００（例えばコントローラＲＣの上位のコンピュータ）がシミュレーションで作業プログラムを実行して、該外部コンピュータ１００が、判定部として、作業プログラムに記述されている手先位置姿勢を目標値とした拘束条件の下で、冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定する。

コンピュータ１００は、選択部として、回転方向を判定した前記関節軸のうち、反転する関節軸が最も少ないときのリンク位置姿勢を選択する。すなわち、図３のＳ２０〜Ｓ７０と同様の処理を行う。なお、この場合、シミュレーションでは、関節軸の各関節角度は、ロータリエンコーダ７１〜７７が検出した検出信号の代わりに、各関節軸の位置指令を使用するものとする。

そして、コンピュータ１００は、コントローラＲＣの記憶部９４に、作業プログラムに記述された各ステップ毎の手先位置姿勢、速度データと、その手先位置姿勢において、反転する関節軸が最も少ないときのリンク位置姿勢を示す姿勢パラメータΦとを関連付けて記憶させる。

コントローラＲＣは制御部として機能して、前記手先位置姿勢が記述された作業プログラムが実行された際、該作業プログラムに記述されている前記手先位置姿勢毎に記憶部９４に関連付けして記憶した姿勢パラメータΦで規定される関節角度に基づいて全関節軸のサーボモータ４１〜４７（回転系アクチュエータ）を制御する。

このようにしても、前記実施形態と同様の効果を実現することができる。
・ 前記実施形態では、作業プログラムの全教示点で規定される移動軌跡の全体において、図３のフローチャートが実行されるようにされている。しかし、移動軌跡の全体において、図３のフローチャートが実行されるのではなく、前記移動軌跡中において、軌跡精度が求められない部分、及び速度変動があっても支障がない部分では、図３のフローチャートにおいて、Ｓ１０の後、Ｓ８０にジャンプするようにしてもよい。この場合、Ｓ１０の後に、Ｓ１０でセットされた手先位置姿勢とともに、Ｓ８０へのジャンプを指示するジャンプ指令等がセットされるものとする。このような手先位置姿勢が示される教示点としては、例えば、マニピュレータ１０の手先を早送りするための教示点が教示されている場合等がある。

・ 前記実施形態では、アクチュエータをサーボモータとしてのＡＣモータを使用したが、ＤＣモータでもよく、ステッピングモータ等を使用してもよい。
・ 前記実施形態では、１つの冗長自由度をもつようにしたが、１つの冗長自由度に限定するものではなく、２つ以上の冗長自由度をもつロボット制御装置及び制御方法にも適用できる。

・ 前記実施形態では、Ｊ２軸、Ｊ４軸、Ｊ６軸の回転方向を、重力加速度が働く方向と一致させたが、他の軸の回転方向を重力加速度が働く方向と一致させてもよい。

ＲＣ…コントローラ（ロボット制御装置）、
１０…マニピュレータ、１１〜１８…リンク、
４１〜４７…サーボモータ（回転系アクチュエータ）、
４９…ツール（エンドエフェクタ）、
９１…ＣＰＵ（判定部、選択部、制御部）、
９４…記憶部、２００…上位コントローラ（判定部、選択部）。

Claims (7)

1. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法において、
   手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定し、
   回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢を選択し、
   前記選択したリンク位置姿勢で、前記手先位置姿勢を目標値として、前記関節軸毎に設けられた回転系アクチュエータを制御することを特徴とする冗長自由度を持つロボットの制御方法。
2. 前記各リンク位置姿勢における複数の所定の関節軸の回転方向の判定には、重力加速度の影響を受ける方向に回転する関節軸の回転方向の判定を含み、
   前記回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢の選択では、前記重力加速度の影響を受ける方向に回転する関節軸の合計反転軸数が最も少ないときのリンク位置姿勢を選択することを特徴とする請求項１に記載の冗長自由度を持つロボットの制御方法。
3. 前記各リンク位置姿勢における複数の所定の関節軸の回転方向の判定は、全関節軸の回転方向の判定であることを特徴とする請求項１に記載の冗長自由度を持つロボットの制御方法。
4. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御装置において、
   手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定する判定部と、
   回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢を選択する選択部と、
   前記選択したリンク位置姿勢で、前記手先位置姿勢を目標値として、前記関節軸毎に設けられた回転系アクチュエータを制御する制御部を備えることを特徴とするロボット制御装置。
5. 前記判定部の判定には、重力加速度の影響を受ける方向に回転する関節軸の回転方向の判定を含み、
   前記選択部が行う選択では、前記重力加速度の影響を受ける方向に回転する関節軸の合計反転軸数が最も少ないときのリンク位置姿勢を選択すること含むことを特徴とする請求項４に記載のロボット制御装置。
6. 前記判定部は、全関節軸の回転方向の判定を行うことを特徴とする請求項４に記載のロボット制御装置。
7. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御システムにおいて、
   シミュレーションにより、作業プログラムに記述された手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合の、各リンク位置姿勢における複数の所定の関節軸の回転方向を判定する判定部と、
   回転方向を判定した前記関節軸のうち、反転する前記関節軸が最も少ないときのリンク位置姿勢を選択する選択部と、
   前記選択部で選択したリンク位置姿勢を、前記作業プログラムに記述された手先位置姿勢と関連付けて記憶する記憶部と、
   前記作業プログラムの実行処理時に、前記手先位置姿勢に関連付けされた前記リンク位置姿勢で、前記手先位置姿勢を目標値として、前記関節軸毎に設けられた回転系アクチュエータを制御する制御部を備えることを特徴とするロボット制御システム。