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

Abstract

【課題】
各関節軸の速度開ループゲインを同一にしているため、各関節軸の動作速度、位相関係が近似値となり、軌跡精度が向上できるとともに、速度リップル（速度むら）が抑制されて制御性能を向上することができる冗長自由度を持つロボットの制御方法及びロボット制御装置並びにロボット制御システムを提供する。
【解決手段】
手先位置姿勢を目標値とした拘束条件として冗長自由度が許容するリンク位置姿勢を変化させて、各リンク位置姿勢毎の全関節軸の負荷イナーシャを算出し、算出した負荷イナーシャの最大値が最小となるイナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算し、対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算し、全関節軸全関節軸の速度系の開ループゲインを、演算した速度開ループゲインに統一した上で、関節軸毎に設けられたサーボモータを制御する。
【選択図】図４

Description

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

従来、多関節ロボットを動作させる場合、各関節軸を駆動するモータの指令値の偏差によって制御される位置ループ、速度ループ及び電流ループで行うのが一般的である。位置、速度、及び電流ループは関節軸毎に独立して構成されており、各ループのループゲイン等は、他の関節軸との関係はなく設定されている（以下、従来技術１という）。

また、特許文献１には、冗長自由度を持つロボットの制御方法が開示されている。この制御方法は、ロボットの各関節部回りの慣性能率（慣性イナーシャ）を冗長自由度の数だけ適宜選択して、用いたジャコビアン行列で表現した関係式により、駆動すべき各関節角度を演算するようにしたものである（以下、従来技術２という）。この方法によれば、全体として動作が円滑となり、極端にある特定の軸に過負荷がかかったり、不自然な姿勢を取ることがなくなるという利点がある。

特公平４−６００３号公報

しかしながら、従来技術１及び従来技術２では、多関節ロボットを動作させる場合、各関節軸の位置、速度、及び電流ループゲインが相互に無関係の大きさに設定されているため、各関節軸の動作速度、位相関係もばらばらになり、ロボットの軌跡精度、速度リップル（速度むら）の制御性能が劣化する問題がある。

本発明の目的は、各関節軸の速度開ループゲインを同一にしているため、各関節軸の動作速度、位相関係が近似値となり、軌跡精度が向上できるとともに、速度リップル（速度むら）が抑制されて制御性能を向上することができる冗長自由度を持つロボットの制御方法及びロボット制御装置並びにロボット制御システムを提供することにある。

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

請求項２の発明は、作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法において、手先位置姿勢毎に以下の第１〜第５ステップをシミュレーションで行って、得られた速度開ループゲインを記憶した後、当該速度開ループゲインで前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法を要旨としている。

（第１ステップ） 付与された手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを算出する。

（第２ステップ） 前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する。

（第３ステップ） 前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する第４ステップ、
（第４ステップ） 前記演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する。

（第５ステップ） 全関節軸の速度系の開ループゲインを、前記演算した速度開ループゲインに統一した上で、前記関節軸毎に設けられた回転系アクチュエータを制御する。
請求項３の発明は、作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御装置において、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを演算する第１演算部と、前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する選択部と、前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する第２演算部と、前記演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する第３演算部と、全関節軸の速度系の開ループゲインを、前記演算した速度開ループゲインに統一する統一部を備えることを特徴とするロボット制御装置を要旨としている。

請求項４の発明は、作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御システムにおいて、シミュレーションにより、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを演算する第１演算部と、前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する選択部と、前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する第２演算部と、前記対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する第３演算部と、前記速度開ループゲインを記憶する記憶部を備え、前記手先位置姿勢が記述された作業プログラムが実行された際、前記手先位置姿勢毎に前記記憶部に記憶した全関節軸の回転系アクチュエータを前記記憶部に記憶した速度開ループゲインで制御する制御部を備えることを特徴とするロボット制御システムを要旨としている。

請求項１及び請求項２の発明によれば、各関節軸の速度開ループゲインを同一にしているため、各関節軸の動作速度、位相関係が近似値となり、軌跡精度が向上できるとともに、速度リップル（速度むら）が抑制されて制御性能を向上することができる冗長自由度を持つロボットの制御方法を提供できる。

請求項３及び請求項４の発明によれば、各関節軸の速度開ループゲインを同一にしているため、各関節軸の動作速度、位相関係が近似値となり、軌跡精度が向上できるとともに、速度リップル（速度むら）が抑制されて制御性能を向上することができるロボット制御装置を提供できる。

