JP2016059971A - ロボット装置、ロボット制御方法、プログラム及び記録媒体 - Google Patents

Abstract

【課題】先端軸に交差する方向のロボットハンドの位置ズレを安定して補正し、かつ先端軸の延びる方向のロボットハンドの到達を迅速に検知する。
【解決手段】ロボットアーム２０１とロボットハンドとの間に設けられた力検出器２０３は力Ｆｘ，Ｆｙ及び力Ｆｚを検出する。ロボットコントローラ３００は、嵌合作業中、カットオフ周波数Ｆ_Ｃ１の第１ローパスフィルタにて、力Ｆｘ，Ｆｙに対してフィルタ処理を行い、力Ｆｘ，Ｆｙが小さくなるようにロボットハンドを移動させて、前記ロボットアーム２０１の軌道を補正し、カットオフ周波数Ｆ_Ｃ１よりも周波数の高いカットオフ周波数Ｆ_Ｃ２の第２ローパスフィルタにて、力Ｆｚに対してフィルタ処理を行い、力Ｆｚに対して閾値判定を行い、力Ｆｚが閾値を上回ったときにロボットハンドの移動を停止させる。
【選択図】図２

Description

本発明は、ロボットアームの力制御に関するものである。

産業用のロボットは、多関節のロボットアームと、ロボットアームの先端に取り付けられたロボットハンドとを有して構成されている。この種のロボットを用いた嵌合作業や作業対象物取得作業では、作業対象物の公差や設置誤差、ロボットハンド内で把持中の作業対象物の把持位置ズレなどが生じる。これにより、作業対象物とロボットハンドとの間に、ロボットアームからロボットハンドに延びる先端軸に対して交差する方向に位置ズレが発生する。

そのため、ロボットの各関節の角度を角度指令値に制御する位置制御のみ行うと、作業対象物とロボットハンドとの位置ズレにより、嵌合ミスや作業対象物取得ミスが発生する。そこで、ロボットがロボットハンドに作用する力を検出する力検出器を有し、制御装置によりロボットアームの力制御が行われている（特許文献１参照）。

力制御には、軌道補正の制御と到達位置検知の制御とがある。軌道補正の制御は、嵌合時やワーク取得時に、作業対象物とロボットハンドとの位置ズレにより発生する接触力が力検出器で検出され、先端軸に交差する方向に発生した接触力が小さくなる（０に近づく）ようにロボットアームの軌道を補正する制御である。到達位置検知の制御は、先端軸の延びる方向であるアプローチ方向の到達位置を検知して、ロボットアームを停止させる制御である。

特開平５−６９３５８号公報

力検出器を有するロボットの構成では、ロボット動作時の振動等により、検出したい力以外の出力が力検出器の出力にノイズとして乗ることがある。力検出器の出力によって軌道補正の制御を行う際に、そのノイズの影響で補正が不安定になることがある。そのため、力検出器の出力にローパスフィルタをかけ、軌道補正の制御を行っている。

しかし、到達位置検知の制御を行う際に、軌道補正の制御を行う場合と同様、力検出器の出力に同様の特性のフィルタを通して制御すると、到達時に発生する力を低い感度でしか検出することができない。そのため、ロボットアームを停止させるタイミングが遅くなる問題があった。ロボットアームを停止させるタイミングが遅くなると、ロボットハンドや力検出器、即ちロボットに過負荷がかかった状態で停止することがあった。

逆に、到達位置検知の制御を優先して、高感度に力を検出するようにフィルタの特性を設定すれば、到達時に発生する力の過渡応答が速くなるので、ロボットへの過負荷を小さくすることは可能である。しかし、軌道補正の制御を行う際には、力検出結果に含まれる振動成分の除去が不十分であり、軌道の補正動作が不安定となっていた。

そこで、本発明は、先端軸に交差する方向のロボットハンドの位置ズレを安定して補正し、かつ先端軸の延びる方向のロボットハンドの到達を迅速に検知することを目的とする。

本発明のロボット装置は、多関節のロボットアームと、前記ロボットアームの先端に取り付けられたロボットハンドと、前記ロボットアームから前記ロボットハンドに延びる先端軸に対して交差する第１方向の第１検出力及び前記先端軸の延びる第２方向の第２検出力を検出する力検出器と、前記ロボットアームの軌道データに基づき、前記ロボットハンドに把持された把持物と対象物との嵌合作業を行うよう、前記ロボットアームの動作を制御する制御部と、を備え、前記制御部は、前記嵌合作業中、第１カットオフ周波数の第１ローパスフィルタにて、前記第１検出力に対してフィルタ処理を行う第１検出力補正処理と、前記嵌合作業中、前記第１検出力が小さくなるように前記ロボットハンドを前記第１方向に移動させて、前記ロボットアームの軌道を補正する第１力制御処理と、前記嵌合作業中、前記第１カットオフ周波数よりも周波数の高い第２カットオフ周波数の第２ローパスフィルタにて、前記第２検出力に対してフィルタ処理を行う第２検出力補正処理と、前記嵌合作業中、前記第２検出力に対して閾値判定を行い、前記第２検出力が前記閾値を上回ったときに前記ロボットハンドの前記第２方向の移動を停止させる第２力制御処理と、を実行することを特徴とする。

本発明によれば、感度の高い到達位置検知の制御と安定した軌道補正の制御とが同時に実施されるので、把持物と対象物との安定した嵌合作業を実現することが可能になる。

