(57)【要約】【課題】作業前の事前準備を不要とし、かつ接触の発生時における反応速度を上げることが可能なロボット装置を提供する。【解決手段】ロボット装置１は、外力を検知する力覚センサを有するロボット本体２と、ロボット制御装置３とを備えている。ロボット制御装置３は、力覚センサにより検知された力覚値に対して設定されたカットオフ周波数でフィルタするフィルタ処理を実行し、フィルタ処理された力覚値に基づきロボット本体を動作させる動作処理を実行する。そして、ロボット制御装置３は、力覚値変化率を演算する変化率演算処理を実行し、力覚値変化率に基づきカットオフ周波数を変更する。これにより、事前準備を要することなく、接触の発生時における反応速度を上げることが可能となる。【選択図】図３
2016203315.tif 000002
【特許請求の範囲】
【請求項１】
外力を検知する力覚センサを有するロボットと、
前記力覚センサにより検知された力覚値に対して設定されたカットオフ周波数でフィルタするフィルタ処理を実行し、前記フィルタ処理された力覚値に基づき前記ロボットを動作させる動作処理を実行する制御装置と、を備えたロボット装置において、
前記制御装置は、前記力覚値の変化率を演算する変化率演算処理を実行し、前記変化率に基づき前記カットオフ周波数を変更する周波数変更処理を実行する、
ことを特徴とするロボット装置。
【請求項２】
前記制御装置は、前記フィルタ処理においてローパスフィルタを用いて前記フィルタ処理を実行し、前記周波数変更処理において前記変化率が大きくなるほど前記カットオフ周波数が高くなるように変更する、
ことを特徴とする請求項１に記載のロボット装置。
【請求項３】
前記制御装置は、前記周波数変更処理において前記変化率に応じて前記カットオフ周波数を段階的に変更する、
ことを特徴とする請求項１または２に記載のロボット装置。
【請求項４】
前記制御装置は、前記力覚値が予め設定した閾値を越えた場合に前記動作処理を停止する、
ことを特徴とする請求項１ないし３のいずれか１項に記載のロボット装置。
【請求項５】
前記制御装置は、前記動作処理において予め作成された作業プログラムに基づき前記ロボットを動作させる、
ことを特徴とする請求項１ないし４のいずれか１項に記載のロボット装置。
【請求項６】
前記制御装置は、前記変化率に基づき前記動作処理を実行する制御周期を変更する周期変更処理を実行する、
ことを特徴とする請求項１ないし５のいずれか１項に記載のロボット装置。
【請求項７】
外力を検知する力覚センサを有するロボットと、
前記力覚センサにより検知された力覚値に基づき前記ロボットを動作させる動作処理を実行する制御装置と、を備えたロボット装置において、
前記制御装置は、前記力覚値の変化率を演算する変化率演算処理を実行し、前記変化率に基づき前記動作処理を実行する制御周期を変更する周期変更処理を実行する、
ことを特徴とするロボット装置。
【請求項８】
前記制御装置は、前記力覚値が予め設定した閾値を越えた場合に前記動作処理を停止する、
ことを特徴とする請求項７に記載のロボット装置。
【請求項９】
外力を検知する力覚センサを有するロボットを制御装置により制御するロボット制御方法において、
前記制御装置が、前記力覚センサにより検知された力覚値に対して設定されたカットオフ周波数でフィルタするフィルタ工程と、
前記制御装置が、前記フィルタされた力覚値に基づき前記ロボットを動作させる動作工程と、
前記制御装置が、前記力覚値の変化率を演算する変化率演算工程と、
前記制御装置が、前記変化率に基づき前記カットオフ周波数を変更する周波数変更工程と、を有する、
ことを特徴とするロボット制御方法。
【請求項１０】
外力を検知する力覚センサを有するロボットを制御装置により制御するロボット制御方法において、
前記制御装置が、前記力覚センサにより検知された力覚値に基づき前記ロボットを動作させる動作工程と、
前記制御装置が、前記力覚値の変化率を演算する変化率演算工程と、
前記制御装置が、前記変化率に基づき前記動作工程を実行する制御周期を変更する周期変更工程と、を有する、
ことを特徴とするロボット制御方法。
【請求項１１】
請求項１ないし８のいずれか１項に記載の各処理を前記ロボット装置の制御装置に実行させるためのプログラム。
【請求項１２】
請求項１１に記載のプログラムを格納したことを特徴としたコンピュータで読み取り可能な記録媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、力覚センサを有するロボットを検知された力覚値に応じて動作させるロボット装置、及びロボット制御方法に関する。
【背景技術】
【０００２】
例えば産業用のロボットにあっては、ロボットアームに外力を検出する力覚センサを備え、その力覚センサにより検出される力覚値を用いてロボットの動作を制御するものがある（特許文献１参照）。力覚センサにより検出される力覚値の信号には、ノイズ成分が含まれるため、そのまま力覚値を用いてロボットを制御すると動作に振動等の影響が生じる虞がある。そのため、検出された力覚値を例えばローパスフィルタ（低域通過フィルタ）などを用いてフィルタ処理してから、ロボットの制御に用いている。
【０００３】
しかしながら、フィルタ処理した力覚値を用いてロボットを動作させると、例えばロボットアームが他の物体（ワーク等）に接触した場合などにあって、フィルタ処理により力覚値の変動を緩やかにしてしまうため、動作の反応に遅れが生じる。このため、ロボットアームと他の物体との接触が強くなる傾向にあり、場合によってはロボットアームが接触した物体に喰い込みを起こしてしまう虞もある。
【０００４】
このように、フィルタ処理によるロボット動作の反応速度と振動成分の除去とは相反する関係となる。そこで、特許文献１では、予めロボットの動作毎に波形データを収集し、それを解析し、ロボットの動作に応じてフィルタ処理における周波数を変更するようにフィルタリングパターンを生成しておくことが提案されている。これにより、物体に接触する予定の動作では力覚値に対するロボットの反応速度を上げ、他の動作では振動成分の除去が可能となるような効果が期待される。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１１−１１０６３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記特許文献１のものは、ロボットに作業を行わせるに先立って、事前に動作に応じた波形データの収集、その解析、及びフィルタリングパターンの生成等の作業が必要である。そのため、ロボットの作業工程を新たに設定する度に事前準備が必要となり、手間がかかるという問題がある。さらに、特許文献１のものは、ロボットアームが予期しない接触を起こした場合などに対応することができないという問題もある。即ち、環境変化や経時変化、誤差などに起因してワークに対する接触位置が手前になった場合や隣接するロボットとの干渉があった場合など、予期できない状況ではフィルタリングパターンで対処できず、フィルタ処理における周波数の変更が間に合わない。そのため、力覚値に対するロボットの反応が遅れ、例えばロボットと他の物体との接触が強くなってしまうなどの問題がある。
【０００７】
そこで本発明は、作業前の事前準備を不要とし、かつ接触の発生時における反応速度を上げることが可能なロボット装置及びロボット制御方法を提供することを目的とするものである。
【課題を解決するための手段】
【０００８】
本発明は、外力を検知する力覚センサを有するロボットと、前記力覚センサにより検知された力覚値に対して設定されたカットオフ周波数でフィルタするフィルタ処理を実行し、前記フィルタ処理された力覚値に基づき前記ロボットを動作させる動作処理を実行する制御装置と、を備えたロボット装置において、前記制御装置は、前記力覚値の変化率を演算する変化率演算処理を実行し、前記変化率に基づき前記カットオフ周波数を変更する周波数変更処理を実行することを特徴とする。
【０００９】
また本発明は、外力を検知する力覚センサを有するロボットと、前記力覚センサにより検知された力覚値に基づき前記ロボットを動作させる動作処理を実行する制御装置と、を備えたロボット装置において、前記制御装置は、前記力覚値の変化率を演算する変化率演算処理を実行し、前記変化率に基づき前記動作処理を実行する制御周期を変更する周期変更処理を実行することを特徴とする。
【００１０】
また本発明は、外力を検知する力覚センサを有するロボットを制御装置により制御するロボット制御方法において、前記制御装置が、前記力覚センサにより検知された力覚値に対して設定されたカットオフ周波数でフィルタするフィルタ工程と、前記制御装置が、前記フィルタされた力覚値に基づき前記ロボットを動作させる動作工程と、前記制御装置が、前記力覚値の変化率を演算する変化率演算工程と、前記制御装置が、前記変化率に基づき前記カットオフ周波数を変更する周波数変更工程と、を有することを特徴とする。
【００１１】
また本発明は、外力を検知する力覚センサを有するロボットを制御装置により制御するロボット制御方法において、前記制御装置が、前記力覚センサにより検知された力覚値に基づき前記ロボットを動作させる動作工程と、前記制御装置が、前記力覚値の変化率を演算する変化率演算工程と、前記制御装置が、前記変化率に基づき前記動作工程を実行する制御周期を変更する周期変更工程と、を有することを特徴とする。
【発明の効果】
【００１２】
本発明によると、力覚値の変化率に基づきフィルタ処理におけるカットオフ周波数、或いは動作処理を実行する制御周期が変更されるので、事前準備を要することなく、接触の発生時における反応速度を上げることができる。
【図面の簡単な説明】
【００１３】
【図１】本実施の形態に係るロボット装置を示す斜視図。
【図２】本ロボット装置の内部構成を示すブロック図。
【図３】本ロボット装置の各機能を示すブロック図。
【図４】第１の実施の形態に係るロボット装置の制御を示すフローチャート。
【図５】力覚値変化率に対するカットオフ周波数の関数の一例を示す図。
【図６】カットオフ周波数を変更する制御におけるロボット本体と他の物体との接触時における力覚値を示すグラフ。
【図７】第２の実施の形態に係るロボット装置の制御を示すフローチャート。
【図８】カットオフ周波数を固定した制御におけるロボットと他の物体との接触時における力覚値を示すグラフ。
【発明を実施するための形態】
【００１４】
＜第１の実施の形態＞
以下、本発明に係る第１の実施の形態を図１乃至図６を参照しつつ説明する。まず、本実施の形態に係るロボット装置１の構成について図１及び図２に沿って説明する。図１は本実施の形態に係るロボット装置を示す斜視図、図２は本ロボット装置の内部構成を示すブロック図である。
【００１５】
本実施の形態に係るロボット装置１は、ロボット本体（ロボット）２とロボット制御装置（制御装置）３とを有して構成されており、ロボット制御装置３には、操作端末４が接続可能となっている。ロボット本体２は、基台２Ｂと、その基台２Ｂに架装されたロボットアーム２Ａと、そのロボットアーム２Ａの先端に設けられたロボットハンド２Ｈとを有している。本実施の形態において、ロボットアーム２Ａは６つの関節と各関節を駆動する関節駆動装置（サーボモータＭ１〜Ｍ６（図２参照）や減速機など）を有する６自由度のマニピュレータであり、つまりロボット本体２は６軸の多関節ロボットとして構成されている。ロボットハンド２Ｈは、作業対象であるワーク１００を把持する複数のフィンガ２ｆを有するツールである。ロボットハンド２Ｈの内部には、アクチュエータ（サーボモータＭＨ（図２参照）や伝達機構など）が内蔵されており、フィンガ２ｆを開閉駆動させる。
【００１６】
また、ロボットアーム２Ａとロボットハンド２Ｈとの間には、ロボットハンド２Ｈにかかる外力を検出する力覚センサ２２Ｆが配設されている。さらに、ロボットアーム２Ａには、各関節（或いは関節駆動装置）に設けられて関節の回転位置（例えば関節を挟んだ２つのフレーム間の相対位置）を検出するエンコーダ等からなる回転位置センサ２２Ｐ１〜２２Ｐ６（図２参照）が配設されている。
【００１７】
一方、操作端末４は、オペレータからの手動操作を受付ける端末である。操作端末４からは、ロボット制御装置３に対し、ロボット本体２の動作を指示したり、制御情報を取得したり、ロボット本体２の動作（教示点）を指定してロボット本体２の動作工程を作業プログラムとして作成したりできる。
【００１８】
詳しくは後述するロボット制御装置３は、上記作業プログラムなどに基づきロボット本体２を駆動制御し、ロボット本体２を動作させる。ロボット本体２の動作結果の情報は、上記回転位置センサ２２Ｐ１〜２２Ｐ６（図２参照）により検出される各関節の位置、上記力覚センサ２２Ｆにより検出される力覚値などの情報であり、ロボット制御装置３に入力される。ロボット制御装置３は、その動作結果の情報に基づき、作業プログラムに応じた動作となるようにサーボモータＭ１〜Ｍ６に駆動指令を与える。これにより、ロボット本体２を駆動制御しつつ軌道を補正したり、或いは作業プログラムの動作指令から外れた動作結果である場合にロボット本体２を停止させたりする。
【００１９】
具体例として、例えばワーク１００を他の部品の穴に嵌合する作業プログラムに応じてロボット本体２を動作させる場合には、ワークから受ける外力を力覚センサ２２Ｆにより力覚値として取得する。そして、ワーク１００の位置を穴に沿って位置合せするような補正演算をしつつサーボモータＭ１〜Ｍ６に駆動指令を与え、ロボット本体２の駆動制御が実行される。また、例えばロボットハンド２Ｈが他の物体（例えば隣接するロボット装置など）に干渉した場合には、力覚値に基づき接触を判定し、ロボット本体２を停止させる制御が実行される。
【００２０】
図２に示すように、ロボット制御装置３は、中央演算装置３１（ＣＰＵ）と、主記憶装置３２（メインメモリ）と、補助記憶装置３３と、各インタフェースを管理する外部通信モジュール３４とを有している。また、外部通信モジュール３４には、モータ制御通信インタフェース（Ｉ／Ｆ）３５、センサ制御通信インタフェース（Ｉ／Ｆ）３６、操作端末通信インタフェース（Ｉ／Ｆ）３７、上位コントローラ通信用インタフェース（Ｉ／Ｆ）３８が接続されている。
【００２１】
モータ制御通信インタフェース３５は、ロボット本体２に配設されたモータ制御回路２１に接続されている。モータ制御回路２１には、ロボットハンド２Ｈに内蔵されたサーボモータＭＨと、ロボットアーム２Ａの各関節の関節駆動装置に内蔵されたサーボモータＭ１〜Ｍ６とが接続されている。
【００２２】
センサ制御通信インタフェース３６は、ロボット本体２に配設されたセンサ制御回路２２に接続されている。センサ制御回路２２には、上述のようにロボットアーム２Ａとロボットハンド２Ｈとの間に配置された力覚センサ２２Ｆと、各関節に配置された回転位置センサ２２Ｐ１〜２２Ｐ６とが接続されている。操作端末通信インタフェース３７は、操作端末４に配設された操作端末制御回路４１に接続されている。なお、本実施の形態においては、力覚センサ２２Ｆと回転位置センサ２２Ｐ１〜２２Ｐ６が接続されたものを説明しているが、例えばモータの出力軸の回転位置を検出するセンサ（エンコーダ）や温度センサなど、どのようなセンサが接続されていてもよい。
【００２３】
操作端末制御回路４１には、使用者の入力操作を受け付ける各種ボタン等で構成される入力インタフェース４２と、例えば文字を表示する画面や音を通知するブザー等で構成される出力インタフェース４３とが接続されている。上位コントローラ通信用インタフェース３８には、本ロボット装置１を含む複数のロボット装置を全体的に管理する上位コンピュータ２００や他のロボット装置のロボット制御装置などに接続される。
【００２４】
上記補助記憶装置３３は、例えばハードディスクドライブ（ＨＤＤ）やソリッドステートドライブ（ＳＳＤ）などの、装置を停止させても記憶内容が失われない不揮発性の記憶装置が用いられる。この中には、本ロボット装置１を制御する各種のプログラムが記憶されていると共に、例えば予め作成された、ロボット本体２の動作順序や軌道（教示点）が記述された作業プログラムも記憶されている。これらの各種のプログラムは、ＣＤ、ＤＶＤなどの記録ディスクや外部フラッシュメモリなどに格納されていてもよく、つまりコンピュータで読取り可能な記録媒体であれば、何れのものであってもよい。
【００２５】
上記中央演算装置３１は、主記憶装置３２或いは補助記憶装置３３に記憶されている値や外部通信モジュール３４で受信した外部からの信号を受信する。これにより、ロボット本体２の軌道演算や軌道の補正演算を行って駆動指令を生成し、或いは停止制御判断などの演算も行って停止指令を生成する。この演算は、中央演算装置３１が随時途中結果を主記憶装置３２に書込み、また途中結果を主記憶装置３２から読込みながら行われ、その演算の結果は補助記憶装置３３に記憶、或いは外部通信モジュール３４を通して外部に通知される。また、上記演算には、制御中に発生したエラー情報、解釈後の作業プログラムの命令の集合などの情報の演算も含まれる。
【００２６】
中央演算装置３１に外部通信モジュール３４を通して受信する情報には、力覚センサ２２Ｆにより検出された力覚値、各回転位置センサ２２Ｐ１〜２２Ｐ６により検出された各関節の位置（角度）などが含まれる。これにより、中央演算装置３１において、ロボット本体２の軌道の補正演算や停止制御の判断等が実行可能となる。なお、力覚値とは、本実施の形態では弾性変形する部材の歪みを歪みゲージ等で検出して発生している力を値としたものを指すが、これに限らず、外力によって生じるトルクやモーメントなどを検出した値であってもよい。
【００２７】
また、中央演算装置３１に外部通信モジュール３４を通して受信する情報には、操作端末４の入力インタフェース４２による操作者の入力操作の情報も含まれる。なお、中央演算装置３１に外部通信モジュール３４を通して受信する情報には、不図示の緊急停止ボタンから送信される非常停止信号等も含まれる。
【００２８】
反対に、中央演算装置３１から外部通信モジュール３４を介してロボット本体２に送信される情報には、中央演算装置３１により生成された駆動指令や停止指令の情報が含まれる。これにより、各サーボモータＭＨ，Ｍ１〜Ｍ６を駆動制御、或いは停止制御し、つまりロボット本体２を駆動制御、或いは停止制御することができる。なお、各関節駆動装置にブレーキ装置を内蔵している場合には、ブレーキの作動を指令することでロボット本体２の停止制御を実行することもできる。
【００２９】
さらに、中央演算装置３１から外部通信モジュール３４を介して操作端末４に送信される情報には、ロボット装置１で発生したエラー情報、軌道や教示点の情報、作業プログラムの情報などが含まれる。これにより、操作端末４の出力インタフェース４３に各種の表示等が行われ、ロボット装置１の操作者は、この操作端末４を通して、ロボット装置１のエラー確認、作業プログラムの実行、教示点の教示作業等を行うことができる。
【００３０】
さらに、中央演算装置３１から外部通信モジュール３４を介して上記上位コンピュータ２００に送信される情報には、生産数やロボット装置の稼働時間・停止時間、ロボット制御装置内で発生したエラー情報なども含まれる。これにより、例えば上位コンピュータ２００で生産状況の管理ができるようになる。
【００３１】
つづいて、上記ロボット装置１の制御機能と信号（データ）の流れを図３に沿って説明する。図３は、本ロボット装置の各機能を示すブロック図であり、図３中の矢印はデータの流れを示している。
【００３２】
ロボット本体２に備えられたセンサ制御回路２２は、制御周期毎にロボット制御装置３に対して、力覚センサ２２Ｆにより検知した力覚値（外力）を送信する。送信された力覚値は、ロボット制御装置３の外部通信モジュール３４から、主記憶装置３２によって機能する力覚値記憶部Ｃ１１に送られる。
【００３３】
力覚値記憶部Ｃ１１は、センサ制御回路２２から受信したフィルタ処理前の力覚値を記憶し、さらに、後述するフィルタ処理部Ｃ１２から受け取るフィルタ処理後の力覚値も別途記憶する機能を有する。なお、センサ制御回路２２から受信した力覚値は、整数のパルス値で表現されている場合があり、その場合には単位系をＮ（ニュートン）に、もしくは力のモーメントを表すＮｍ（ニュートンメートル）に変換し、実数値として力覚値を記憶する。
【００３４】
中央演算装置３１によって機能するフィルタ処理部Ｃ１２は、力覚値記憶部Ｃ１１からフィルタ処理前の力覚値を読込み、低域通過フィルタ（ローパスフィルタ）による処理を実行する。そして、その演算結果をフィルタ処理後の力覚値として力覚値記憶部Ｃ１１に送信して記憶する。これにより、フィルタ処理前とフィルタ処理後の力覚値は、どちらも制御周期毎に力覚値記憶部Ｃ１１に一時的に格納される。格納された力覚値は、フィルタ処理部Ｃ１２、後述の力覚値変化率計算部Ｃ１３及び接触検知部Ｃ１７で使用した後に不要となると、力覚値記憶部Ｃ１１内から適宜破棄される。
【００３５】
中央演算装置３１によって機能する力覚値変化率計算部Ｃ１３は、力覚値記憶部Ｃ１１から力覚値を受信して、力覚値の時間に対する変化率を計算して力覚値変化率（力覚値の変化率）とし、その力覚値変化率をカットオフ周波数算出部Ｃ１４に送信する。中央演算装置３１によって機能するカットオフ周波数算出部Ｃ１４は、力覚値変化率計算部Ｃ１３から受信した力覚値変化率からカットオフ周波数を演算し、上記フィルタ処理部Ｃ１２に算出したカットオフ周波数を送信する。このように、上記フィルタ処理部Ｃ１２において低域通過フィルタの処理で使用されるカットオフ周波数には、カットオフ周波数算出部Ｃ１４により制御周期毎に与えられる値を使用する。これにより、事前に定められたカットオフ周波数を用いるのではなく、各制御周期で使用するカットオフ周波数を動的に変更することを可能としている。
【００３６】
中央演算装置３１によって機能する作業プログラム解釈部Ｃ１６は、操作端末４内の入力インタフェース４２から作業プログラムの開始指示、停止指示、その他の指示など、つまり操作者からの指示を受信する。作業プログラムの開始指示を受信した場合、作業プログラム解釈部Ｃ１６は、まず補助記憶装置３３によって機能する作業プログラム記憶部Ｃ１５からロボット言語で記述された作業プログラムを読込み、作業プログラムを解釈する。即ち、読み込んだ作業プログラムに応じて、ロボット本体２の動作内容に関する命令を抽出し、中央演算装置３１によって機能する動作指令部Ｃ１８に送信する。そして、動作指令部Ｃ１８は、ロボット本体２の動作指令を生成し、モータ制御回路２１にその動作指令を送信して、作業プログラムに応じたロボット本体２の動作を実現する。なお、上記操作端末４から作業プログラムの開始指示や停止指示を受信するものを説明したが、これに限らず、上位コンピュータ２００やその他の外部機器から作業プログラムの開始指示、停止指示、その他の指示などを受信することも可能である。さらに、中央演算装置３１の自己判断で作業プログラムを開始や停止などを実行するようにしても良い。
【００３７】
一方、作業プログラム解釈部Ｃ１６が作業プログラムから力制御開始の命令を抽出した場合、作業プログラム解釈部Ｃ１６は、中央演算装置３１によって機能する接触検知部Ｃ１７に力制御開始指令を送信する。また、力制御開始指令を送信する際は、力制御の実行時の条件である力制御条件も送信する。力制御開始指令を受けた接触検知部Ｃ１７は、力覚値記憶部Ｃ１１から取得できるフィルタ処理後の力覚値に応じて接触状態を判断し、その判断結果を動作指令部Ｃ１８に送信する。これにより、動作指令部Ｃ１８は、上記作業プログラム解釈部Ｃ１６による作業プログラムの動作内容を、接触状態に応じて変更（補正）して動作指令を生成し、ロボット本体２のモータ制御回路２１に送信する。これにより、作業プログラムに応じてかつロボットハンド２Ｈに受ける外力に応じた、ロボット本体２の動作を可能にする。
【００３８】
即ち、例えばワーク１００を他の部品に嵌合する場合などは、ワーク１００が他の部品に接触したことにより受ける外力を検出しながら、その外力が適宜な値となるようにロボット本体２の動作を制御する。それにより、ロボット本体２は、ワーク１００の他の部品への嵌合をスムーズに実行できる。このようなロボット制御のことを、本実施の形態では力制御という。また、上記力制御条件とは、例えばワーク１００の形状や他の部品の形状、嵌合する際のワーク１００の位置や姿勢などの条件ということになる。
【００３９】
その後、作業プログラム解釈部Ｃ１６が作業プログラムから力制御停止の命令を抽出した場合、作業プログラム解釈部Ｃ１６は、接触検知部Ｃ１７に力制御停止指令を送信する。力制御停止指令を受けた接触検知部Ｃ１７は、接触状態に関数指令を動作指令部Ｃ１８に送信しなくなる。これにより、動作指令部Ｃ１８は、上記作業プログラム解釈部Ｃ１６による作業プログラムの動作内容だけで動作指令を生成し、ロボット本体２のモータ制御回路２１に送信することになる。そのため、力制御が終了して、作業プログラムに応じたロボット本体２の動作に戻ることになる。
【００４０】
そして、操作端末４の入力インタフェース４２に停止指示の入力があった場合、或いは作業プログラム解釈部Ｃ１６において作業プログラムの実行終了が判断された場合は、作業プログラム解釈部Ｃ１６は動作指令部Ｃ１８にロボット本体２の停止を指令する。これにより、動作指令部Ｃ１８は、モータ制御回路２１に各サーボモータＭＨ，Ｍ１〜Ｍ６の停止を指令し、ロボット本体２は停止することになる。
【００４１】
ここで、作業プログラムが前工程と後工程の作業プログラム、即ち例えばワークをロボットハンド２Ｈで把持して他の部品に組付ける作業プログラムと、次の動作の作業プログラムとに分かれているものとする。次の動作としては、例えばワークをロボットハンド２Ｈから離して次の作業位置にロボット本体２を移動する動作が考えられる。この場合は、前工程の作業プログラムに従って上記力制御の実行中に、接触検知部Ｃ１７において予め設定した閾値以上の力覚値を検出すると、動作指令部Ｃ１８に閾値以上の力が加わったことを送信する。そして、動作指令部Ｃ１８は、モータ制御回路２１に指令してロボット本体２を停止制御する。これにより、ワークが他の部品に食い込み等を起こすことなく、前工程の作業プログラムは終了することになる。その後、作業プログラム解釈部Ｃ１６により後工程の作業プログラムの実行が開始され、動作指令部Ｃ１８は、モータ制御回路２１に指令してロボット本体２を動作制御する。これにより、ロボット本体２は、後工程の作業プログラムに従って次の動作が開始される。このように、接触検知部Ｃ１７は、作業プログラムの停止（終了）を判断する接触検知を行うことになる。
【００４２】
また、上記力制御の実行中にあって、例えば作業プログラムでは想定していない外力が力覚センサ２２Ｆに加わると、接触検知部Ｃ１７において予め設定した閾値以上の力覚値を検出する。この場合は、接触検知部Ｃ１７は、動作指令部Ｃ１８に接触状態として閾値以上の外力が加わったことを送信し、それを受けた動作指令部Ｃ１８は、モータ制御回路２１に指令してロボット本体２を直ちに停止制御することになる。なお、想定していない外力であるか想定していた外力であるかは、ロボット本体２の位置や姿勢、作業プログラムの進行状況などで判断してもよいし、上位コンピュータ２００によるシステム全体の情報やカメラ等で撮像した画像情報などから判断してもよい。
【００４３】
続いて、ロボットハンド２Ｈが他の物体に接触して停止する場合の制御について図４に沿って説明する。図４は、第１の実施の形態に係るロボット装置の制御を示すフローチャートである。詳しくは、作業プログラムに基づき力制御を開始して接触停止判定の力覚値の閾値（接触判定の閾値）が設定された後、ロボット本体２の動作を開始してから動作停止するまでのフローチャートである。
【００４４】
ロボット制御装置３は、例えば操作端末４からの指示により力制御を実行する作業プログラムの開始が指令されると、図４に示す制御を開始する。例えばこの作業プログラムにおける力制御の情報には、ロボットハンド２Ｈの接触時にロボット本体２を停止させるための力覚値の閾値が含まれている。なお、接触判定の閾値は、作業プログラムに含まれているものではなく、例えばロボット本体２の設計上の値などに基づき、予め設定して主記憶装置３２に保存しておいてもよい。
【００４５】
まず、本制御を開始すると、ロボット本体２の制御状態を動作状態にする（Ｓ１）。続いて、ロボット制御装置３の動作指令部Ｃ１８は、作業プログラムの動作内容に基づき、各制御周期のロボット動作目標位置をモータ制御回路２１に送信する（Ｓ２、動作処理、動作工程）。モータ制御回路２１は各サーボモータＭＨ，Ｍ１〜Ｍ６に対し、受信した動作目標位置に移動するための指令値を生成して送信し、それに従い各サーボモータＭＨ，Ｍ１〜Ｍ６が駆動され、ロボット本体２が動作目標位置に向けて１制御周期分だけ駆動される。そして、制御周期カウンタＩを１増加させ（Ｓ３）、次の制御周期に移る。
【００４６】
次に、力覚値変化率計算部Ｃ１３において、この制御周期における低域通過フィルタの処理で使用するカットオフ周波数Ｆｃを決定する（Ｓ４、周波数変更処理、周波数変更工程）。カットオフ周波数Ｆｃは、力覚値変化率に基づき図５に示す関数Ｇ０１から決定する。図５は、力覚値変化率に対するカットオフ周波数の関数の一例を示す図である。図５に示すように、カットオフ周波数Ｆｃは、力覚値変化率Ｒが０〜８である場合に５Ｈｚ、力覚値変化率Ｒが８〜１０である場合に２０Ｈｚ、力覚値変化率Ｒが１０以上である場合に８０Ｈｚに決定される。つまり力覚値変化率が大きいほどカットオフ周波数が高くなり、低域周波数成分をカットしないようになる。なお、図５に示す関数Ｇ０１は、段階的な関数となっているが、必ずしもカットオフ周波数Ｆｃと力覚値変化率Ｒにこのような関係性を持たせる必要はなく、例えば二次関数を使用してもよい。
【００４７】
ついで、力覚センサ２２Ｆからセンサ制御回路２２を介して現在の力覚値を取得し、力覚値記憶部Ｃ１１に割り当てられたフィルタ処理前の力覚値変数領域Ｆｒに保存する（Ｓ５）。続けて、フィルタ処理部Ｃ１２が低域通過フィルタの処理後の力覚値を計算して取得し、その結果を同じく力覚値記憶部Ｃ１１に割り当てられたフィルタ処理後の力覚値変数領域Ｆに保存する（Ｓ６、フィルタ処理、フィルタ工程）。
【００４８】
本実施の形態で求めるフィルタ処理後の力覚値Ｆ（Ｉ）の計算には、以下の低域通過フィルタの計算式を用いた。
【００４９】
Ｆ（Ｉ）＝（１−Ｄ）・Ｆｒ（Ｉ）＋Ｄ・Ｆ（Ｉ−１）
Ｄ＝１／（１＋２・ＰＩ・Ｔ・Ｆｃ）
なお、前回の制御周期でのフィルタ処理後の力覚値をＦ（Ｉ−１）、現在の制御周期でのフィルタ処理前の力覚値をＦｒ（Ｉ）、制御周期をＴ、カットオフ周波数をＦｃ、円周率をＰＩとしている。また、低域通過フィルタの計算式は、上記計算式に限ったものではなく、カットオフ周波数を変更可能であれば、どのような低域通過フィルタを使用しても良い。
【００５０】
続けて、次回の制御周期で適用する低域通過フィルタのカットオフ周波数を決定する前準備として、フィルタ処理後（フィルタ済）の力覚値の変化率を計算する（Ｓ７、変化率演算処理、変化率演算工程）。本実施の形態では、力覚値変化率Ｒを、フィルタ済の現在の制御周期における力覚値をＦ（Ｉ）、フィルタ済の前回の制御周期における力覚値をＦ（Ｉ−１）、制御周期をＴｃとして、以下の計算式により計算する。
【００５１】
Ｒ＝（Ｆ（Ｉ）−Ｆ（Ｉ−１））／Ｔｃ
なお、力覚値の変化率を計算する計算式は、上記計算式に限るものではなく、時間に対する力覚値の変化量と力覚値変化率Ｒに相関性をもつような計算式であれば、どのような計算式を用いてもよい。また、本実施の形態においては、低域通過フィルタの処理後の力覚値を用いて変化率を計算することで、ノイズ等により変化率が急上昇して閾値を越えてしまうような誤判定を防止している。しかしながら、設定した閾値に余裕があったり、力覚値のノイズによる変化が生じ難かったりする場合には、フィルタ処理前の力覚値を用いて変化率を演算するようにしてもよい。フィルタ処理前の力覚値を用いて変化率を演算した方が、接触を検知する上でレスポンスの向上を見込むことができる。
【００５２】
ついで、接触検知部Ｃ１７は、上述した接触判定の閾値と現在の低域通過フィルタの処理後の力覚値を比較し（Ｓ８）、現在のフィルタ処理後の力覚値Ｆ（Ｉ）が接触判定の閾値を超えているか否かの判定を行う。力覚値Ｆ（Ｉ）が接触判定の閾値を超えている場合は、接触検知部Ｃ１７が接触を検知したことを動作指令部Ｃ１８に送信する。
【００５３】
そして、動作指令部Ｃ１８は、ロボット本体２が目標位置に到達したこと、或いは接触を検知したこと（力覚値が閾値を越えたか）を検知するまで上記ステップＳ２〜ステップＳ８の制御を繰り返す（Ｓ９のＮｏ）。その後、何も接触せずにロボット本体２が目標位置に到達した場合、或いは目標位置に到達する前に接触を判定した場合は（Ｓ９のＹｅｓ）は、ロボット本体２の制御状態を停止状態にし、以上で本制御を終了する。
【００５４】
次に、本ロボット装置１において接触が発生した際の力覚値の変化について説明する。図６は、カットオフ周波数を変更する制御におけるロボット本体と他の物体との接触時における力覚値を示すグラフである。図６においては、フィルタ処理前の力覚値Ｆｒ（Ｉ）とフィルタ処理後の力覚値Ｆ（Ｉ）とを示している。また、図６に示すグラフは、上述したように力覚値変化率Ｒに応じて低域通過フィルタのカットオフ周波数が５Ｈｚ、２０Ｈｚ、８０Ｈｚの３種類に切換えられる場合のグラフである。比較例として、低域通過フィルタのカットオフ周波数を５Ｈｚに固定した場合について図８に示す。図８はカットオフ周波数を固定した制御におけるロボットと他の物体との接触時における力覚値を示すグラフである。
【００５５】
図６に示すように、ロボット本体２の動作を開始し、接触が生じる時点ｔ１までは、カットオフ周波数が５Ｈｚに設定された低域通過フィルタでフィルタ処理を行うことになる。この場合は、フィルタ処理前の力覚値Ｆｒ（Ｉ）とフィルタ処理後の力覚値Ｆ（Ｉ）とがほぼ同じ値であり、フィルタ処理前の力覚値Ｆｒ（Ｉ）にノイズが生じてもカットされ、フィルタ処理後の力覚値Ｆ（Ｉ）は安定している。時点ｔ１にロボットハンド２Ｈが接触すると、フィルタ処理前の力覚値Ｆｒ（Ｉ）が上昇を開始する。これにより、フィルタ処理後の力覚値Ｆ（Ｉ）もやや遅れて上昇を開始する。
【００５６】
フィルタ処理後の力覚値Ｆ（Ｉ）が上昇を開始すると、力覚値変化率Ｒが大きくなるので、それに応じて低域通過フィルタのカットオフ周波数が５Ｈｚから例えば８０Ｈｚに切換えられて変更される。これにより、フィルタ処理前の力覚値Ｆｒ（Ｉ）に対するフィルタ処理後の力覚値Ｆ（Ｉ）の追従が良好になる。このため、時点ｔ２にフィルタ処理前の力覚値Ｆｒ（Ｉ）が接触判定の閾値Ｆｔに到達してから、検知遅れ時間Ｔｌ後の時点ｔ３に、フィルタ処理後の力覚値Ｆ（Ｉ）も接触判定の閾値Ｆｔに到達する。従って、この時点ｔ３にロボット本体２の制御状態が停止状態にされ、実際に接触によりロボット本体２に作用する外力であるフィルタ処理前の力覚値Ｆｒ（Ｉ）は、外力の超過量Ｆｏで済む。
【００５７】
一方、図８に示すように、低域通過フィルタのカットオフ周波数が５Ｈｚに固定されている場合、時点ｔ１にロボットハンド２Ｈが接触し、フィルタ処理前の力覚値Ｆ’ｒ（Ｉ）が上昇を開始した場合、フィルタ処理後の力覚値Ｆ’（Ｉ）の追従が遅れる。このため、実際に生じる外力であるフィルタ処理前の力覚値Ｆ’ｒ（Ｉ）が時点ｔ２に接触判定の閾値Ｆｔに到達しても、検知遅れ時間Ｔ’ｌ後の時点ｔ４までフィルタ処理後の力覚値Ｆ’（Ｉ）が接触判定の閾値Ｆｔに到達しない。そのため、実際に接触によりロボット本体２に作用する外力であるフィルタ処理前の力覚値Ｆ’ｒ（Ｉ）は、外力の超過量Ｆ’ｏとなって、閾値Ｆｔよりも大きく超過して、ロボット本体２が他の物体に食い込み等を起こす可能性がある。
【００５８】
以上のように、本ロボット装置１では、力覚値変化率に基づき低域通過フィルタのカットオフ周波数を変更するので、事前準備を要することなく、接触の発生時における反応速度を上げることができる。
【００５９】
また、力覚値変化率が大きくなるほどカットオフ周波数が高くなるように変更するので、フィルタ処理前の力覚値に対してフィルタ処理後の力覚値の追従性を良好にすることができ、反応速度を上げることができる。
【００６０】
さらに、力覚値変化率に応じてカットオフ周波数を段階的に変更するので、演算処理が簡易になり、中央演算装置３１の負荷を低減することができる。
【００６１】
また、力覚値が予め設定した閾値を越えた場合に動作処理を停止するので、接触が発生した場合にロボット本体２を反応よく停止することができ、ロボットと他の物体との接触が強くなってしまうことの防止を図ることができる。
【００６２】
＜第２の実施の形態＞
ついで、上記第１の実施の形態を一部変更した第２の実施の形態を図７に沿って説明する。図７は、第２の実施の形態に係るロボット装置の制御を示すフローチャートである。本第２の実施の形態は、第１の実施の形態における制御に、制御の動作処理を実行する際の制御周期を変更する部分を加えたものである。なお、本第２の実施の形態では、第１の実施の形態の制御に制御周期の変更の制御を追加したものを説明するが、カットオフ周波数の変更の制御に代えて制御周期の変更の制御だけを行うものであってもよい。
【００６３】
詳細には、第２の実施の形態に係るロボット装置１の制御では、図７に示すように、第１の実施の形態における制御におけるステップＳ２とステップＳ３との間にステップＳ１１を追加したものである（図４参照）。ここでは、ステップＳ１〜Ｓ２、及びステップＳ３〜１０は、第１の実施の形態と同様であるため、その説明を省略する。
【００６４】
ステップＳ１１（周期変更処理、周期変更工程）は、全体の動作処理を実行する制御周期Ｔｃを決定し変更する処理であり、各制御周期で実行される。勿論であるが、必ずしも各制御周期で変化させる必要はない。制御周期Ｔｃは、例えば以下の計算式によって決定する。
【００６５】
Ｔｃ＝５    ［ｍｓ］      ：Ｒ＜８
Ｔｃ＝Ｔｃ  ［ｍｓ］      ：８≦Ｒ≦１０
Ｔｃ＝２    ［ｍｓ］      ：１０＜Ｒ
【００６６】
このように、力覚値が上昇する際にあっては、力覚値変化率Ｒが１０を超えるまでは制御周期が５Ｈｚに設定され、力覚値変化率Ｒが１０を超えると制御周期が２Ｈｚに設定される。また、力覚値が下降する際にあっては、力覚値変化率Ｒが８を下回るまでは制御周期が２Ｈｚに設定され、力覚値変化率Ｒが８を下回ると制御周期が５Ｈｚに設定される。従って、力覚値に大きな変化が加わったときに制御周期Ｔｃが短くなり、力覚値の変化が小さいときには制御周期Ｔｃが長くなる。
【００６７】
なお、上記の制御周期Ｔｃを決定する計算式では力覚値変化率Ｒの値によって、制御周期を変更しているが、力覚値変化率Ｒだけではなく力覚値Ｆ（Ｉ）も利用し、一定の力覚値Ｆａ以上の力が加わっている間は常に制御周期を短くするなどしてもよい。この場合は例えば以下の計算式が用いられる。
【００６８】
Ｔｃ＝５    ［ｍｓ］      ：Ｒ＜８  かつ  ｜Ｆ（Ｉ）｜＜Ｆａ
Ｔｃ＝Ｔｃ  ［ｍｓ］      ：８≦Ｒ≦１０  かつ  ｜Ｆ（Ｉ）｜＜Ｆａ
Ｔｃ＝２    ［ｍｓ］      ：１０＜Ｒ  または  Ｆａ≦｜Ｆ（Ｉ）｜
【００６９】
そして、ステップＳ３では、ステップＳ１１で計算した制御周期Ｔｃを参照し、前回のステップＳ３の実行が終了した時から計算した制御周期Ｔｃの時間が経過するまで待機し、制御周期カウンタを１増加させる。
【００７０】
以上のような本第２の実施の形態におけるロボット装置１の制御では、力覚値変化率Ｒに基づき動作処理を実行する制御周期Ｔｃが変更されるので、事前準備を要することなく、接触の発生時における反応速度を上げることができる。
【００７１】
特に力覚値が予め設定した閾値を越えた場合に動作処理を停止する本制御にあっては、接触が発生して力覚値変化率Ｒが大きくなったときに制御周期Ｔｃが短くすることができる。そのため、接触が発生した場合にロボット本体２を反応よく停止することができ、ロボットと他の物体との接触が強くなってしまうことの防止を図ることができる。
【００７２】
なお、以上説明した第１及び第２の実施の形態においては、力覚センサから取得する力覚値に温度ドリフト等のバイアス変化が生じていないことを前提に説明した。しかし、例えばフィルタ処理前の力覚値Ｆｒ（Ｉ）やフィルタ処理後の力覚値Ｆ（Ｉ）を、力制御開始時の力覚値を０とした差分で演算するように置換えれば、本実施の形態の制御をそのまま適用し、温度ドリフト等のバイアス変化に対応可能である。
【００７３】
また、以上説明した第１及び第２の実施の形態においては、ロボットハンド２Ｈとロボットアーム２Ａとの間に力覚センサ２２Ｆを設け、ロボットハンド２Ｈに接触等の外力が生じた場合を想定して説明を行った。しかし、例えば各関節などに力覚センサを配置して、それらの力覚値変化率を演算すれば、ロボットアームのどの部位に接触等の外力が加わっても対応できる。
【００７４】
また、以上説明した第１及び第２の実施の形態においては、ロボット本体２が他の物体と接触した場合について説明した。しかし、本発明の効果はこれに限らず、例えばワークを他の部品に組付ける際に、力覚値を用いてコンプライアンス制御する場合などにも、反応速度が上昇することは言うまでもない。
【００７５】
また、以上説明した第１及び第２の実施の形態においては、多関節ロボットを一例として説明したが、これに限らず、パラレルリンクロボットなど、他の形態のロボットにあっても本発明を適用し得る。
【００７６】
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。
【符号の説明】
【００７７】
１…ロボット装置：２…ロボット（ロボット本体）：３…制御装置（ロボット制御装置）：２２Ｆ…力覚センサ：Ｆ（Ｉ），Ｆｒ（Ｉ）…力覚値：Ｆｃ…カットオフ周波数：Ｒ…力覚値の変化率（力覚値変化率）：Ｔｃ…制御周期：Ｓ２…動作処理、動作工程：Ｓ４…周波数変更処理、周波数変更工程：Ｓ６…フィルタ処理、フィルタ工程：Ｓ７…変化率演算処理、変化率演算工程：Ｓ１１…周期変更処理、周期変更工程
【図１】
2016203315.tif 000003
【図２】
2016203315.tif 000004
【図３】
2016203315.tif 000005
【図４】
2016203315.tif 000006
【図５】
2016203315.tif 000007
【図６】
2016203315.tif 000008
【図７】
2016203315.tif 000009
【図８】
2016203315.tif 000010