冗長自由度を有するマニピュレータのスケルトン図。 一実施形態のロボット制御装置の概略構成図。 ロボット制御装置の制御系全体のブロック線図。 ロボット制御装置が実行するプログラムのフローチャート。 ロボット制御装置が実行するプログラムのフローチャート。 速度ループのブロック線図。 （ａ）、（ｂ）は、速度ループのブロック線図。 姿勢パラメータの説明図。 負荷イナーシャが最大値を示す関節軸 他の実施形態のロボット制御システム。

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

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

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

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

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

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

図１に示すように、第１関節２１には第１サーボモータ４１が取り付けられており、電力が供給されることにより、第２リンク１２を図示しない減速機を介して第１リンク１１に対して旋回させる。

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

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

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

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

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

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

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

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

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

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

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

（一般的に行われているロボット制御装置の制御系の動作）
ここで、説明の便宜上、一般的に行われているロボット制御装置の制御系の動作を図３示す制御系のブロック線図を用いて説明する。なお、説明の便宜上、各構成部材の符号を、既に説明した各構成部材の符号と同一にして説明する。なお、図３において、一点鎖線から左側は、コントローラＲＣ側の動作を表わし、右側は、サーボモータ側の動作を表わしている。

図３に示すように、コントローラＲＣにおいては、まずＣＰＵ９１が、ＲＡＭ９３に格納された作業プログラムに記述されているツール４９の目標位置及び目標姿勢に基づいて逆変換演算を行って求めた各サーボモータの回転位置を位置指令θｓ^＊とする。

この位置指令θｓ^＊と、ロータリエンコーダ７１〜７７にて求めた各サーボモータ４１〜４７の実位置θｋとの偏差を算出する。
そして、この算出した位置偏差に、位置制御部１１０では、Ｐ制御（比例制御）にて所定の位置ゲインＫ０を乗じて、サーボモータの目標速度（速度指令ωｓ^＊）を算出する。

さらにこの速度指令ωｓ^＊と、実位置θｋから求められた各サーボモータ４１〜４７の実速度ωとの速度偏差に、速度制御部１２０では、ＰＩＤ制御にて、所定の速度ゲインＫ１を乗じて、各サーボモータ４１〜４７に対する電流指令ｉｑ^＊を算出し、この電流指令ｉｑ^＊を電流制御部１３０に出力する。電流制御部１３０は、図示しない電流検出回路にて検出された各サーボモータ４１〜４７の実電流をＡ／Ｄ変換して取り込み、この実電流ｉｑが電流指令となるように、制御指令を出力して図２に示す前記ＰＷＭジェネレータ５１〜５７にてＰＷＭ信号を生成する。具体的には、電流指令ｉｑ^＊と実電流ｉｑとの電流偏差に対して、ＰＩ制御にて所定の電流ゲインＫ２を乗じることにより、ＰＷＭ信号を生成する。そして、この生成したＰＷＭ信号を各サーボアンプ６１〜６７に出力し、各サーボモータ４１〜４７の通電電流を制御する。なお、図３において、実電流をＡ／Ｄ変換にて取り込む手順を表わすブロックＰ６に記載のＫＡ/Ｄ は、実電流をデジタル値に変換する際の変換定数を表わす。

この結果、各サーボモータ４１〜４７のモータ巻線には、各サーボアンプ６１〜６７から、ＰＷＭ信号に応じて駆動電圧が印加され、モータ巻線の端子電圧は、この駆動電圧と、モータの回転角速度に逆起電力定数Ｋｅを乗じて得られる逆起電圧とを合成した電圧となる。そして、各モータ巻線には、この端子電圧に、モータインダクタンスＬ及びモータ抵抗Ｒをパラメータとする係数｛１／（Ｌｓ＋Ｒ）｝を乗じた電流（つまり実電流）が流れる。

また、モータ巻線に電流が流れると、各サーボモータ４１〜４７においては、回転子に、実電流とトルク定数Ｋｔとにより決定されるモータトルクＴＭが発生し、モータ軸のイナーシャＪによる遅れ（１／Ｊ）を伴って回転角加速度が発生し、その回転角加速度を積分（１／Ｓ）した回転角速度をさらに積分（１／Ｓ）した回転位置に制御される。そして、この回転位置が、各サーボモータ４１〜４７に設けられたロータリエンコーダ等のセンサにて検出され、その検出信号がコントローラＲＣ内にフィードバックされる。このようにして、コントローラＲＣは、各サーボモータ４１〜４７の回転位置，速度をフィードバック制御するサーボ系の制御装置として構成され、各サーボモータ４１〜４７の回転位置、及びツール４９の位置を制御する。

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

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

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

（Ｓ２０）
次に、Ｓ２０で、ＣＰＵ９１は、Ｓ１０でセットした教示点（手先位置姿勢）において、イナーシャ最小姿勢の導出処理を行う。