本発明の第１実施形態に係るロボット装置を示す模式図である。 本発明の第１実施形態に係るロボット装置を示すブロック図である。 本発明の第１実施形態に係るロボット装置のロボットコントローラの構成を示すブロック図である。 本発明の第１実施形態における嵌合物と被嵌合物との嵌合作業を行っている状態を示す説明図である。 本発明の第２実施形態に係るロボット装置を示す模式図である。 本発明の第２実施形態における嵌合物と被嵌合物との嵌合作業を説明するための説明図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。

［第１実施形態］
図１は、本発明の第１実施形態に係るロボット装置を示す模式図である。図２は、本発明の第１実施形態に係るロボット装置を示すブロック図である。図１に示すロボット装置１００は、把持物と対象物との嵌合作業を行い組立部品Ｗを製造する製造方法を実行するものである。把持物及び対象物のうち、一方（第１実施形態では把持物）が、嵌合物Ｗ_１であり、他方（第１実施形態では対象物）が、嵌合物Ｗ_１が嵌合する穴Ｈ_２を有する被嵌合物Ｗ_２である。被嵌合物Ｗ_２は、第１実施形態では、基台Ｂに載置されているものとするが、別のロボットに把持されていてもよい。嵌合物Ｗ_１は、円柱状のピンであり、被嵌合物Ｗ_２の穴Ｈ_２は、嵌合物Ｗ_１が嵌合する円柱状のピン穴である。

ロボット装置１００は、多関節のロボット２００と、ロボット２００にケーブル等で接続されたロボットコントローラ３００と、を備えている。

ロボット２００は、多関節（例えば垂直多関節）のロボットアーム２０１と、エンドエフェクタであるロボットハンド２０２と、ロボットハンド２０２（フィンガ）に作用した力を検出する力検出器２０３と、を有している。ロボットアーム２０１は、複数のリンクが複数の関節Ｊ_１〜Ｊ_６で旋回又は回転可能に連結されて構成されている。ロボットアーム２０１の基端（基端リンク、ベース部ともいう）が基台Ｂの上面に固定されている。ロボットアーム２０１の先端（先端リンク、フランジ部ともいう）には、力検出器２０３を介してロボットハンド２０２が取り付けられている。なお、図１では、ロボットアーム２０１の先端リンクが力検出器２０３と一体に形成されている場合について図示しているが、先端リンクと力検出器２０３とが別体に形成されていて、力検出器２０３が先端リンクに固定具等で固定されている場合であってもよい。ロボットハンド２０２は、複数のフィンガを有し、嵌合物Ｗ_１を把持又は把持解放することができるよう構成されている。

ロボット２００（ロボットアーム２０１）の基端には、ワールド座標系Σ_０が設定されている。ワールド座標系Σ_０は、基台Ｂに垂直なＺ_０軸と、Ｚ_０軸に対して交差（直交）し、互いに交差（直交）するＸ_０軸及びＹ_０軸とで規定されている。

また、ロボット２００（ロボットアーム２０１）の先端には、手先座標系Σ_１が設定されている。手先座標系Σ_１は、ロボットアーム２０１の先端からロボットハンド２０２に向かって延びる先端軸であるＺ_１軸と、Ｚ_１軸に対して交差（直交）し、互いに交差（直交）するＸ_１軸及びＹ_１軸とで規定されている。

力検出器２０３は、いわゆる力覚センサであり、負荷されている力の大きさ及び方向を検出することがきるよう構成されている。即ち、力検出器２０３は、ロボットハンド２０２（フィンガ）に作用した、Ｘ_１軸及びＹ_１軸の方向（第１方向）の力（第１検出力）Ｆｘ，Ｆｙ、及びＺ_１軸の方向（第２方向）の力（第２検出力）Ｆｚを検出する。

またロボットアーム２０１は、各関節Ｊ_１〜Ｊ_６に対応して設けられた、各関節Ｊ_１〜Ｊ_６を駆動するモータ（図２）２１１と、各モータ２１１の回転位置を検出する位置検出器２１２と、各モータ２１１の回転を制御するモータ制御器２１３と、を有している。

各モータ２１１は、ブラシレスＤＣモータやＡＣモータ等のサーボモータである。各位置検出器２１２は、ロータリエンコーダで構成されている。各モータ制御器２１３は、各位置検出器２１２で検出した各関節Ｊ_１〜Ｊ_６の角度がロボットコントローラ３００から取得した軌道、即ち角度指令値（位置指令値）となるように、各モータ２１１の回転を制御する。

図３は、本発明の第１実施形態に係るロボット装置１００のロボットコントローラ３００の構成を示すブロック図である。図３に示すように、ロボットコントローラ３００は、コンピュータで構成されている。ロボットコントローラ３００は、コンピュータの中枢となる制御部（演算部）としてのＣＰＵ（Central Processing Unit）３０１を備えている。また、ロボットコントローラ３００は、記憶部として、ＲＯＭ（Read Only Memory）３０２、ＲＡＭ（Random Access Memory）３０３、及びＨＤＤ（Hard Disk Drive）３０４を備えている。また、ロボットコントローラ３００は、記録ディスクドライブ３０５及び各種のインタフェース３１１〜３１４を備えている。

ＣＰＵ３０１には、ＲＯＭ３０２、ＲＡＭ３０３、ＨＤＤ３０４、記録ディスクドライブ３０５及びインタフェース３１１〜３１４が、バス３１０を介して接続されている。

