JP2013208671A - ロボット制御方法、ロボット制御装置、及びロボット制御システム - Google Patents

Abstract

【課題】
冗長自由度を利用して、エンドエフェクタにおける目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度及び軌跡精度を向上できる。
【解決手段】
マニピュレータの手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を算出する段階（Ｓ２０４）と、リンク位置姿勢の手先の誤差拡大率を演算する段階（Ｓ２０６）とを組み合わせて、複数のリンク位置姿勢を取得するとともに、リンク位置姿勢毎の誤差拡大率を複数取得し、該複数の誤差拡大率の中から最小の誤差拡大率を選択する。最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように各関節軸のサーボモータに位置指令として付与する。
【選択図】図４

Description

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

従来、マニピュレータを制御するロボット制御装置では、各関節軸での回転角の制御誤差がマニピュレータの先端のエンドエフェクタでは拡大されて、位置決め精度、及び軌跡精度が劣化する。例えば、ある点で位置決めした状態で、関節軸に±１パルスのサーボ変動があると、マニピュレータの作動によりマニピュレータの先端に設けられたエンドエフェクタで拡大される場合がある。

このような位置決め精度を高精度に行うための技術として特許文献１が公知である。特許文献１では、マニピュレータ先端の位置をカメラで撮像して、フィードバックすることにより、目標位置との誤差が軽減されて、位置決め精度が向上する。

特開平５−３０１１８５号公報

ところが、特許文献１では、カメラ等の撮像装置が必要となるとともに、リアルタイムでの制御に時間がかかる問題がある。
本発明の目的は、冗長自由度を利用して、エンドエフェクタにおける目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度及び軌跡精度を向上できるロボット制御方法、ロボット制御装置及びロボット制御システムを提供することにある。

上記問題点を解決するために、請求項１の発明は、作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御方法において、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階と、前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階とを組み合わせて、複数のリンク位置姿勢を取得するとともに、リンク位置姿勢毎の誤差拡大率を複数取得し、該複数の誤差拡大率の中から最小の誤差拡大率を選択し、前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与することを特徴とするロボット制御方法を要旨としている。

請求項２の発明は、請求項１において、前記リンク位置姿勢を算出する際、前記手先の目標位置姿勢を固定したと仮定して、前記冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されており、前記複数のリンク位置姿勢を取得する際は、前記姿勢パラメータを所定値分変化する毎に、前記手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出することを特徴とする。

請求項３の発明は、請求項１または請求項２において、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階を第１段階とし、前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階を第２段階としたとき、第１段階と第２段階を１つの組み合わせとし、この組み合わせの処理を複数回行うことを特徴とする。

請求項４の発明は、請求項１または請求項２において、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階を第１段階とし、前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階を第２段階としたとき、第１段階を複数回処理し、その後、算出されたリンク位置姿勢毎に第２段階を複数回行うことを特徴とする。

請求項５の発明は、作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御方法において、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階と、前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階とを組み合わせて、複数のリンク位置姿勢を取得するとともに、リンク位置姿勢毎の誤差拡大率を複数取得する段階と、該複数の誤差拡大率の中から最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度を記憶部に格納する段階と、作業プログラムに従って前記マニピュレータの手先の目標位置姿勢が与えられたとき、前記記憶部から前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与することを特徴とするロボット制御方法を要旨としている。

請求項６の発明は、作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御装置において、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部と、前記リンク位置姿勢毎の前記手先の誤差拡大率を演算する誤差拡大率演算部と、該複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部と、前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与する位置指令付与部を備えることを特徴とするロボット制御装置を要旨としている。

請求項７の発明は、請求項６において、前記リンク位置姿勢算出部は、前記リンク位置姿勢を算出する際、前記手先の目標位置姿勢を固定したと仮定して、前記冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータを定義して、前記複数のリンク位置姿勢を取得する際は、前記姿勢パラメータを所定値分変化する毎に、前記手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出することを特徴とする。

