(57)【特許請求の範囲】
【請求項１】
関節を有し、先端にエンドエフェクタが取り付けられたロボットと、
前記ロボットの動作を制御する制御装置と、を備えたロボット装置であって、
前記関節は、モータと、前記モータの回転を減速する減速機と、前記モータの回転角度を検出する第１角度検出部と、前記減速機の出力軸の回転角度を検出する第２角度検出部と、を有し、
前記制御装置は、
前記第１角度検出部の角度検出結果を用いて、所定作業を行う作業開始位置に前記エンドエフェクタが移動するよう前記ロボットの動作を制御する移動制御モードと、
前記第１角度検出部の角度検出結果を用いて、前記所定作業の最中に、前記エンドエフェクタに作用する力のうち、所定方向の力成分が小さくなるように前記ロボットの動作を制御するコンプライアンス制御モードと、を実行可能であり、
前記第１角度検出部の角度検出結果及び前記第２角度検出部の角度検出結果を用いて、前記関節に作用するトルクを算出し、前記関節のトルクの算出結果を用いて、前記エンドエフェクタに作用する力を算出することを特徴とするロボット装置。
【請求項２】
前記制御装置は、前記コンプライアンス制御モードにおいて、前記所定作業を開始する前に算出した力のうち前記所定方向の力成分と、前記所定作業の最中に算出した力のうち前記所定方向の力成分との差が小さくなるように前記ロボットの動作を制御することを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記制御装置は、前記第１角度検出部の角度検出結果を前記減速機の減速比で前記関節の角度に換算した結果と、前記第２角度検出部の角度検出結果との差分、及び前記減速機のねじり剛性から、前記関節に作用するトルクを算出することを特徴とする請求項１又は２に記載のロボット装置。
【請求項４】
前記制御装置は、前記関節のトルクの算出結果及び前記ロボットのリンクパラメータから、前記エンドエフェクタに作用する力を算出することを特徴とする請求項１乃至３のいずれか１項に記載のロボット装置。
【請求項５】
前記エンドエフェクタは、複数のフィンガーを有するロボットハンドであり、
前記所定作業は、前記ロボットハンドに把持させたワークを別のワークに嵌合させる嵌合作業、又は前記ロボットハンドに把持させたワークを別のワークから抜き取る抜き取り作業であることを特徴とする請求項１乃至４のいずれか１項に記載のロボット装置。
【請求項６】
前記ロボットは、複数の関節を有し、前記複数の関節は各々、前記モータと、前記減速機と、前記第１角度検出部と、前記第２角度検出部と、を有し、
前記制御装置は、前記複数の関節にそれぞれ作用するトルクを算出し、これらトルクの算出結果を合成して、前記エンドエフェクタに作用する力を算出することを特徴とする請求項１乃至５のいずれか１項に記載のロボット装置。
【請求項７】
前記制御装置は、
前記モータの回転位置が入力を受けた位置指令に近づくように前記モータに電流を供給して前記ロボットの関節角度を制御する複数の関節制御部と、
前記各関節制御部に位置指令を出力するメイン制御部と、を有することを特徴とする請求項６に記載のロボット装置。
【請求項８】
前記各関節制御部は、前記各関節に作用するトルクを算出して、トルクの算出結果を前記メイン制御部に出力し、
前記メイン制御部は、予め設定された教示点に基づいて位置指令を生成し、
前記移動制御モードでは、前記各関節制御部に前記教示点に基づく位置指令を出力し、
前記コンプライアンス制御モードでは、前記各関節制御部から入力を受けたトルクの算出結果を用いて前記エンドエフェクタに作用する力を算出し、算出した力のうち前記所定方向の力成分が小さくなるように前記教示点に基づく位置指令を補正して、前記各関節制御部に補正した位置指令を出力することを特徴とする請求項７に記載のロボット装置。
【請求項９】
前記第１角度検出部及び前記第２角度検出部は、エンコーダであることを特徴とする請求項１乃至８のいずれか１項に記載のロボット装置。
【請求項１０】
関節を有するロボットの前記関節が、モータと、前記モータの回転を減速する減速機と、前記モータの回転角度を検出する第１角度検出部と、前記減速機の出力軸の回転角度を検出する第２角度検出部と、を有しており、制御装置が前記ロボットの動作を制御するロボット制御方法であって、
前記制御装置が、前記第１角度検出部の角度検出結果を用いて、前記ロボットの先端に取り付けられたエンドエフェクタが所定作業を行う作業開始位置に移動するよう前記ロボットを制御する移動制御工程と、
前記制御装置が、前記第１角度検出部の角度検出結果及び前記第２角度検出部の角度検出結果を用いて、前記関節に作用するトルクを算出するトルク算出工程と、
前記制御装置が、前記関節のトルクの算出結果を用いて、前記エンドエフェクタに作用する力を算出する力算出工程と、
前記制御装置が、前記所定作業の最中に、前記エンドエフェクタに作用する力のうち、所定方向の力成分が小さくなるように、前記第１角度検出部の角度検出結果を用いて、前記ロボットの動作を制御するコンプライアンス制御工程と、を備えたことを特徴とするロボット制御方法。
【請求項１１】
前記コンプライアンス制御工程では、前記制御装置が、前記所定作業を開始する前に算出した力のうち前記所定方向の力成分と、前記所定作業の最中に算出した力のうち前記所定方向の力成分との差が小さくなるように前記ロボットの動作を制御することを特徴とする請求項１０に記載のロボット制御方法。
【請求項１２】
前記トルク算出工程では、前記制御装置が、前記第１角度検出部の角度検出結果を前記減速機の減速比で前記関節の角度に換算した結果と、前記第２角度検出部の角度検出結果との差分、及び前記減速機のねじり剛性から、前記関節に作用するトルクを算出することを特徴とする請求項１０又は１１に記載のロボット制御方法。
【請求項１３】
前記力算出工程では、前記制御装置が、前記関節のトルクの算出結果及び前記ロボットのリンクパラメータから、前記エンドエフェクタに作用する力を算出することを特徴とする請求項１０乃至１２のいずれか１項に記載のロボット制御方法。
【請求項１４】
前記ロボットは、複数の関節を有し、前記複数の関節は各々、前記モータと、前記減速機と、前記第１角度検出部と、前記第２角度検出部と、を有し、
前記トルク算出工程では、前記制御装置が、前記複数の関節にそれぞれ作用するトルクを算出し、
前記力算出工程では、前記制御装置が、前記トルク算出工程にて算出した複数のトルクの算出結果を合成して、前記エンドエフェクタに作用する力を算出することを特徴とする請求項１０乃至１３のいずれか１項に記載のロボット制御方法。
【請求項１５】
前記第１角度検出部及び前記第２角度検出部は、エンコーダであることを特徴とする請求項１０乃至１４のいずれか１項に記載のロボット制御方法。
【請求項１６】
コンピュータに、請求項１０乃至１５のいずれか１項に記載のロボット制御方法の各工程を実行させるためのプログラム。
【請求項１７】
請求項１６に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【請求項１８】
請求項１０乃至１５のいずれか１項に記載のロボット制御方法により前記ロボットを制御し、第１ワークを第２ワークに組み付けて物品を製造する物品の製造方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、作業時にロボットハンド等のエンドエフェクタに発生する外力に対して柔軟にするロボット装置、ロボット制御方法、プログラム、記録媒体、及び物品の製造方法に関する。
【背景技術】
【０００２】
ロボット装置を用いて、部品同士の接触を伴う部品組立の作業、例えば嵌合部品を被嵌合部品に嵌合する嵌合作業を実施する場合、ロボットアームに配置した力センサを利用してコンプライアンス制御を行うのが一般的である。
【０００３】
コンプライアンス制御は、嵌合部品を把持したロボットハンドが受ける力（力及び力のモーメントを含む）に対し、ロボットアームに仮想的なばね性や粘性を持たせ、作業時に発生する反力に対して柔軟にロボットアームを動作させる制御である。コンプライアンス制御によって、ロボットアームの位置決め誤差や被嵌合部品の位置誤差、ロボットハンドの把持繰り返し性等を補正した嵌合作業が実施できる。
【０００４】
しかし、ロボットアームに力センサを設ける場合、力センサの機械的強度が、ロボットアームの機械的強度に比べて低いため、力センサの許容荷重以上の力が必要な組み立て作業はロボット装置で実現できないという問題があった。
【０００５】
上記の問題に対し、特許文献１では、ロボットアームに力センサを設けずに、コンプライアンス制御を実現する方法を提案している。その方法は、コンプライアンス制御したい方向を定め、その方向に対する指令位置を逐次、ロボットアームの各関節軸を駆動するモータのエンコーダの現在位置に更新することで、指定方向に対する剛性を低下させるものである。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特許第２６１９２２７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
一般に、ロボットアームは、高速低トルクのモータの出力を減速機で低速高トルクに変換して関節を駆動する関節駆動装置を備えている。この種の減速機は、減速機自体の撓み変形により、入力軸と出力軸との間でねじれが生じる。
【０００８】
しかし、特許文献１は、減速機を有するロボットアームを対象にコンプライアンス制御を行うものではない。即ち、特許文献１では、モータのエンコーダの情報のみでコンプライアンス制御を行っており、減速機の入力軸と出力軸の間で生じるねじれ角は考慮されていない。よって、減速機を有するロボットアームに特許文献１のコンプライアンス制御を適用した場合、減速機のねじれ角の分の誤差が生じ、コンプライアンス制御の精度が低いものであった。
【０００９】
また、エンドエフェクタに外力が作用した際にエンドエフェクタの変位量を大きくするためには、サーボ制御における位置や速度のゲインを小さくする必要があり、応答速度が制限される場合があった。
【００１０】
そこで、本発明は、力センサを用いずに、精度よくかつ応答が速いコンプライアンス制御を実現することを目的とする。
【課題を解決するための手段】
【００１１】
本発明のロボット装置は、関節を有し、先端にエンドエフェクタが取り付けられたロボットと、前記ロボットの動作を制御する制御装置と、を備えたロボット装置であって、前記関節は、モータと、前記モータの回転を減速する減速機と、前記モータの回転角度を検出する第１検出部と、前記減速機の出力軸の回転角度を検出する第２角度検出部と、を有し、前記制御装置は、前記第１角度検出部の角度検出結果を用いて、所定作業を行う作業開始位置に前記エンドエフェクタが移動するよう前記ロボットの動作を制御する移動制御モードと、前記第１角度検出部の角度検出結果を用いて、前記所定作業の最中に、前記エンドエフェクタに作用する力のうち、所定方向の力成分が小さくなるように前記ロボットの動作を制御するコンプライアンス制御モードと、を実行可能であり、前記第１角度検出部の角度検出結果及び前記第２角度検出部の角度検出結果を用いて、前記関節に作用するトルクを算出し、前記関節のトルクの算出結果を用いて、前記エンドエフェクタに作用する力を算出することを特徴とする。
【発明の効果】
【００１２】
本発明によれば、力センサを用いずに、精度よくかつ応答が速いコンプライアンス制御を実現することができる。
【図面の簡単な説明】
【００１３】
【図１】第１実施形態に係るロボット装置を示す斜視図である。
【図２】ロボット装置のロボットアームの関節を示す部分断面図である。
【図３】ロボット装置の制御装置の構成を示すブロック図である。
【図４】ロボット装置の要部構成を示した機能ブロック図である。
【図５】第１実施形態に係るロボット制御方法を示すフローチャートである。
【図６】第２実施形態に係るロボット制御方法を示すフローチャートである。
【発明を実施するための形態】
【００１４】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１５】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置を示す斜視図である。ロボット装置１００は、ロボット２００と、ロボット２００の動作を制御する制御装置３００と、ユーザの操作によりロボット２００の動作を教示する教示部としてのティーチングペンダント４００と、を備えている。
【００１６】
ロボット２００は、垂直多関節型のロボットアーム２０１と、ロボットアーム２０１の先端に取り付けられた、エンドエフェクタとしてのロボットハンド２０２と、を有している。
【００１７】
ロボットアーム２０１は、作業台に固定されるベース部（基端リンク）２１０と、変位や力を伝達する複数のリンク２１１〜２１６とが関節Ｊ１〜Ｊ６で屈曲（旋回）又は回転可能に連結されている。第１実施形態では、ロボットアーム２０１は、屈曲する３軸と回転する３軸の６軸の関節Ｊ１〜Ｊ６で構成されている。ここで、屈曲とは２つのリンクの結合部のある点で折れ曲がること、回転とは２つのリンクの長手方向の回転軸でリンクが相対的に回ることをいい、それぞれを屈曲部、回転部と呼ぶ。ロボットアーム２０１は、６つの関節Ｊ１〜Ｊ６から構成され、関節Ｊ１，Ｊ４，Ｊ６が回転部、関節Ｊ２，Ｊ３，Ｊ５が屈曲部である。
【００１８】
ロボットハンド２０２は、複数のフィンガー２２０を有し、リンク（先端リンク）２１６に取り付けられている。複数のフィンガー２２０を閉動作させることにより、第１ワークであるワークＷ１を把持することができ、複数のフィンガー２２０を開動作させることにより、ワークＷ１を把持解放することができる。ロボットハンド２０２は、複数のフィンガー２２０を用いてワークＷ１を把持することにより、第１ワークであるワーク（嵌合部品）Ｗ１を第２ワークであるワーク（被嵌合部品）Ｗ２に嵌合する嵌合作業を行うことができる。
【００１９】
ロボットアーム２０１は、各関節Ｊ１〜Ｊ６に対して設けられ、各関節Ｊ１〜Ｊ６をそれぞれ駆動するための複数（６つ）の関節駆動装置２３０を有している。なお、図１では、関節駆動装置２３０は、便宜上、関節Ｊ２にのみ図示しているが、他の関節Ｊ１，Ｊ３〜Ｊ６において図示を省略しているが、他の関節Ｊ１，Ｊ３〜Ｊ６にも、同様の構成の関節駆動装置２３０が配置されている。
【００２０】
図２は、ロボットアーム２０１の関節Ｊ２を示す部分断面図である。以下、関節Ｊ２を例に代表して説明し、他の関節Ｊ１，Ｊ３〜Ｊ６については、同様の構成であるため、説明を省略する。
【００２１】
関節駆動装置２３０は、電動モータである回転モータ（以下、「モータ」という）２３１と、モータ２３１の回転軸２３２の回転を減速する減速機２３３と、を有している。関節Ｊ２は、モータ２３１の回転軸２３２（減速機２３３の入力軸）の回転角度を検出する第１角度検出部であるエンコーダ（入力軸エンコーダ）２３５を有する。また、関節Ｊ２は、リンク１２１に対するリンク１２２の角度（減速機２３３の出力軸の回転角度）を検出する第２角度検出部であるエンコーダ（出力軸エンコーダ）２３６を有している。即ち、エンコーダ２３６は、関節Ｊ２の角度（関節角度）を検出する。モータ２３１は、サーボモータであり、例えばブラシレスＤＣサーボモータやＡＣサーボモータである。
【００２２】
エンコーダ２３５は、アブソリュート型のロータリーエンコーダが望ましく、１回転の絶対角度エンコーダ、絶対角度エンコーダの回転総数のカウンタ、及びカウンタに電力を供給するバックアップ電池を有して構成される。ロボットアーム２０１への電源の供給がオフになっても、このバックアップ電池が有効であれば、ロボットアーム２０１への電源供給のオン／オフに関係なく、カウンタにおいて回転総数が保持される。したがって、ロボットアーム２０１の姿勢が制御可能となる。なお、エンコーダ２３５は、回転軸２３２に取り付けられているが、減速機２３３の入力軸に取り付けてもよい。
【００２３】
エンコーダ２３６は、隣り合う２つのリンク間の相対角度を検出するロータリーエンコーダである。関節Ｊ２においては、エンコーダ２３６は、リンク２１１とリンク２１２との間の相対角度を検出するロータリーエンコーダである。エンコーダ２３６は、リンク２１１にエンコーダスケールを設け、リンク２１２に検出ヘッドを設けた構成、或いは逆の構成となる。
【００２４】
また、リンク２１１とリンク２１２とは、クロスローラベアリング２３７を介して回転自在に結合されている。モータ２３１は、モータカバー２３８で覆われて保護されている。モータ２３１とエンコーダ２３５との間には、不図示のブレーキユニットが設けられている。ブレーキユニットの主な機能は、電源オフ時のロボットアーム２０１の姿勢の保持である。
【００２５】
減速機２３３は、第１実施形態では、小型軽量で減速比の大きい波動歯車減速機である。減速機２３３は、モータ２３１の回転軸２３２に結合された、入力軸であるウェブジェネレータ２４１と、リンク２１２に固定された、出力軸であるサーキュラスプライン２４２と、を備えている。なお、サーキュラスプライン２４２は、リンク２１２に直結されているが、リンク２１２に一体に形成されていてもよい。
【００２６】
また、減速機２３３は、ウェブジェネレータ２４１とサーキュラスプライン２４２との間に配置され、リンク２１１に固定されたフレクスプライン２４３を備えている。フレクスプライン２４３は、ウェブジェネレータ２４１の回転に対して減速比Ｎで減速され、サーキュラスプライン２４２に対して相対的に回転する。従って、モータ２３１の回転軸２３２の回転は、減速機２３３で１／Ｎの減速比で減速されて、フレクスプライン２４３が固定されたリンク２１１に対してサーキュラスプライン２４２が固定されたリンク２１２を相対的に回転運動させ、関節Ｊ２を屈曲させる。
【００２７】
図３は、ロボット装置１００の制御装置３００の構成を示すブロック図である。制御装置３００は、メイン制御部３３０と、複数（関節の数に対応した数：第１実施形態では６つ）の関節制御部３４０と、を有する。
【００２８】
メイン制御部３３０は、コンピュータで構成されており、演算部としてのＣＰＵ（Central  Processing  Unit）３０１を備えている。また、メイン制御部３３０は、記憶部として、ＲＯＭ（Read  Only  Memory）３０２、ＲＡＭ（Random  Access  Memory）３０３、ＨＤＤ（Hard  Disk  Drive）３０４を備えている。また、メイン制御部３３０は、記録ディスクドライブ３０５及び各種のインタフェース３１１〜３１３を備えている。
【００２９】
ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５及び各種のインタフェース３１１〜３１３が、バスを介して接続されている。ＲＯＭ３０２には、ＢＩＯＳ等の基本プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００３０】
ＨＤＤ３０４は、ＣＰＵ３０１の演算処理結果や外部から取得した各種データ等を記憶する記憶装置であると共に、ＣＰＵ３０１に、後述する演算処理を実行させるためのプログラム３２０を記録するものである。ＣＰＵ３０１は、ＨＤＤ３０４に記録（格納）されたプログラム３２０に基づいてロボット制御方法の各工程を実行する。
【００３１】
記録ディスクドライブ３０５は、記録ディスク３２１に記録された各種データやプログラム等を読み出すことができる。なお、メイン制御部３３０には、書き換え可能な不揮発性メモリや外付けＨＤＤ等の不図示の外部記憶装置が接続されていてもよい。
【００３２】
教示部であるティーチングペンダント４００は、インタフェース３１１に接続されている。ティーチングペンダント４００は、ユーザの入力操作により、ロボット２００を教示する教示点、即ち各関節Ｊ１〜Ｊ６の目標関節角度（各関節Ｊ１〜Ｊ６のモータ２３１の目標回転位置）を指定するものである。教示点のデータは、インタフェース３１１及びバスを通じてＨＤＤ３０４に出力される。
【００３３】
ＨＤＤ３０４は、ティーチングペンダント４００により指定された教示点のデータを格納することができる。ＣＰＵ３０１は、ＨＤＤ３０４に設定（格納）された教示点のデータを読み出すことができる。
【００３４】
表示部である表示装置（モニタ）５００は、インタフェース３１２に接続されており、ＣＰＵ３０１の制御の下、画像を表示する。
【００３５】
インタフェース３１３には、関節制御部３４０が接続されている。なお、第１実施形態では、ロボットアーム２０１が６つの関節Ｊ１〜Ｊ６を有しているので、制御装置３００は、６つの関節制御部３４０を有するが、図３では、関節制御部３４０が１つだけ図示し、残りの５つは図示を省略している。各関節制御部３４０は、制御装置３００の筐体内に配置されている。なお、各関節制御部３４０の配置位置は、筐体内に限定するものではなく、例えばロボットアーム２０１に配置されていてもよい。
【００３６】
ＣＰＵ３０１は、予め設定された教示点に基づき、ロボットアーム２０１の軌道を計算し、モータ２３１の回転軸２３２の目標回転位置（回転角度の制御量）を示す位置指令の信号を所定時間間隔で各関節制御部３４０に出力する。
【００３７】
関節制御部３４０は、ＣＰＵ３５１、記憶部としてのＥＥＰＲＯＭ３５２及びＲＡＭ３５３、インタフェース３６１、検出回路３６２，３６３並びにモータ駆動回路３６５を備えており、これらがバスを介して接続されて構成されている。
【００３８】
ＣＰＵ３５１は、プログラム３７０に従って演算処理を実行する。ＥＥＰＲＯＭ３５２は、プログラム３７０を記憶する記憶装置である。ＲＡＭ３５３は、ＣＰＵ３５１の演算処理結果等、各種データを一時的に記憶する記憶装置である。
【００３９】
メイン制御部３３０は、複数（６つ）のインタフェース３１３（図３では１つのみ図示）を有している。各インタフェース３１３と各関節制御部３４０のインタフェース３６１とがケーブル等で接続されており、メイン制御部３３０と各関節制御部３４０との間で信号の送受信を行うことができる。
【００４０】
エンコーダ２３５は、検出回路３６２に接続され、エンコーダ２３６は、検出回路３６３に接続されている。エンコーダ２３５，２３６からは、検出した角度検出値を示すパルス信号が出力される。
【００４１】
検出回路３６２，３６３は、エンコーダ２３５，２３６からパルス信号を取得し、ＣＰＵ３５１にて取得可能な信号に変換してＣＰＵ３５１に出力する。
【００４２】
モータ駆動回路３６５は、例えば半導体スイッチング素子を有するモータドライバであり、入力した電流指令に応じて、パルス幅変調された３相交流のＰＷＭ波形の電圧をモータ２３１に出力することで、モータ２３１に電流を供給する。
【００４３】
各関節制御部３４０のＣＰＵ３５１は、メイン制御部３３０のＣＰＵ３０１から入力を受けた位置指令にモータ２３１の回転位置（回転角度）が近づくようにモータ２３１への電流の出力量（電流指令）を演算し、電流指令をモータ駆動回路３６５に出力する。
【００４４】
モータ駆動回路３６５は、入力を受けた電流指令に対応する電流をモータ２３１に供給する。そして、モータ２３１は、モータ駆動回路２６５から電力供給を受けて駆動トルクを発生し、減速機２３３の入力軸であるウェブジェネレータ２４１にトルクを伝達する。減速機２３３において、出力軸であるサーキュラスプライン２４２は、ウェブジェネレータ２４１の回転に対して１／Ｎの回転数で回転する。これにより、リンク２１２がリンク２１１に対して相対的に回転する。
【００４５】
このように、各関節制御部３４０は、モータ２３１の回転位置がメイン制御部３３０から入力を受けた位置指令に近づくようにモータ２３１に電流を供給して各関節Ｊ１〜Ｊ６の関節角度を制御する。
【００４６】
なお、第１実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４，ＥＥＰＲＯＭ３５２であり、ＨＤＤ３０４，ＥＥＰＲＯＭ３５２にプログラム３２０，３７０が格納される場合について説明するが、これに限定するものではない。プログラム３２０，３７０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム３２０，３７０を供給するための記録媒体としては、図３に示す記録ディスク３２１、不図示の外部記憶装置等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、不揮発性メモリ、ＲＯＭ等を用いることができる。
【００４７】
図４は、本発明の第１実施形態に係るロボット装置の要部構成を示した機能ブロック図である。制御装置３００については、プログラム３２０に基づくＣＰＵ３０１の機能をブロック化して図示し、プログラム３７０に基づくＣＰＵ３５１の機能及びモータ駆動回路３６５の機能をブロック化して図示している。ロボット２００については、ロボットアーム２０１の関節Ｊ１をブロック化して図示している。
【００４８】
制御装置３００は、メイン制御部３３０及び各関節Ｊ１〜Ｊ６に対応する関節制御部３４０を有している。図４では、関節Ｊ１と関節Ｊ１に対応する関節制御部３４０のみ図示しており、図示は省略しているが、制御装置３００は、他の関節Ｊ２〜Ｊ６それぞれに対応する関節制御部３４０を複数有している。
【００４９】
メイン制御部３３０は、軌道計算部３３１、位置判断部３３２、制御切替部３３３、力記憶部３３４、力算出部３３５、リンクパラメータ記憶部３３６、補正量算出部３３７からなる。各関節制御部３４０は、モータ制御部３４１、トルク算出部３４２、減速機定数記憶部３４３からなる。
【００５０】
メイン制御部３３０のＣＰＵ３０１は、プログラム３２０により軌道計算部３３１、位置判断部３３２、制御切替部３３３、力算出部３３５及び補正量算出部３３７として機能する。力記憶部３３４、リンクパラメータ記憶部３３６は、例えばＨＤＤ３０４である。
【００５１】
また、各関節制御部３４０のモータ制御部３４１は、プログラム３７０により動作するＣＰＵ３５１及びモータ駆動回路３６５の機能である。トルク算出部３４２は、プログラム３７０により動作するＣＰＵ３５１の機能である。減速機定数記憶部３４３は、例えばＥＥＰＲＯＭ３５２である。
【００５２】
まず、メイン制御部３３０の制御動作について説明する。軌道計算部３３１は、教示点のデータに基づいて、ロボットアーム２０１の動作（軌道）を計算する。教示点は、関節空間又はタスク空間上の点として、作業者が操作するティーチングペンダント４００により設定される。
【００５３】
ロボットアーム２０１の自由度を表すパラメータを関節角度として、ロボットアーム２０１の関節Ｊ１〜Ｊ６の関節角度をそれぞれθ１〜θ６とする。ロボットアーム２０１のコンフィグレーションは（θ１，θ２，θ３，θ４，θ５，θ６）で表され、関節空間上では、１つの点とみなすことができる。このように、ロボットアーム２０１の自由度を表すパラメータ（例えば、関節角度や伸縮長さ）を座標軸の値とした場合、ロボットアーム２０１のコンフィグレーションは関節空間上の点として表現することができる。つまり、関節空間は、ロボットアーム２０１の関節角度を座標軸とする空間である。
【００５４】
また、ロボットハンド２０２には、ツールセンターポイント（ＴＣＰ）が設定されている。ＴＣＰは、位置を表す３つのパラメータ（ｘ，ｙ，ｚ）と、姿勢（回転）を表す３つのパラメータ（α，β，γ）、即ち６つのパラメータ（ｘ，ｙ，ｚ，α，β，γ）で表され、タスク空間上では、１つの点としてみなすことができる。つまり、タスク空間は、これら６つの座標軸で規定された空間である。
【００５５】
軌道計算部３３１は、設定された複数の教示点を繋ぐロボットアーム２０１の経路を所定の補間方法（例えば、直線補間や円弧補間、関節補間等）で生成する。そして、軌道計算部３３１は、生成したロボットアーム２０１の経路から、ロボットアーム２０１の軌道を生成する。
【００５６】
ここで、ロボットアーム２０１の経路とは、関節空間又はタスク空間の点の順序集合である。ロボットアーム２０１の軌道とは、時間をパラメータとして経路を表したものであり、第１実施形態では、時刻毎の各関節Ｊ１〜Ｊ６のモータ２３１の位置指令の集合である。
【００５７】
軌道データは、ロボットアーム２０１を動作させる前に予め計算して記憶部、例えばＨＤＤ３０４に記憶（設定）させておく。
【００５８】
なお、軌道データの計算は、メイン制御部３３０のＣＰＵ３０１が行う場合について説明するが、不図示の他のコンピュータに行わせ、メイン制御部３３０の記憶部、例えばＨＤＤ３０４に予め記憶（設定）してもよい。
【００５９】
位置判断部３３２は、ＨＤＤ３０４から読み出した、教示点に基づくロボットアーム２０１の軌道のデータを用いて、ロボットアーム２０１の先端に取り付けられたロボットハンド２０２が所定作業の作業開始位置又は作業完了位置へ達したか否かを判断する。第１実施形態では、所定作業は、ロボットハンド２０２に把持させたワークＷ１を別のワークＷ２に嵌合させる嵌合作業である。
【００６０】
制御切替部３３３は、各関節Ｊ１〜Ｊ６、図４では関節Ｊ１の駆動を制御する関節制御部３４０のモータ制御部３４１に指令する位置指令について、補正量算出部３３７から出力される補正量を位置指令に加えるか否かを切り替える。
【００６１】
具体的には、関節制御部３４０は、教示点に基づき算出された位置指令の補正を行う制御モード（コンプライアンス制御モード）と、教示点に基づき算出された位置指令の補正を行わない制御モード（通常制御モード）と、を有する。
【００６２】
コンプライアンス制御モードは、嵌合作業の最中に、ロボットハンド２０２に作用する力のうち、所定方向（例えば嵌合方向と交差（直交）する方向）の力成分が小さくなるように位置指令を補正してロボットアーム２０１の動作を制御する力制御モードである。つまり、コンプライアンス制御モードでは、軌道計算部３３１にて計算した位置指令に、補正量算出部３３７にて算出した補正量を加算する補正を行う。
【００６３】
通常制御モードは、嵌合作業を行わせる作業開始位置にロボットハンド２０２が移動するようロボットアーム２０１の動作を制御する制御モード（移動制御モード）を含み、この制御モードのときは、軌道計算部３３１にて算出した位置指令を補正しない。
【００６４】
制御切替部３３３は、位置判断部３３２にてロボットハンド２０２が作業開始位置に到達したと判断すれば、通常制御モードからコンプライアンス制御モードに切り替える。また、制御切替部３３３は、位置判断部３３２にてロボットハンド２０２が作業完了位置に到達したと判断すれば、コンプライアンス制御モードから通常制御モードに切り替える。なお、制御切替部３３３は、位置判断部３３２にてロボットハンド２０２が作業完了位置に到達していないと判断した場合は、コンプライアンス制御モードを継続する。
【００６５】
力算出部３３５は、後述する各トルク算出部３４２からのトルクの算出結果を用いて、ロボットハンド２０２に作用する力を算出する。具体的には、力算出部３３５は、各軸のトルク算出部３４２からのトルクの計算結果と、リンクパラメータ記憶部３３６に記憶されているロボットアーム２０１のリンクパラメータから、ロボットハンド２０２の任意の位置に生じさせている合成の力を算出する。リンクパラメータ記憶部３３６には、予めロボットアーム２０１のリンクパラメータが記憶（設定）されている。ロボットアーム２０１のリンクパラメータは、ロボットアーム２０１を構成する各リンク２１０〜２１６の長さや各関節Ｊ１〜Ｊ６の位置関係を示したパラメータである。算出する基準となるロボットハンド２０２の任意位置については、例えばロボットハンド２０２の掌面の中心とする。
【００６６】
力記憶部３３４は、制御切替部３３３が通常制御モード（移動制御モード）からコンプライアンス制御モードに切り替える直前の力算出部３３５により算出した力を記憶する。なお、力記憶部３３４は、所定作業（嵌合作業）を行う直前に力算出部３３５により算出した力を記憶するのが好ましいが、直前に限定するものではなく、所定作業（嵌合作業）を行う前であればよい。
【００６７】
補正量算出部３３７は、力記憶部３３４に記憶された嵌合作業を開始する前にロボットハンド２０２に生じていた力と、嵌合作業中に力算出部３３５が算出するロボットハンド２０２に生じている力との差を算出する。具体的には、補正量算出部３３７は、嵌合作業を開始する前に算出した力のうち所定方向の力成分と、嵌合作業の最中に算出した力のうち所定方向の力成分との差を算出する。そして、補正量算出部３３７は、所定方向の力成分の差を小さくするための位置指令の補正量を算出する。
【００６８】
補正量については、ロボットハンド２０２を中心とするツール座標系の任意の軸方向に対して算出可能である。例えば、ワークＷ１の嵌合作業を実施する場合に、嵌合方向への力成分は補正せずに、嵌合方向と交差（直交）する方向（所定方向）に対する力成分の補正は実施するように補正量を算出する。
【００６９】
次に、各関節制御部３４０について説明する。モータ制御部３４１は、通常制御モード時は、軌道計算部３３１からの位置指令を入力し、コンプライアンス制御モード時には、軌道計算部３３１からの位置指令と補正量算出部３３７の補正量とを足し合わせた値の位置指令を入力する。軌道計算部３３１からの位置指令とは、上述したように、教示点に基づき算出した位置指令である。モータ制御部３４１は、入力した位置指令とエンコーダ２３５の値とを参照して、モータ２３１の回転位置が位置指令に近づくようにモータ２３１の位置制御（フィードバック制御）を行う。
【００７０】
トルク算出部３４２は、エンコーダ２３５の角度検出結果及びエンコーダ２３６の角度検出結果を用いて、関節に作用するトルクを算出する。
【００７１】
具体的に説明すると、トルク算出部３４２は、まず、エンコーダ２３５により検出された角度検出結果を減速機２３３の減速比で関節の角度に換算する。具体的には、エンコーダ２３５の角度検出結果に減速比Ｎ（例えば５０）を除算する。減速機２３３の減速比Ｎは、予めＥＥＰＲＯＭ３５２に記憶（設定）されている。
【００７２】
次に、トルク算出部３４２は、角度換算した角度情報と、エンコーダ２３６からの角度情報の差分に対して、減速機定数記憶部３４３に予め記憶（設定）されている減速機２３３のねじり剛性（回転方向に対するばね定数）を乗算する。これにより、トルク算出部３４２は、関節に生じているトルクを算出する。上記トルク計算を各関節Ｊ１〜Ｊ６にて実施することにより、各関節Ｊ１〜Ｊ６に生じているトルクを算出する。
【００７３】
次に、制御装置３００によりロボット２００の動作を制御するロボット制御方法について説明する。図５は、本発明の第１実施形態に係るロボット制御方法を示すフローチャートである。第１実施形態では、ロボット２００（ロボットハンド２０２）に行わせる所定作業は、ワークＷ１をワークＷ２に嵌合する嵌合作業である。
【００７４】
まず、メイン制御部３３０のＣＰＵ３０１は、ロボットアーム２０１の制御を通常制御モードにする（Ｓ１）。
【００７５】
次に、ＣＰＵ３０１は、ワークＷ１を把持して取り出すための位置（ワーク取り出し位置）にロボットハンド２０２が移動するようロボットアーム２０１の動作を制御する（Ｓ２）。
【００７６】
次に、ＣＰＵ３０１は、ロボットハンド２０２がワーク取り出し位置に移動したら、ロボットハンド２０２にワークＷ１を把持させるようロボットハンド２０２の動作を制御する（Ｓ３）。
【００７７】
次に、ＣＰＵ３０１は、ロボット２００によりワークＷ１の嵌合作業を開始する作業開始位置にロボットハンド２０２を位置合わせするよう、ロボットアーム２０１の動作を制御する（Ｓ４：移動制御工程）。この場合、ＣＰＵ３０１は、通常制御モード（移動制御モード）に設定しているので、ロボットハンド２０２を作業開始位置に高速に移動することができる。作業開始位置は、ワークＷ２の近傍の位置に設定されている。
【００７８】
ロボットハンド２０２を作業開始位置に移動させる際、各関節制御部３４０のＣＰＵ３５１は、上述した計算方法で関節に作用するトルクを算出する（Ｓ５：トルク算出工程）。そして、メイン制御部３３０のＣＰＵ３０１は、上述した計算方法でロボットハンド２０２に作用する力を算出する（Ｓ６：力算出工程）。
【００７９】
次に、ＣＰＵ３０１は、作業開始位置にロボットハンド２０２が到達したか否かを判断する（Ｓ７：判断工程）。
【００８０】
ＣＰＵ３０１は、ステップＳ７にて、ロボットハンド２０２が作業開始位置に到達していないと判断した場合には（Ｓ７：Ｎｏ）、ステップＳ４へ戻り、通常制御モードによりロボットアーム２０１の動作を制御する。
【００８１】
ＣＰＵ３０１は、ステップＳ７にて、ロボットハンド２０２が作業開始位置に到達したと判断した場合には（Ｓ７：Ｙｅｓ）、ロボットアーム２０１の制御を、通常制御モードからコンプライアンス制御モードへ切り替える（Ｓ８）。コンプライアンス制御モードでは、通常制御モードでの位置指令に加えて、作業中にロボットアーム２０１やワークＷ１に生じる外力を小さくするように位置を補正するように制御する。
【００８２】
ここで、ＣＰＵ３０１は、ステップＳ８にて作業開始位置でコンプライアンス制御モードへ切り替える直前の力の値を、力記憶部３３４（例えばＨＤＤ３０４）に記憶させる。力記憶部３３４に記憶させた力の値は、嵌合作業によって生じる外力を受けていない状態でのロボットハンド２０２に作用する力となるので、コンプライアンス制御の基準値となる。
【００８３】
ロボットハンド２０２を嵌合方向の作業完了位置に移動させる際、各関節制御部３４０のＣＰＵ３５１は、上述した計算方法で関節に作用するトルクを算出する（Ｓ９：トルク算出工程）。そして、メイン制御部３３０のＣＰＵ３０１は、上述した計算方法でロボットハンド２０２に作用する力を算出する（Ｓ１０：力算出工程）。
【００８４】
ＣＰＵ３０１は、ワークＷ１を把持したロボットハンド２０２がワークＷ１をワークＷ２に嵌合する嵌合方向に移動するようロボットアーム２０１の動作を制御する（Ｓ１１：コンプライアンス制御工程）。このステップＳ１１において、ＣＰＵ３０１は、嵌合作業中、ロボットハンド２０２に作用する力のうち、嵌合方向と直交する方向（所定方向）の力成分が小さくなるようにロボットアーム２０１の動作を制御する。
【００８５】
次に、ＣＰＵ３０１は、嵌合作業が完了したか否か、即ち作業完了位置に到達したか否かを判断する（Ｓ１２：作業完了判断工程）。ＣＰＵ３０１は、嵌合作業が未完了であれば（Ｓ１２：Ｎｏ）、ステップＳ９に戻り、嵌合作業を継続する。
【００８６】
ＣＰＵ３０１は、嵌合作業が完了したと判断した場合は（Ｓ１２：Ｙｅｓ）、ロボットアーム２０１の制御をコンプライアンス制御モードから通常制御モードに切り替える（Ｓ１３）。そして、ＣＰＵ３０１は、ロボットハンド２０２のフィンガー２２０を開方向へ移動させて、ワークＷ１の把持解放を行い、ロボットアーム２０１を所定の位置姿勢へ移動させる（Ｓ１４）。これにより動作が終了する。
【００８７】
次に、第１実施形態のロボット装置１００を用いて、力検知及び応答速度を計測した結果を示す。表１は、ロボットハンド２０２に対して、１［Ｎ］の負荷と１０［Ｎ］の負荷を印加した場合のＪ３軸のエンコーダ２３５とエンコーダ２３６の値の変化を計測したデータを示したものである。
【００８８】
【表１】
0006584102.tif 000002
【００８９】
表１より、エンコーダ２３５については、力印加中と無負荷状態では、エンコーダの最低分解能以上の差が得られなかったが、エンコーダ２３６については、ほぼ印加力に比例した角度変化が得られていることが分かる。本計測結果の角度変化量に対して減速機２３３のねじり剛性を乗算することで、関節にかかるトルクを算出でき、そのトルクによって生じる力を全関節軸分を剛性することにより、ロボットハンド２０２に対して印加されている力を算出することが可能である。
【００９０】
表２はロボットアーム２０１のゲインを上述のケースより下げた状態で、ロボットハンド２０２に対して、１［Ｎ］の負荷と１０［Ｎ］の負荷を印加した場合のＪ３軸のエンコーダ２３５とエンコーダ２３６の値の変化量を計測したデータを示したものである。
【００９１】
【表２】
0006584102.tif 000003
【００９２】
その結果、ゲインを下げた状態では、力印加時と、無負荷状態でのエンコーダ２３５の値の変化量は検出可能であった。しかし、印加力が大きくなると、エンコーダ２３５の値の変化量が急激に大きくなるために、エンコーダ２３５のみで、精度良く印加力を算出することは困難であることが分かる。エンコーダ２３６でも、印加力が大きくなると、エンコーダ値の変化量が急激に大きくなっている。しかし、エンコーダ２３６の値の変化量から、エンコーダ２３５の値の変化量にＪ３軸の減速比１／２４０を乗算した値を引くと、ほぼ印加力に比例した角度変化が得られていることが分かる。
【００９３】
表１及び表２より、エンコーダ２３５のみでは検知できなかった力を、エンコーダ２３６を併用することで精度良く検知することが可能であることが分かる。また、ゲインの高い状態でも低印加力から高印加力まで力を検知できるので、ロボットアーム２０１の応答速度が速いままでの力検出が可能となる。
【００９４】
以上、第１実施形態によれば、力センサを使用せずにコンプライアンス制御を実現できるので、力センサの許容荷重以上の力（挿入力）が必要な組み立て作業でも、コンプライアンス性を確保できる。
【００９５】
また、第１実施形態では、ＣＰＵ３５１が２つのエンコーダ２３５，２３６の検出結果を用いて関節のトルクを算出してロボットハンド２０２に作用する力を算出している。したがって、エンコーダ２３５のみでコンプライアンス制御する場合よりもコンプライアンス制御の精度が向上する。
【００９６】
また、第１実施形態では、コンプライアンス制御モード、通常制御モードのいずれの制御モードでも、各関節制御部３４０のＣＰＵ３５１は、エンコーダ２３５の角度検出値を位置指令に近づけるフィードバック制御を行う。ロボットハンド２０２に加わる力をエンコーダ２３５の値の変位量で検出するためには、ゲインを下げる必要があるが、第１実施形態では、エンコーダ２３６で検出できるので、ゲインを下げる必要がない。そのため、サーボの応答が速く、各関節を高速に目標関節角度に制御することができる。
【００９７】
サーボ応答性が高いので、通常制御モード（移動制御モード）時には、ロボットハンド２０２を高速に作業開始位置へ到達させることができるので、作業時間を短縮することができる。よって、ロボット装置１００による製品の生産性が高まる。
【００９８】
また、第１実施形態によれば、複数の関節制御部３４０それぞれにおいてトルクを算出するようにしたので、メイン制御部３３０における計算負荷が低減し、よりロボットアーム２０１の動作を高速化することができる。
【００９９】
また、第１実施形態によれば、コンプライアンス制御モードにおいて、嵌合作業を開始する前に算出した力のうち所定方向の力成分と、嵌合作業の最中に算出した力のうち所定方向の力成分との差が小さくなるようにロボットアーム２０１の動作を制御する。即ち、嵌合作業を開始する前の力を基準値とし、嵌合作業中にロボットハンド２０２にかかる力が基準値に近づけられることとなる。よって、嵌合作業中に、ワークＷ１，Ｗ２或いはロボットアーム２０１に過度な負荷が掛かるのが抑制され、嵌合作業が円滑に行われる。
【０１００】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置におけるロボット制御方法について説明する。第２実施形態では、ワークＷ１をワークＷ２から抜き取る作業におけるロボット制御方法について説明する。
【０１０１】
図６は、本発明の第２実施形態に係るロボット制御方法を示すフローチャートである。第２実施形態において、ロボット装置の装置構成は、上記第１実施形態と略同様であり、ＣＰＵ３０１の制御動作、即ちＣＰＵ３０１を動作させるプログラム３２０が上記第１実施形態と異なる。したがって、第２実施形態において、上記第１実施形態と同様の構成については説明を省略し、異なる点について説明する。
【０１０２】
第２実施形態においても、上記第１実施形態と同様、ＣＰＵ３０１がプログラム３２０に基づき、ＣＰＵ３５１がプログラム３７０に基づき、ロボット制御方法の各工程を実行する。第２実施形態では、ロボット２００（ロボットハンド２０２）による所定作業は、ロボットハンド２０２に把持させたワークＷ１を別のワークＷ２から抜き取る抜き取り作業である。
【０１０３】
メイン制御部３３０のＣＰＵ３０１は、ロボットアーム２０１の制御を通常制御モードにする（Ｓ２１）。
【０１０４】
次に、ＣＰＵ３０１は、ロボットハンド２０２にワークＷ１を把持させるために、ロボットハンド２０２のフィンガー２２０を開方向へ移動させて、ワークＷ１の把持の準備をする（Ｓ２２）。
【０１０５】
次に、ＣＰＵ３０１は、ロボット２００によりワークＷ１の抜き取り作業を開始する作業開始位置にロボットハンド２０２を位置合わせするよう、ロボットアーム２０１の動作を制御する（Ｓ２３：移動制御工程）。この場合、ＣＰＵ３０１は、通常制御モード（移動制御モード）に設定しているので、ロボットハンド２０２を作業開始位置に高速に移動することができる。作業開始位置は、ワークＷ１の位置に設定されている。なお、ステップＳ２２の動作は、ステップＳ２３のロボットアーム２０１の動作中に行ってもよい。
【０１０６】
ロボットハンド２０２を作業開始位置に移動させる際、各関節制御部３４０のＣＰＵ３５１は、第１実施形態で説明した計算方法で関節に作用するトルクを算出する（Ｓ２４：トルク算出工程）。そして、メイン制御部３３０のＣＰＵ３０１は、第１実施形態で説明した計算方法でロボットハンド２０２に作用する力を算出する（Ｓ２５：力算出工程）。
【０１０７】
次に、ＣＰＵ３０１は、作業開始位置にロボットハンド２０２が到達したか否かを判断する（Ｓ２６：判断工程）。
【０１０８】
ＣＰＵ３０１は、ステップＳ２６にて、ロボットハンド２０２が作業開始位置に到達していないと判断した場合には（Ｓ２６：Ｎｏ）、ステップＳ２３へ戻り、通常制御モードによりロボットアーム２０１の動作を制御する。
【０１０９】
ＣＰＵ３０１は、ステップＳ２６にて、ロボットハンド２０２が作業開始位置に到達したと判断した場合には（Ｓ２６：Ｙｅｓ）、ロボットアーム２０１の制御を、通常制御モードからコンプライアンス制御モードへ切り替える（Ｓ２７）。コンプライアンス制御モードでは、通常制御モードでの位置指令に加えて、作業中にロボットアーム２０１やワークＷ１に生じる外力を小さくするように位置を補正するように制御する。
【０１１０】
ここで、ＣＰＵ３０１は、ステップＳ２７にて作業開始位置でコンプライアンス制御モードへ切り替える直前の力の値を、力記憶部３３４（例えばＨＤＤ３０４）に記憶させる。力記憶部３３４に記憶させた力の値は、抜き取り作業によって生じる外力を受けていない状態でのロボットハンド２０２に作用する力となるので、コンプライアンス制御の基準値となる。
【０１１１】
次にＣＰＵ３０１は、ロボットハンド２０２のフィンガー２２０を閉方向に移動させてロボットハンド２０２にワークＷ１を把持させる（Ｓ２８）。
【０１１２】
ロボットハンド２０２を抜き取り方向の作業完了位置に移動させる際、各関節制御部３４０のＣＰＵ３５１は、第１実施形態で説明した計算方法で関節に作用するトルクを算出する（Ｓ２９：トルク算出工程）。そして、メイン制御部３３０のＣＰＵ３０１は、第１実施形態で説明した計算方法でロボットハンド２０２に作用する力を算出する（Ｓ３０：力算出工程）。
【０１１３】
ＣＰＵ３０１は、ワークＷ１を把持したロボットハンド２０２がワークＷ１をワークＷ２から抜き取る抜き取り方向に移動するようロボットアーム２０１の動作を制御する（Ｓ３１：コンプライアンス制御工程）。このステップＳ３１において、ＣＰＵ３０１は、抜き取り作業中、ロボットハンド２０２に作用する力のうち、抜き取り方向と直交する方向（所定方向）の力成分が小さくなるようにロボットアーム２０１の動作を制御する。
【０１１４】
コンプライアンス制御モードでは、抜き取り作業中に生じる外力に対して、力算出部３３５が算出する力と、力記憶部３３４に記憶された力との差分が小さくなるように補正する。これにより、ワークＷ１，Ｗ２或いはロボットアーム２０１に過度な負荷が掛かるのが抑制され、ワークＷ１が抜けない、又はワークＷ１，Ｗ２に傷がつく等の問題が生じるのが防止される。
【０１１５】
次に、ＣＰＵ３０１は、抜き取り作業が完了したか否か、即ち作業完了位置に到達したか否かを判断する（Ｓ３２：作業完了判断工程）。ＣＰＵ３０１は、抜き取り作業が未完了であれば（Ｓ３２：Ｎｏ）、ステップＳ２９に戻り、抜き取り作業を継続する。
【０１１６】
ＣＰＵ３０１は、抜き取り作業が完了したと判断した場合は（Ｓ３２：Ｙｅｓ）、ロボットアーム２０１の制御をコンプライアンス制御モードから通常制御モードに切り替える（Ｓ３３）。そして、ＣＰＵ３０１は、ワークＷ１を所定の位置へ移動させる（Ｓ３４）。これにより本動作が終了する。
【０１１７】
以上、第２実施形態によれば、力センサを使用せずにコンプライアンス制御を実現できるので、力センサの許容荷重以上の力が必要な組み立て作業でも、コンプライアンス性を確保できる。
【０１１８】
また、第２実施形態によれば、ＣＰＵ３５１が２つのエンコーダ２３５，２３６の検出結果を用いて関節のトルクを算出してロボットハンド２０２に作用する力を算出している。したがって、エンコーダ２３５のみでコンプライアンス制御する場合よりもコンプライアンス制御の精度が向上する。
【０１１９】
また、第２実施形態では、コンプライアンス制御モード、通常制御モードのいずれの制御モードでも、各関節制御部３４０のＣＰＵ３５１は、エンコーダ２３５の角度検出値を位置指令に近づけるフィードバック制御を行う。モータ２３１の角度を直接検出するため、サーボ応答性が高く（サーボの応答が速く）、各関節を高速に目標関節角度に制御することができる。
【０１２０】
サーボ応答性が高いので、通常制御モード（移動制御モード）時には、ロボットハンド２０２を高速に作業開始位置へ到達させることができるので、作業時間を短縮することができる。よって、ロボット装置１００による製品の生産性が高まる。
【０１２１】
また、第２実施形態によれば、複数の関節制御部３４０それぞれにおいてトルクを算出するようにしたので、メイン制御部３３０における計算負荷が低減し、よりロボットアーム２０１の動作を高速化することができる。
【０１２２】
第２実施形態によれば、コンプライアンス制御モードにおいて、抜き取り作業を開始する前に算出した力のうち所定方向の力成分と、抜き取り作業の最中に算出した力のうち所定方向の力成分との差が小さくなるようにロボットアーム２０１の動作を制御する。即ち、抜き取り作業を開始する前の力を基準値とし、抜き取り作業中にロボットハンド２０２にかかる力が基準値に近づけられることとなる。これにより、ワークＷ１を把持後、抜き取り動作を実施する前に、ワークＷ１に対するロボットハンド２０２の位置ずれや、抜き取り軸に対する傾きによる外力を補正できる。よって、抜き取り作業中に、ワークＷ１，Ｗ２或いはロボットアーム２０１に過度な負荷が掛かるのが抑制され、抜き取り時にワークＷ１，Ｗ２が傷つくのが抑制され、抜き取り作業が円滑に行われる。
【０１２３】
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、本発明の実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、本発明の実施形態に記載されたものに限定されない。
【０１２４】
［その他の実施形態］
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【０１２５】
また、上記実施形態では、トルク計算を各関節制御部３４０が行う場合について説明したが、これに限定するものではなく、メイン制御部３３０が各関節のトルク計算を行ってもよい。この場合、各関節Ｊ１〜Ｊ６におけるエンコーダ２３５，２３６の検出結果は、メイン制御部３３０に送信されるようにすればよい。また、メイン制御部３３０のＣＰＵ３０１の数又は各関節制御部３４０のＣＰＵ３５１の数は、１つに限定するものではなく、複数あってもよい。
【０１２６】
また、上記実施形態では、複数の関節Ｊ１〜Ｊ６全てに減速機２３３が配置されている場合について説明したが、これに限定するものではなく、複数の関節Ｊ１〜Ｊ６のうち少なくとも１つの関節に減速機２３３が配置されている場合であってもよい。なお、ロボットアーム２０１の関節が１つの場合には、当該関節に減速機２３３が配置されていればよい。これらの場合、減速機２３３が配置されている関節について２つのエンコーダ２３５，２３６の検出値からトルクを計算すればよい。そして、トルク計算結果を用いて、ロボットハンド２０２に作用する力を計算すればよい。
【０１２７】
また、エンドエフェクタが、ロボットハンド２０２である場合について説明したが、これに限定するものではなく、例えば工具等が取り付けられたツールである場合であっても本発明は適用可能である。
【符号の説明】
【０１２８】
１００…ロボット装置、２０１…ロボットアーム、２０２…ロボットハンド（エンドエフェクタ）、２３０…関節駆動装置、２３１…モータ、２３３…減速機、２３５…エンコーダ（第１角度検出部）、２３６…エンコーダ（第２角度検出部）、３００…制御装置
【図１】
0006584102.tif 000004
【図２】
0006584102.tif 000005
【図３】
0006584102.tif 000006
【図４】
0006584102.tif 000007
【図５】
0006584102.tif 000008
【図６】
0006584102.tif 000009