ＲＯＭ３０２には、ＢＩＯＳ等の起動プログラムが格納されている。ＲＡＭ３０３は、ＣＰＵ３０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ３０４には、プログラム３２１が格納（記録）される。そして、ＣＰＵ３０１がプログラム３２１を読み出して実行することにより、図２に示す各部３３２，３３３，３３５，３３６，３３７，３３８として機能し、後述するロボット制御方法の各工程を実行する。なお、ＨＤＤ３０４には、ユーザにより作成されたロボット動作プログラム３２０が格納される。記録ディスクドライブ３０５は、記録ディスク３２２に記録された各種データやプログラム等を読み出すことができる。

インタフェース３１１には、ロボット２００の各モータ制御器２１３が接続されている。ＣＰＵ３０１は、バス３１０及びインタフェース３１１を介して、各モータ制御器２１３に、関節Ｊ_１〜Ｊ_６の角度指令値（位置指令値）を出力し、ロボット２００の動作を制御する。

インタフェース３１２には、マウスやキーボード等、作業者が操作して、操作に応じた指令をＣＰＵ３０１に送信する入力装置６００が接続されている。インタフェース３１３には、画像を表示するディスプレイ等の表示装置７００が接続されている。インタフェース３１４には、ＵＳＢメモリ等の書き換え可能な不揮発性メモリ、或いは外付けＨＤＤ等の外部記憶装置８００が接続されている。

ＣＰＵ３０１は、軌道データに基づき、ロボットハンド２０２に把持された嵌合物Ｗ_１と被嵌合物Ｗ_２との嵌合作業を行うようロボット２００（ロボットアーム２０１）の動作を制御する。

軌道データは、ＣＰＵ３０１が、ロボット動作プログラム３２０に基づいて生成する。ロボット動作プログラム３２０は、ロボット２００に対してどのような動きをさせるかを指示する動作指示であり、ロボット専用のプログラミング言語を用いてユーザにより作成されたプログラムである。ロボット２００に何らかの動作を行わせる際には、ロボット動作プログラム３２０内で位置制御や力制御などの制御方法や位置、速度、力目標値等のパラメータをプログラムし、動作の指示を行う。

ＣＰＵ３０１は、図３に示すプログラム３２１を実行することにより、図２に示す軌道計算部３３２、位置出力部３３３、軌道補正計算部３３５、軌道補正フィルタ部３３６、到達位置判定部３３７及び到達位置判定フィルタ部３３８として機能する。

軌道計算部３３２は、ロボット動作プログラム３２０に記載されている情報をもとに、ロボット２００（ロボットアーム２０１）の経路を作成し、ロボット２００の経路に基づいて、ロボット２００（ロボットアーム２０１）の軌道を生成する。

ここで、ロボット２００の経路とは、ロボット２００の各関節Ｊ_１〜Ｊ_６の軌跡である。換言すれば、ロボット２００の経路とは、関節Ｊ_１〜Ｊ_６の角度を座標軸とした関節空間における点（ポーズ）の順序集合である。ロボット２００の軌道とは、時間をパラメータとしてポーズ（経路）を表したものであり、第１実施形態では、所定時間（例えば２ｍｓ）間隔のロボット２００の関節Ｊ_１〜Ｊ_６の角度指令値の集合である。

ここで、ＣＰＵ３０１によるロボットアーム２０１の動作の制御は、大別して、位置制御と力制御の２つの方法がある。

位置制御は、ロボット動作プログラム３２０により生成した軌道に従ってロボットアーム２０１を動作させる制御である。力制御は、ロボット動作プログラム３２０により生成した軌道に従ってロボットアーム２０１を動作させているときに、力検出器２０３により検出された力Ｆｘ，Ｆｙ，Ｆｚに基づいて動作を修正する制御である。

また、力制御は、軌道補正の制御と到達位置検知の制御とがある。軌道補正の制御は、第１検出力である力Ｆｘ，Ｆｙが小さくなるように（０に近づくように）ロボットハンド２０２をＸ_１軸，Ｙ_１軸の方向に移動させて、ロボットアーム２０１の軌道を補正する制御である。到達位置検知の制御は、第２検出力である力Ｆｚに対して閾値判定を行い、力Ｆｚが閾値を上回ったときにロボットハンド２０２のＺ_１軸の方向の移動を停止させる制御である。

ユーザは、ＣＰＵ３０１（ロボットコントローラ３００）に位置制御でロボット２００を制御させる際は、位置制御を行う旨をロボット動作プログラム３２０に記載する。軌道計算部３３２は、このように記述されたロボット動作プログラム３２０に基づき、目標位置までの軌道を生成する。

位置出力部３３３は、生成された軌道（角度指令値）を、所定時間（例えば２ｍｓ）毎にモータ制御器２１３に通知する。モータ制御器２１３は、位置出力部３３３から受領した軌道と位置検出器２１２から取得した現在位置を用いてモータ２１１をフィードバック制御する。

この位置制御の際には、図２に示す軌道補正計算部３３５、軌道補正フィルタ部３３６、到達位置判定部３３７及び到達位置判定フィルタ部３３８は機能しない。

次に、ユーザは、ＣＰＵ３０１（ロボットコントローラ３００）に力制御でロボット２００を制御させる際は、力制御を行う旨をロボット動作プログラム３２０に記載する。ロボット動作プログラム３２０内では、到達位置検知の制御や軌道補正の制御の力制御を実施する旨が記載される。