請求項８の発明は、作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御システムにおいて、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部と、前記リンク位置姿勢毎の前記手先の誤差拡大率を演算する誤差拡大率演算部と、該複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部と、を備えたシミュレーション装置と、前記選択部により選択された前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度を格納する記憶部と、作業プログラムに従って前記マニピュレータの手先の目標位置姿勢が与えられたとき、前記記憶部から前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与する位置指令付与部を備えることを特徴とするロボット制御システムを要旨としている。

請求項１乃至請求項５の発明によれば、冗長自由度を利用して、エンドエフェクタ（すなわち、マニピュレータの手先）における目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度及び軌跡精度を向上できるロボット制御方法を提供できる。

また、請求項２の発明によれば、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されていることにより、リンク位置姿勢の変更は、所定値の増減だけでできる。

また、請求項３の発明によれば、マニピュレータの手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を算出する段階を第１段階とし、リンク位置姿勢の手先の誤差拡大率を演算する段階を第２段階としたとき、第１段階と第２段階を１つの組み合わせとし、この組み合わせの処理を複数回行うことにより、請求項１または請求項２の効果を容易に実現できる。

また、請求項４の発明によれば、マニピュレータの手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を算出する段階を第１段階とし、リンク位置姿勢の手先の誤差拡大率を演算する段階を第２段階としたとき、第１段階を複数回処理し、その後、算出されたリンク位置姿勢毎に第２段階を複数回行うことにより、請求項１または請求項２の効果を容易に実現できる。

請求項５の発明によれば、予め記憶部に、最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度が格納されているため、作業プログラムに従ってマニピュレータの手先の目標位置姿勢が与えられたときは、コンピュータ（ＣＰＵ）の演算負荷を軽減でき、高速な演算処理ができる高コストのＣＰＵが不要となり、コストを低減できる。

請求項６乃至請求項８の発明によれば、冗長自由度を利用して、エンドエフェクタ（すなわち、マニピュレータの手先）における目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度及び軌跡精度を向上できるロボット制御装置を提供できる。

また、請求項７の発明によれば、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されていることにより、リンク位置姿勢の変更は、所定値の増減だけでできる。

また、請求項８の発明によれば、予め記憶部に、最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度が格納されているため、作業プログラムに従ってマニピュレータの手先の目標位置姿勢が与えられたときは、コンピュータ（ＣＰＵ）の演算負荷を軽減でき、高速な演算処理ができる高コストのＣＰＵが不要となり、コストを低減できる。

第１実施形態の冗長性を有するマニピュレータのスケルトン図。 一実施形態のロボット制御装置の概略構成図。 ロボット制御装置の制御フローチャート。 ロボット制御装置の制御フローチャート。 姿勢パラメータの説明図。 第２実施形態のロボット制御装置の制御フローチャート。 他の実施形態のロボット制御システムの概略図。

（第１実施形態）
以下、本発明を具体化した第１実施形態の作業自由度に対して冗長自由度を有するロボット制御装置及びロボット制御方法を図１〜図５を参照して説明する。

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

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

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

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

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

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

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

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

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

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

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

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

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

なお、回転角度検出器としては、ロータリエンコーダに限定するものではなく、レゾルバ、或いは、ポテンショメータであってもよい。
なお、第１サーボモータ４１〜第７サーボモータ４７に対してロータリエンコーダ７１〜７７を設ける代わりに、リンク１１〜１８または第１関節２１〜第７関節２７に、リンク１１〜１８の回転角度（関節角度）を直接に検出可能なセンサを取り付けてもよい。

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

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

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

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

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

（Ｓ２０）
次に、ＣＰＵ９１は、Ｓ２０で、手先をＳ１０でセットした教示点（手先位置）において、誤差拡大率が最小の姿勢（すなわち、リンク位置姿勢）決定を行う。このＳ２０の処理の詳細を図４のフローチャートを参照して説明する。

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

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

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

（Ｓ２０４）
Ｓ２０４では、ＣＰＵ９１は、前記手先位置姿勢から関節角度を求めるために逆運動学演算を行う。

ここで、第１関節２１〜第７関節２７の関節角度θ１，θ２，θ３，…，θ７とし、手先座標（ｘ，ｙ，ｚ）及び手先姿勢（ａ，ｂ，ｃ）とすると、ベクトルθ及び手先位置姿勢Ｘは下記のように表される。

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

