(57)【特許請求の範囲】
【請求項１】
多関節のロボットアームと、
前記ロボットアームの先端に取り付けられたロボットハンドと、
前記ロボットハンドに把持された把持物の移動方向の力としての第１検出力及び前記移動方向と交差する方向の力としての第２検出力を検出する力検出器と、
前記ロボットアームの軌道データに基づき、前記把持物を前記移動方向にある対象物に向けて、離れた位置から移動作業を行うよう、前記ロボットアームの動作を制御する制御部と、を備え、
前記制御部は、
前記移動作業中、第１カットオフ周波数の第１ローパスフィルタにて、前記第１検出力に対してフィルタ処理を行う第１検出力補正処理と、
前記移動作業中、前記把持物と前記対象物とが接触して前記第１検出力が閾値を上回ったときに前記ロボットハンドの前記移動方向への移動を停止させる第１力制御処理と、
前記移動作業中、前記第１カットオフ周波数よりも周波数の低い第２カットオフ周波数の第２ローパスフィルタにて、前記第２検出力に対してフィルタ処理を行う第２検出力補正処理と、
前記移動作業中、前記第２検出力が小さくなるように前記ロボットアームの軌道を前記移動方向とは交差する方向に補正する第２力制御処理と、を実行することを特徴とするロボット装置。
【請求項２】
前記把持物及び前記対象物のうち、一方が嵌合物であり、他方が、前記嵌合物が嵌合する穴を有する被嵌合物であり、
前記制御部は、前記移動作業として、前記嵌合物を前記被嵌合物の穴に嵌合させる嵌合作業を行うことを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記嵌合物は、円柱状の円柱部と、前記円柱部の先端部と後端部との間の中間部における外周面に突設された突起部と、を有しており、
前記穴は、前記円柱部が嵌合する円柱状の円柱穴と、前記円柱穴に繋がり、前記突起部が嵌合する溝と、を有しており、
前記制御部は、前記嵌合作業として、前記円柱部を前記円柱穴に嵌合させ、かつ前記突起部を前記溝に嵌合させる作業中、前記円柱部の先端部を前記円柱穴に嵌合させて前記第１力制御処理にて前記把持物の前記移動方向の移動を停止させたとき、前記第１検出力が前記閾値を下回るまで、前記把持物を、前記移動方向を軸に回転させることを特徴とする請求項２に記載のロボット装置。
【請求項４】
多関節のロボットアームの先端に、力検出器を介してロボットハンドが取り付けられており、前記力検出器が、前記ロボットハンドに把持された把持物の移動方向の力としての第１検出力、及び前記移動方向と交差する方向の力としての第２検出力を検出するよう構成されており、制御部が、前記ロボットアームの軌道データに基づき、前記把持物を前記移動方向にある対象物に向けて、離れた位置から移動作業を行うよう前記ロボットアームの動作を制御するロボット制御方法であって、
前記制御部が、前記移動作業中、第１カットオフ周波数の第１ローパスフィルタにて、前記第１検出力に対してフィルタ処理を行う第１検出力補正工程と、
前記制御部が、前記移動作業中、前記把持物と前記対象物とが接触して前記第１検出力が閾値を上回ったときに前記ロボットハンドの前記移動方向への移動を停止させる第１力制御工程と、
前記制御部が、前記移動作業中、前記第１カットオフ周波数よりも周波数の低い第２カットオフ周波数の第２ローパスフィルタにて、前記第２検出力に対してフィルタ処理を行う第２検出力補正工程と、
前記制御部が、前記移動作業中、前記第２検出力が小さくなるように前記ロボットアームの軌道を前記移動方向とは交差する方向に補正する第２力制御工程と、を備えたことを特徴とするロボット制御方法。
【請求項５】
コンピュータに、請求項４に記載のロボット制御方法の各工程を実行させるためのプログラム。
【請求項６】
請求項５に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアームの力制御に関するものである。
【背景技術】
【０００２】
産業用のロボットは、多関節のロボットアームと、ロボットアームの先端に取り付けられたロボットハンドとを有して構成されている。この種のロボットを用いた嵌合作業や作業対象物取得作業では、作業対象物の公差や設置誤差、ロボットハンド内で把持中の作業対象物の把持位置ズレなどが生じる。これにより、作業対象物とロボットハンドとの間に、ロボットアームからロボットハンドに延びる先端軸に対して交差する方向に位置ズレが発生する。
【０００３】
そのため、ロボットの各関節の角度を角度指令値に制御する位置制御のみ行うと、作業対象物とロボットハンドとの位置ズレにより、嵌合ミスや作業対象物取得ミスが発生する。そこで、ロボットがロボットハンドに作用する力を検出する力検出器を有し、制御装置によりロボットアームの力制御が行われている（特許文献１参照）。
【０００４】
力制御には、軌道補正の制御と到達位置検知の制御とがある。軌道補正の制御は、嵌合時やワーク取得時に、作業対象物とロボットハンドとの位置ズレにより発生する接触力が力検出器で検出され、先端軸に交差する方向に発生した接触力が小さくなる（０に近づく）ようにロボットアームの軌道を補正する制御である。到達位置検知の制御は、先端軸の延びる方向であるアプローチ方向の到達位置を検知して、ロボットアームを停止させる制御である。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平５−６９３５８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
力検出器を有するロボットの構成では、ロボット動作時の振動等により、検出したい力以外の出力が力検出器の出力にノイズとして乗ることがある。力検出器の出力によって軌道補正の制御を行う際に、そのノイズの影響で補正が不安定になることがある。そのため、力検出器の出力にローパスフィルタをかけ、軌道補正の制御を行っている。
【０００７】
しかし、到達位置検知の制御を行う際に、軌道補正の制御を行う場合と同様、力検出器の出力に同様の特性のフィルタを通して制御すると、到達時に発生する力を低い感度でしか検出することができない。そのため、ロボットアームを停止させるタイミングが遅くなる問題があった。ロボットアームを停止させるタイミングが遅くなると、ロボットハンドや力検出器、即ちロボットに過負荷がかかった状態で停止することがあった。
【０００８】
逆に、到達位置検知の制御を優先して、高感度に力を検出するようにフィルタの特性を設定すれば、到達時に発生する力の過渡応答が速くなるので、ロボットへの過負荷を小さくすることは可能である。しかし、軌道補正の制御を行う際には、力検出結果に含まれる振動成分の除去が不十分であり、軌道の補正動作が不安定となっていた。
【０００９】
そこで、本発明は、移動方向と交差する方向のロボットハンドの位置ズレを安定して補正し、かつ移動方向のロボットハンドの到達を迅速に検知することを目的とする。
【課題を解決するための手段】
【００１０】
本発明のロボット装置は、多関節のロボットアームと、前記ロボットアームの先端に取り付けられたロボットハンドと、前記ロボットハンドに把持された把持物の移動方向の力としての第１検出力及び前記移動方向と交差する方向の力としての第２検出力を検出する力検出器と、前記ロボットアームの軌道データに基づき、前記把持物を前記移動方向にある対象物に向けて、離れた位置から移動作業を行うよう、前記ロボットアームの動作を制御する制御部と、を備え、前記制御部は、前記移動作業中、第１カットオフ周波数の第１ローパスフィルタにて、前記第１検出力に対してフィルタ処理を行う第１検出力補正処理と、前記移動作業中、前記把持物と前記対象物とが接触して前記第１検出力が閾値を上回ったときに前記ロボットハンドの前記移動方向への移動を停止させる第１力制御処理と、前記移動作業中、前記第１カットオフ周波数よりも周波数の低い第２カットオフ周波数の第２ローパスフィルタにて、前記第２検出力に対してフィルタ処理を行う第２検出力補正処理と、前記移動作業中、前記第２検出力が小さくなるように前記ロボットアームの軌道を前記移動方向とは交差する方向に補正する第２力制御処理と、を実行することを特徴とする。
【発明の効果】
【００１１】
本発明によれば、感度の高い到達位置検知の制御と安定した軌道補正の制御とが同時に実施される。
【図面の簡単な説明】
【００１２】
【図１】本発明の第１実施形態に係るロボット装置を示す模式図である。
【図２】本発明の第１実施形態に係るロボット装置を示すブロック図である。
【図３】本発明の第１実施形態に係るロボット装置のロボットコントローラの構成を示すブロック図である。
【図４】本発明の第１実施形態における嵌合物と被嵌合物との嵌合作業を行っている状態を示す説明図である。
【図５】本発明の第２実施形態に係るロボット装置を示す模式図である。
【図６】本発明の第２実施形態における嵌合物と被嵌合物との嵌合作業を説明するための説明図である。
【発明を実施するための形態】
【００１３】
以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
【００１４】
［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置を示す模式図である。図２は、本発明の第１実施形態に係るロボット装置を示すブロック図である。図１に示すロボット装置１００は、把持物と対象物との嵌合作業を行い組立部品Ｗを製造する製造方法を実行するものである。把持物及び対象物のうち、一方（第１実施形態では把持物）が、嵌合物Ｗ１であり、他方（第１実施形態では対象物）が、嵌合物Ｗ１が嵌合する穴Ｈ２を有する被嵌合物Ｗ２である。被嵌合物Ｗ２は、第１実施形態では、基台Ｂに載置されているものとするが、別のロボットに把持されていてもよい。嵌合物Ｗ１は、円柱状のピンであり、被嵌合物Ｗ２の穴Ｈ２は、嵌合物Ｗ１が嵌合する円柱状のピン穴である。
【００１５】
ロボット装置１００は、多関節のロボット２００と、ロボット２００にケーブル等で接続されたロボットコントローラ３００と、を備えている。
【００１６】
ロボット２００は、多関節（例えば垂直多関節）のロボットアーム２０１と、エンドエフェクタであるロボットハンド２０２と、ロボットハンド２０２（フィンガ）に作用した力を検出する力検出器２０３と、を有している。ロボットアーム２０１は、複数のリンクが複数の関節Ｊ１〜Ｊ６で旋回又は回転可能に連結されて構成されている。ロボットアーム２０１の基端（基端リンク、ベース部ともいう）が基台Ｂの上面に固定されている。ロボットアーム２０１の先端（先端リンク、フランジ部ともいう）には、力検出器２０３を介してロボットハンド２０２が取り付けられている。なお、図１では、ロボットアーム２０１の先端リンクが力検出器２０３と一体に形成されている場合について図示しているが、先端リンクと力検出器２０３とが別体に形成されていて、力検出器２０３が先端リンクに固定具等で固定されている場合であってもよい。ロボットハンド２０２は、複数のフィンガを有し、嵌合物Ｗ１を把持又は把持解放することができるよう構成されている。
【００１７】
ロボット２００（ロボットアーム２０１）の基端には、ワールド座標系Σ０が設定されている。ワールド座標系Σ０は、基台Ｂに垂直なＺ０軸と、Ｚ０軸に対して交差（直交）し、互いに交差（直交）するＸ０軸及びＹ０軸とで規定されている。
【００１８】
また、ロボット２００（ロボットアーム２０１）の先端には、手先座標系Σ１が設定されている。手先座標系Σ１は、ロボットアーム２０１の先端からロボットハンド２０２に向かって延びる先端軸であるＺ１軸と、Ｚ１軸に対して交差（直交）し、互いに交差（直交）するＸ１軸及びＹ１軸とで規定されている。
【００１９】
力検出器２０３は、いわゆる力覚センサであり、負荷されている力の大きさ及び方向を検出することがきるよう構成されている。即ち、力検出器２０３は、ロボットハンド２０２（フィンガ）に作用した、Ｘ１軸及びＹ１軸の方向の力（第２検出力）Ｆｘ，Ｆｙ、及びＺ１軸の方向の力（第１検出力）Ｆｚを検出する。
【００２０】
またロボットアーム２０１は、各関節Ｊ１〜Ｊ６に対応して設けられた、各関節Ｊ１〜Ｊ６を駆動するモータ（図２）２１１と、各モータ２１１の回転位置を検出する位置検出器２１２と、各モータ２１１の回転を制御するモータ制御器２１３と、を有している。
【００２１】
各モータ２１１は、ブラシレスＤＣモータやＡＣモータ等のサーボモータである。各位置検出器２１２は、ロータリエンコーダで構成されている。各モータ制御器２１３は、各位置検出器２１２で検出した各関節Ｊ１〜Ｊ６の角度がロボットコントローラ３００から取得した軌道、即ち角度指令値（位置指令値）となるように、各モータ２１１の回転を制御する。
【００２２】
図３は、本発明の第１実施形態に係るロボット装置１００のロボットコントローラ３００の構成を示すブロック図である。図３に示すように、ロボットコントローラ３００は、コンピュータで構成されている。ロボットコントローラ３００は、コンピュータの中枢となる制御部（演算部）としてのＣＰＵ（Central  Processing  Unit）３０１を備えている。また、ロボットコントローラ３００は、記憶部として、ＲＯＭ（Read  Only  Memory）３０２、ＲＡＭ（Random  Access  Memory）３０３、及びＨＤＤ（Hard  Disk  Drive）３０４を備えている。また、ロボットコントローラ３００は、記録ディスクドライブ３０５及び各種のインタフェース３１１〜３１４を備えている。
【００２３】
ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５及びインタフェース３１１〜３１４が、バス３１０を介して接続されている。
【００２４】
ＲＯＭ３０２には、ＢＩＯＳ等の起動プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ３０４には、プログラム３２１が格納（記録）される。そして、ＣＰＵ３０１がプログラム３２１を読み出して実行することにより、図２に示す各部３３２，３３３，３３５，３３６，３３７，３３８として機能し、後述するロボット制御方法の各工程を実行する。なお、ＨＤＤ３０４には、ユーザにより作成されたロボット動作プログラム３２０が格納される。記録ディスクドライブ３０５は、記録ディスク３２２に記録された各種データやプログラム等を読み出すことができる。
【００２５】
インタフェース３１１には、ロボット２００の各モータ制御器２１３が接続されている。ＣＰＵ３０１は、バス３１０及びインタフェース３１１を介して、各モータ制御器２１３に、関節Ｊ１〜Ｊ６の角度指令値（位置指令値）を出力し、ロボット２００の動作を制御する。
【００２６】
インタフェース３１２には、マウスやキーボード等、作業者が操作して、操作に応じた指令をＣＰＵ３０１に送信する入力装置６００が接続されている。インタフェース３１３には、画像を表示するディスプレイ等の表示装置７００が接続されている。インタフェース３１４には、ＵＳＢメモリ等の書き換え可能な不揮発性メモリ、或いは外付けＨＤＤ等の外部記憶装置８００が接続されている。
【００２７】
ＣＰＵ３０１は、軌道データに基づき、ロボットハンド２０２に把持された嵌合物Ｗ１と被嵌合物Ｗ２との嵌合作業を行うようロボット２００（ロボットアーム２０１）の動作を制御する。
【００２８】
軌道データは、ＣＰＵ３０１が、ロボット動作プログラム３２０に基づいて生成する。ロボット動作プログラム３２０は、ロボット２００に対してどのような動きをさせるかを指示する動作指示であり、ロボット専用のプログラミング言語を用いてユーザにより作成されたプログラムである。ロボット２００に何らかの動作を行わせる際には、ロボット動作プログラム３２０内で位置制御や力制御などの制御方法や位置、速度、力目標値等のパラメータをプログラムし、動作の指示を行う。
【００２９】
ＣＰＵ３０１は、図３に示すプログラム３２１を実行することにより、図２に示す軌道計算部３３２、位置出力部３３３、軌道補正計算部３３５、軌道補正フィルタ部３３６、到達位置判定部３３７及び到達位置判定フィルタ部３３８として機能する。
【００３０】
軌道計算部３３２は、ロボット動作プログラム３２０に記載されている情報をもとに、ロボット２００（ロボットアーム２０１）の経路を作成し、ロボット２００の経路に基づいて、ロボット２００（ロボットアーム２０１）の軌道を生成する。
【００３１】
ここで、ロボット２００の経路とは、ロボット２００の各関節Ｊ１〜Ｊ６の軌跡である。換言すれば、ロボット２００の経路とは、関節Ｊ１〜Ｊ６の角度を座標軸とした関節空間における点（ポーズ）の順序集合である。ロボット２００の軌道とは、時間をパラメータとしてポーズ（経路）を表したものであり、第１実施形態では、所定時間（例えば２ｍｓ）間隔のロボット２００の関節Ｊ１〜Ｊ６の角度指令値の集合である。
【００３２】
ここで、ＣＰＵ３０１によるロボットアーム２０１の動作の制御は、大別して、位置制御と力制御の２つの方法がある。
【００３３】
位置制御は、ロボット動作プログラム３２０により生成した軌道に従ってロボットアーム２０１を動作させる制御である。力制御は、ロボット動作プログラム３２０により生成した軌道に従ってロボットアーム２０１を動作させているときに、力検出器２０３により検出された力Ｆｘ，Ｆｙ，Ｆｚに基づいて動作を修正する制御である。
【００３４】
また、力制御は、軌道補正の制御と到達位置検知の制御とがある。軌道補正の制御は、第２検出力である力Ｆｘ，Ｆｙが小さくなるように（０に近づくように）ロボットハンド２０２をＸ１軸，Ｙ１軸の方向に移動させて、ロボットアーム２０１の軌道を補正する制御である。到達位置検知の制御は、第１検出力である力Ｆｚに対して閾値判定を行い、力Ｆｚが閾値を上回ったときにロボットハンド２０２のＺ１軸の方向の移動を停止させる制御である。
【００３５】
ユーザは、ＣＰＵ３０１（ロボットコントローラ３００）に位置制御でロボット２００を制御させる際は、位置制御を行う旨をロボット動作プログラム３２０に記載する。軌道計算部３３２は、このように記述されたロボット動作プログラム３２０に基づき、目標位置までの軌道を生成する。
【００３６】
位置出力部３３３は、生成された軌道（角度指令値）を、所定時間（例えば２ｍｓ）毎にモータ制御器２１３に通知する。モータ制御器２１３は、位置出力部３３３から受領した軌道と位置検出器２１２から取得した現在位置を用いてモータ２１１をフィードバック制御する。
【００３７】
この位置制御の際には、図２に示す軌道補正計算部３３５、軌道補正フィルタ部３３６、到達位置判定部３３７及び到達位置判定フィルタ部３３８は機能しない。
【００３８】
次に、ユーザは、ＣＰＵ３０１（ロボットコントローラ３００）に力制御でロボット２００を制御させる際は、力制御を行う旨をロボット動作プログラム３２０に記載する。ロボット動作プログラム３２０内では、到達位置検知の制御や軌道補正の制御の力制御を実施する旨が記載される。
【００３９】
ＣＰＵ３０１は、記載されたロボット動作プログラム３２０の情報を解析する。到達位置判定フィルタ部３３８は、ロボット動作プログラム３２０内で到達位置検知の制御が指定された場合に、ロボット動作プログラム３２０で指定されたローパスフィルタを設定する。軌道補正フィルタ部３３６は、ロボット動作プログラム３２０内で軌道補正の制御が指定された場合に、ロボット動作プログラム３２０で指定されたローパスフィルタを設定する。第１実施形態では、嵌合物Ｗ１と被嵌合物Ｗ２との嵌合作業を行う際には、軌道補正の制御及び到達位置検知の制御の両方を同時に実行する。
【００４０】
軌道補正フィルタ部３３６及び到達位置判定フィルタ部３３８は、力検出器２０３で出力された力Ｆｘ，Ｆｙ，Ｆｚのデータの高周波のノイズ成分をカットするためのローパスフィルタ（デジタルフィルタ）として機能する。各フィルタ部３３６，３３８は、カットオフ周波数ＦＣ１，ＦＣ２がパラメータとして指定され、そのパラメータはロボット動作プログラム３２０内で記載することができる。具体的には、軌道補正フィルタ部３３６は、第２カットオフ周波数であるカットオフ周波数ＦＣ１の第２ローパスフィルタとして動作し、力Ｆｘ，Ｆｙに対してフィルタ処理を行う。到達位置判定フィルタ部３３８は、嵌合作業中、第１カットオフ周波数であるカットオフ周波数ＦＣ２の第１ローパスフィルタとして動作し、力Ｆｚに対してフィルタ処理を行う。
【００４１】
軌道計算部３３２は、力制御が指定された場合、ロボット動作プログラム３２０に記載された動作指示通りに、位置制御と同様な方法で軌道を生成する。位置出力部３３３は、所定時間（例えば２ｍｓ）毎に角度指令値をモータ制御器２１３に通知する。モータ制御器２１３は、モータ２１１の回転をフィードバック制御し、ロボット２００を動作させる。
【００４２】
同時に、各フィルタ部３３６，３３８は、力検出器２０３よりロボット２００にかかる力Ｆｘ，Ｆｙ，Ｆｚを取得する。具体的には、軌道補正フィルタ部３３６は力Ｆｘ，Ｆｙを取得し、到達位置判定フィルタ部３３８は力Ｆｚを取得する。
【００４３】
軌道補正フィルタ部３３６は、嵌合作業中、カットオフ周波数ＦＣ１の第２ローパスフィルタを用いて、力Ｆｘ，Ｆｙに対してフィルタ処理を行う（第２検出力補正処理、第２検出力補正工程）。同時に、到達位置判定フィルタ部３３８は、嵌合作業中、カットオフ周波数ＦＣ２の第１ローパスフィルタを用いて、力Ｆｚに対してフィルタ処理を行う（第１検出力補正処理、第１検出力補正工程）。
【００４４】
軌道補正計算部３３５は、軌道補正フィルタ部３３６を通して取得した力Ｆｘ，Ｆｙのデータに合わせた（比例する）軌道補正量を計算し、軌道計算部３３２で計算した軌道に加えることで、軌道を補正する。即ち、軌道補正計算部３３５は、嵌合作業中、力Ｆｘ，Ｆｙが小さくなるように（０に近づくように）ロボットハンド２０２をＸ１軸，Ｙ１軸の方向に移動するよう、ロボットアーム２０１の軌道を補正する（第２力制御処理、第２力制御工程）。
【００４５】
位置出力部３３３は、補正された軌道データ（補正された角度指令値）をモータ制御器２１３に通知する。これにより、モータ制御器２１３は、補正された軌道データに基づいてモータ２１１の回転を制御する。このように、ＣＰＵ３０１は、力制御による軌道補正の制御を実施する。
【００４６】
一方、到達位置判定部３３７は、ロボット動作プログラム３２０に記載された力目標値（閾値）と到達位置判定フィルタ部３３８を通して取得した力Ｆｚのデータとを比較する。そして、到達位置判定部３３７は、力Ｆｚが力目標値（閾値）を上回ったタイミングで位置出力部３３３にロボットハンド２０２がＺ１軸の方向に移動するのを停止させる停止指令を通知する。即ち、到達位置判定部３３７は、嵌合作業中、力Ｆｚに対して閾値判定を行い、力Ｆｚが閾値を上回ったときにロボットハンド２０２のＺ１軸の方向の移動が停止するようロボットアーム２０１の動作を制御する（第１力制御処理，第１力制御工程）。
【００４７】
第１実施形態では、到達位置判定部３３７は、ロボットハンド２０２のＸ１軸の方向及びＹ１軸の方向の移動も停止するよう停止指令を通知する。つまり、第１実施形態では、停止指令はロボットアーム２０１の動作を停止させる指令である。
【００４８】
位置出力部３３３は、停止指令を受信したタイミングで各モータ制御器２１３へ停止するよう通知し、各モータ制御器２１３はモータ２１１を停止させる。このように、ＣＰＵ３０１は、力制御による到達位置検知の制御を実施する。
【００４９】
ここで、位置出力部３３３から停止指令をモータ制御器２１３が受信した際、モータ２１１を即停止させず、モータ制御器２１３が各モータ２１１を減速させて停止しても良い。また、位置出力部３３３で到達位置判定部３３７から停止指令を受信した際に、軌道計算部３３２を通して減速軌道を計算し、算出された軌道をモータ制御器２１３に通知して、停止させても良い。
【００５０】
次に、ロボットコントローラ３００によるロボットハンド２０２が嵌合物Ｗ１を把持し、嵌合物Ｗ１を被嵌合物Ｗ２の穴Ｈ２に嵌合させる嵌合作業を行う場合の力制御について、具体的に説明する。図４は、嵌合物Ｗ１と被嵌合物Ｗ２との嵌合作業を行っている状態を示す説明図である。
【００５１】
図４に示すように、被嵌合物Ｗ２の穴Ｈ２は、凹み穴であり、底部Ｂ２が形成されている。穴Ｈ２の開口端には、嵌合物Ｗ１を誘い込むための面取り部Ｃ２が形成されている。なお、面取り部は、嵌合物Ｗ１及び被嵌合物Ｗ２のうち少なくとも一方に形成されていればよく、嵌合物Ｗ１に面取り部が形成されていても、嵌合物Ｗ１と被嵌合物Ｗ２の両方に面取り部が形成されていてもよい。
【００５２】
嵌合物Ｗ１と被嵌合物Ｗ２はそれぞれ、不図示の供給装置により供給され、取得位置に供給された嵌合物Ｗ１をロボットハンド２０２で把持し、被嵌合物Ｗ２は組付け位置に供給される。
【００５３】
このような供給方法をとると、ロボットハンド２０２に対する嵌合物Ｗ１の位置ズレや、被嵌合物Ｗ２の組み付け位置に対する位置ズレにより、被嵌合物Ｗ２に対する嵌合物Ｗ１の相対的な位置ズレが生じることがある。また、嵌合物Ｗ１及び被嵌合物Ｗ２は製作時の公差分バラつきが生じている。
【００５４】
上記の要因から被嵌合物Ｗ２に対する嵌合物Ｗ１の組付け方向（図４に示すＺ１軸の方向）と組付け方向に鉛直な面方向（図４に示すＸ１，Ｙ１軸の方向）それぞれの相対位置ズレが生じる。位置制御だけで嵌合作業を行うと、被嵌合物Ｗ２の穴Ｈ２に嵌合物Ｗ１が嵌合されない場合がある。
【００５５】
そのため、第１実施形態では、穴Ｈ２の中心軸に対する嵌合物Ｗ１の中心軸の、Ｘ１，Ｙ１軸の方向の相対的なズレΔＸ，ΔＹを補正する軌道補正の制御と、Ｚ１軸の方向の到達位置検知の制御の２つの力制御を同時に行う。
【００５６】
ここで、デジタルフィルタを介した力検出の感度は、デジタルフィルタのカットオフ周波数で決まる。即ち、カットオフ周波数を高い周波数にするほど、感度が高くなり、逆にカットオフ周波数を低い周波数にするほど、感度が低くなる。
【００５７】
到達位置検知の制御を実施する際に、仮に接触したことを検知する感度が低い場合、嵌合物Ｗ１を底部Ｂ２に押し付け続けることとなり、ロボットハンド２０２と力検出器２０３、即ちロボット２００に過負荷がかかった状態を維持し続けることとなる。
【００５８】
そこで、第１実施形態では、到達位置検知の制御に使用する到達位置判定フィルタ部３３８におけるローパスフィルタのカットオフ周波数ＦＣ２を、カットオフ周波数ＦＣ１よりも高い周波数に設定し、急峻な力の変化を検知できるような設定とする。
【００５９】
また、嵌合物Ｗ１と被嵌合物Ｗ２の穴Ｈ２とのＸ１，Ｙ１軸の方向のズレを補正するための軌道補正の制御では、相対位置ズレの影響で、嵌合物Ｗ１の下端が面取り部Ｃ２に接触することで、Ｘ１，Ｙ１軸の方向に力Ｆｘ，Ｆｙが発生する。その発生した力Ｆｘ，Ｆｙを力検出器２０３で検出し、軌道補正計算部３３５は、軌道補正の制御を実施する。
【００６０】
ここで、力検出器２０３はロボット２００に設置されているため、面取り部Ｃ２に接触して発生した力以外もロボット２００（ロボットアーム２０１）の振動によるノイズが同時に検出される。そのため、軌道補正フィルタ部３３６で使用するローパスフィルタのカットオフ周波数ＦＣ１をカットオフ周波数ＦＣ２よりも低い周波数に設定し、力の急峻な変化を無視するような設定とする。
【００６１】
例えば、カットオフ周波数ＦＣ１を２０Ｈｚに設定し、カットオフ周波数ＦＣ２を１００Ｈｚに設定する。これらカットオフ周波数ＦＣ１，ＦＣ２は、実験を行って決定すればよい。
【００６２】
以上、第１実施形態によれば、軌道補正フィルタ部３３６に使用する第２ローパスフィルタと、到達位置判定フィルタ部３３８に使用する第１ローパスフィルタとを別々に設定している。そして、第１ローパスフィルタのカットオフ周波数ＦＣ２を第２ローパスフィルタのカットオフ周波数ＦＣ１よりも高い周波数に設定している。
【００６３】
これにより、軌道補正の制御では、軌道補正フィルタ部３３６によって、力検出器２０３により検出された力Ｆｘ，Ｆｙにおいて周波数ＦＣ１以上の高周波のノイズがカットされる。したがって、安定した軌道補正を実施することができる。
【００６４】
また、到達位置検知の制御では、軌道補正フィルタ部３３６により、力検出器２０３により検出された力Ｆｚにおいて、周波数ＦＣ２以上の周波数のノイズはカットされ、周波数ＦＣ１〜ＦＣ２の周波数成分については、カットされずに残っている。よって、軌道補正フィルタ部３３６に用いるフィルタよりも感度良く力を検出することができる。これにより、Ｚ１軸方向の到達位置に達した時に発生する力Ｆｚを感度よく検知することが可能になる。
【００６５】
このように、感度の高い到達位置検知の制御と安定した軌道補正の制御とが同時に実施されるので、嵌合物Ｗ１と被嵌合物Ｗ２との安定した嵌合作業を実現することが可能になる。
【００６６】
［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置によるロボット制御について説明する。図５は、本発明の第２実施形態に係るロボット装置を示す模式図である。なお、第２実施形態のロボット装置は、上記第１実施形態のロボット装置と同様の構成であるため、同一符号を付して説明を省略する。第２実施形態では、嵌合を行う嵌合物及び被嵌合物の形状が上記第１実施形態と異なる。なお、ロボットハンド２０２は、ロボットアーム２０１の先端リンクが関節Ｊ６で回転することによりＺ１軸を中心に回転する。
【００６７】
嵌合物Ｗ１０は、円柱状の円柱部Ｗ１１と、円柱部Ｗ１１の中心軸の延びる方向の先端部と後端部との間の中間部における外周面に突設された突起部であるボスＷ１２と、を有している。被嵌合物Ｗ２０には、穴Ｈ２０が形成されている。穴Ｈ２０は、円柱部Ｗ１１が嵌合する円柱状の円柱穴Ｈ２１と、円柱穴Ｈ２１に繋がり、ボスＷ１２が嵌合する溝であるボス穴Ｈ２２と、を有している。
【００６８】
第２実施形態では、ロボットハンド２０２が把持する把持物が、嵌合物Ｗ１０であり、対象物が被嵌合物Ｗ２０である。第２実施形態では、嵌合作業として、円柱部Ｗ１１を円柱穴Ｈ２１に嵌合させ、かつボスＷ１２をボス穴Ｈ２２に嵌合させる作業を行う。
【００６９】
以下、嵌合物Ｗ１０の回転し押し付けながらボスＷ１２をボス穴Ｈ２２に嵌合させる動作について説明する。
【００７０】
ＣＰＵ３０１は、ロボットハンド２０２によってボスＷ１２を有した嵌合物Ｗ１０を把持させ、ロボットハンド２０２が把持している嵌合物Ｗ１０を被嵌合物Ｗ２０に設けられた穴Ｈ２０に嵌合する嵌合作業を行う。この嵌合作業はボスＷ１２をボス穴Ｈ２２に嵌合することで完了する。
【００７１】
円柱穴Ｈ２１は上記第１実施形態の穴Ｈ２と同様の凹み穴であり、円柱穴Ｈ２１の開口端に、円柱部Ｗ１１の先端部を誘い込むための面取り部が形成されている。なお、面取り部は、円柱部Ｗ１１及び円柱穴Ｈ２１のうち少なくとも一方に形成されていればよく、円柱部Ｗ１１に面取り部が形成されていても、円柱部Ｗ１１と円柱穴Ｈ２１の両方に面取り部が形成されていてもよい。
【００７２】
また、嵌合物Ｗ１０及び被嵌合物Ｗ２０は、上記第１実施形態と同様の方法で供給される。また、嵌合物Ｗ１０及び被嵌合物Ｗ２０の公差、嵌合物Ｗ１０の把持位置、被嵌合物Ｗ２０の設置位置の影響で、被嵌合物Ｗ２０に対して嵌合物Ｗ１０に相対位置のズレが生じる。このため、第２実施形態においても、到達位置検知の制御と軌道補正の制御の２つの力制御で嵌合作業を行う。
【００７３】
ここで、ボスＷ１２は、円柱部Ｗ１１の先端部（下端部）より一定距離上方に離れた場所に取り付けられている。
【００７４】
図６は、本発明の第２実施形態における嵌合物Ｗ１０と被嵌合物Ｗ２０との嵌合作業を説明するための説明図である。図６（ａ）は、嵌合作業の途中状態を示し、図６（ｂ）は、嵌合作業の終了状態を示している。
【００７５】
ＣＰＵ３０１は、図６（ａ）に示すように、円柱部Ｗ１１の先端部を穴Ｈ２０の円柱穴Ｈ２１に嵌合する工程を実施した後、図６（ｂ）に示すように、ボスＷ１２をボス穴Ｈ２２に嵌合するために位相を合わせて嵌合する２つの工程を実施する。
【００７６】
まず、図６（ａ）に示す円柱部Ｗ１１の先端部を穴Ｈ２０に嵌合する処理は、上記第１実施形態に記載の内容とほぼ同様の方法で嵌合作業を行うため、詳細な説明は省略する。上記第１実施形態と異なる点として、第２実施形態では、ＣＰＵ３０１は、到達位置検知の制御により、ボスＷ１２を被嵌合物Ｗ２０上面に接触させたとき、ロボットアーム２０１の動作を停止させる点である。即ち、到達位置判定部３３７は、嵌合作業中、円柱部Ｗ１１の先端部を円柱穴Ｈ２１に嵌合させて、力Ｆｚが閾値を上回ったとき、嵌合物Ｗ１０のＺ１軸の方向の移動を停止させる。
【００７７】
次に、ＣＰＵ３０１は、ボスＷ１２をボス穴Ｈ２２に嵌合させる際に、図６（ｂ）に示すように、ロボットハンド２０２で把持している嵌合物Ｗ１０の円柱部Ｗ１１の先端部を円柱穴Ｈ２１に嵌合させている状態で、嵌合物Ｗ１０をＺ１軸まわりに回転させる。
【００７８】
即ち、ＣＰＵ３０１は、嵌合作業中、円柱部Ｗ１１の先端部を円柱穴Ｈ２１に嵌合させて第１力制御処理にて嵌合物Ｗ１０のＺ１軸方向の移動を停止させたとき、力Ｆｚが閾値を下回るまで、嵌合物Ｗ１０を、Ｚ１軸を中心に回転させる。具体的には、ロボットハンド２０２をロボットアーム２０１に対してＺ１軸を中心に回転させる。
【００７９】
ボスＷ１２がボス穴Ｈ２２に到達すると、ボス穴Ｈ２２に到達したタイミングでＺ１軸方向の力Ｆｚが抜け、力Ｆｚが閾値を下回る。その力Ｆｚを到達位置検知の力制御で検知することで、ボスＷ１２をボス穴Ｈ２２に嵌合するための位相が検知可能になる。
【００８０】
ここで、ボス穴Ｈ２２の位相を検知する際、仮に到達位置検知の感度が低いと、組付け位相に到達しても閾値を下回る力を検知せずに通り過ぎてしまい、ボス穴Ｈ２２の位相を検知することができなくなるおそれがある。そのため、上記第１実施形態と同様、到達位置検知に使用する到達位置判定フィルタ部３３８で使用するフィルタのカットオフ周波数ＦＣ２をカットオフ周波数ＦＣ１よりも大きく設定し、急峻な力の変化を検知できるような設定とする。その後、検知した位相で嵌合物Ｗ１０をＺ１軸の方向に押し込んで、再度、閾値判定を行うことにより、嵌合物Ｗ１０を穴Ｈ２０に嵌合する嵌合作業が完了する。
【００８１】
ここで、嵌合物Ｗ１０を回転させる際、嵌合物Ｗ１０を回転させる回転軸と穴Ｈ２０の回転軸にズレが生じている場合、回転中にＸ１，Ｙ１軸の方向の力Ｆｘ，Ｆｙが生じることがある。
【００８２】
よって、第２実施形態では、ロボットハンド２０２と力検出器２０３、即ちロボット２００に過負荷がかかるのを回避するため、嵌合物Ｗ１０を回転させるのと同時に、回転軸のズレを補正するための軌道補正の制御を実施する。
【００８３】
この軌道補正の制御では、回転時のロボット２００の振動によるノイズを検出し、嵌合物Ｗ１０と穴Ｈ２０が接触することによって発生する力以外の力も同時に検出する。そのため、上記第１実施形態と同様、軌道補正フィルタ部３３６で使用するフィルタのカットオフ周波数ＦＣ１を小さくし、力の急峻な変化を無視するような設定とする。
【００８４】
以上の如くカットオフ周波数ＦＣ１，ＦＣ２を設定することで、感度の高い到達位置検知によりボスＷ１２を組付ける位相を検知するのと同時に、嵌合物Ｗ１０と穴Ｈ２０との相対位置ズレにより発生する回転軸のズレを吸収した嵌合作業を実現できる。
【００８５】
なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。
【００８６】
上記第２実施形態では、力Ｆｚを検知して到達位置検知を実施したが、力Ｆｚだけでなく、力Ｆｚの時間変化量を使用して、到達位置検知を実施しても良い。また、上記第２実施形態で使用した到達位置検知では、挿入方向の力Ｆｚをモニタリングして検知していたが、Ｚ軸まわりの回転方向の力成分を利用して、到達位置検知を実施してもよい。
【００８７】
上記第１，第２実施形態では、ロボットハンドが嵌合物を把持して嵌合物と被嵌合物との嵌合作業を行う場合について説明したが、これに限定するものではない。ロボットハンドが被嵌合物を把持して嵌合作業を行う場合であっても本発明は適用可能である。
【００８８】
上記実施形態の各処理動作は具体的にはＣＰＵ３０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体をロボットコントローラに供給し、ロボットコントローラを構成するコンピュータが記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。
【００８９】
また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４であり、ＨＤＤ３０４にプログラム３２１が格納される場合について説明したが、これに限定するものではない。プログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図３に示すＲＯＭ３０２、記録ディスク３２２、外部記憶装置８００等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、書き換え可能な不揮発性のメモリ（例えばＵＳＢメモリ）、ＲＯＭ等を用いることができる。
【００９０】
また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。
【００９１】
また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。
【００９２】
さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。
【００９３】
また、上記実施形態では、コンピュータがＨＤＤ等の記録媒体に記録されたプログラムを実行することにより、処理を行う場合について説明したが、これに限定するものではない。プログラムに基づいて動作する演算部の一部又は全部の機能をＡＳＩＣやＦＰＧＡ等の専用ＬＳＩで構成してもよい。なお、ＡＳＩＣはApplication  Specific  Integrated  Circuit、ＦＰＧＡはField-Programmable  Gate  Arrayの頭字語である。
【符号の説明】
【００９４】
１００…ロボット装置、２０１…ロボットアーム、２０２…ロボットハンド、２０３…力検出器、３０１…ＣＰＵ（制御部）、ＦＣ１…カットオフ周波数（第２カットオフ周波数）、ＦＣ２…カットオフ周波数（第１カットオフ周波数）
【図１】
0006460690.tif 000002
【図２】
0006460690.tif 000003
【図３】
0006460690.tif 000004
【図４】
0006460690.tif 000005
【図５】
0006460690.tif 000006
【図６】
0006460690.tif 000007