ＣＰＵ３０１は、記載されたロボット動作プログラム３２０の情報を解析する。到達位置判定フィルタ部３３８は、ロボット動作プログラム３２０内で到達位置検知の制御が指定された場合に、ロボット動作プログラム３２０で指定されたローパスフィルタを設定する。軌道補正フィルタ部３３６は、ロボット動作プログラム３２０内で軌道補正の制御が指定された場合に、ロボット動作プログラム３２０で指定されたローパスフィルタを設定する。第１実施形態では、嵌合物Ｗ_１と被嵌合物Ｗ_２との嵌合作業を行う際には、軌道補正の制御及び到達位置検知の制御の両方を同時に実行する。

軌道補正フィルタ部３３６及び到達位置判定フィルタ部３３８は、力検出器２０３で出力された力Ｆｘ，Ｆｙ，Ｆｚのデータの高周波のノイズ成分をカットするためのローパスフィルタ（デジタルフィルタ）として機能する。各フィルタ部３３６，３３８は、カットオフ周波数Ｆ_Ｃ１，Ｆ_Ｃ２がパラメータとして指定され、そのパラメータはロボット動作プログラム３２０内で記載することができる。具体的には、軌道補正フィルタ部３３６は、第１カットオフ周波数であるカットオフ周波数Ｆ_Ｃ１の第１ローパスフィルタとして動作し、力Ｆｘ，Ｆｙに対してフィルタ処理を行う。到達位置判定フィルタ部３３８は、嵌合作業中、第２カットオフ周波数であるカットオフ周波数Ｆ_Ｃ２の第２ローパスフィルタとして動作し、力Ｆｚに対してフィルタ処理を行う。

軌道計算部３３２は、力制御が指定された場合、ロボット動作プログラム３２０に記載された動作指示通りに、位置制御と同様な方法で軌道を生成する。位置出力部３３３は、所定時間（例えば２ｍｓ）毎に角度指令値をモータ制御器２１３に通知する。モータ制御器２１３は、モータ２１１の回転をフィードバック制御し、ロボット２００を動作させる。

同時に、各フィルタ部３３６，３３８は、力検出器２０３よりロボット２００にかかる力Ｆｘ，Ｆｙ，Ｆｚを取得する。具体的には、軌道補正フィルタ部３３６は力Ｆｘ，Ｆｙを取得し、到達位置判定フィルタ部３３８は力Ｆｚを取得する。

軌道補正フィルタ部３３６は、嵌合作業中、カットオフ周波数Ｆ_Ｃ１の第１ローパスフィルタを用いて、力Ｆｘ，Ｆｙに対してフィルタ処理を行う（第１検出力補正処理、第１検出力補正工程）。同時に、到達位置判定フィルタ部３３８は、嵌合作業中、カットオフ周波数Ｆ_Ｃ２の第２ローパスフィルタを用いて、力Ｆｚに対してフィルタ処理を行う（第２検出力補正処理、第２検出力補正工程）。

軌道補正計算部３３５は、軌道補正フィルタ部３３６を通して取得した力Ｆｘ，Ｆｙのデータに合わせた（比例する）軌道補正量を計算し、軌道計算部３３２で計算した軌道に加えることで、軌道を補正する。即ち、軌道補正計算部３３５は、嵌合作業中、力Ｆｘ，Ｆｙが小さくなるように（０に近づくように）ロボットハンド２０２をＸ_１軸，Ｙ_１軸の方向に移動するよう、ロボットアーム２０１の軌道を補正する（第１力制御処理、第１力制御工程）。

位置出力部３３３は、補正された軌道データ（補正された角度指令値）をモータ制御器２１３に通知する。これにより、モータ制御器２１３は、補正された軌道データに基づいてモータ２１１の回転を制御する。このように、ＣＰＵ３０１は、力制御による軌道補正の制御を実施する。

一方、到達位置判定部３３７は、ロボット動作プログラム３２０に記載された力目標値（閾値）と到達位置判定フィルタ部３３８を通して取得した力Ｆｚのデータとを比較する。そして、到達位置判定部３３７は、力Ｆｚが力目標値（閾値）を上回ったタイミングで位置出力部３３３にロボットハンド２０２がＺ_１軸の方向に移動するのを停止させる停止指令を通知する。即ち、到達位置判定部３３７は、嵌合作業中、力Ｆｚに対して閾値判定を行い、力Ｆｚが閾値を上回ったときにロボットハンド２０２のＺ_１軸の方向の移動が停止するようロボットアーム２０１の動作を制御する（第２力制御処理，第２力制御工程）。

第１実施形態では、到達位置判定部３３７は、ロボットハンド２０２のＸ_１軸の方向及びＹ_１軸の方向の移動も停止するよう停止指令を通知する。つまり、第１実施形態では、停止指令はロボットアーム２０１の動作を停止させる指令である。

位置出力部３３３は、停止指令を受信したタイミングで各モータ制御器２１３へ停止するよう通知し、各モータ制御器２１３はモータ２１１を停止させる。このように、ＣＰＵ３０１は、力制御による到達位置検知の制御を実施する。

ここで、位置出力部３３３から停止指令をモータ制御器２１３が受信した際、モータ２１１を即停止させず、モータ制御器２１３が各モータ２１１を減速させて停止しても良い。また、位置出力部３３３で到達位置判定部３３７から停止指令を受信した際に、軌道計算部３３２を通して減速軌道を計算し、算出された軌道をモータ制御器２１３に通知して、停止させても良い。