この各関節角度θ１，θ２，θ３，…，θ７が当該姿勢パラメータΦにおけるリンク位置姿勢となる。

（Ｓ２０６）
Ｓ２０６では、ＣＰＵ９１は、姿勢パラメータΦにおけるｘ，ｙ，ｚ，ａ，ｂ，ｃのそれぞれの方向における、エンドエフェクタ変動／アクチュエータ変動、すなわち、各方向の誤差拡大率Σξを、式（３）〜式（８）により算出する。

なお、後述するＳ２１４において、誤差拡大率の最小を選択する場合、特定の方向のみの誤差拡大率、例えば、ｘ，ｙ，ｚ，ａ，ｂ，ｃのいずれか１つの方向のみの誤差拡大率の最小を選択する場合は、その方向に関する誤差拡大率Σξを、式（３）〜式（８）のいずれか１つの式を用いて算出すればよい。

また、後述するＳ２１４において、ｘ，ｙ，ｚ，ａ，ｂ，ｃのうち、２個〜５個の方向を組み合わせた場合、例えば、ｘ及びｙ方向、ｘ及びｚ方向、ｙ及びｚ方向の２つの方向、或いはいずれか３つの方向、或いは４つの方向、或いは５つの方向における誤差拡大率の総合的な誤差拡大率の場合には、それらの方向に関連した式（３）〜式（８）を使用して当該方向の誤差拡大率を算出すればよい。

（Ｓ２０８）
Ｓ２０８では、ＣＰＵ９１は、前記姿勢パラメータΦと、Ｓ２０６で算出した、誤差拡大率Σξを、記憶部９４に格納する。

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

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

（Ｓ２１４）
Ｓ２１２からＳ２１４に移行した場合、ＣＰＵ９１は、Ｓ２０８において記憶部９４に格納した誤差拡大率Σξの中から、最小のものを選択する。すなわち、姿勢パラメータΦを順次更新した上で、各姿勢パラメータ毎の各方向（或いは特定方向）における誤差拡大率Σξが取得されているため、この中から、ＣＰＵ９１は最小の誤差拡大率Σξを選択するのである。

ここで、特定方向のみの誤差拡大率Σξの最小値を選択する場合は、その方向のみであるので、容易に選択できる。一方、複数の方向の誤差拡大率が格納されている場合、例えば、ｘ及びｙ方向、ｘ及びｚ方向、或いは、ｙ及びｚ方向のように２つの方向の誤差拡大率Σξが記憶部９４に格納されている場合は、ＣＰＵ９１は、両方向の誤差拡大率が平均的に最小になるものを選択する。さらに、３つ以上の方向の誤差拡大率が記憶部９４に格納されている場合には、ＣＰＵ９１は、それらの方向の誤差拡大率Σξが平均的に最小になるものを選択する。このようにして誤差拡大率Σξが最小となるリンク位置姿勢が選択され、この結果を利用して、ＣＰＵ９１は、誤差拡大率Σξが最小となるリンク位置姿勢、すなわち姿勢パラメータΦを導出する。

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

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

本実施形態では、下記の特徴を有する。
（１） 本実施形態のロボット制御方法は、マニピュレータ１０の手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を算出する段階（Ｓ２０４）と、
リンク位置姿勢の手先の誤差拡大率を演算する段階（Ｓ２０６）とを組み合わせて、複数のリンク位置姿勢を取得するとともに、リンク位置姿勢毎の誤差拡大率Σξを複数取得し、該複数の誤差拡大率の中から最小の誤差拡大率を選択する。そして、最小の誤差拡大率となるリンク位置姿勢における各関節軸（Ｊ１軸〜Ｊ７軸）の関節角度となるように各関節軸（Ｊ１軸〜Ｊ７軸）のサーボモータ４１〜４７（アクチュエータ）に位置指令として付与する。この結果、本実施形態によれば、冗長自由度を利用して、エンドエフェクタ（すなわち、マニピュレータの手先）における目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度及び軌跡精度を向上できる。