このＳ２０の処理の詳細を図５のフローチャートを参照して説明する。
（Ｓ２１）
Ｓ２１では、ＣＰＵ９１は、姿勢パラメータΦを初期化する、本実施形態ではΦ＝０にして初期化するが、初期値は０に限定するものではない。姿勢パラメータΦについて説明する。

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

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

（Ｓ２２）
Ｓ２２では、ＣＰＵ９１は姿勢パラメータΦとして所定値を加算して更新する。すなわち、仮想的に姿勢パラメータΦを増加させてリンクの位置姿勢を変えたものとするのである。

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

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

（Ｓ２４）
Ｓ２４では、ＣＰＵ９１は、各関節軸の負荷イナーシャＩ１，Ｉ２，Ｉ３，…，Ｉ７を算出し、記憶部９４に格納する。なお、負荷イナーシャは、式（３）のように、特許文献１で説明されているように関節角度の関数として表すことができ、公知であるため、算出方法の詳細な説明は省略する。

（Ｓ２５）
Ｓ２５では、ＣＰＵ９１は、姿勢パラメータΦが、予め設定された上限値Φｍａｘ以下であれば、Ｓ２２に戻り、姿勢パラメータΦが予め設定された上限値Φｍを超えた場合には、Ｓ２６に移行する。この上限値Φｍは、予め試験等により設定されたものである。

（Ｓ２６）
Ｓ２６では、ＣＰＵ９１は、Ｓ２４で、姿勢パラメータΦが更新される毎に算出した全関節軸の負荷イナーシャのうち、負荷イナーシャの最大値が最小となるリンク位置姿勢、すなわち、イナーシャ最小リンク位置姿勢を選択（導出）する。

図９に示すように、例えば、姿勢パラメータΦ１〜Φｎ毎にＪ１軸〜Ｊ７軸の関節軸について、算出した負荷イナーシャが算出されたとする。ここで、Φ１〜Φｎは、上限値Φｍａｘまで、リンク位置姿勢が変化したことを意味している。

ここで、姿勢パラメータΦ１〜Φｎ毎に負荷イナーシャの最大値を示す欄において、姿勢パラメータΦ１では、Ｊ２軸の負荷イナーシャが最大値となり、姿勢パラメータΦ２では、Ｊ２軸の負荷イナーシャが最大値となり、姿勢パラメータΦ３では、Ｊ２軸の負荷イナーシャが最大値となったとし、残りの姿勢パラメータでは、負荷イナーシャの最大値がこれらの値よりも例えば大きいものとする。

このような場合、最大値の欄Ｊｍａｘ示されている負荷イナーシャの中で、最小値の負荷イナーシャを有する姿勢パラメータは、Φ２となり、この姿勢パラメータΦ２がイナーシャ最小リンク位置姿勢として選択される。

次に、図４のフローチャートに戻って説明する。
（Ｓ３０）
Ｓ３０では、ＣＰＵ９１は、Ｓ３０で選択した、イナーシャ最小リンク位置姿勢の全関節軸（Ｊ１軸〜Ｊ７軸）の対ロータイナーシャ比を算出する。すなわち、Ｊ１軸〜Ｊ７軸の各サーボモータ４１〜４７のロータイナーシャは、既知、すなわち、記憶部９４に予め記憶されており、このため、ＣＰＵ９１は、このロータイナーシャと、Ｓ２０で算出して得た各Ｊ１軸〜Ｊ７軸の負荷イナーシャに基づいて対ロータイナーシャ比（＝負荷イナーシャ/ロータイナーシャ）を算出する。

（Ｓ４０）
Ｓ４０では、ＣＰＵ９１は、Ｓ３０で算出したＪ１軸〜Ｊ７軸の関節軸についての対ロータイナーシャ比が最大値の対ロータイナーシャ比の関節軸について、速度開ループゲインを演算する。

まず、速度開ループについて説明する。
図６は、図３で説明した制御系のうち、特に速度ループについて簡略化して図示している。ここで、電流制御部１３０は、ＰＩ制御が行われて、その制御周期が速度制御部１２０よりも短い制御周期であるため、その伝達関数は１と近似できる。このため、図６は、図７（ａ）に示すように書き換えることができる。

また、速度ループを開ループ（速度開ループ）とすると、図７（ｂ）に示すように表すことができる。そして、図７（ｂ）に示す、Ａ点から、Ｂ点のゲイン、すなわち、速度開ループゲインＧ０は、式（４）で表すことができる。