次に、ロボットコントローラ３００によるロボットハンド２０２が嵌合物Ｗ_１を把持し、嵌合物Ｗ_１を被嵌合物Ｗ_２の穴Ｈ_２に嵌合させる嵌合作業を行う場合の力制御について、具体的に説明する。図４は、嵌合物Ｗ_１と被嵌合物Ｗ_２との嵌合作業を行っている状態を示す説明図である。

図４に示すように、被嵌合物Ｗ_２の穴Ｈ_２は、凹み穴であり、底部Ｂ_２が形成されている。穴Ｈ_２の開口端には、嵌合物Ｗ_１を誘い込むための面取り部Ｃ_２が形成されている。なお、面取り部は、嵌合物Ｗ_１及び被嵌合物Ｗ_２のうち少なくとも一方に形成されていればよく、嵌合物Ｗ_１に面取り部が形成されていても、嵌合物Ｗ_１と被嵌合物Ｗ_２の両方に面取り部が形成されていてもよい。

嵌合物Ｗ_１と被嵌合物Ｗ_２はそれぞれ、不図示の供給装置により供給され、取得位置に供給された嵌合物Ｗ_１をロボットハンド２０２で把持し、被嵌合物Ｗ_２は組付け位置に供給される。

このような供給方法をとると、ロボットハンド２０２に対する嵌合物Ｗ_１の位置ズレや、被嵌合物Ｗ_２の組み付け位置に対する位置ズレにより、被嵌合物Ｗ_２に対する嵌合物Ｗ_１の相対的な位置ズレが生じることがある。また、嵌合物Ｗ_１及び被嵌合物Ｗ_２は製作時の公差分バラつきが生じている。

上記の要因から被嵌合物Ｗ_２に対する嵌合物Ｗ_１の組付け方向（図４に示すＺ_１軸の方向）と組付け方向に鉛直な面方向（図４に示すＸ_１，Ｙ_１軸の方向）それぞれの相対位置ズレが生じる。位置制御だけで嵌合作業を行うと、被嵌合物Ｗ_２の穴Ｈ_２に嵌合物Ｗ_１が嵌合されない場合がある。

そのため、第１実施形態では、穴Ｈ_２の中心軸に対する嵌合物Ｗ_１の中心軸の、Ｘ_１，Ｙ_１軸の方向の相対的なズレΔＸ，ΔＹを補正する軌道補正の制御と、Ｚ_１軸の方向の到達位置検知の制御の２つの力制御を同時に行う。

ここで、デジタルフィルタを介した力検出の感度は、デジタルフィルタのカットオフ周波数で決まる。即ち、カットオフ周波数を高い周波数にするほど、感度が高くなり、逆にカットオフ周波数を低い周波数にするほど、感度が低くなる。

到達位置検知の制御を実施する際に、仮に接触したことを検知する感度が低い場合、嵌合物Ｗ_１を底部Ｂ_２に押し付け続けることとなり、ロボットハンド２０２と力検出器２０３、即ちロボット２００に過負荷がかかった状態を維持し続けることとなる。

そこで、第１実施形態では、到達位置検知の制御に使用する到達位置判定フィルタ部３３８におけるローパスフィルタのカットオフ周波数Ｆ_Ｃ２を、カットオフ周波数Ｆ_Ｃ１よりも高い周波数に設定し、急峻な力の変化を検知できるような設定とする。

また、嵌合物Ｗ_１と被嵌合物Ｗ_２の穴Ｈ_２とのＸ_１，Ｙ_１軸の方向のズレを補正するための軌道補正の制御では、相対位置ズレの影響で、嵌合物Ｗ_１の下端が面取り部Ｃ_２に接触することで、Ｘ_１，Ｙ_１軸の方向に力Ｆｘ，Ｆｙが発生する。その発生した力Ｆｘ，Ｆｙを力検出器２０３で検出し、軌道補正計算部３３５は、軌道補正の制御を実施する。

ここで、力検出器２０３はロボット２００に設置されているため、面取り部Ｃ_２に接触して発生した力以外もロボット２００（ロボットアーム２０１）の振動によるノイズが同時に検出される。そのため、軌道補正フィルタ部３３６で使用するローパスフィルタのカットオフ周波数Ｆ_Ｃ１をカットオフ周波数Ｆ_Ｃ２よりも低い周波数に設定し、力の急峻な変化を無視するような設定とする。

例えば、カットオフ周波数Ｆ_Ｃ１を２０Ｈｚに設定し、カットオフ周波数Ｆ_Ｃ２を１００Ｈｚに設定する。これらカットオフ周波数Ｆ_Ｃ１，Ｆ_Ｃ２は、実験を行って決定すればよい。

以上、第１実施形態によれば、軌道補正フィルタ部３３６に使用する第１ローパスフィルタと、到達位置判定フィルタ部３３８に使用する第２ローパスフィルタとを別々に設定している。そして、第２ローパスフィルタのカットオフ周波数Ｆ_Ｃ２を第１ローパスフィルタのカットオフ周波数Ｆ_Ｃ１よりも高い周波数に設定している。

これにより、軌道補正の制御では、軌道補正フィルタ部３３６によって、力検出器２０３により検出された力Ｆｘ，Ｆｙにおいて周波数Ｆ_Ｃ１以上の高周波のノイズがカットされる。したがって、安定した軌道補正を実施することができる。