（２）本実施形態の制御方法は、リンク位置姿勢を算出する際、手先の目標位置姿勢を固定したと仮定して、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータΦが定義されている。そして、複数のリンク位置姿勢を取得する際は、姿勢パラメータΦを所定値分変化する毎に、手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出する。この結果、本実施形態によれば、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されていることにより、リンク位置姿勢の変更は、所定値の増減だけでできる。

（３）本実施形態の制御方法は、マニピュレータ１０の手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を算出する段階（Ｓ２０４）を第１段階とし、リンク位置姿勢の前記手先の誤差拡大率を演算する段階（Ｓ２０６）を第２段階としたとき、Ｓ２１２で姿勢パラメータΦが、上限値Φｍａｘ以下の場合は、Ｓ２０４とＳ２０６を１つの組み合わせとし、この組み合わせの処理を複数回行うことになる。この結果、上記（１）、及び（２）の効果を容易に実現できる。

（４） 本実施形態のロボット制御装置のＣＰＵ９１は、マニピュレータ１０の手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部として機能する。また、ＣＰＵ９１は、リンク位置姿勢毎の手先の誤差拡大率を演算する誤差拡大率演算部として機能するとともに、前記複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部として機能する。さらに、ＣＰＵ９１は、最小の誤差拡大率となるリンク位置姿勢における各関節軸（Ｊ１軸〜Ｊ７軸）の関節角度となるように各関節軸（Ｊ１軸〜Ｊ７軸）のサーボモータ４１〜４７（アクチュエータ）に位置指令として付与する位置指令付与部として機能する。この結果、本実施形態のロボット制御装置ＲＣによれば、冗長自由度を利用して、エンドエフェクタ（すなわち、マニピュレータの手先）における目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度及び軌跡精度を向上できるロボット制御装置を提供できる。

（５） 本実施形態のロボット制御装置は、ＣＰＵ９１はリンク位置姿勢算出部として、リンク位置姿勢を算出する際、手先の目標位置姿勢を固定したと仮定して、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータΦを定義して、複数のリンク位置姿勢を取得する際は、姿勢パラメータΦを所定値分変化する毎に、手先の目標位置姿勢と該所定値分変化した姿勢パラメータΦに基づいてリンク位置姿勢を算出する。この結果、本実施形態によれば、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されていることにより、リンク位置姿勢の変更は、所定値の増減だけでできる。

（第２実施形態）
次に、第２実施形態のロボット制御方法について説明する。なお、第２実施形態のコントローラーＲＣのハード構成は、第１実施形態と同一であるため、説明を省略する。第２実施形態では、第１実施形態の図３のＳ２０の処理が図６に示すように異なっている。すなわち、Ｓ２０４の次にＳ２０８Ａの処理を行うことと、Ｓ２１２で「ＹＥＳ」と判定したときに、Ｓ２１３の処理を行った後、Ｓ２１４の処理を行うところが異なっている。具体的に、説明する。

（Ｓ２０８Ａ）
すなわち、Ｓ２０８Ａでは、ＣＰＵ９１は、Ｓ２０４で算出した各関節角度θ１，θ２，θ３，…，θ７と、姿勢パラメータΦを関連付けて記憶部９４に格納する。

（Ｓ２１３）
Ｓ２１３では、ＣＰＵ９１は、記憶部９４に格納した姿勢パラメータΦを順次読み出して、第１実施形態のＳ２０６と同様に、ＣＰＵ９１は、姿勢パラメータΦにおけるｘ，ｙ，ｚ，ａ，ｂ，ｃのそれぞれの方向における、エンドエフェクタ変動／アクチュエータ変動、すなわち、各方向の誤差拡大率Σξを、式（３）〜式（８）により算出する。

なお、後述するＳ２１４において、誤差拡大率の最小を選択する場合、特定の方向のみの誤差拡大率、例えば、ｘ，ｙ，ｚ，ａ，ｂ，ｃのいずれか１つの方向のみの誤差拡大率の最小を選択する場合は、その方向に関する誤差拡大率Σξを、式（３）〜式（８）のいずれか１つの式を用いて算出すればよい。