Ｇ０＝Ｇ１×Ｇ２×１／Ｊ ……（４）
Ｇ１は、速度制御部１２０のゲインであり、Ｇ２はトルク定数Ｋｔを伝達関数として表したものである。

ここで、式（４）を式（５）に置き換えるものとする。
Ｇ０＝Ｋｖ×Ｇ２×１／Ｊ ……（５）
式（５）において、Ｋｖは、速度制御部１２０の比例ゲインとする。すなわち、Ｇ１はＫｖと同じ速度比例ゲインである。

そして、本実施形態では、１/Ｊを１/Ｊｍａｘとして、Ｇ０を求める。
すなわち、式（６）にする。
Ｇ０＝Ｋｖ×Ｇ２×１/Ｊｍａｘ ……（６）
Ｊｍａｘは、前記最大値の対ロータイナーシャ比の関節軸の負荷イナーシャである。

ＣＰＵ９１は、速度制御部１２０の比例ゲインＫｖを、速度開ループゲインによる応答性が良好であって、安定性を損なわないまで、大きくしていく。この応答性が良好であって、安定性を損なわないとは、例えば、ボード線図でいう、ゲイン余裕及び位相余裕で判定する。また、比例ゲインＫｖを大きくすればするほど、応答性は良くなるが、安定性は悪くなるため、ＣＰＵ９１は、予め設定された閾値内で、比例ゲインＫｖを決定することにより、速度開ループゲインＧ０を求める。

なお、速度制御部１２０において、ＰＩＤ制御を行うが、積分制御における積分ゲイン及び微分制御の微分ゲインについては、変更しないものとする。
（Ｓ５０）
次に、ＣＰＵ９１は、Ｓ５０では、他の関節軸についての、速度制御部１２０に関して、速度開ループを行うことを前提として、速度開ループゲインを前記Ｇ０と同一とする。すなわち、速度開ループゲインを全関節軸において、統一する。

（Ｓ６０）
Ｓ６０では、ＣＰＵ９１は、Ｓ２６で導出した姿勢パラメータΦとなるように、Ｓ１０でセットした手先位置姿勢を目標値とした各関節軸の回転系アクチュエータの制御指令を、今回の制御周期分の指令としてＰＷＭジェネレータ５１〜５７にそれぞれ出力する。

すなわち、本実施形態では、図３の位置制御部１１０では、一般的に行われている、速度指令ωｓ^＊を出力し、速度制御部１２０では、速度開ループゲインＧ０を使用して、電流指令を出力し、電流制御部１３０（ＰＷＭジェネレータ５１〜５７を含む）では、その電流指令と実電流ｉｑの偏差を入力してＰＷＭ信号を生成し、ＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じて各関節軸の第１サーボモータ４１〜第７サーボモータ４７を作動させることにより、各リンク１２〜１８を回転させる。

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

本実施形態では、下記の特徴を有する。
（１） 本実施形態のロボットの制御方法は、手先位置姿勢を目標値とした拘束条件の下で、冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを算出する。そして、複数変化させた場合のリンク位置姿勢のうち、算出した全関節軸（Ｊ１軸〜Ｊ７軸）の負荷イナーシャの最大値が、最小となるイナーシャ最小リンク位置姿勢を選択する。また、イナーシャ最小リンク位置姿勢のときの全関節軸（Ｊ１軸〜Ｊ７軸）の対ロータイナーシャ比を演算する。さらに、演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算し、全関節軸全関節軸（Ｊ１軸〜Ｊ７軸）の速度系の開ループゲインを、演算した速度開ループゲインに統一した上で、関節軸毎に設けられたサーボモータ４１〜４７（回転系アクチュエータ）を制御する。

この結果、本実施形態によれば、各関節軸の速度開ループゲインを同一にしているため、各関節軸の動作速度、位相関係が近似値となり、軌跡精度が向上できるとともに、速度リップル（速度むら）が抑制されて制御性能を向上することができる冗長自由度を持つロボットの制御方法を提供できる。

（２） 本実施形態のロボット制御装置は、ＣＰＵ９１が、第１演算部として、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを演算する。

また、ＣＰＵ９１は、選択部として、複数変化させた場合のリンク位置姿勢のうち、算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する。また、ＣＰＵ９１は、第２演算部として、イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する。さらに、ＣＰＵ９１は、第３演算部として、演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する。そして、ＣＰＵ９１は、統一部として、全関節軸の速度系の開ループゲインを、前記演算した速度開ループゲインに統一する。この結果、本実施形態によれば、各関節軸の速度開ループゲインを同一にしているため、各関節軸の動作速度、位相関係が近似値となり、軌跡精度が向上できるとともに、速度リップル（速度むら）が抑制されて制御性能を向上することができる冗長自由度を持つロボット制御装置を提供できる。