また、到達位置検知の制御では、軌道補正フィルタ部３３６により、力検出器２０３により検出された力Ｆｚにおいて、周波数Ｆ_Ｃ２以上の周波数のノイズはカットされ、周波数Ｆ_Ｃ１〜Ｆ_Ｃ２の周波数成分については、カットされずに残っている。よって、軌道補正フィルタ部３３６に用いるフィルタよりも感度良く力を検出することができる。これにより、Ｚ_１軸方向の到達位置に達した時に発生する力Ｆｚを感度よく検知することが可能になる。

このように、感度の高い到達位置検知の制御と安定した軌道補正の制御とが同時に実施されるので、嵌合物Ｗ_１と被嵌合物Ｗ_２との安定した嵌合作業を実現することが可能になる。

［第２実施形態］
次に、本発明の第２実施形態に係るロボット装置によるロボット制御について説明する。図５は、本発明の第２実施形態に係るロボット装置を示す模式図である。なお、第２実施形態のロボット装置は、上記第１実施形態のロボット装置と同様の構成であるため、同一符号を付して説明を省略する。第２実施形態では、嵌合を行う嵌合物及び被嵌合物の形状が上記第１実施形態と異なる。なお、ロボットハンド２０２は、ロボットアーム２０１の先端リンクが関節Ｊ_６で回転することによりＺ_１軸を中心に回転する。

嵌合物Ｗ_１０は、円柱状の円柱部Ｗ_１１と、円柱部Ｗ_１１の中心軸の延びる方向の先端部と後端部との間の中間部における外周面に突設された突起部であるボスＷ_１２と、を有している。被嵌合物Ｗ_２０には、穴Ｈ_２０が形成されている。穴Ｈ_２０は、円柱部Ｗ_１１が嵌合する円柱状の円柱穴Ｈ_２１と、円柱穴Ｈ_２１に繋がり、ボスＷ_１２が嵌合する溝であるボス穴Ｈ_２２と、を有している。

第２実施形態では、ロボットハンド２０２が把持する把持物が、嵌合物Ｗ_１０であり、対象物が被嵌合物Ｗ_２０である。第２実施形態では、嵌合作業として、円柱部Ｗ_１１を円柱穴Ｈ_２１に嵌合させ、かつボスＷ_１２をボス穴Ｈ_２２に嵌合させる作業を行う。

以下、嵌合物Ｗ_１０の回転し押し付けながらボスＷ_１２をボス穴Ｈ_２２に嵌合させる動作について説明する。

ＣＰＵ３０１は、ロボットハンド２０２によってボスＷ_１２を有した嵌合物Ｗ_１０を把持させ、ロボットハンド２０２が把持している嵌合物Ｗ_１０を被嵌合物Ｗ_２０に設けられた穴Ｈ_２０に嵌合する嵌合作業を行う。この嵌合作業はボスＷ_１２をボス穴Ｈ_２２に嵌合することで完了する。

円柱穴Ｈ_２１は上記第１実施形態の穴Ｈ_２と同様の凹み穴であり、円柱穴Ｈ_２１の開口端に、円柱部Ｗ_１１の先端部を誘い込むための面取り部が形成されている。なお、面取り部は、円柱部Ｗ_１１及び円柱穴Ｈ_２１のうち少なくとも一方に形成されていればよく、円柱部Ｗ_１１に面取り部が形成されていても、円柱部Ｗ_１１と円柱穴Ｈ_２１の両方に面取り部が形成されていてもよい。

また、嵌合物Ｗ_１０及び被嵌合物Ｗ_２０は、上記第１実施形態と同様の方法で供給される。また、嵌合物Ｗ_１０及び被嵌合物Ｗ_２０の公差、嵌合物Ｗ_１０の把持位置、被嵌合物Ｗ_２０の設置位置の影響で、被嵌合物Ｗ_２０に対して嵌合物Ｗ_１０に相対位置のズレが生じる。このため、第２実施形態においても、到達位置検知の制御と軌道補正の制御の２つの力制御で嵌合作業を行う。

ここで、ボスＷ_１２は、円柱部Ｗ_１１の先端部（下端部）より一定距離上方に離れた場所に取り付けられている。

図６は、本発明の第２実施形態における嵌合物Ｗ_１０と被嵌合物Ｗ_２０との嵌合作業を説明するための説明図である。図６（ａ）は、嵌合作業の途中状態を示し、図６（ｂ）は、嵌合作業の終了状態を示している。

ＣＰＵ３０１は、図６（ａ）に示すように、円柱部Ｗ_１１の先端部を穴Ｈ_２０の円柱穴Ｈ_２１に嵌合する工程を実施した後、図６（ｂ）に示すように、ボスＷ_１２をボス穴Ｈ_２２に嵌合するために位相を合わせて嵌合する２つの工程を実施する。

まず、図６（ａ）に示す円柱部Ｗ_１１の先端部を穴Ｈ_２０に嵌合する処理は、上記第１実施形態に記載の内容とほぼ同様の方法で嵌合作業を行うため、詳細な説明は省略する。上記第１実施形態と異なる点として、第２実施形態では、ＣＰＵ３０１は、到達位置検知の制御により、ボスＷ_１２を被嵌合物Ｗ_２０上面に接触させたとき、ロボットアーム２０１の動作を停止させる点である。即ち、到達位置判定部３３７は、嵌合作業中、円柱部Ｗ_１１の先端部を円柱穴Ｈ_２１に嵌合させて、力Ｆｚが閾値を上回ったとき、嵌合物Ｗ_１０のＺ_１軸の方向の移動を停止させる。

