(57)【特許請求の範囲】
【請求項１】
ロボットが、関節と前記関節の関節位置を検出する位置検出部とを有するロボットアームと、前記ロボットアームの先端に取り付けられたエンドエフェクタと、前記エンドエフェクタに作用する力を検出する力検出部と、を有し、
制御部が、前記関節の関節位置指令値を取得して、前記関節の動作を制御する制御方法であって、
前記制御部が、前記位置検出部の位置検出値に基づき前記エンドエフェクタの実位置を求める位置算出工程と、
前記制御部が、前記力検出部の力検出値が力指令値に近づくように前記エンドエフェクタに対する第１位置補正指令値を求める第１補正指令算出工程と、
前記制御部が、前記エンドエフェクタに対する位置指令値を取得し、前記位置指令値を、前記第１位置補正指令値で補正し、補正した当該位置指令値と前記実位置との差分値を用いて、前記エンドエフェクタに対する第２位置補正指令値を求める第２補正指令算出工程と、
前記制御部が、前記第１位置補正指令値を前記第２位置補正指令値で補正し、当該補正結果に基づき前記関節位置指令値を補正する指令値補正工程と、を備えたことを特徴とする制御方法。
【請求項２】
前記指令値補正工程では、
前記制御部が、
前記位置指令値に基づき前記関節に対応する前記関節位置指令値を求め、
前記第１位置補正指令値を前記第２位置補正指令値で補正した補正結果を、前記関節の位置に変換し、前記関節位置指令値を補正することを特徴とする請求項１に記載の制御方法。
【請求項３】
前記第２補正指令算出工程では、前記制御部が、前記ロボットアームの固有振動数以下をカットオフ周波数とするローパスフィルタで前記差分値を補正した値を、前記第２位置補正指令値とすることを特徴とする請求項１又は２に記載の制御方法。
【請求項４】
前記第２補正指令算出工程では、前記制御部が、前記力指令値と前記力検出値との差分に比例する重み係数を、前記差分値に乗算して得られる補正量を用いて前記第２位置補正指令値を求めることを特徴とする請求項１乃至３のいずれか１項に記載の制御方法。
【請求項５】
前記第２補正指令算出工程では、前記制御部が、前記ロボットアームの固有振動数以下をカットオフ周波数とするローパスフィルタで前記補正量を補正した値を、前記第２位置補正指令値とすることを特徴とする請求項４に記載の制御方法。
【請求項６】
前記第２補正指令算出工程では、前記制御部が、前記ロボットアームの固有振動数を、前記力検出値に基づいて算出することを特徴とする請求項３又は５に記載の制御方法。
【請求項７】
前記ロボットアームの姿勢に対応付けた前記カットオフ周波数の値を予め記憶部に記憶させておき、
前記第２補正指令算出工程では、前記制御部が、前記ロボットアームの姿勢に対する前記カットオフ周波数の値を前記記憶部から読み出すことを特徴とする請求項３又は５に記載の制御方法。
【請求項８】
請求項１乃至７のいずれか１項に記載の制御方法により前記ロボットを動作させて物品を製造する物品の製造方法。
【請求項９】
関節と前記関節の関節位置を検出する位置検出部とを有するロボットアームと、
前記ロボットアームの先端に取り付けられたエンドエフェクタと、
前記エンドエフェクタに作用する力を検出する力検出部と、
前記関節の関節位置指令値を取得して、前記関節の動作を制御する制御部と、を備え、
前記制御部は、
前記位置検出部の位置検出値に基づき前記エンドエフェクタの実位置を求める位置算出処理と、
前記力検出部の力検出値が力指令値に近づくように前記エンドエフェクタに対する第１位置補正指令値を求める第１補正指令算出処理と、
前記エンドエフェクタに対する位置指令値を取得し、前記位置指令値を、前記第１位置補正指令値で補正し、補正した当該位置指令値と前記実位置との差分値を用いて、前記エンドエフェクタに対する第２位置補正指令値を求める第２補正指令算出処理と、
前記第１位置補正指令値を前記第２位置補正指令値で補正し、当該補正結果に基づき前記関節位置指令値を補正する指令値補正処理と、を実行することを特徴とするロボット装置。
【請求項１０】
前記制御部は、前記第２補正指令算出処理において、前記ロボットアームの固有振動数以下をカットオフ周波数とするローパスフィルタで前記差分値を補正した値を、前記第２位置補正指令値とすることを特徴とする請求項９に記載のロボット装置。
【請求項１１】
前記制御部は、前記第２補正指令算出処理において、前記力指令値と前記力検出値との差分に比例する重み係数を、前記差分値に乗算して得られる補正量を用いて前記第２位置補正指令値を求めることを特徴とする請求項９又は１０に記載のロボット装置。
【請求項１２】
前記制御部は、前記第２補正指令算出処理において、前記ロボットアームの固有振動数以下をカットオフ周波数とするローパスフィルタで前記補正量を補正した値を、前記第２位置補正指令値とすることを特徴とする請求項１１に記載のロボット装置。
【請求項１３】
前記制御部は、前記第２補正指令算出処理において、前記ロボットアームの固有振動数を、前記力検出値に基づいて算出することを特徴とする請求項１０又は１２に記載のロボット装置。
【請求項１４】
前記ロボットアームの姿勢に対応付けた前記カットオフ周波数の値を予め記憶する記憶部を更に備え、
前記制御部は、前記第２補正指令算出処理において、前記ロボットアームの姿勢に対する前記カットオフ周波数の値を前記記憶部から読み出すことを特徴とする請求項１０又は１２に記載のロボット装置。
【請求項１５】
コンピュータに、請求項１乃至７のいずれか１項に記載の制御方法の各工程を実行させるための制御プログラム。
【請求項１６】
請求項１５に記載のプログラムを記録した、コンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアームの関節の動作を制御する制御方法、物品の製造方法、ロボット装置、制御プログラム及び記録媒体に関する。
【背景技術】
【０００２】
現在、産業分野において、ロボットアームとエンドエフェクタとの間に設けられた力覚センサに基づく力制御により、製品の組立が行われている。精密部品の組立に用いられるロボットアームには、より高精度な力制御性が求められている。
【０００３】
ロボットアームは、関節位置指令値に基づき動作する位置制御方式が主流であり、力制御も位置制御を基に行うことが一般的である。即ち、各関節における駆動部（例えば電動のサーボモータ）を制御する駆動制御部に指令する関節位置指令値を、力検知結果に基づいて補正して、各駆動制御部に出力する。各駆動制御部は、各関節位置を各関節位置指令値に近づけるように位置制御する。これにより、力検知値を目標とする力指令値に近づけながら、エンドエフェクタの位置を制御することができる。
【０００４】
そのため、位置制御系に基づき力制御を行う制御方式では、力制御性を上げるためには位置制御性の向上が必要である。ロボットアームの位置制御性を向上させるために位置制御帯域を高くすればよいが、各関節軸に搭載された減速器の固有振動数が十数Ｈｚと低いため、位置制御帯域を十分高くすることができず、位置制御系の応答性を上げることができない。よって、位置制御系において力制御を行う場合、位置制御系の制御遅れにより、力指令値に対し力検知値がオーバーシュートとアンダーシュートとを繰り返し振動的になる。
【０００５】
特に、精密な部品同士を一定の力で押し付けて組立を行うなど、ワーク同士を接触させる場合、位置制御系の制御遅れによる力制御時の振動がワークへの負荷となり、ワークの変形や破損へとつながるおそれがある。
【０００６】
そこで、力制御時のワークへの負荷を低減する手法として、特許文献１が提案されている。特許文献１では、位置制御系に基づいたロボットアームの力制御において、力指令値と力覚センサの力検知値との差が小さくなるような位置補正指令値を生成する。位置補正指令値は、慣性、粘性、剛性のパラメータからなる二次系のモデルで演算することができる。特許文献１では、製品組立を行う前に、慣性、粘性のパラメータをあるアルゴリズムに従い自動的に変更し、ロボットアームに所定の動作を自動的にさせるパラメータ探索動作をさせ、そのときの力検知値から、最適なパラメータを試行錯誤的に求めている。製品組立中は、予め設定されたパラメータに基づいて力制御する。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特許第４９６２５５１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
特許文献１のような慣性、粘性の各パラメータを変更する制御方法は、一種のローパスフィルタであるため、力指令値と力検知値との差とから生成される位置補正指令値の帯域を調整していることと同義である。つまり、力制御時のオーバーシュートが大きいときは、位置制御系の制御遅れが大きいことが原因であるので、生成される位置補正指令値の帯域が低くなるように、各パラメータが調整される。このようにすることで、位置補正指令値に対するロボットアームの制御遅れが小さくなるので、力制御時のオーバーシュートが小さくなり、ワークへの負荷を抑制することができる。
【０００９】
しかしながら、力制御時のオーバーシュートを抑えるようにすればするほど、生成される位置補正指令値は帯域が低い指令値となり、ロボットアームの動作速度は遅くなり、力検知値が力指令値に達するまでの時間が長くなる。
【００１０】
そこで、本発明は、力制御時の力のオーバーシュートを抑え、かつ、ロボットアームの動作を高速化することを目的とする。
【課題を解決するための手段】
【００１１】
本発明の制御方法は、ロボットが、関節と前記関節の関節位置を検出する位置検出部とを有するロボットアームと、前記ロボットアームの先端に取り付けられたエンドエフェクタと、前記エンドエフェクタに作用する力を検出する力検出部と、を有し、制御部が、前記関節の関節位置指令値を取得して、前記関節の動作を制御する制御方法であって、前記制御部が、前記位置検出部の位置検出値に基づき前記エンドエフェクタの実位置を求める位置算出工程と、前記制御部が、前記力検出部の力検出値が力指令値に近づくように前記エンドエフェクタに対する第１位置補正指令値を求める第１補正指令算出工程と、前記制御部が、前記エンドエフェクタに対する位置指令値を取得し、前記位置指令値を、前記第１位置補正指令値で補正し、補正した当該位置指令値と前記実位置との差分値を用いて、前記エンドエフェクタに対する第２位置補正指令値を求める第２補正指令算出工程と、前記制御部が、前記第１位置補正指令値を前記第２位置補正指令値で補正し、当該補正結果に基づき前記関節位置指令値を補正する指令値補正工程と、を備えたことを特徴とする。
【００１２】
本発明のロボット装置は、関節と前記関節の関節位置を検出する位置検出部とを有するロボットアームと、前記ロボットアームの先端に取り付けられたエンドエフェクタと、前記エンドエフェクタに作用する力を検出する力検出部と、前記関節の関節位置指令値を取得して、前記関節の動作を制御する制御部と、を備え、前記制御部は、前記位置検出部の位置検出値に基づき前記エンドエフェクタの実位置を求める位置算出処理と、前記力検出部の力検出値が力指令値に近づくように前記エンドエフェクタに対する第１位置補正指令値を求める第１補正指令算出処理と、前記エンドエフェクタに対する位置指令値を取得し、前記位置指令値を、前記第１位置補正指令値で補正し、補正した当該位置指令値と前記実位置との差分値を用いて、前記エンドエフェクタに対する第２位置補正指令値を求める第２補正指令算出処理と、前記第１位置補正指令値を前記第２位置補正指令値で補正し、当該補正結果に基づき前記関節位置指令値を補正する指令値補正処理と、を実行することを特徴とする。
【発明の効果】
【００１３】
本発明によれば、位置指令値とエンドエフェクタの実位置との差分値を用いて、この差分値を抑制するような第２位置補正指令値を求め、第２位置補正指令値で力のフィードバック値である第１位置補正指令値を補正している。これにより、位置指令値とエンドエフェクタの実位置との差分値が小さくなるように補正されるため、力制御時の力のオーバーシュートを抑制することができる。また、第１位置補正指令値を第２位置補正指令値で補正することにより、制御帯域を高くすることができるので、ロボットアームの動作の高速化につながる。したがって、ロボットによる組立作業等の作業時に部品にかかる負荷を低減することができ、かつ作業時間を短縮化することができるので、生産性が向上する。
【図面の簡単な説明】
【００１４】
【図１】本発明の実施形態に係るロボット装置を示す概略図である。
【図２】本発明の実施形態に係るロボットアームの関節を示す部分断面図である。
【図３】本発明の実施形態に係るロボット装置の制御装置の構成を示すブロック図である。
【図４】本発明の実施形態に係るロボット装置の制御系の力制御時の制御ブロックを示す図である。
【図５】本発明の実施形態に係るロボット装置の位置制御部の制御ブロックを示す図である。
【図６】本発明の実施形態に係るロボット装置のローパスフィルタ部により帯域を設定するときの処理を示すフローチャートである。
【図７】（ａ）は、本発明の実施形態に係るロボット装置のロボットアームを位置制御モードで力制御開始位置に動作させている途中の状態を示す模式図である。（ｂ）は、本発明の実施形態に係るロボット装置のロボットアームを位置制御モードで力制御開始位置に動作させた状態を示す模式図である。（ｃ）は、本発明の実施形態に係るロボット装置のロボットアームを力制御モードで組立完了位置に動作させている途中の状態を示す模式図である。（ｄ）は、組立品を構成するそれぞれの部品を示す模式図である。
【図８】ローパスフィルタの帯域を求める際に位置制御モードにてロボットアームを動作させた際の位置指令値と力検知値の結果を示すグラフである。
【図９】（ａ）は、本発明の実施形態のロボット装置における組立完了位置付近でのＺ軸方向の力検知値の結果を示すグラフである。（ｂ）は、比較例のロボット装置における組立完了位置付近でのＺ軸方向の力検知値の結果を示すグラフである。
【図１０】（ａ）〜（ｄ）は、本発明の実施形態に係るロボット装置の制御系におけるフィードフォワード指令算出部の変形例を示す図である。
【発明を実施するための形態】
【００１５】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。図１は、本発明の実施形態に係るロボット装置を示す概略図である。ロボット装置５００は、ロボット１００と、ロボット１００を制御する制御装置２００と、を備えている。
【００１６】
ロボット１００は、垂直多関節のロボットアーム１０１と、エンドエフェクタとしてのロボットハンド１０２と、力検知部である力覚センサ１０３と、を有している。ロボットハンド１０２は、力覚センサ１０３を介してロボットアーム１０１の先端に取り付けられている。即ち、力覚センサ１０３は、ロボットアーム１０１とロボットハンド１０２との間に配置されている。
【００１７】
ロボットアーム１０１は、架台の上面に固定されたベース部（基端リンク）１１０と、変位や力を伝達する複数のリンク１１１〜１１６とが複数の関節Ｊ１〜Ｊ６で屈曲（旋回）又は回転可能に連結されている。本実施形態では、ロボットアーム１０１は、６軸の関節Ｊ１〜Ｊ６を有している。ロボットアーム１０１の基端が固定端であり、ロボットアーム１０１の先端が自由端である。したがって、ロボットアーム１０１の複数の関節Ｊ１〜Ｊ６が旋回又は回転動作することにより、ロボットハンド１０２の位置及び姿勢を変更することができる。
【００１８】
ロボットハンド１０２は、複数のフィンガー（爪）と、複数のフィンガーを駆動させる不図示の駆動装置とを有している。駆動装置が複数のフィンガーを閉動作させることにより、部品Ｗ１を把持することができ、複数のフィンガーを開動作させることにより、部品Ｗ１を把持解放することができる。ロボットハンド１０２は、複数のフィンガーを用いて部品Ｗ１を把持することにより、部品Ｗ１を別の部品Ｗ２に嵌合する嵌合作業を行い、組み立てることにより、物品（組立品）を製造することができる。また、ロボットハンド１０２は、組立時に用いるツール等を把持することができる。
【００１９】
力覚センサ１０３は、互いに直交する３軸それぞれの方向の荷重Ｆｘ，Ｆｙ，Ｆｚと、各軸まわりのモーメントＭｘ，Ｍｙ，Ｍｚと、を検知可能である。以下、荷重及びモーメントを「力」という。即ち、力覚センサ１０３は、ロボットハンド１０２に作用する力を検知する。力覚センサ１０３の力検知値（力計測値）は、制御装置２００に、所定の周期で送信される。
【００２０】
ロボットアーム１０１は、各関節Ｊ１〜Ｊ６に配置され、各関節Ｊ１〜Ｊ６をそれぞれ駆動する複数（６つ）の駆動装置（駆動部）１３０を有している。
【００２１】
図２は、ロボットアーム１０１の関節Ｊ２を示す部分断面図である。以下、関節Ｊ２を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６については、同様の構成であるため、説明を省略する。
【００２２】
駆動装置１３０は、電動モータである回転モータ（以下、「モータ」という）１３１と、モータ１３１の回転軸１３２の回転を減速する減速機１３３と、を有している。関節Ｊ２には、当該関節Ｊ２の関節角度（関節位置）を検知する角度検知部（位置検知部）であるロータリーエンコーダ（以下、「エンコーダ」という）１３５が配置されている。ここで、「関節角度を検知する」とは、関節角度そのものの角度を検知する場合と、関節角度に対応するモータ角度を検知する場合とがあり、本実施形態では、関節角度に対応するモータ角度を検知することを意味する。
【００２３】
本実施形態のエンコーダ１３５は、モータ１３１の回転軸１３２（減速機１３３の入力軸）の回転角度を検知する。減速機１３３の減速比をＮ（例えば５０）とすると、エンコーダ１３５は、関節角度に対応する角度、本実施形態では関節角度のＮ倍の角度を検知する。したがって、基準角度に対する関節Ｊ２の回転角度が１［°］であれば、エンコーダ１３５により検知されるモータ１３１の回転角度は５０［°］となる。よって、モータ１３１の回転軸１３２の回転角度は、関節そのものの角度とは倍率が異なるだけで、関節そのものの角度と等価であり、以下、関節角度とは、モータ１３１の回転角度のことをいう。
【００２４】
なお、本実施形態のロボットアーム１０１の各関節は、旋回又は回転する関節であるため、関節位置とは、関節角度（旋回又は回転角度）のことであるが、直動する関節の場合は、関節位置（直動位置）ということになる。直動関節の場合も、位置検知部は、関節位置を検知することとなる。
【００２５】
モータ１３１は、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。エンコーダ１３５は、アブソリュート型のロータリーエンコーダが望ましく、１回転の絶対角度エンコーダ、絶対角度エンコーダの回転総数のカウンタ、及びカウンタに電力を供給するバックアップ電池を有して構成される。ロボットアーム１０１への電源の供給がオフになっても、このバックアップ電池が有効であれば、ロボットアーム１０１への電源供給のオン／オフに関係なく、カウンタにおいて回転総数が保持される。したがって、ロボットアーム１０１の姿勢が制御可能となる。なお、エンコーダ１３５は、回転軸１３２に取り付けられているが、減速機１３３の入力軸に取り付けてもよい。また、リンク１１１とリンク１１２とは、クロスローラベアリング１３７を介して回転自在に結合されている。
【００２６】
モータ１３１は、モータカバー１３８で覆われて保護されている。モータ１３１とエンコーダ１３５との間には、不図示のブレーキユニットが設けられている。ブレーキユニットの主な機能は、電源オフ時のロボットアーム１０１の姿勢の保持である。
【００２７】
減速機１３３は、本実施形態では、小型軽量で減速比の大きい波動歯車減速機である。減速機１３３は、モータ１３１の回転軸１３２に結合された、入力軸であるウェブジェネレータ１４１と、リンク１１２に固定された、出力軸であるサーキュラスプライン１４２と、を備えている。なお、サーキュラスプライン１４２は、リンク１１２に直結されているが、リンク１１２に一体に形成されていてもよい。
【００２８】
また、減速機１３３は、ウェブジェネレータ１４１とサーキュラスプライン１４２との間に配置され、リンク１１１に固定されたフレクスプライン１４３を備えている。フレクスプライン１４３は、ウェブジェネレータ１４１の回転に対して減速比Ｎで減速され、サーキュラスプライン１４２に対して相対的に回転する。従って、モータ１３１の回転軸１３２の回転は、減速機１３３で１／Ｎの減速比で減速されて、フレクスプライン１４３が固定されたリンク１１１に対してサーキュラスプライン１４２が固定されたリンク１１２を相対的に回転運動させ、関節Ｊ２を屈曲させる。
【００２９】
制御装置２００は、６つのモータ１３１のそれぞれを選択的に駆動制御することで、ロボット１００の先端（ロボットハンド１０２）の位置及び姿勢を任意に移動させることができる。
【００３０】
図３は、ロボット装置５００の制御装置２００の構成を示すブロック図である。制御装置２００は、メイン制御装置２３０と、複数（関節の数に対応した数：本実施形態では６つ）の関節制御装置２４０と、を有する。各関節制御装置２４０は、フィードバック制御により各関節の位置制御を行う。また、メイン制御装置２３０は、力覚センサ１０３の力検知値から、力覚センサ１０３にかかる力が、予め設定された所望の値（力指令値）になるようにフィードバック制御を行う。
【００３１】
メイン制御装置２３０は、コンピュータで構成されており、制御部としてのＣＰＵ（Central  Processing  Unit）２０１を備えている。また、メイン制御装置２３０は、記憶部として、ＲＯＭ（Read  Only  Memory）２０２、ＲＡＭ（Random  Access  Memory）２０３、ＨＤＤ（Hard  Disk  Drive）２０４を備えている。また、メイン制御装置２３０は、記録ディスクドライブ２０５、インタフェース２１３及び検知回路２１４を備えている。
【００３２】
ＣＰＵ２０１には、ＲＯＭ２０２、ＲＡＭ２０３、ＨＤＤ２０４、記録ディスクドライブ２０５、インタフェース２１３及び検知回路２１４が、バスを介して接続されている。ＲＯＭ２０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ２０３は、ＣＰＵ２０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００３３】
ＨＤＤ２０４は、ＣＰＵ２０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ２０１に、後述する演算処理を実行させるためのプログラム２２０を記録するものである。ＣＰＵ２０１は、ＨＤＤ２０４に記録（格納）されたプログラム２２０に基づいてロボット制御方法の各工程を実行する。
【００３４】
記録ディスクドライブ２０５は、記録ディスク２２１に記録された各種データやプログラム等を読み出すことができる。なお、メイン制御装置２３０には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の不図示の外部記憶装置が接続されていてもよい。
【００３５】
力覚センサ１０３は、検知回路２１４に接続されている。検知回路２１４は、力覚センサ１０３により検知された力検知値を取得する。検知回路２１４は、力検知値を示す信号を、ＣＰＵ２０１にて取得可能な信号に変換してＣＰＵ２０１に出力する。
【００３６】
インタフェース２１３には、関節制御装置２４０が接続されている。なお、本実施形態では、ロボットアーム１０１が６つの関節Ｊ１〜Ｊ６を有しているので、制御装置２００は、６つの関節制御装置２４０を有するが、図３では、関節制御装置２４０を１つだけ図示し、残りの５つは図示を省略している。各関節制御装置２４０は、制御装置２００の筐体内に配置されている。なお、各関節制御装置２４０の配置位置は、筐体内に限定するものではなく、例えばロボットアーム１０１に配置されていてもよい。
【００３７】
ＣＰＵ２０１は、教示点のデータ及びロボットプログラムに基づきロボットアーム１０１の軌道（ロボット１００の手先、即ちロボットハンド１０２の位置指令値）を計算する。
【００３８】
ＣＰＵ２０１は、位置制御モードと、力制御モードとに切り替えられる。これら制御モードのうち、いずれの制御モードでロボットアーム１０１を動作させるかは、例えばロボットプログラムに記述されている。ＣＰＵ２０１は、位置制御モード時は、ＨＤＤ２０４等の記憶部に予め設定（記憶）されている軌道データに従ってロボットアーム１０１の動作を制御する。ＣＰＵ２０１は、力制御モード時には、力覚センサ１０３の検知結果に基づき軌道データを補正して、ロボットアーム１０１の動作を制御する。ＣＰＵ２０１は、これら制御モードのいずれかに設定された場合、軌道データ（位置指令値）から各関節Ｊ１〜Ｊ６の関節角度指令値（関節位置指令値）を計算し、各関節角度指令値の信号を、所定時間間隔で各関節制御装置２４０に出力する。これにより、ＣＰＵ２０１は、ロボットアーム１０１の動作を制御する。
【００３９】
ここで、本実施形態ではエンコーダ１３５は、モータ１３１の回転軸１３２の回転角度（回転位置）を検知するので、関節角度指令値は、モータ１３１の回転軸１３２の回転角度に対する角度指令値ということになる。よって、以下、関節の関節角度指令値とは、モータ１３１の角度指令値のことをいう。
【００４０】
なお、本実施形態では、関節位置指令値を関節角度指令値と名称を変えているが、ロボットアーム１０１の各関節が旋回又は回転する関節であるためであり、関節角度指令値は、関節位置指令値のことである。なお、直動する関節の場合は、そのまま関節位置指令値ということになる。
【００４１】
また、図示は省略するが、ロボットハンド１０２の駆動装置に対してもハンド制御装置が設けられており、ＣＰＵ２０１は、ハンド制御装置に指令することで、ロボットハンド１０２の動作を制御する。
【００４２】
関節制御装置２４０は、プリント回路板で構成されている。関節制御装置２４０は、ＣＰＵ２５１、記憶部としてのＥＥＰＲＯＭ２５２及びＲＡＭ２５３、インタフェース２６１、検知回路２６２，２６３並びにＰＷＭ生成器２６５を備えており、これらがバスを介して接続されて構成されている。
【００４３】
ＣＰＵ２５１は、プログラム２７０に従って演算処理を実行する。ＥＥＰＲＯＭ２５２は、プログラム２７０を記憶する記憶装置である。ＲＡＭ２５３は、ＣＰＵ２５１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００４４】
メイン制御装置２３０は、複数（６つ）のインタフェース２１３（図３では１つのみ図示）を有している。各インタフェース２１３と各関節制御装置２４０のインタフェース２６１とがケーブル等で接続されており、メイン制御装置２３０と各関節制御装置２４０との間で信号の送受信を行うことができる。
【００４５】
エンコーダ１３５は、検知回路２６２に接続されている。エンコーダ１３５からは、検知した角度検知値を示すパルス信号が出力される。検知回路２６２は、エンコーダ１３５からパルス信号を取得し、ＣＰＵ２５１にて取得可能な信号に変換してＣＰＵ２５１に出力する。ＣＰＵ２５１は、これらエンコーダ１３５の検知結果をＣＰＵ２０１に出力する。
【００４６】
電流センサ１３６は、検知回路２６３に接続され、モータ１３１に供給される電流を検知する。検知回路２６３は、電流センサ１３６により検知された電流の電流値を示す信号を取得し、ＣＰＵ２５１にて取得可能な信号に変換してＣＰＵ２５１に出力する。
【００４７】
ＰＷＭ生成器２６５は、例えば半導体スイッチング素子を有するモータドライバであり、入力した電流指令値に応じて、パルス幅変調された３相交流のＰＷＭ波形の電圧をモータ１３１に出力することで、モータ１３１に電流を供給する。
【００４８】
各関節制御装置２４０のＣＰＵ２５１は、メイン制御装置２３０のＣＰＵ２０１から入力を受けた関節角度指令値に関節角度が近づくように、モータ１３１への電流の出力量（電流指令）を演算し、電流指令をＰＷＭ生成器２６５に出力する。
【００４９】
ＰＷＭ生成器２６５は、入力を受けた電流指令に対応する電流をモータ１３１に供給する。そして、モータ１３１は、ＰＷＭ生成器２６５から電力供給を受けて駆動トルクを発生し、減速機１３３の入力軸であるウェブジェネレータ１４１にトルクを伝達する。減速機１３３において、出力軸であるサーキュラスプライン１４２は、ウェブジェネレータ１４１の回転に対して１／Ｎの回転数で回転する。これにより、リンク１１２がリンク１１１に対して相対的に回転する。
【００５０】
このように、各関節制御装置２４０は、関節角度がメイン制御装置２３０から入力を受けた関節角度指令値に近づくようにモータ１３１に電流を供給して、各関節Ｊ１〜Ｊ６の関節角度を制御する。
【００５１】
なお、本実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ２０４であり、ＨＤＤ２０４にプログラム２２０が格納される場合について説明するが、これに限定するものではない。プログラム２２０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム２２０を供給するための記録媒体としては、図３に示す記録ディスク２２１、不図示の外部記憶装置等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＲＯＭ等を用いることができる。
【００５２】
図４は、本発明の実施形態に係るロボット装置の制御系の力制御時の制御ブロックを示す図である。図５は、本発明の実施形態に係るロボット装置の位置制御部の制御ブロックを示す図である。
【００５３】
メイン制御装置２３０のＣＰＵ２０１は、プログラム２２０を読み出して実行することにより、図４に示すフィードフォワード指令算出部１０，力制御量演算部２０，逆キネマティクス演算部３０，加算部４０，キネマティクス演算部５０として機能する。また、各関節制御装置２４０のＣＰＵ２５１は、プログラム２７０を読み出して実行することにより、図５に示す減算器６１、位置制御器６２、速度変換器６３、減算器６４、速度制御器６５、減算器６６及び電流制御器６７として機能する。これら各器６１〜６７、ＰＷＭ生成器２６５、電流センサ１３６、モータ１３１及びエンコーダ１３５で、位置制御部６０が構成されている。
【００５４】
メイン制御装置２３０のＣＰＵ２０１は、各関節Ｊ１〜Ｊ６の関節角度の目標値である関節角度指令値θｉ＊（ｉ＝１〜６）を求め、各関節制御装置２４０のＣＰＵ２５１に、各関節角度指令値θｉ＊を出力する。位置制御部６０は、関節角度指令値θｉ＊を入力し、ロボットアーム１０１の各関節Ｊ１〜Ｊ６の位置制御（角度制御）を行う。なお、位置制御部６０は、関節が６つあるので、６つあるが、図４では、位置制御部６０を代表して１つ図示だけ図示し、残りの５つは図示を省略している。
【００５５】
まず、図５に示す位置制御部６０について詳細に説明する。位置制御部６０は、一般にカスケード制御と呼ばれている制御方式をとっており、位置制御器６２、速度制御器６５、電流制御器６７を直列に接続した構成となっている。カスケード制御方式の特徴として、各制御器の制御周期を個別の設定できるという特徴がある。本実施形態において、各制御器の制御周期は、電流制御器６７＜速度制御器６５＜位置制御器６２となるように設定されている。
【００５６】
減算器６１は、関節Ｊｉの関節角度指令値（モータ１３１の角度指令値）θｉ＊とエンコーダ１３５により検知された角度検知値θｉとの差分値（θｉ＊−θｉ）、つまり角度偏差を求める。
【００５７】
位置制御器６２は、差分値（θｉ＊−θｉ）に対し、比例や積分や微分演算を行うＰＩＤ制御などの一般的なフィードバック制御系を構成している。本実施形態では、位置制御器６２は、比例演算のみを行うＰ制御とし、速度指令値ωｉ＊を出力する。
【００５８】
速度変換器６３は、角度値θｉを時間で微分してモータ１３１の速度値ωｉを算出する。減算器６４は、速度指令値ωｉ＊と速度値ωｉとの差分値（ωｉ＊−ωｉ）、つまり速度偏差を求める。速度制御器６５は、差分値（ωｉ＊−ωｉ）に対し、ＰＩＤ制御などを行う一般的なフィードバック制御系を構成している。本実施形態では、速度制御器６５は、比例と積分演算を行うＰＩ制御とし、電流指令値Ｉｉ＊を出力する。
【００５９】
電流センサ１３６は、モータ１３１に供給されている電流、即ちＰＷＭ生成器２６５の出力電流の電流値Ｉｉを検知する。減算器６６は、電流指令値Ｉｉ＊と電流値Ｉｉとの差分値（Ｉｉ＊−Ｉｉ）、つまり電流偏差を求める。電流制御器６７は、差分値（Ｉｉ＊−Ｉｉ）に対し、ＰＩＤ制御などを行う一般的なフィードバック制御系を構成している。本実施形態では、電流制御器６７は、比例、積分、微分演算を行うＰＩＤ制御を行う。
【００６０】
なお、位置制御器６２、速度制御器６５、電流制御器６７のＰＩＤ制御などの演算に用いられる各制御定数は、制御帯域や制御安定性、各軸のモータ１３１に接続される機械負荷の固有振動等を考慮した値に個別に設定される。
【００６１】
以上、各関節制御装置２４０のＣＰＵ２５１は、メイン制御装置２３０のＣＰＵ２０１から指令値θ１＊〜θ６＊を入力し、モータ１３１の回転角度θ１〜θ６が指令値θ１＊〜θ６＊に近づくように各関節Ｊ１〜Ｊ６のモータ１３１の回転角度を制御する。このように、メイン制御装置２３０のＣＰＵ２０１は、各関節制御装置２４０のＣＰＵ２５１に関節角度指令値θ１＊〜θ６＊を出力する（指令する）ことで、各関節Ｊ１〜Ｊ６の動作（関節角度）を制御することとなる。
【００６２】
次に、メイン制御装置２３０における制御動作について詳細に説明する。図４に示す力制御量演算部２０は、減算部２１、力制御部２２、加算部２３及びヤコビ行列演算部２４からなる。フィードフォワード指令算出部１０は、差分値演算部１１、重み係数演算部１２、乗算部１３及びローパスフィルタ部１４からなる。重み係数演算部１２は、減算部１５と、比例係数乗算部１６とからなる。
【００６３】
ここで、本実施形態では、ロボットハンド１０２の位置に対する位置指令値（軌道データ）Ｐ０＊及びロボットハンド１０２に作用する力に対する力指令値Ｆ０＊は、ＨＤＤ２０４等の記憶部に予め記憶（設定）されている。ＣＰＵ２０１は、位置指令値Ｐ０＊及び力指令値Ｆ０＊をＨＤＤ２０４から読み出して取得する。図４の制御ブロックでは、差分値演算部１１及び逆キネマティクス演算部３０が位置指令値Ｐ０＊を取得し、減算部１５及び減算部２１が力指令値Ｆ０＊を取得することとなる。
【００６４】
キネマティクス演算部５０は、各関節Ｊ１〜Ｊ６のエンコーダ１３５により検知された角度検知値θ１〜θ６に基づき、ロボットハンド１０２の位置（実位置）Ｐ０を、順運動学計算により求める（位置算出工程、位置算出処理）。
【００６５】
力制御量演算部２０は、力指令値Ｆ０＊と力覚センサ１０３により検知された力検知値Ｆ０との差（Ｆ０＊−Ｆ０）を低減するフィードバック制御によるロボットハンド１０２の位置に対する位置補正指令値Ｐ１＊を演算する。
【００６６】
フィードフォワード指令算出部１０は、位置指令値Ｐ０＊を位置補正指令値Ｐ１＊で補正した位置指令値（Ｐ０＊＋Ｐ１＊）と、実位置Ｐ０との差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）を用いて、フィードフォワード指令値Ｐ３＊を算出する。
【００６７】
力制御量演算部２０は、位置補正指令値Ｐ１＊をフィードフォワード指令値Ｐ３＊で補正して、位置補正値（Ｐ１＊＋Ｐ３＊）を求め、この補正結果（Ｐ１＊＋Ｐ３＊）を、各関節Ｊ１〜Ｊ６の角度（位置）に変換した関節補正値θ１ｉ＊を求める。
【００６８】
逆キネマティクス演算部３０は、ロボットアーム１０１の位置指令値Ｐ０＊から逆運動学計算により、各関節の角度（位置）に対する関節指令値θ０ｉ＊を求める。
【００６９】
加算部４０は、各関節指令値θ０ｉ＊を各関節補正値θ１ｉ＊で補正、具体的には、関節指令値θ０ｉ＊に関節補正値θ１ｉ＊を加算する補正を行い、各関節角度指令値θｉ＊を求める（関節位置指令算出工程、関節位置指令算出処理）。位置制御部６０は、各関節角度指令値θｉ＊に従って各関節の角度を制御する。
【００７０】
力制御量演算部２０について詳細に説明する。まず、減算部２１は、力指令値Ｆ０＊と力検知値Ｆ０との差分、具体的には、力指令値Ｆ０＊から力検知値Ｆ０を減算することで、差分（Ｆ０＊−Ｆ０）、つまり力偏差を求める。
【００７１】
力制御部２２は、差分（Ｆ０＊−Ｆ０）が小さくなるように、即ち力検知値Ｆ０が力指令値Ｆ０＊に近づくように、フィードバック制御によりロボットハンド１０２の位置に対する位置補正指令値Ｐ１＊を求める（補正指令算出工程、補正指令算出処理）。この力制御部２２は、ロボットハンド１０２に力が作用した際のロボットアーム１０１の位置の応答特性を決めるブロックである。力制御部２２は、慣性（Ｍ）、粘性（Ｃ）、剛性（Ｋ）からなる二次系の伝達関数でモデル化することができる。慣性（Ｍ）、粘性（Ｃ）、剛性（Ｋ）のパラメータは、力検知値Ｆ０が所望の応答特性となるように調整される。力制御部２２におけるパラメータの値は、ユーザが設定してもよいが、自動で適した値を探索して設定してもよい。
【００７２】
加算部２３は、フィードフォワード指令算出部１０にて算出されたフィードフォワード指令値Ｐ３＊で位置補正指令値（フィードバック指令値）Ｐ１＊を補正する。具体的には、加算部２３は、位置補正指令値Ｐ１＊にフィードフォワード指令値Ｐ３＊を加算する補正を行う。
【００７３】
ヤコビ行列演算部２４は、加算部２３にて補正された補正結果（Ｐ１＊＋Ｐ３＊）を各関節の角度に変換して、関節補正値θ１ｉ＊を求める（関節補正値算出工程、関節補正値算出処理）。具体的には、ヤコビ行列演算部２４は、ヤコビ行列を用いて、関節補正値θ１ｉ＊を求める。
【００７４】
本実施形態では、位置補正指令値Ｐ１＊は、力指令値Ｆ０＊に対する力検知値Ｆ０のずれを解消するのに必要な位置指令値Ｐ０＊のフィードバック制御量に相当する。この位置補正指令値Ｐ１＊を、指令値（Ｐ０＊＋Ｐ１＊）に対する実位置Ｐ０のずれ量に応じたフィードフォワード指令値Ｐ３＊で補正している。このように、位置補正指令値Ｐ１＊をフィードフォワード指令値Ｐ３＊で補正しているので、力制御を行っているときの力のオーバーシュートを抑制できる。更に、位置補正指令値Ｐ１＊をフィードフォワード指令値Ｐ３＊で補正することで、帯域が高くなり、ロボットアーム１０１の動作速度を高速化することができる。
【００７５】
以下、フィードフォワード指令算出部１０におけるフィードフォワード指令算出処理（フィードフォワード指令算出工程）について詳細に説明する。
【００７６】
差分値演算部（位置偏差演算部）１１は、位置指令値Ｐ０＊を位置補正指令値Ｐ１＊で補正した位置指令値（Ｐ０＊＋Ｐ１＊）と実位置Ｐ０との差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）、つまり位置偏差を求める。本実施形態では（Ｐ０＊＋Ｐ１＊）がロボット１００への指令値になるため、差分値にＰ１＊分のオフセットが発生するのを抑制している。差分値演算部１１の演算により、ロボット１００の手先位置、即ちロボットハンド１０２の位置における指令値と実位置とのずれ量である差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）が求まる。
【００７７】
重み係数演算部１２は、力指令値Ｆ０＊と力検知値Ｆ０との差分に比例する重み係数を演算する。詳細に説明すると、重み係数演算部１２の減算部１５は、力指令値Ｆ０＊と力検知値Ｆ０との差分として、力指令値Ｆ０＊から力検知値Ｆ０を減算した値（Ｆ０＊−Ｆ０）、つまり力偏差を求める。重み係数演算部１２の比例係数乗算部１６は、差分値（Ｆ０＊−Ｆ０）に比例係数Ｖを乗算することで、重み係数Ｇ＝（Ｆ０＊−Ｆ０）×Ｖを求める。ここで、比例係数Ｖは正数の定数である。重み係数Ｇの値は、力差分値（Ｆ０＊−Ｆ０）が大きくなるほど大きくなる。逆に、重み係数Ｇの値は、力差分値（Ｆ０＊−Ｆ０）が小さくなるほど小さくなる。
【００７８】
乗算部１３は、重み係数Ｇを差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）に乗算する。その乗算結果を位置補正量Ｐ２＊とする。
【００７９】
ローパスフィルタ部１４は、ロボットアーム１０１の動作姿勢に応じて制御帯域を変える可変帯域ローパスフィルタである。このローパスフィルタは、デジタルローパスフィルタである。ローパスフィルタ部１４は、ロボットアーム１０１の固有振動数以下をカットオフ周波数とするローパスフィルタで位置補正量Ｐ２＊を補正した値を、フィードフォワード指令値Ｐ３＊とする。本実施形態では、ローパスフィルタ部１４は、位置指令値Ｐ０＊と力制御直前の力覚センサ１０３の力検知値Ｆ０とから、ロボットアーム１０１の固有振動数の演算を行い、ローパスフィルタの帯域（即ち、カットオフ周波数）を決定する。ローパスフィルタ部１４は、位置補正量Ｐ２＊に対してフィルタ処理を行い、高域をカットしたフィードフォワード指令値Ｐ３＊を生成する。
【００８０】
ロボットアーム１０１の姿勢に応じて各関節Ｊ１〜Ｊ６にかかる負荷が変わるため、ロボットアーム１０１の姿勢に応じてロボットアーム１０１の固有振動数が変化する。ロボットアーム１０１の各関節Ｊ１〜Ｊ６に配置された減速機１３３の制御帯域は、十数［Ｈｚ］程度である。位置補正量Ｐ２＊によっては、ロボットアーム１０１が加振し、動作が振動的になる可能性がある。そのため、本実施形態では、ローパスフィルタを用いて、位置補正量Ｐ２＊に対してフィルタ処理を施し、フィードフォワード指令値Ｐ３＊を生成している。これにより、ロボットアーム１０１の振動を抑制することができる。
【００８１】
また、フィードフォワード指令値Ｐ３＊による補正効果を最大限得るためには、ローパスフィルタの帯域の上限値（カットオフ周波数）をできるだけ高く設定する必要がある。本実施形態では、カットオフ周波数を、ロボットアーム１０１の固有振動数に設定している。このカットオフ周波数は、ロボットアーム１０１の固有振動数に応じて、即ちロボットアーム１０１の姿勢に応じて設定される。
【００８２】
ローパスフィルタ部１４の帯域設定手順について詳細に説明する。図６は、本発明の実施形態に係るロボット装置のローパスフィルタ部により帯域を設定するときの処理を示すフローチャートである。ローパスフィルタの帯域の設定は、力制御を開始する前に行う。例えば、力制御を開始する直前の位置である力制御開始位置にロボットアーム１０１を移動させた際に、ローパスフィルタの帯域の設定を行う。ロボットアーム１０１の力制御直前の姿勢は力制御時の姿勢と大きく変わらないため、適した帯域を設定することが可能となる。
【００８３】
ローパスフィルタ部１４は、ロボットアーム１０１の姿勢が力制御を行う姿勢に近い状態での位置制御時の位置指令値Ｐ０＊から、ロボット１００の手先、即ちロボットハンド１０２の加速度を算出し、加速度の周波数特性を求める（Ｓ１）。
【００８４】
また、ローパスフィルタ部１４は、前記位置制御時に、力覚センサ１０３により検知された力検知値Ｆ０の加速度成分の周波数特性を求める（Ｓ２）。
【００８５】
次に、ローパスフィルタ部１４は、ステップＳ１で求めた加速度の周波数成分とステップＳ２で求めた加速度の周波数成分との差分をとり、差分をとった周波数特性からロボットアーム１０１の固有振動数を算出する（Ｓ３）。
【００８６】
次に、ローパスフィルタ部１４は、ステップＳ３で求めた固有振動数の値以下の値、本実施形態では、固有振動数の値をローパスフィルタの制御帯域の上限値（カットオフ周波数）として設定する（Ｓ４）。
【００８７】
なお、ロボットアーム１０１の姿勢（即ち、位置指令値）に対応付けたカットオフ周波数の値を予めＨＤＤ２０４等の記憶部に記憶させておいてもよい。そして、ローパスフィルタ部１４は、ロボットアーム１０１の姿勢（即ち、位置指令値）に対するカットオフ周波数の値をＨＤＤ２０４から読み出すようにしてもよい。例えば、ＨＤＤ２０４には、ロボットアーム１０１の姿勢とカットオフ周波数の値とを対応付けたテーブルデータを記憶させておけばよい。このようにすることで、力制御開始前のローパスフィルタの帯域を決定するための演算処理を省略することができる。
【００８８】
図７は、本発明の実施形態に係るロボット装置による組立品の製造方法を説明するための模式図である。図７（ａ）は、本発明の実施形態に係るロボット装置のロボットアームを位置制御モードで力制御開始位置に動作させている途中の状態を示す模式図である。図７（ｂ）は、本発明の実施形態に係るロボット装置のロボットアームを位置制御モードで力制御開始位置に動作させた状態を示す模式図である。図７（ｃ）は、本発明の実施形態に係るロボット装置のロボットアームを力制御モードで組立完了位置に動作させている途中の状態を示す模式図である。図７（ｄ）は、組立品を構成するそれぞれの部品を示す模式図である。
【００８９】
ロボットハンド１０２により把持された組立ワーク（部品）Ｗ１を、架台５２０に固定された被組立ワーク（部品）Ｗ２に力制御を用いて組み付ける。図７（ｄ）に示すように、組立ワークＷ１は円筒状の部材であり、被組立ワークＷ２は組立ワークＷ１と嵌合される凸形状部を有している。組立ワークＷ１を被組立ワークＷ２に嵌合した際、Ｚ軸方向に５［Ｎ］で押し付けて組立完了となる。
【００９０】
まず、ＣＰＵ２０１は、位置制御モードにて、図７（ａ）の状態から図７（ｂ）の状態へ、ロボットアーム１０１を動作させる。次に、ＣＰＵ２０１は、力制御モードにて、図７（ｂ）の状態から図７（ｃ）の状態へ、ロボットアーム１０１を動作させる。図７（ｃ）において、組立完了位置付近で、組立ワークＷ１と被組立ワークＷ２とがＺ軸方向に接触している。
【００９１】
本実施形態では、ＣＰＵ２０１は、図７（ａ）の状態から図７（ｂ）の状態へロボットアーム１０１を位置制御にて動作させている。このときに、ＣＰＵ２０１は、力覚センサ１０３から力検知値Ｆ０を取得して、位置指令値Ｐ０＊と力検知値Ｆ０を用いて、図６のフローチャートに従ってローパスフィルタの制御帯域（カットオフ周波数）を求める。
【００９２】
図８は、ローパスフィルタの帯域を求める際に位置制御モードにてロボットアーム１０１を動作させた際の位置指令値Ｐ０＊と力検知値Ｆ０の結果を示すグラフである。図８に示す結果から、ロボットアーム１０１の固有振動数は１８．５［Ｈｚ］と求まった。したがって、本実施形態では、ローパスフィルタの制御帯域を１８．５［Ｈｚ］とした。このように、ロボットアーム１０１の姿勢により各関節にかかる負荷が変わり、ロボットアーム１０１の固有振動数が姿勢により変化するため、姿勢に応じてローパスフィルタの帯域を設定することは有効である。
【００９３】
図９（ａ）は、本発明の実施形態のロボット装置における組立完了位置付近でのＺ軸方向の力検知値Ｆ０の結果を示すグラフである。本実施形態のロボット装置５００では、比例係数Ｖを２５とした。図９（ｂ）は、比較例のロボット装置における組立完了位置付近でのＺ軸方向の力検知値Ｆ０の結果を示すグラフである。比較例のロボット装置では、比例係数Ｖをゼロ、つまりフィードフォワード指令値Ｐ３＊をゼロとして力制御を行った。
【００９４】
本実施形態では、フィードフォワード指令値Ｐ３＊を力制御部２２の演算結果Ｐ１＊に加算し、位置制御部６０の制御遅れを補正している。このため、図９（ａ）に示すように、図９（ｂ）と比較して、力指令値Ｆ０＊に対する力検知値Ｆ０のオーバーシュートが抑制されている。よって、力制御時の組立ワークＷ１に対する負荷を低減することができる。また、比例係数Ｖを大きくすると、補正効果が大きくなるので、力制御時の立ち上がりがよくなる。
【００９５】
また、組立ワークＷ１と被組立ワークＷ２とがＺ軸方向に接触した直後は、力差分（Ｆ０＊−Ｆ０）が大きく、重み係数Ｇの値も合わせて大きくなるため、フィードフォワード指令値Ｐ３＊による補正結果（Ｐ１＊＋Ｐ３＊）も大きくなる。したがって、組立ワークＷ１と被組立ワークＷ２とがＺ軸方向に接触したときの力検知値Ｆ０の立ち上がりが急峻となる。以上より、組立時のワークへの負荷を低減し、かつ力指令値Ｆ０＊に達するまでの時間が短くなる。よって、力制御時の組立時間を短縮することができる。
【００９６】
したがって、ロボット１００による組立作業等の作業時に部品にかかる負荷を低減することができ、かつ作業時間を短縮化することができるので、生産性が向上する。
【００９７】
なお、以上説明したフィードフォワード指令算出部１０において、必要に応じて、重み係数演算部１２やローパスフィルタ部１４を省略できる。図１０（ａ）〜図１０（ｄ）は、本発明の実施形態に係るロボット装置の制御系におけるフィードフォワード指令算出部の変形例を示す図である。
【００９８】
図４に示すフィードフォワード指令算出部１０において、図１０（ａ）に示すように、重み係数演算部１２、乗算部１３及びローパスフィルタ部１４を省略してもよい。この場合、差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）が、フィードフォワード指令値Ｐ３＊ということになる。このような構成であっても、力制御時の力の収束時間を短縮することができ、力のオーバーシュートを抑制することができ、ロボットアーム１０１の動作速度を向上させることができる。
【００９９】
また、図４に示すフィードフォワード指令算出部１０において、図１０（ｂ）に示すように、ローパスフィルタ部１４を省略してもよい。この場合、差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）を重み係数Ｇで補正した値（Ｐ０＊＋Ｐ１＊−Ｐ０）×Ｇ、即ち位置補正量Ｐ２＊が、フィードフォワード指令値Ｐ３＊ということになる。この場合、図１０（ａ）に示す構成とした場合よりも、力の立ち上がりが速くなる。
【０１００】
また、図４に示すフィードフォワード指令算出部１０において、図１０（ｃ）に示すように、重み係数演算部１２及び乗算部１３を省略してもよい。この場合、差分値（Ｐ０＊＋Ｐ１＊−Ｐ０）にローパスフィルタのフィルタ処理を施した結果が、フィードフォワード指令値Ｐ３＊ということになる。この場合、図１０（ａ）に示す構成とした場合よりも、ロボットアーム１０１の振動を抑制することができる。
【０１０１】
また、図４、図１０（ａ）、図１０（ｂ）及び図１０（ｃ）に示すフィードフォワード指令算出部１０の差分値演算部１１において、図１０（ｄ）に示すように、位置指令値Ｐ０＊と実位置Ｐ０との差分値（Ｐ０＊−Ｐ０）を演算するようにしてもよい。
【０１０２】
本発明は、以上説明した実施形態及び変形例に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。
【０１０３】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１０４】
また、上記実施形態では、ロボットアーム１０１が垂直多関節のロボットアームの場合について説明したが、これに限定するものではない。ハンドが取り付けられるアームが、例えば、水平多関節のロボットアーム、パラレルリンクのロボットアーム、直交ロボット等、種々のロボットアームであってもよい。
【０１０５】
また、上記実施形態では、関節角度を、モータ１３１の回転軸の回転角度とし、関節角度指令値を、モータ１３１の回転軸の回転角度に対する角度指令値とした場合について説明したが、これに限定するものではない。
【０１０６】
エンコーダ１３５が、減速機１３３の出力軸の回転角度を検知する場合には、関節角度は、減速機１３３の出力軸の回転角度であり、関節角度指令値は、モータ１３１の回転軸の回転角度に対する角度指令値ということになる。この場合、角度偏差を求める際には、エンコーダ１３５により検知された値に減速機１３３の減速比Ｎを乗算するなど、エンコーダ１３５の検知値と角度指令値との間で整合を図ればよい。エンコーダ１３５が一方のリンクに対する他方のリンクの相対角度を検知する場合であっても同様である。
【符号の説明】
【０１０７】
１００…ロボット、１０１…ロボットアーム、１０２…ロボットハンド（エンドエフェクタ）、１０３…力覚センサ（力検知部）、１３５…エンコーダ（位置検知部）、２０１…ＣＰＵ（制御部）、５００…ロボット装置
【図１】
0006700679.tif 000002
【図２】
0006700679.tif 000003
【図３】
0006700679.tif 000004
【図４】
0006700679.tif 000005
【図５】
0006700679.tif 000006
【図６】
0006700679.tif 000007
【図７】
0006700679.tif 000008
【図８】
0006700679.tif 000009
【図９】
0006700679.tif 000010
【図１０】
0006700679.tif 000011