また、後述するＳ２１４において、ｘ，ｙ，ｚ，ａ，ｂ，ｃのうち、２個〜５個の方向を組み合わせた場合、例えば、ｘ及びｙ方向、ｘ及びｚ方向、ｙ及びｚ方向の２つの方向、或いはいずれか３つの方向、或いは４つの方向、或いは５つの方向における誤差拡大率の総合的な誤差拡大率の場合には、それらの方向に関連した式（３）〜式（８）を使用して当該方向の誤差拡大率を算出すればよい。なお、以下では、
このようにして、各姿勢パラメータΦに関する、各方向（或いは特定の方向）の誤差拡大率を算出した後、Ｓ２１４に移行する。

このように、第２実施形態では、Ｓ２１２において、「ＮＯ」と判定される毎に、Ｓ２０４が複数回実行される。また、Ｓ２１２において、「ＹＥＳ」と判定されると、Ｓ２１３で、各姿勢パラメータの誤差拡大率が複数回実行されることになる。

本実施形態では、第１実施形態の（４）、及び（５）の効果の他に下記の特徴を有する。
（１） 本実施形態のロボット制御方法は、マニピュレータ１０の手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を算出する段階（Ｓ２０４）を第１段階とし、リンク位置姿勢の前記手先の誤差拡大率を演算する段階（Ｓ２１３）を第２段階としたとき、第１段階を複数回処理し、その後、算出されたリンク位置姿勢毎に第２段階を複数回行うことにより、第１実施形態の（１）と同様の効果を容易に実現できる。

なお、本発明の実施形態は前記実施形態に限定されるものではなく、下記のように変更しても良い。
・ 前記各実施形態では、作業プログラムの実行時に、Ｓ２０の処理、すなわち、誤差拡大率の最小のリンク位置姿勢決定をリアルタイムに行うようにしている。この制御方法に代えて、ロボット制御システムとして具体化してもよい。

具体的には、図７に示すように、ティーチングプレイバック方式で作成された作業プログラムを、コントローラーＲＣと通信が可能に接続されたコンピュータ１００（例えばコントローラーＲＣの上位のコンピュータ）がシミュレーション装置としてシミュレーションで作業プログラムを実行する。そして、該コンピュータ１００が、該作業プログラムでの教示点での手先位置姿勢を手先の目標位置姿勢とし、それぞれの教示点毎に、複数の誤差拡大率を算出して該複数の誤差拡大率の中から最小となる誤差拡大率を選択する。この選択方法は、第１実施形態、または第２実施形態のＳ２０での求め方と同様に行えばよい。

そして、教示点毎のこの最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度を、その教示点が記述されている作業プログラムのステップと関連付けてファイル化して、該コンピュータ１００が図示しない記憶部に記憶する。この後、コンピュータ１００は、前記作業プログラムの教示点と関連付けされたファイルを、コントローラーＲＣに出力して記憶部９４に記憶する。

コントローラーＲＣは、前記作業プログラムが実行されるときに、教示点での手先位置姿勢等をセットした場合に、該教示点（ステップ）と関連付けされたファイルを記憶部９４から読み出すようにする。例えば、第１実施形態の図３のフローチャートにおいて、Ｓ２０の代わりに、このファイルを読込するステップに変えればよい。

このようにしても、前記実施形態と同様の効果を実現することができる。
また、このロボット制御システムによれば、予め記憶部９４に、最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度が格納されているため、作業プログラムに従ってマニピュレータ１０の手先の目標位置姿勢が与えられたときは、コンピュータ（ＣＰＵ）の演算負荷を軽減でき、高速な演算処理ができる高コストのＣＰＵが不要となり、コストを低減できる。

・ なお、作業プログラム上の教示点を結ぶ全ての軌道上について行う必要はなく、特定の軌道上のみ、第１、第２実施形態並びに上記システムにおいて、位置決め精度及び軌跡精度が必要なところのみを限定的に行うようにしてもよい。