次に、ＣＰＵ３０１は、ボスＷ_１２をボス穴Ｈ_２２に嵌合させる際に、図６（ｂ）に示すように、ロボットハンド２０２で把持している嵌合物Ｗ_１０の円柱部Ｗ_１１の先端部を円柱穴Ｈ_２１に嵌合させている状態で、嵌合物Ｗ_１０をＺ_１軸まわりに回転させる。

即ち、ＣＰＵ３０１は、嵌合作業中、円柱部Ｗ_１１の先端部を円柱穴Ｈ_２１に嵌合させて第２力制御処理にて嵌合物Ｗ_１０のＺ_１軸方向の移動を停止させたとき、力Ｆｚが閾値を下回るまで、嵌合物Ｗ_１０を、Ｚ_１軸を中心に回転させる。具体的には、ロボットハンド２０２をロボットアーム２０１に対してＺ_１軸を中心に回転させる。

ボスＷ_１２がボス穴Ｈ_２２に到達すると、ボス穴Ｈ_２２に到達したタイミングでＺ_１軸方向の力Ｆｚが抜け、力Ｆｚが閾値を下回る。その力Ｆｚを到達位置検知の力制御で検知することで、ボスＷ_１２をボス穴Ｈ_２２に嵌合するための位相が検知可能になる。

ここで、ボス穴Ｈ_２２の位相を検知する際、仮に到達位置検知の感度が低いと、組付け位相に到達しても閾値を下回る力を検知せずに通り過ぎてしまい、ボス穴Ｈ_２２の位相を検知することができなくなるおそれがある。そのため、上記第１実施形態と同様、到達位置検知に使用する到達位置判定フィルタ部３３８で使用するフィルタのカットオフ周波数Ｆ_Ｃ２をカットオフ周波数Ｆ_Ｃ１よりも大きく設定し、急峻な力の変化を検知できるような設定とする。その後、検知した位相で嵌合物Ｗ_１０をＺ_１軸の方向に押し込んで、再度、閾値判定を行うことにより、嵌合物Ｗ_１０を穴Ｈ_２０に嵌合する嵌合作業が完了する。

ここで、嵌合物Ｗ_１０を回転させる際、嵌合物Ｗ_１０を回転させる回転軸と穴Ｈ_２０の回転軸にズレが生じている場合、回転中にＸ_１，Ｙ_１軸の方向の力Ｆｘ，Ｆｙが生じることがある。

よって、第２実施形態では、ロボットハンド２０２と力検出器２０３、即ちロボット２００に過負荷がかかるのを回避するため、嵌合物Ｗ_１０を回転させるのと同時に、回転軸のズレを補正するための軌道補正の制御を実施する。

この軌道補正の制御では、回転時のロボット２００の振動によるノイズを検出し、嵌合物Ｗ_１０と穴Ｈ_２０が接触することによって発生する力以外の力も同時に検出する。そのため、上記第１実施形態と同様、軌道補正フィルタ部３３６で使用するフィルタのカットオフ周波数Ｆ_Ｃ１を小さくし、力の急峻な変化を無視するような設定とする。

以上の如くカットオフ周波数Ｆ_Ｃ１，Ｆ_Ｃ２を設定することで、感度の高い到達位置検知によりボスＷ_１２を組付ける位相を検知するのと同時に、嵌合物Ｗ_１０と穴Ｈ_２０との相対位置ズレにより発生する回転軸のズレを吸収した嵌合作業を実現できる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。

上記第２実施形態では、力Ｆｚを検知して到達位置検知を実施したが、力Ｆｚだけでなく、力Ｆｚの時間変化量を使用して、到達位置検知を実施しても良い。また、上記第２実施形態で使用した到達位置検知では、挿入方向の力Ｆｚをモニタリングして検知していたが、Ｚ軸まわりの回転方向の力成分を利用して、到達位置検知を実施してもよい。

上記第１，第２実施形態では、ロボットハンドが嵌合物を把持して嵌合物と被嵌合物との嵌合作業を行う場合について説明したが、これに限定するものではない。ロボットハンドが被嵌合物を把持して嵌合作業を行う場合であっても本発明は適用可能である。

上記実施形態の各処理動作は具体的にはＣＰＵ３０１により実行されるものである。従って上述した機能を実現するプログラムを記録した記録媒体をロボットコントローラに供給し、ロボットコントローラを構成するコンピュータが記録媒体に格納されたプログラムを読み出し実行することによって達成されるようにしてもよい。この場合、記録媒体から読み出されたプログラム自体が上述した実施形態の機能を実現することになり、プログラム自体及びそのプログラムを記録した記録媒体は本発明を構成することになる。

また、上記実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ３０４であり、ＨＤＤ３０４にプログラム３２１が格納される場合について説明したが、これに限定するものではない。プログラムは、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラムを供給するための記録媒体としては、図３に示すＲＯＭ３０２、記録ディスク３２２、外部記憶装置８００等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、ハードディスク、光ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、磁気テープ、書き換え可能な不揮発性のメモリ（例えばＵＳＢメモリ）、ＲＯＭ等を用いることができる。

また、上記実施形態におけるプログラムを、ネットワークを介してダウンロードしてコンピュータにより実行するようにしてもよい。