なお、本発明の実施形態は前記実施形態に限定されるものではなく、下記のように変更しても良い。
・ 前記実施形態では、Ｓ２０〜Ｓ５０の処理及びＳ２１〜Ｓ２６の処理は、作業プログラムの実行時にリアルタイムに行うようにしている。この制御方法に代えて、Ｓ２０〜Ｓ５０の処理を、下記のように具体化してもよい。

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

そして、コンピュータ１００は、選択部として、複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する。すなわち、図４のＳ２０、図５のＳ２１〜Ｓ２６と同様の処理を行う。

そして、コンピュータ１００は、第２演算部として、イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する。さらに、コンピュータ１００は、第３演算部として、前記対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する。

そして、コンピュータ１００は、コントローラＲＣの記憶部９４に、作業プログラムに記述された各ステップ毎の手先位置姿勢、速度データと、その手先位置姿勢における姿勢パラメータΦ（イナーシャ最小姿勢の姿勢パラメータΦ）とを関連付けるとともに、そのときに使用する前記速度開ループゲインを関連付けて記憶させる。

コントローラＲＣは制御部として機能して、前記手先位置姿勢が記述された作業プログラムが実行された際、前記手先位置姿勢毎に記憶部９４に記憶した全関節軸の回転系アクチュエータを前記記憶部に記憶した速度開ループゲインで制御する。

このようにしても、前記実施形態と同様の効果を実現することができる。
・ 前記実施形態では、アクチュエータをサーボモータとしてのＡＣモータを使用したが、ＤＣモータでもよく、ステッピングモータ等を使用してもよい。

・ 前記実施形態では、１つの冗長自由度をもつようにしたが、１つの冗長自由度に限定するものではなく、２つ以上の冗長自由度をもつロボット制御装置及び制御方法にも適用できる。

ＲＣ…コントローラ（ロボット制御装置）、
１０…マニピュレータ、１１〜１８…リンク、
４１〜４７…サーボモータ（回転系アクチュエータ）、
９１…ＣＰＵ（第１演算部、第２演算部、第３演算部、選択部、及び統一部）、
９４…記憶部。

Claims (4)

1. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法において、
   手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを算出し、
   前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択し、
   前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算し、
   前記演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算し、
   全関節軸の速度系の開ループゲインを、前記演算した速度開ループゲインに統一した上で、前記関節軸毎に設けられた回転系アクチュエータを制御することを特徴とするロボットの制御方法。
2. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法において、
   手先位置姿勢毎に以下の第１〜第５ステップをシミュレーションで行って、得られた速度開ループゲインを記憶した後、当該速度開ループゲインで前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボットの制御方法。
   （第１ステップ） 付与された手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを算出する。
   （第２ステップ） 前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する。
   （第３ステップ）前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する第４ステップ、
   （第４ステップ）前記演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する。
   （第５ステップ） 全関節軸の速度系の開ループゲインを、前記演算した速度開ループゲインに統一した上で、前記関節軸毎に設けられた回転系アクチュエータを制御する。
3. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御装置において、
   手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを演算する第１演算部と、
   前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する選択部と、
   前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する第２演算部と、
   前記演算された対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する第３演算部と、
   全関節軸の速度系の開ループゲインを、前記演算した速度開ループゲインに統一する統一部を備えることを特徴とするロボット制御装置。
4. 作業自由度に対して少なくとも１つの冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを備え、前記関節軸毎に設けられた回転系アクチュエータにより駆動するロボット制御システムにおいて、
   シミュレーションにより、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を複数変化させた場合、各リンク位置姿勢毎の全関節軸の負荷イナーシャを演算する第１演算部と、
   前記複数変化させた場合の前記リンク位置姿勢のうち、前記算出した全関節軸の負荷イナーシャの最大値が、最小となるリンク位置姿勢（以下、イナーシャ最小リンク位置姿勢という）を選択する選択部と、
   前記イナーシャ最小リンク位置姿勢のときの全関節軸の対ロータイナーシャ比を演算する第２演算部と、
   前記対ロータイナーシャ比が最大となった関節軸の速度開ループゲインを演算する第３演算部と、
   前記速度開ループゲインを記憶する記憶部を備え、
   前記手先位置姿勢が記述された作業プログラムが実行された際、前記手先位置姿勢毎に前記記憶部に記憶した全関節軸の回転系アクチュエータを前記記憶部に記憶した速度開ループゲインで制御する制御部を備えることを特徴とするロボット制御システム。