この場合、例えば、特定の軌道上の教示点の手先位置姿勢の場合は、その旨を記すデータが当該作業プログラムのステップに記述されており、この記述されている場合に、図３に示すフローチャートにおいて、Ｓ１０からＳ２０に移行し、そうでない場合には、Ｓ１０からＳ３０にジャンプするようにする。

このようにすれば特定の軌道領域のみ、位置決め精度、及び軌跡精度を良くすることができる。このようにするとＣＰＵ９１の演算負荷を軽減できる。
・ また、姿勢パラメータΦの大きな変動を抑制するために、ある姿勢の範囲で決定するようにしてもよい。例えば、初期位置は、上限値Φｍａｘ以下の中で姿勢パラメータΦの全範囲で最小の誤差拡大率であるものを探索するようにし、次からは、例えば、１つ前の位置での姿勢パラメータΦ±ΔΦの範囲内で、最小の誤差拡大率であるものを探索するようにする等、制限を加えるようにしてもよい。

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

ＲＣ…コントローラー（制御装置）、
１０…マニピュレータ、１１〜１８…リンク、
４１〜４７…サーボモータ（アクチュエータ）、
９１…ＣＰＵ（リンク位置姿勢算出部、誤差拡大率演算部、選択部及び位置指令付与部）。

Claims (8)

1. 作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御方法において、
   前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階と、
   前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階とを組み合わせて、複数のリンク位置姿勢を取得するとともに、リンク位置姿勢毎の誤差拡大率を複数取得し、
   該複数の誤差拡大率の中から最小の誤差拡大率を選択し、
   前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与することを特徴とするロボット制御方法。
2. 前記リンク位置姿勢を算出する際、前記手先の目標位置姿勢を固定したと仮定して、前記冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されており、
   前記複数のリンク位置姿勢を取得する際は、前記姿勢パラメータを所定値分変化する毎に、前記手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出することを特徴とする請求項１に記載のロボット制御方法。
3. 前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階を第１段階とし、前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階を第２段階としたとき、
   第１段階と第２段階を１つの組み合わせとし、この組み合わせの処理を複数回行うことを特徴とする請求項１または請求項２に記載のロボット制御方法。
4. 前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階を第１段階とし、前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階を第２段階としたとき、
   第１段階を複数回処理し、その後、算出されたリンク位置姿勢毎に第２段階を複数回行うことを特徴とする請求項１または請求項２に記載のロボット制御方法。
5. 作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御方法において、
   前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を算出する段階と、
   前記リンク位置姿勢の前記手先の誤差拡大率を演算する段階とを組み合わせて、複数のリンク位置姿勢を取得するとともに、リンク位置姿勢毎の誤差拡大率を複数取得する段階と、
   該複数の誤差拡大率の中から最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度を記憶部に格納する段階と、
   作業プログラムに従って前記マニピュレータの手先の目標位置姿勢が与えられたとき、前記記憶部から前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与することを特徴とするロボット制御方法。
6. 作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御装置において、
   前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部と、
   前記リンク位置姿勢毎の前記手先の誤差拡大率を演算する誤差拡大率演算部と、
   該複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部と、
   前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与する位置指令付与部を備えることを特徴とするロボット制御装置。
7. 前記リンク位置姿勢算出部は、前記リンク位置姿勢を算出する際、前記手先の目標位置姿勢を固定したと仮定して、前記冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータを定義して、前記複数のリンク位置姿勢を取得する際は、前記姿勢パラメータを所定値分変化する毎に、前記手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出することを特徴とする請求項６に記載のロボット制御装置。
8. 作業自由度に対して冗長自由度を備えるように複数のリンクが各関節軸にて連結されたマニピュレータを、前記関節軸毎に設けられたアクチュエータに位置指令を付与することにより制御するロボットの制御システムにおいて、
   前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部と、
   前記リンク位置姿勢毎の前記手先の誤差拡大率を演算する誤差拡大率演算部と、
   該複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部と、
   を備えたシミュレーション装置と、
   前記選択部により選択された前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度を格納する記憶部と、
   作業プログラムに従って前記マニピュレータの手先の目標位置姿勢が与えられたとき、前記記憶部から前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに前記位置指令として付与する位置指令付与部を備えることを特徴とするロボット制御システム。