(57)【要約】【課題】さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができるロボットの制御方法及びロボット制御装置を提供する。【解決手段】作業自由度に対して１つの冗長自由度を備えるマニピュレータ１０Ａ，１０Ｂの関節軸毎に設けられたサーボモータ（回転系アクチュエータ）を制御するＣＰＵを備えたコントローラーＲＣ１，ＲＣ２は、作業プログラムの内容に応じて、ＣＰＵのサーボモータに対する制御を精度向上軌跡制御または精度向上位置決め制御に切り換える。この結果、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができる。【選択図】図１
2014124733.tif 000002
【特許請求の範囲】
【請求項１】
作業自由度に対して少なくとも１つの冗長自由度を備えるマニピュレータの関節軸毎に設けられた回転系アクチュエータを制御する精度向上軌跡制御部及び精度向上位置決め制御部を備えたロボット制御装置において、
作業プログラムの内容に応じて、前記精度向上軌跡制御部または前記精度向上位置決め制御部による前記回転系アクチュエータに対する制御を切り換える制御切替部を備えるロボット制御装置。
【請求項２】
前記マニピュレータは、複数のリンクが各関節軸に連結されており、
前記精度向上軌跡制御部は、
手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を示す姿勢パラメータを順次変化させる毎に、少なくとも各リンクの関節軸の慣性力と遠心力・コリオリ力と重力に基づく負荷トルクを算出する負荷トルク算出部と、
前記姿勢パラメータの変化の中で、前記負荷トルクと前記回転系アクチュエータの定格トルク比が最小となるリンク位置姿勢を求めるリンク姿勢算出部と、
前記手先位置姿勢を目標値とした各関節軸の回転系アクチュエータの制御指令に、前記負荷トルクと前記回転系アクチュエータの定格トルク比が前記最小となるときの各負荷トルクとなるフィードフォワード値を付与する付与部を含む請求項１に記載のロボット制御装置。
【請求項３】
前記負荷トルク算出部は、各リンクの関節軸の慣性力と遠心力・コリオリ力と重力の他に摩擦トルクを含む負荷トルクを算出するものである請求項２に記載のロボット制御装置。
【請求項４】
前記精度向上位置決め制御部は、
前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部と、
前記リンク位置姿勢毎の前記手先の誤差拡大率を演算する誤差拡大率演算部と、
該複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部と、
前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに位置指令として付与する位置指令付与部を含む請求項２または請求項３に記載のロボット制御装置。
【請求項５】
前記リンク位置姿勢算出部は、前記リンク位置姿勢を算出する際、前記手先の目標位置姿勢を固定したと仮定して、前記冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータを定義して、前記複数のリンク位置姿勢を取得する際は、前記姿勢パラメータを所定値分変化する毎に、前記手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出するものである請求項４に記載のロボット制御装置。
【請求項６】
作業自由度に対して少なくとも１つの冗長自由度を備えるマニピュレータの関節軸毎に設けられた回転系アクチュエータを制御する精度向上軌跡制御部及び精度向上位置決め制御部によりロボットを制御するロボットの制御方法において、
作業プログラムの内容に応じて、前記精度向上軌跡制御部または前記精度向上位置決め制御部による前記回転系アクチュエータに対する制御を切り換えるロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置及びロボットの制御方法に関する。
【背景技術】
【０００２】
特許文献１のロボット制御装置では、ロボットアームの運動に伴って、所定のサンプリング周期毎の角度の出力値と、前記角度の目標値との偏差と角速度の目標値との偏差とからの状態フィードバック値とにより、目標角加速度を修正して慣性力、コリオリ力、求心力、重力をリアルタイムで計算し、それらと等価のトルクを関節駆動アクチュエータに発生させてロボットアームの運動を制御するようにしている。
【０００３】
上記のように前記慣性力等をリアルタイムで計算してロボットアームを回転駆動するモータに対し、トルクを与えることにより、制御性を向上させることができる。
また、特許文献２では、表示部に対して撮像カメラで撮影した視覚情報と共に、マニピュレータ本体の現在位置から目標への予定経路情報に基づくマニピュレータ位置情報の双方を表示するようにして、操作部を表示部の視覚情報及びマニピュレータ位置情報に対応して操作し得るように構成し、高精度な位置決め制御を実現するようにしている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平４−９８３０４号公報
【特許文献２】特開平５−３０１１８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１では、慣性力等をリアルタイムで計算してモータにトルクを与えるため、従来よりは制御性はよいが、ロボット姿勢によっては、過大なトルクを発生させなければならない場合があり、この場合は逆に制御性が劣化し、高精度な軌跡制御ができない。
【０００６】
また、特許文献２では、アクチュエータ軸での回転角制御誤差がエンドエフェクタで拡大されて、位置決め精度及び軌跡精度が劣化し、例えば、ある点で位置決めした状態でアクチュエータ軸に±１パルスのサーボ変動があると、その動きがマニピュレータの先端で拡大される場合がある。マニピュレータの先端の位置をカメラ等でフィードバックすれば、これらの誤差は軽減されるが、リアルタイムでの制御に時間がかかる問題がある。
【０００７】
本発明の目的は、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができるロボット制御装置を提供することにある。
【０００８】
また、本発明の他の目的は、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができるロボットの制御方法を提供することにある。
【課題を解決するための手段】
【０００９】
上記問題点を解決するために、本ロボット制御装置は、作業自由度に対して少なくとも１つの冗長自由度を備えるマニピュレータの関節軸毎に設けられた回転系アクチュエータを制御する精度向上軌跡制御部及び精度向上位置決め制御部を備えたロボット制御装置において、作業プログラムの内容に応じて、前記精度向上軌跡制御部または前記精度向上位置決め制御部による前記回転系アクチュエータに対する制御を切り換える制御切替部を備えるものである。
【００１０】
前記マニピュレータは、複数のリンクが各関節軸に連結されており、前記精度向上軌跡制御部は、手先位置姿勢を目標値とした拘束条件の下で、前記冗長自由度が許容するリンク位置姿勢を示す姿勢パラメータを順次変化させる毎に、少なくとも各リンクの関節軸の慣性力と遠心力・コリオリ力と重力に基づく負荷トルクを算出する負荷トルク算出部と、前記姿勢パラメータの変化の中で、前記負荷トルクと前記回転系アクチュエータの定格トルク比が最小となるリンク位置姿勢を求めるリンク姿勢算出部と、前記手先位置姿勢を目標値とした各関節軸の回転系アクチュエータの制御指令に、前記負荷トルクと前記回転系アクチュエータの定格トルク比が前記最小となるときの各負荷トルクとなるフィードフォワード値を付与する付与部を含むことが好ましい。
【００１１】
前記負荷トルク算出部は、各リンクの関節軸の慣性力と遠心力・コリオリ力と重力の他に摩擦トルクを含む負荷トルクを算出するものであることが好ましい。
前記精度向上位置決め制御部は、前記マニピュレータの手先の目標位置姿勢に基づいて、前記冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部と、前記リンク位置姿勢毎の前記手先の誤差拡大率を演算する誤差拡大率演算部と、該複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部と、前記最小の誤差拡大率となるリンク位置姿勢における各関節軸の関節角度となるように前記各関節軸のアクチュエータに位置指令として付与する位置指令付与部を含むことが好ましい。
【００１２】
前記リンク位置姿勢算出部は、前記リンク位置姿勢を算出する際、前記手先の目標位置姿勢を固定したと仮定して、前記冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータを定義して、前記複数のリンク位置姿勢を取得する際は、前記姿勢パラメータを所定値分変化する毎に、前記手先の目標位置姿勢と該所定値分変化した姿勢パラメータに基づいてリンク位置姿勢を算出するものであることが好ましい。
【００１３】
また、本ロボットの制御方法は、作業自由度に対して少なくとも１つの冗長自由度を備えるマニピュレータの関節軸毎に設けられた回転系アクチュエータを制御する精度向上軌跡制御部及び精度向上位置決め制御部によりロボットを制御するロボットの制御方法において、作業プログラムの内容に応じて、前記精度向上軌跡制御部または前記精度向上位置決め制御部による前記回転系アクチュエータに対する制御を切り換えるものである。
【発明の効果】
【００１４】
本ロボット制御装置によれば、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができる。
また、本ロボットの制御方法によれば、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができる。
【図面の簡単な説明】
【００１５】
【図１】本発明の一実施形態の一対のマニピュレータのスケルトン図。
【図２】一実施形態の冗長性を有するマニピュレータのスケルトン図。
【図３】一実施形態のロボット制御装置の概略構成図。
【図４】一実施形態の作業プログラムによる全体の制御フローチャート。
【図５】姿勢パラメータの説明図。
【図６】ロボット制御装置の高精度軌跡制御部の制御フローチャート。
【図７】ロボット制御装置の高精度軌跡制御部の制御フローチャート。
【図８】ロボット制御装置の高精度位置決め制御部の制御フローチャート。
【図９】ロボット制御装置の高精度位置決め制御部の制御フローチャート。
【図１０】他の実施形態のロボット制御装置の高精度位置決め制御部の制御フローチャート。
【発明を実施するための形態】
【００１６】
以下、本発明を具体化した一実施形態のロボット制御装置及びロボットの制御方法を図１〜図９を参照して説明する。
本実施形態のロボット制御装置は、図１に示すように、一対の多関節ロボットであるマニピュレータ１０Ａ，１０Ｂをそれぞれ制御する一対のロボット制御装置（コントローラー）ＲＣ１，ＲＣ２（図３参照）に具体化したものである。
【００１７】
まず、本実施形態の作業自由度に対して冗長自由度を有するロボットであるマニピュレータ１０Ａについて説明する。なお、マニピュレータ１０Ｂの構成はマニピュレータ１０Ａと同一構成であるため、同一構成には同一符号を付してその説明を省略する。
【００１８】
図２に示すように、マニピュレータ１０Ａは、８個のリンク１１〜１８が７個の関節２１〜２７により直列に連結されて形成されている。多関節ロボットであるマニピュレータ１０Ａは、７個の関節２１〜２７においてリンク１２〜１８が旋回することのできる７自由度（自由度ｎ＝７）を有するロボットであり、その作業空間の次元数（次元数ｍ）は６であって、１（＝ｎ−ｍ）の冗長性を有する。すなわち、本実施形態のマニピュレータは、作業自由度６に対して、１つの冗長自由度を有する。
【００１９】
第１リンク１１は一端が床面ＦＬに固定され、他端が第１関節２１の一側に接続されている。第１関節２１の他側には、第２リンク１２の一端が接続され、第２リンク１２の他端には第２関節２２の一側が接続されている。以下同様に、第３リンク１３、第４リンク１４、第５リンク１５、第６リンク１６、第７リンク１７及び第８リンク１８が、それぞれ第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７を介して順に連結されている。
【００２０】
第１関節２１の他側は一側に対して、矢印３１に示すように図２において上下方向に延びる軸を中心に回転可能とされており、これにより、第２リンク１２は隣接する第１リンク１１に対して、第１関節２１の回転軸（Ｊ１軸）を中心に矢印３１方向に旋回可能である。
【００２１】
また、第２関節２２の他側は一側に対して、矢印３２に示すように、図２において紙面に垂直な方向に延びる軸（Ｊ２軸）を中心に回転可能とされている。これにより、第３リンク１３は隣接する第２リンク１２に対して、第２関節２２の回転軸を中心に矢印３２方向に回転可能である。
【００２２】
以下、第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７についてもそれぞれ、回転可能とされており、第４リンク１４、第５リンク１５、第６リンク１６、第７リンク１７及び第８リンク１８も、それぞれ関節２３〜２７の回転軸（Ｊ３軸〜Ｊ７軸）を中心に、矢印３３〜３７方向に旋回可能である。尚、本願の全体にわたって、第１関節２１〜２７を介して連結されているリンク１１〜１８同士を、互いに隣接するリンク１１〜１８という。また、Ｊ１軸〜Ｊ７軸は、関節軸に相当する。
【００２３】
図２に示すように、第１関節２１には第１サーボモータ４１が取り付けられており、電力が供給されることにより、第２リンク１２を図示しない減速機を介して第１リンク１１に対して旋回させる。
【００２４】
また、第２関節２２には第２サーボモータ４２が取り付けられており、電力が供給されることにより、第３リンク１３を図示しない減速機を介して第２リンク１２に対して旋回させる。以下、同様に、第３関節２３、第４関節２４、第５関節２５、第６関節２６及び第７関節２７にはそれぞれサーボモータ４３〜４７が取り付けられており、電力が供給されることにより、各々リンク１４〜１８を図示しない減速機を介して旋回させる。
【００２５】
なお、各モータは、各関節内に設けられるが、図２では、説明の便宜上、関節とは分離して図示している。また、本実施形態では回転系アクチュエータとしてサーボモータであるＡＣモータが使用されているが、限定されるものではない。
【００２６】
第８リンク１８の先端には、ツール４９が取り付けられている。ツール４９は第８リンク１８とともに、第７関節２７の回転軸（Ｊ７軸）を中心に図２に示すように矢印３７方向に旋回可能とされている。ツール４９は、例えば、ワークＷ１等を把持可能なハンドである。
【００２７】
上述したようにマニピュレータ１０Ａは、第１サーボモータ４１〜第７サーボモータ４７を駆動して第２リンク１２〜第８リンク１８を回転させることにより、第２リンク１２〜第８リンク１８の回転角度が累積して先端部にあるツール４９に働くため、ツール４９の先端の位置および姿勢を、その作業内容に応じた目標位置および目標姿勢に一致させることが可能である。
【００２８】
次に、図３を参照して、前記マニピュレータ１０Ａを制御するロボット制御装置としてのコントローラーＲＣ１を中心とした多関節ロボットの電気的な構成を説明する。なお、マニピュレータ１０Ｂを制御するロボット制御装置としてのコントローラーＲＣ２の構成は、コントローラーＲＣ１と同一構成であるため、説明を省略する。
【００２９】
コントローラーＲＣ１は、コンピュータ９０と、コンピュータ９０に電気的に接続されたＰＷＭジェネレータ５１〜５７と、ＰＷＭジェネレータ５１〜５７に電気的に接続されたサーボアンプ６１〜６７を有する。各サーボアンプ６１〜６７はそれぞれ第１サーボモータ４１〜第７サーボモータ４７に電気的に接続されている。
【００３０】
コンピュータ９０は、後述する作業プログラムに基づいて制御指令をＰＷＭジェネレータ５１〜５７に出力し、ＰＷＭジェネレータ５１〜５７は、当該制御令指令に基づいてＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じてサーボモータ４１〜４７を作動させることにより、各リンク１２〜１８を回転させる。
【００３１】
前記サーボモータ４１〜４７にはロータリエンコーダ７１〜７７が内蔵されており、インターフェイス８０を介してコンピュータ９０と接続されている。ロータリエンコーダ７１〜７７は、各々のサーボモータ４１〜４７の回動角度を検出することにより、すなわち、リンク１２〜１８のそれぞれが隣接するリンク１１〜１７に対する回転角度（関節角度）を検出して、その検出信号をコントローラーＲＣ１に送信する。ロータリエンコーダ７１〜７７は、回転角度検出器に相当する。なお、回転角度検出器としては、ロータリエンコーダに限定するものではなく、レゾルバ、或いは、ポテンショメータであってもよい。
【００３２】
また、第１サーボモータ４１〜第７サーボモータ４７に対してロータリエンコーダ７１〜７７を設ける代わりに、リンク１１〜１８または第１関節２１〜第７関節２７に、リンク１１〜１８の回転角度（関節角度）を直接に検出可能なセンサを取り付けてもよい。
【００３３】
前記コンピュータ９０は、ＣＰＵ９１、ＲＯＭ９２、ＲＡＭ９３、ハードディスク等の不揮発性の記憶部９４、インターフェイス９５及び通信インターフェイス９７等を備え、バス９６を介して電気的に接続されている。
【００３４】
記憶部９４には、各種データ、ロボットに各種作業を行わせるための作業プログラム、各種パラメータ等が記憶されている。すなわち、本実施形態のロボットは、ティーチングプレイバック方式で作動するロボットであり、前記作業プログラムが実行されることにより、前記マニピュレータ１０Ａが動作する。ＲＯＭ９２は、コンピュータ９０自体のシステムプログラムが記憶されている。ＲＡＭ９３は、ＣＰＵ９１の作業用のメモリであって、各種演算等が実行されるときに一時的にデータが格納される。
【００３５】
ＣＰＵ９１は、負荷トルク算出部、リンク姿勢算出部及び付与部を含む精度向上軌跡制御部に相当するとともに、リンク位置姿勢算出部、誤差拡大率演算部、選択部、及び位置指令付与部を含む精度向上位置決め制御部に相当する。また、ＣＰＵ９１は、前記精度向上軌跡制御部と精度向上位置決め制御部を切り替える制御切替部に相当する。
【００３６】
通信インターフェイス９７は、コントローラーＲＣ２との通信を行うための通信器である。コントローラーＲＣ１とコントローラーＲＣ２は、共に通信インターフェイス９７を介して通信を行うことにより、協調制御を行う。
【００３７】
コントローラーＲＣ１には入力装置８２が前記インターフェイス９５を介して接続されている。入力装置８２は、図示しないモニター画面及び各種入力キー等を有する操作盤であり、ユーザーが各種のデータを入力操作可能とされている。入力装置８２は、多関節ロボットの電源スイッチが設けられるとともに、コンピュータ９０に対して、マニピュレータ１０Ａの先端部にあるツール４９の先端（以下、手先という）の最終目標位置および最終目標姿勢、ツール４９の先端の補間点における位置および姿勢の入力、並びに、冗長性を利用したマニピュレータ１０Ａの姿勢変更のためのジョグ操作等による入力が可能となっている。
【００３８】
（実施形態の作用）
次に、本実施形態による多関節ロボットの両コントローラーＲＣ１，ＲＣ２の作用を説明する。
【００３９】
図４、図６〜図９は、両コントローラーＲＣ１，ＲＣ２の各ＣＰＵ９１が実行する作業プログラムに書き込まれている教示点（最終目標位置、並びにツール４９の先端の補間点における位置）に手先を位置させるときの制御フローチャートである。前記ロータリエンコーダ７１〜７７は下記各ステップが実行される制御周期よりも、十分に速い検出周期で回転角度（すなわち、関節角度）を検出している。
【００４０】
図４は一実施形態のロボット制御装置ＲＣ１，ＲＣ２の両装置及びロボットを含むロボットシステム全体において、ロボット制御装置ＲＣ１，ＲＣ２のそれぞれの作業プログラムが実行された際の制御フローチャートである。
【００４１】
まず、両作業プログラムがそれぞれ起動されると、各作業プログラムに基づいて、コントローラーＲＣ１，ＲＣ２は、通信インターフェイス９７を介して相互に通信を行って協調制御により図示しない搬送装置上の共通のワークＷ１をツール４９によりハンドリングしてワークＷ１を取り出し（Ｓ１）、図１に示す二点鎖線で示すよう作業台Ｄ上方の所定高さの位置まで搬送する（Ｓ２）、このＳ２での工程を協調搬送工程という。この協調搬送工程では、ＣＰＵ９１（精度向上軌跡制御部）により精度向上軌跡制御を行う。この精度向上軌跡制御については、後述する。
【００４２】
次に、両マニピュレータ１０Ａ，１０Ｂにより前記作業台Ｄ上方の所定高さへ前記ワークＷ１を位置した後は、両ロボット制御装置のＣＰＵ９１（制御切替部）は、それぞれ精度向上位置決め制御に切り替える（Ｓ３）。すなわち、両ＣＰＵ９１（制御切替部）は、それぞれの作業プログラムの協調搬送工程の実行のための最終ステップの次のステップの組付け工程のための最初のステップで精度向上位置決め制御に切り替えて、ＣＰＵ９１（精度向上位置決め制御部）による後述する精度向上位置決め制御により、前記ワークＷ１を、作業台Ｄ上に予め固定された他のワークＷ２に対して組み付けを行う（Ｓ４）。この工程を組付け工程という。
【００４３】
＜１．精度向上軌跡制御＞
次に、上記のＳ２で行われる精度向上軌跡制御を、図６及び図７のフローチャートを参照して説明する。以下で説明するＣＰＵ９１は、コントローラーＲＣ１，ＲＣ２のそれぞれのＣＰＵ９１のことである。
【００４４】
（Ｓ１０）
まず、Ｓ（ステップ）１０では、ＣＰＵ９１は、記憶部９４の作業プログラムに書き込まれている教示点（手先位置）、手先姿勢及び速度データをＲＡＭ９３の所定領域にセットする。なお、手先位置及び手先姿勢を合わせて手先位置姿勢という。
【００４５】
（Ｓ２０）
次に、ＣＰＵ９１は、Ｓ２０で、手先をＳ１０でセットした教示点（手先位置）において、負荷トルク／定格トルク比最小姿勢決定を行う。このＳ２０の処理の詳細を図７のフローチャートを参照して説明する。
【００４６】
（Ｓ２１）
Ｓ２１では、ＣＰＵ９１は、姿勢パラメータΦを初期化する、本実施形態ではΦ＝０にして初期化するが、初期値は０に限定するものではない。姿勢パラメータΦについて説明する。
【００４７】
姿勢パラメータΦは、冗長自由度を有する前記マニピュレータ１０Ａ、１０Ｂが、手先位置を固定した場合、すなわち、手先位置姿勢を拘束した場合において、その冗長自由度により許容されるリンク位置姿勢を示すものである。例えば、図５に示すようにマニピュレータ１０Ａの第４関節２４は、第２関節２２（以下、第１基準点Ｗという）を中心とし、第３リンク１３〜第４リンク１４のリンク長の合計を半径とした球Ａ１と、第６関節２６（以下、第２基準点Ｋという）を中心とし、第５リンク１５〜第６リンク１６のリンク長の合計を半径とする球Ａ２とが形成する交差円Ｅ上の移動が可能である。従って、本実施形態では、この交差円Ｅ上に第４関節２４が位置するようにして、リンク位置姿勢が変わる。
【００４８】
前記交差円Ｅの中心を通る中心軸Ｏは、図５に示すように、第１基準点Ｗ（第２関節２２中心）と第２基準点Ｋ（第６関節２６中心）を通過する軸である。第４関節２４はこの交差円Ｅ上に位置するため、姿勢パラメータΦは、交差円Ｅ上のリンク位置姿勢を示すパラメータとして表すことができる。そこで、交差円Ｅ上の適宜の位置Ｒから、変更された位置までの角度を、ここでは、姿勢パラメータΦとして定義されている。本実施形態では位置Ｒは、第４関節２４の現在位置とする。
【００４９】
（Ｓ２２）
Ｓ２２では、ＣＰＵ９１は姿勢パラメータΦとして所定値を加算して更新する。すなわち、仮想的に姿勢パラメータΦを増加させてリンクの位置姿勢を変えたものとするのである。
【００５０】
（Ｓ２３）
Ｓ２３では、手先位置姿勢から関節角度を求めるために逆変換演算を行う。
ここで、第１関節２１〜第７関節２７の関節角度ｑ１，ｑ２，ｑ３，…，ｑ７とし、手先座標（ｘ，ｙ，ｚ）及び手先姿勢（ａ，ｂ，ｃ）とすると、ベクトルｑ及び手先位置姿勢Ｘは下記のように表される。
【００５１】
【数１】
2014124733.tif 000003
各関節角度ｑ１，ｑ２，ｑ３，…，ｑ７は、式（２）に示すように表すことができ、これらの式は、逆変換式である。本ステップでは、式（２）により、手先位置姿勢Ｘと姿勢パラメータΦにより、各関節角度ｑ１，ｑ２，ｑ３，…，ｑ７を算出する。
【００５２】
【数２】
2014124733.tif 000004
（Ｓ２４）
Ｓ２４では、ＣＰＵ９１は、各関節軸に設けられたモータの負荷トルクを式（３）により算出し、記憶部９４に格納する。
【００５３】
式（３）の右辺において、第１項は慣性力、第２項は遠心力・コリオリ力より導かれるトルク、第３項は重力負荷より導かれるトルク、第４項は摩擦トルク、及び第５項はアクチュエータ慣性トルクである。これらの式中、右辺の第１項から第３項は、ラグランジュ法により導かれた運動方程式であり、公知の式である。また、右辺の第４項及び第５項はアクチュエータの影響を考慮に入れた項である。
【００５４】
【数３】
2014124733.tif 000005
なお、関節ｉの角速度、各加速度は、求められた関節角度ｑｉと、Ｓ１０でセットされた速度データに基づいて算出する。
【００５５】
第１項の慣性力については下記の通りである。
【００５６】
【数４】
2014124733.tif 000006
【００５７】
【数５】
2014124733.tif 000007
【００５８】
【数６】
2014124733.tif 000008
なお、「既知」とかかれているパラメータ等は、記憶部９４に格納されている。
【００５９】
各軸の各関節軸に設けられたモータの負荷トルクの算出が終了すると、ＣＰＵ９１は、Ｓ２５に移行する。
（Ｓ２５）
Ｓ２５では、ＣＰＵ９１は、姿勢パラメータΦが予め設定された上限値Φｍａｘ以下であればＳ２２に戻り、姿勢パラメータΦが予め設定された上限値Φｍを超えた場合にはＳ２６に移行する。この上限値Φｍは予め試験等により設定されたものである。
【００６０】
（Ｓ２６）
Ｓ２６では、ＣＰＵ９１は、Ｓ２４で算出した各負荷トルクと、予め記憶部９４に格納されている各関節軸のモータの定格トルクの比（負荷トルク／定格トルク）を算出して、最小の比となった負荷トルクが算出された姿勢パラメータΦを導出する。
【００６１】
次に、図６のフローチャートに戻って説明する。
（Ｓ３０）
Ｓ３０では、ＣＰＵ９１は、前記最小の比となった負荷トルクが算出された姿勢パラメータΦにおいて、該負荷トルクとなるトルクフィードフォワード値、すなわち、前記負荷トルクを生成するためのモータ電流値を公知の方法で算出する。
【００６２】
（Ｓ４０）
Ｓ４０では、ＣＰＵ９１は、Ｓ１０でセットした手先位置姿勢を目標値とした各関節軸の回転系アクチュエータの制御指令に、前記算出したトルクフィードフォワード値を付与し、そのトルクフィードフォワード値が付与された制御指令を、今回の制御周期分の指令としてＰＷＭジェネレータ５１〜５７にそれぞれ出力する。ＰＷＭジェネレータ５１〜５７は、その制御指令に基づくＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じて各関節軸の第１サーボモータ４１〜第７サーボモータ４７を作動させることにより、各リンク１２〜１８を回転させる。
【００６３】
（Ｓ５０）
ＣＰＵ９１は、Ｓ５０において、各ロータリエンコーダ７１〜７７からの検出信号に基づいて、最終目標位置（すなわち、協調搬送工程での最終目標位置）、及び最終目標姿勢（すなわち、協調搬送工程での最終の目標位置姿勢）に手先が到達したか否かを判定し、達していないと判定した場合はＳ１０に戻り、反対に作業プログラムの最終の目標位置姿勢に達していると判定した場合は、このフローチャートを終了する。なお、Ｓ１０に戻った場合、作業プログラムに記述されている次の手先位置姿勢、速度データをセットする。
【００６４】
＜２．精度向上位置決め制御＞
次に、図４のＳ４での組付け工程で行われる精度向上位置決め制御を、図８及び図９のフローチャートを参照して説明する。
【００６５】
図８及び図９は、コントローラーＲＣ１，ＲＣ２の各ＣＰＵ９１が実行する作業プログラムに書き込まれている教示点（最終目標位置、並びにツール４９の先端の補間点における位置を含む）に手先を位置させるときの制御フローチャートである。前記ロータリエンコーダ７１〜７７は下記各ステップが実行される制御周期よりも、十分に短い検出周期で回転角度（すなわち、検出関節角度）を検出している。
【００６６】
（Ｓ１００）
まず、Ｓ１００では、ＣＰＵ９１は、記憶部９４の作業プログラムに書き込まれている教示点（手先位置）、手先姿勢及び速度データをＲＡＭ９３の所定領域にセットする。なお、手先位置及び手先姿勢を合わせて手先位置姿勢という。このＳ１００セットされた手先位置姿勢は、手先の目標位置姿勢となる。
【００６７】
（Ｓ２００）
次に、ＣＰＵ９１は、Ｓ２００で、手先をＳ１００でセットした教示点（手先位置）において、誤差拡大率が最小の姿勢（すなわち、リンク位置姿勢）決定を行う。このＳ２００の処理の詳細を図９のフローチャートを参照して説明する。
【００６８】
（Ｓ２０２）
Ｓ２０２では、ＣＰＵ９１は、前記姿勢パラメータΦを初期化する、本実施形態ではΦ＝０にして初期化するが、初期値は０に限定するものではない。
【００６９】
（Ｓ２０４）
Ｓ２０４では、ＣＰＵ９１は、前記手先位置姿勢から関節角度を求めるために逆運動学演算を行う。
【００７０】
ここで、第１関節２１〜第７関節２７の関節角度ｑ１，ｑ２，ｑ３，…，ｑ７とし、手先座標（ｘ，ｙ，ｚ）及び手先姿勢（ａ，ｂ，ｃ）とすると、ベクトルｑ及び手先位置姿勢Ｘは、前記式（１）で表される。
【００７１】
各関節角度ｑ１，ｑ２，ｑ３，…，ｑ７は、前記式（２）に示すように表すことができ、これらの式は、逆変換式である。本ステップでは、式（２）により、手先位置姿勢Ｘと姿勢パラメータΦにより、各関節角度ｑ１，ｑ２，ｑ３，…，ｑ７を算出する。
【００７２】
この各関節角度ｑ１，ｑ２，ｑ３，…，ｑ７が当該姿勢パラメータΦにおけるリンク位置姿勢となる。
（Ｓ２０６）
Ｓ２０６では、ＣＰＵ９１は、姿勢パラメータΦにおけるｘ，ｙ，ｚ，ａ，ｂ，ｃのそれぞれの方向における、エンドエフェクタ変動／アクチュエータ変動、すなわち、各方向の誤差拡大率Σξを、式（７）〜式（１２）により算出する。
【００７３】
なお、後述するＳ２１４において、誤差拡大率の最小を選択する場合、特定の方向のみの誤差拡大率、例えば、ｘ，ｙ，ｚ，ａ，ｂ，ｃのいずれか１つの方向のみの誤差拡大率の最小を選択する場合は、その方向に関する誤差拡大率Σξを、式（７）〜式（１２）のいずれか１つの式を用いて算出すればよい。
【００７４】
また、後述するＳ２１４において、ｘ，ｙ，ｚ，ａ，ｂ，ｃのうち、２個〜５個の方向を組み合わせた場合、例えば、ｘ及びｙ方向、ｘ及びｚ方向、ｙ及びｚ方向の２つの方向、或いはいずれか３つの方向、或いは４つの方向、或いは５つの方向における誤差拡大率の総合的な誤差拡大率の場合には、それらの方向に関連した式（７）〜式（１２）を使用して当該方向の誤差拡大率を算出すればよい。式（７）〜式（１２）中のｇは、ｑとΦとから各方向（ｘ，ｙ，ｚ，ａ，ｂ，ｃ）の位置を表す関数である。例えば、ｘ＝ｇｘ（ｑ，Φ）は、ｘにおける位置を表す関数である。
【００７５】
【数７】
2014124733.tif 000009
（Ｓ２０８）
Ｓ２０８では、ＣＰＵ９１は、前記姿勢パラメータΦと、Ｓ２０６で算出した、誤差拡大率Σξを、記憶部９４に格納する。
【００７６】
（Ｓ２１０）
Ｓ２１０では、ＣＰＵ９１は姿勢パラメータΦに対して所定値を加算して更新する。すなわち、仮想的に姿勢パラメータΦを増加させてリンク位置姿勢を変えたものとするのである。この所定値は、例えば１°であるが、この値に限定されるものではない。
【００７７】
（Ｓ２１２）
Ｓ２１２では、ＣＰＵ９１は、姿勢パラメータΦが、予め設定された上限値Φｍａｘ以下であれば、Ｓ２０４に戻り、姿勢パラメータΦが予め設定された上限値Φｍを超えた場合には、「ＹＥＳ」と判定し、Ｓ２１４に移行する。この上限値Φｍは、予め試験等により設定されたものである。
【００７８】
（Ｓ２１４）
Ｓ２１２からＳ２１４に移行した場合、ＣＰＵ９１は、Ｓ２０８において記憶部９４に格納した誤差拡大率Σξの中から、最小のものを選択する。すなわち、姿勢パラメータΦを順次更新した上で、各姿勢パラメータ毎の各方向（或いは特定方向）における誤差拡大率Σξが取得されているため、この中から、ＣＰＵ９１は最小の誤差拡大率Σξを選択するのである。
【００７９】
ここで、特定方向のみの誤差拡大率Σξの最小値を選択する場合は、その方向のみであるので、容易に選択できる。一方、複数の方向の誤差拡大率が格納されている場合、例えば、ｘ及びｙ方向、ｘ及びｚ方向、或いは、ｙ及びｚ方向のように２つの方向の誤差拡大率Σξが記憶部９４に格納されている場合は、ＣＰＵ９１は、両方向の誤差拡大率が平均的に最小になるものを選択する。さらに、３つ以上の方向の誤差拡大率が記憶部９４に格納されている場合には、ＣＰＵ９１は、それらの方向の誤差拡大率Σξが平均的に最小になるものを選択する。このようにして誤差拡大率Σξが最小となるリンク位置姿勢が選択され、この結果を利用して、ＣＰＵ９１は、誤差拡大率Σξが最小となるリンク位置姿勢、すなわち姿勢パラメータΦを導出する。
【００８０】
次に、図８のフローチャートに戻って説明する。
（Ｓ３００）
Ｓ３００では、ＣＰＵ９１は、前記誤差拡大率Σξが最小となる姿勢パラメータΦの各関節軸の関節角度となるように今回の制御周期分の位置指令（制御指令）としてＰＷＭジェネレータ５１〜５７にそれぞれ出力する。ＰＷＭジェネレータ５１〜５７は、その位置指令に基づくＰＷＭ信号をサーボアンプ６１〜６７に出力する。サーボアンプ６１〜６７は、その出力に応じて各関節軸の第１サーボモータ４１〜第７サーボモータ４７を作動させることにより、各リンク１２〜１８を回転させる。なお、速度データの使用は、ＣＰＵ９１は、速度フィードバック制御を行う際に使用しているが、本発明とは関係しないため、説明を省略する。
【００８１】
（Ｓ４００）
ＣＰＵ９１は、Ｓ４００において、各ロータリエンコーダ７１〜７７からの検出信号に基づいて、最終目標位置、及び最終目標姿勢（すなわち、最終の目標位置姿勢）に手先が到達したか否かを判定し、達していないと判定した場合はＳ１０に戻り、反対に作業プログラムの最終の目標位置姿勢に達していると判定した場合は、このフローチャートを終了する。なお、Ｓ１０に戻った場合、作業プログラムに記述されている次の手先位置姿勢、速度データをセットする。
【００８２】
本実施形態では、下記の特徴を有する。
（１）  本実施形態のロボット制御装置では、ロボット制御装置を、作業自由度に対して１つの冗長自由度を備えるマニピュレータ１０Ａ，１０Ｂの関節軸毎に設けられた第１サーボモータ４１〜４７（回転系アクチュエータ）を制御するＣＰＵ９１（精度向上軌跡制御部及び精度向上位置決め制御部）を備えた一対のコントローラーＲＣ１，ＲＣ２としている。そして、コントローラーＲＣ１，ＲＣ２の各ＣＰＵ９１は、作業プログラムの内容に応じて、第１サーボモータ４１〜第７サーボモータ４７（回転系アクチュエータ）に対する制御を切り換える制御切替部として機能する。この結果、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができる。
【００８３】
（２）  本実施形態のロボット制御装置は、手先位置姿勢を目標値とした拘束条件の下で、冗長自由度が許容するリンク位置姿勢を示す姿勢パラメータΦを順次変化させる毎に、各リンクの関節軸の慣性力、遠心力・コリオリ力、重力、摩擦トルク及びアクチュエータ慣性トルクに基づく負荷トルクを算出するＣＰＵ９１（負荷トルク算出部）を備えている。また、前記ＣＰＵ９１は、姿勢パラメータΦの変化の中で、負荷トルクと第１サーボモータ４１〜第７サーボモータ４７（回転系アクチュエータ）の定格トルク比が最小となるリンク位置姿勢を求めるリンク姿勢算出部として機能する。
【００８４】
さらに、ＣＰＵ９１は、手先位置姿勢を目標値とした各関節軸の第１サーボモータ４１〜第７サーボモータ４７の制御指令に、負荷トルクと第１サーボモータ４１〜第７サーボモータ４７の定格トルク比が最小となるときの各負荷トルクとなるフィードフォワード値を付与する付与部として機能する。この結果、本実施形態によれば、冗長自由度を利用して制御性を向上することができ、さらには、第１サーボモータ４１〜第７サーボモータ４７のトルク容量を必要最小限にできるため、第１サーボモータ４１〜第７サーボモータ４７の小型化、ひいてはロボットの小型化を実現できるロボット制御装置を提供できる。
【００８５】
（３）  本実施形態のロボット制御装置は、ＣＰＵ９１（負荷トルク算出部）は、各リンクの関節軸の慣性力と遠心力・コリオリ力と重力の他に摩擦トルクを含む負荷トルクを算出する。この結果、本実施形態のロボット制御装置によれば、負荷トルクに摩擦トルクが含まれることにより、さらに、制御性の精度が高めることができる。
【００８６】
（４）  本実施形態のロボット制御装置のＣＰＵ９１（精度向上位置決め制御部）は、マニピュレータ１０の手先の目標位置姿勢に基づいて、冗長自由度が許容するリンク位置姿勢を複数算出するリンク位置姿勢算出部として機能する。また、ＣＰＵ９１は、リンク位置姿勢毎の手先の誤差拡大率を演算する誤差拡大率演算部として機能するとともに、前記複数の誤差拡大率の中から最小の誤差拡大率を選択する選択部として機能する。さらに、ＣＰＵ９１は、最小の誤差拡大率となるリンク位置姿勢における各関節軸（Ｊ１軸〜Ｊ７軸）の関節角度となるように各関節軸（Ｊ１軸〜Ｊ７軸）の第１サーボモータ４１〜第７サーボモータ４７（回転系アクチュエータ）に位置指令として付与する位置指令付与部として機能する。この結果、本実施形態のロボット制御装置ＲＣ１，ＲＣ２によれば、冗長自由度を利用して、エンドエフェクタ（すなわち、マニピュレータの手先）における目標位置姿勢との誤差の拡大率が最小となるように関節軸を駆動するアクチュエータを制御でき、このために特別なセンサを要することなく、位置決め精度を向上できる。
【００８７】
（５）  本実施形態のロボット制御装置ＲＣ１，ＲＣ２のＣＰＵ９１はリンク位置姿勢算出部として、リンク位置姿勢を算出する際、手先の目標位置姿勢を固定したと仮定して、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータΦを定義して、複数のリンク位置姿勢を取得する際は、姿勢パラメータΦを所定値分変化する毎に、手先の目標位置姿勢と該所定値分変化した姿勢パラメータΦに基づいてリンク位置姿勢を算出する。この結果、本実施形態によれば、冗長自由度により許容されたリンク位置姿勢を示す姿勢パラメータが定義されていることにより、リンク位置姿勢の変更は、所定値の増減だけでできる。
【００８８】
（６）  本実施形態のロボットの制御方法は、作業自由度に対して１つの冗長自由度を備えるマニピュレータ１０Ａ，１０Ｂの関節軸毎に設けられた第１サーボモータ４１〜第７サーボモータ４７（回転系アクチュエータ）を制御するＣＰＵ９１（精度向上軌跡制御部及び精度向上位置決め制御部）を備えたコントローラーＲＣ１，ＲＣ２（ロボット制御装置）によるロボットの制御方法において、コントローラーＲＣ１，ＲＣ２は、作業プログラムの内容に応じて、ＣＰＵ９１の第１サーボモータ４１〜第７サーボモータ４７に対する制御を精度向上軌跡制御部による制御または精度向上位置決め制御部による制御に切り換えるようにしている。この結果、さまざまな作業プログラムに応じて、高速動作においても、低速動作においても、精度を向上した軌跡制御及び位置決め動作ができる。
【００８９】
（第２実施形態）
次に、第２実施形態のロボット制御装置及びロボットの制御方法について説明する。なお、第２実施形態のコントローラーＲＣ１，ＲＣ２のハード構成は、第１実施形態と同一であるため、説明を省略する。第２実施形態では、第１実施形態の図８のＳ２００の処理が図１０に示すように異なっている。すなわち、Ｓ２０４の次にＳ２０６の代わりにＳ２０８Ａの処理を行うことと、Ｓ２１２で「ＹＥＳ」と判定したときに、Ｓ２１３の処理を行った後、Ｓ２１４の処理を行うところが異なっている。
【００９０】
（Ｓ２０８Ａ）
Ｓ２０８Ａでは、ＣＰＵ９１は、Ｓ２０４で算出した各関節角度ｑ１，ｑ２，ｑ３，…，ｑ７と、姿勢パラメータΦを関連付けて記憶部９４に格納する。
【００９１】
（Ｓ２１３）
Ｓ２１３では、ＣＰＵ９１は、記憶部９４に格納した姿勢パラメータΦを順次読み出して、第１実施形態のＳ２０６と同様に、ＣＰＵ９１は、姿勢パラメータΦにおけるｘ，ｙ，ｚ，ａ，ｂ，ｃのそれぞれの方向における、エンドエフェクタ変動／アクチュエータ変動、すなわち、各方向の誤差拡大率Σξを、式（７）〜式（１２）により算出する。
【００９２】
なお、Ｓ２１４において、誤差拡大率の最小を選択する場合、特定の方向のみの誤差拡大率、例えば、ｘ，ｙ，ｚ，ａ，ｂ，ｃのいずれか１つの方向のみの誤差拡大率の最小を選択する場合は、その方向に関する誤差拡大率Σξを、式（７）〜式（１２）のいずれか１つの式を用いて算出すればよい。
【００９３】
また、Ｓ２１４において、ｘ，ｙ，ｚ，ａ，ｂ，ｃのうち、２個〜５個の方向を組み合わせた場合、例えば、ｘ及びｙ方向、ｘ及びｚ方向、ｙ及びｚ方向の２つの方向、或いはいずれか３つの方向、或いは４つの方向、或いは５つの方向における誤差拡大率の総合的な誤差拡大率の場合には、それらの方向に関連した式（７）〜式（１２）を使用して当該方向の誤差拡大率を算出すればよい。
【００９４】
このようにして、各姿勢パラメータΦに関する、各方向（或いは特定の方向）の誤差拡大率を算出した後、Ｓ２１４に移行する。
このように、第２実施形態では、Ｓ２１２において、「ＮＯ」と判定される毎に、Ｓ２０４が複数回実行される。また、Ｓ２１２において、「ＹＥＳ」と判定されると、Ｓ２１３で、各姿勢パラメータの誤差拡大率が複数回実行されることになる。
【００９５】
このように構成した本実施形態では、第１実施形態の（１）〜（６）と同様の効果を奏する。
なお、本発明の実施形態は前記実施形態に限定されるものではなく、下記のように変更しても良い。
【００９６】
・  前記実施形態では、回転系アクチュエータをサーボモータとしてのＡＣモータを使用したが、ＤＣモータでもよく、ステッピングモータ等を使用してもよい。
・前記実施形態では、式（３）において、第４項の摩擦トルク、及び第５項はアクチュエータ慣性トルクを加算するようにしたが、いずれか一方、或いは両方の加算を省略するようにしてもよい。この場合は、前記実施形態よりは制御性が若干劣るが、これらの場合でも、従来技術よりも制御性が向上する。
【００９７】
・  Ｓ２１２において、姿勢パラメータΦの大きな変動を抑制するために、ある姿勢の範囲で決定するようにしてもよい。例えば、初期位置は、上限値Φｍａｘ以下の中で姿勢パラメータΦの全範囲で最小の誤差拡大率であるものを探索するようにし、次からは、例えば、１つ前の位置での姿勢パラメータΦ±ΔΦの範囲内で、最小の誤差拡大率であるものを探索するようにする等、制限を加えるようにしてもよい。
【００９８】
・  前記実施形態では、一対のマニピュレータ１０Ａ，１０ＢのそれぞれのコントローラーＲＣ１，ＲＣ２（ロボット制御装置）の制御方法及びロボット制御装置としたが、単一のロボットの制御方法及び単一のロボット制御装置に具体化することも可能である。例えば、単一のロボットの制御方法にあっては、ロボットは、ツール交換装置を備えたものとし、そのツールの使用目的に応じて軌跡精度の向上が必要な工程では、精度向上軌跡制御部がその工程で行い、位置決め精度の向上が必要な工程では、精度向上位置決め制御が行うようにしてもよい。制御の順序は限定されない。
【００９９】
例えば、精度向上軌跡制御部では、搬送、溶接、またはシーリングを行い、精度向上位置決め制御部では、組み付け、またはスポット溶接を行うことが挙げられる。
・  前記実施形態では、１つの冗長自由度をもつようにしたが、１つの冗長自由度に限定するものではなく、２つ以上の冗長自由度をもつロボット制御装置及び制御方法にも適用できる。
【０１００】
・  前記実施形態では、ツール４９はワークＷ１を把持可能なハンドとしたが、ロボットの作業に応じて他のツールに変更してもよい。
・  前記実施形態では、一対のマニピュレータ１０Ａ，１０ＢのそれぞれのコントローラーＲＣ１，ＲＣ２（ロボット制御装置）の制御方法及びロボット制御装置としたが、３台以上のロボットの制御方法及び３台以上のロボット制御装置に具体化してもよい。この場合も、制御の順序は問わない。
【符号の説明】
【０１０１】
ＲＣ１，ＲＣ２…コントローラー（ロボット制御装置）、
１０Ａ，１０Ｂ…マニピュレータ、１１〜１８…リンク、
４１〜４７…サーボモータ（回転系アクチュエータ）、
９１…ＣＰＵ（精度向上軌跡制御部、リンク姿勢算出部、負荷トルク算出部、付与部、精度向上位置決め制御部、リンク位置姿勢算出部、誤差拡大率演算部、選択部、及び位置指令付与部）、
９４…記憶部。
【図１】
2014124733.tif 000010
【図２】
2014124733.tif 000011
【図３】
2014124733.tif 000012
【図４】
2014124733.tif 000013
【図５】
2014124733.tif 000014
【図６】
2014124733.tif 000015
【図７】
2014124733.tif 000016
【図８】
2014124733.tif 000017
【図９】
2014124733.tif 000018
【図１０】
2014124733.tif 000019