また、コンピュータが読み出したプログラムコードを実行することにより、上記実施形態の機能が実現されるだけに限定するものではない。そのプログラムコードの指示に基づき、コンピュータ上で稼働しているＯＳ（オペレーティングシステム）等が実際の処理の一部または全部を行い、その処理によって前述した実施形態の機能が実現される場合も含まれる。

さらに、記録媒体から読み出されたプログラムコードが、コンピュータに挿入された機能拡張ボードやコンピュータに接続された機能拡張ユニットに備わるメモリに書き込まれてもよい。そのプログラムコードの指示に基づき、その機能拡張ボードや機能拡張ユニットに備わるＣＰＵ等が実際の処理の一部または全部を行い、その処理によって上記実施形態の機能が実現される場合も含まれる。

また、上記実施形態では、コンピュータがＨＤＤ等の記録媒体に記録されたプログラムを実行することにより、処理を行う場合について説明したが、これに限定するものではない。プログラムに基づいて動作する演算部の一部又は全部の機能をＡＳＩＣやＦＰＧＡ等の専用ＬＳＩで構成してもよい。なお、ＡＳＩＣはApplication Specific Integrated Circuit、ＦＰＧＡはField-Programmable Gate Arrayの頭字語である。

１００…ロボット装置、２０１…ロボットアーム、２０２…ロボットハンド、２０３…力検出器、３０１…ＣＰＵ（制御部）、Ｆ_Ｃ１…カットオフ周波数（第１カットオフ周波数）、Ｆ_Ｃ２…カットオフ周波数（第２カットオフ周波数）

Claims (6)

1. 多関節のロボットアームと、
   前記ロボットアームの先端に取り付けられたロボットハンドと、
   前記ロボットアームから前記ロボットハンドに延びる先端軸に対して交差する第１方向の第１検出力及び前記先端軸の延びる第２方向の第２検出力を検出する力検出器と、
   前記ロボットアームの軌道データに基づき、前記ロボットハンドに把持された把持物と対象物との嵌合作業を行うよう、前記ロボットアームの動作を制御する制御部と、を備え、
   前記制御部は、
   前記嵌合作業中、第１カットオフ周波数の第１ローパスフィルタにて、前記第１検出力に対してフィルタ処理を行う第１検出力補正処理と、
   前記嵌合作業中、前記第１検出力が小さくなるように前記ロボットハンドを前記第１方向に移動させて、前記ロボットアームの軌道を補正する第１力制御処理と、
   前記嵌合作業中、前記第１カットオフ周波数よりも周波数の高い第２カットオフ周波数の第２ローパスフィルタにて、前記第２検出力に対してフィルタ処理を行う第２検出力補正処理と、
   前記嵌合作業中、前記第２検出力に対して閾値判定を行い、前記第２検出力が前記閾値を上回ったときに前記ロボットハンドの前記第２方向の移動を停止させる第２力制御処理と、を実行することを特徴とするロボット装置。
2. 前記把持物及び前記対象物のうち、一方が嵌合物であり、他方が、前記嵌合物が嵌合する穴を有する被嵌合物であり、
   前記制御部は、前記嵌合作業として、前記嵌合物を前記被嵌合物の穴に嵌合させる作業を行うことを特徴とする請求項１に記載のロボット装置。
3. 前記嵌合物は、円柱状の円柱部と、前記円柱部の先端部と後端部との間の中間部における外周面に突設された突起部と、を有しており、
   前記穴は、前記円柱部が嵌合する円柱状の円柱穴と、前記円柱穴に繋がり、前記突起部が嵌合する溝と、を有しており、
   前記制御部は、前記嵌合作業として、前記円柱部を前記円柱穴に嵌合させ、かつ前記突起部を前記溝に嵌合させる作業中、前記円柱部の先端部を前記円柱穴に嵌合させて前記第２力制御処理にて前記把持物の前記第２方向の移動を停止させたとき、前記第２検出力が前記閾値を下回るまで、前記把持物を、前記先端軸を中心に回転させることを特徴とする請求項２に記載のロボット装置。
4. 多関節のロボットアームの先端に、力検出器を介してロボットハンドが取り付けられており、前記力検出器が、前記ロボットアームから前記ロボットハンドに延びる先端軸に対して交差する第１方向の第１検出力、及び前記先端軸の延びる第２方向の第２検出力を検出するよう構成されており、制御部が、前記ロボットアームの軌道データに基づき、前記ロボットハンドに把持された把持物と対象物との嵌合作業を行うよう前記ロボットアームの動作を制御するロボット制御方法であって、
   前記制御部が、前記嵌合作業中、第１カットオフ周波数の第１ローパスフィルタにて、前記第１検出力に対してフィルタ処理を行う第１検出力補正工程と、
   前記制御部が、前記嵌合作業中、前記第１検出力が小さくなるように前記ロボットハンドを前記第１方向に移動させて、前記ロボットアームの軌道を補正する第１力制御工程と、
   前記制御部が、前記嵌合作業中、前記第１カットオフ周波数よりも周波数の高い第２カットオフ周波数の第２ローパスフィルタにて、前記第２検出力に対してフィルタ処理を行う第２検出力補正工程と、
   前記制御部が、前記嵌合作業中、前記第２検出力に対して閾値判定を行い、前記第２検出力が前記閾値を上回ったときに前記ロボットハンドの前記第２方向の移動を停止させる第２力制御工程と、を備えたことを特徴とするロボット制御方法。
5. コンピュータに、請求項４に記載のロボット制御方法の各工程を実行させるためのプログラム。
6. 請求項５に記載のプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images