JP6960242B2 - ロボットシステム及びロボットシステムの制御方法 - Google Patents

Description

本発明は、ロボットシステム及びロボットシステムの制御方法に関する。

従来からマスタースレーブマニピュレータとその制御方法が知られている（例えば特許文献１参照）。

このマスタースレーブマニピュレータは、力センサの測定した反力があらかじめ設定した力目標値を超えると接触モードに切り替えるよう構成されている。この接触モードにおいては、作業対象物の法線方向にほぼ一致するようにあらかじめ設定しておいた力制御方向、すなわち、作業対象物に向かう方向及び作業対象物から離れる方向に関しては、スレーブアームをマスターアームの動作に追従するように動作させずに、あらかじめ設定した力目標値と、力センサの測定した反力が一致するように力制御によって動作させるように構成されている。これによって、硬い作業対象物に接触しながら作業を行うマスタースレーブマニピュレータを安定に制御することができる。

特開平８−２８１５７３号公報

しかし、特許文献１に記載のマスタースレーブマニピュレータは、接触モードにおいては、マスタアームへの入力が無効化されるため、モード切替操作を行わなければ、マスタアームを操作して作業対象物から離れる方向にスレーブアームを動作させることができず、操作性に難点があった。

上記課題を解決するため、本発明のある態様に係るロボットシステムは、作業端を有するスレーブアームと、前記作業端に保持したワークが該ワークを組み付ける対象物に接触したときに前記作業端又は前記作業端に保持した前記ワークに作用する反力の方向及び大きさを検出するスレーブ側力検出部と、前記スレーブアームを駆動するスレーブアーム駆動部と、前記作業端の移動方向を規定するスレーブ動作指令に基づき前記スレーブアーム駆動部を制御するスレーブ側制御部と、を含むスレーブユニットと、操作端を有するマスタアームと、前記操作端に操作者が加えた操作力の方向及び大きさを検出するマスタ側力検出部と、前記マスタアームを駆動するマスタアーム駆動部と、前記操作端の移動方向を規定するマスタ動作指令に基づき前記マスタアーム駆動部を制御するマスタ側制御部と、を含むマスタユニットと、前記操作力及び前記反力に基づいて、前記スレーブ動作指令と、該スレーブ動作指令の前記作業端の移動方向に対応する移動方向に前記操作端を移動させる前記マスタ動作指令とを生成するシステム制御部と、を有するロボットシステムであって、前記システム制御部は、前記操作力及び前記反力に基づいて前記作業端の移動方向を算出する換算部と、前記反力が予め設定された許容値を超えるときは前記対象物を押圧する方向への前記作業端の移動を規制するように前記作業端の前記移動方向を修正する規制部と、修正した前記作業端の前記移動方向に基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する指令生成部と、を有する。

この構成によれば、ワークの押付動作を規制することができ、スレーブアームの動作によるワーク及び対象物の破損を防止することができる。また、作業端に保持したワークが対象物を押圧している状態において、作業端が対象物から離れる方向と対応する方向に操作端に力を加えることによって、スレーブアームの作業端を対象物から離れる方向に移動させることができ、操作性を向上させることができる。

前記スレーブ動作指令は位置指令であってもよい。

この構成によれば、位置指令に基づいて動作するスレーブアームを用いたロボットシステムにおいて、ワークまたは対象物の破損を効果的に防止することができる。

前記規制部は力加減部を有し、前記力加減部は、前記反力が前記許容値を超えるときは前記作業端が前記対象物を押圧する方向における前記作業端の位置を一時的に維持するように前記作業端の前記移動方向を修正してもよい。

この構成によれば、位置指令に基づいて動作するスレーブアームを用いたロボットシステムにおいて、ワークまたは対象物の破損を適切に防止することができる。

前記換算部は、前記操作力及び前記反力の差分に基づいて前記作業端の移動方向及び移動速度を規定する原速度ベクトルを算出し、前記規制部は力加減部を有し、前記力加減部は、前記反力の前記対象物を押圧する方向に向かう押圧方向成分が前記許容値を超え且つ前記原速度ベクトルが前記対象物を押圧する方向に向かう押圧方向成分を含むときは該押圧方向成分が０になるように前記原速度ベクトルを修正して加減速度ベクトルを算出し、前記指令生成部は、前記加減速度ベクトルに基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成してもよい。

この構成によれば、スレーブアームの動作によるワーク及び対象物の破損を効果的に防止することができる。

前記力加減部は、前記加減速度ベクトルの前記対象物を押圧する方向成分である押圧方向成分と直交する方向の成分が前記原速度ベクトルの前記対象物を押圧する方向と直交する方向の成分と同じ値となるように前記原速度ベクトルを修正して前記加減速度ベクトルを算出してもよい。

この構成によれば、スレーブアームの動作によるワークまたは対象物の破損を防止するとともに、操作感の悪化を防止することができる。

前記規制部は、力緩和部を有し、前記力緩和部は、前記反力の押圧方向成分が前記許容値を超えるときは前記作業端が前記対象物を押圧する方向と反対方向に前記作業端を移動させるように前記作業端の移動方向を修正してもよい。

この構成によれば、ワークと対象物とが接触したときに反力を緩和することができる。

前記換算部は、前記操作力及び前記反力の差分に基づいて前記作業端の移動方向及び移動速度を規定する原速度ベクトルを算出し、前記力緩和部は、前記反力が前記許容値を超えるときは前記許容値からの乖離値に基づいて前記原速度ベクトルの前記対象物を押圧する方向成分である押圧方向成分が小さくなるように前記原速度ベクトルを修正して緩和速度ベクトルを算出し、前記指令生成部は、前記緩和速度ベクトルに基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成してもよい。

この構成によれば、ワークと対象物とが接触したときに反力を適切に緩和することができる。

前記力緩和部は、前記許容値から前記反力を減算した値を積算した値に所定のゲインを乗算した調整積算値を前記原速度ベクトルの前記押圧方向成分に加算して前記原速度ベクトルを修正して前記緩和速度ベクトルを算出してもよい。

この構成によれば、ワークと対象物とが接触したときに反力を適切に緩和することができる。

前記力緩和部は、前記反力が前記許容値を超えた後で前記反力が許容値内に収まると、前記調整積算値を０にしてもよい。

この構成によれば、ワークと対象物とが離間し、作業性が低下することを防止することができる。

前記規制部は力加減部を有し、前記力加減部は、前記反力が前記許容値を超えるときは前記作業端が前記対象物を押圧する方向における前記作業端の位置を一時的に維持するように前記作業端の前記移動方向を修正してもよい。

この構成によれば、位置指令に基づいて動作するスレーブアームを用いたロボットシステムにおいて、ワークまたは対象物の破損を適切に防止することができる。

前記規制部は力加減部を更に有し、前記力加減部は、前記反力の前記対象物を押圧する方向に向かう押圧方向成分が前記許容値を超え且つ前記緩和速度ベクトルが前記押圧方向成分を含むときは該押圧方向成分が０になるように前記緩和速度ベクトルを修正して加減速度ベクトルを算出し、前記指令生成部は、前記加減速度ベクトルに基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成してもよい。

この構成によれば、スレーブアームの動作によるワーク及び対象物の破損を適切に防止することができる。

前記力緩和部は、前記緩和速度ベクトルの前記対象物を押圧する方向成分である押圧方向成分と直交する方向の成分が前記原速度ベクトルの前記対象物を押圧する方向と直交する方向の成分と同じ値になるように前記原速度ベクトルを修正して前記緩和速度ベクトルを算出し、前記力加減部は、前記加減速度ベクトルの前記対象物を押圧する方向と直交する方向の成分が前記緩和速度ベクトルの前記対象物を押圧する方向と直交する方向の成分と同じ値となるように前記緩和速度ベクトルを修正して前記加減速度ベクトルを算出してもよい。

この構成によれば、スレーブアームの動作によるワークまたは対象物の破損を防止するとともに、操作感の悪化を防止することができる。

上記課題を解決するため、本発明のある態様に係るロボットシステムの制御方法は、作業端を有するスレーブアームと、前記作業端に保持したワークが該ワークを組み付ける対象物に接触したときに前記作業端又は前記作業端に保持した前記ワークに作用する反力の方向及び大きさを検出するスレーブ側力検出部と、前記スレーブアームを駆動するスレーブアーム駆動部と、前記作業端の移動方向を規定するスレーブ動作指令に基づき前記スレーブアーム駆動部を制御するスレーブ側制御部と、を含むスレーブユニットと、操作端を有するマスタアームと、前記操作端に操作者が加えた操作力の方向及び大きさを検出するマスタ側力検出部と、前記マスタアームを駆動するマスタアーム駆動部と、前記操作端の移動方向を規定するマスタ動作指令に基づき前記マスタアーム駆動部を制御するマスタ側制御部と、を含むマスタユニットと、前記操作力及び前記反力に基づいて、前記スレーブ動作指令と、該スレーブ動作指令の前記作業端の移動方向に対応する移動方向に前記操作端を移動させる前記マスタ動作指令とを生成するシステム制御部と、を有するロボットシステムの制御方法であって、前記システム制御部が前記操作力及び前記反力に基づいて前記作業端の移動方向を算出する換算ステップと、前記システム制御部が前記反力が予め設定された許容値を超えるときは前記対象物を押圧する方向への前記作業端の移動を規制するように前記作業端の移動方向を修正する規制ステップと、前記システム制御部が修正した前記作業端の移動方向に基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する指令生成ステップと、を有する。

この構成によれば、ワークの押付動作を規制することができ、スレーブアームの動作によるワーク及び対象物の破損を防止することができる。また、作業端に保持したワークが対象物を押圧している状態において、作業端が対象物から離れる方向と対応する方向に操作端に力を加えることによって、スレーブアームの作業端を対象物から離れる方向に移動させることができ、操作性を向上させることができる。

本発明は、スレーブアームの動作によるワーク及び対象物の破損を防止することができ、また、操作性を向上させることができるという効果を奏する。

本発明の実施の形態１に係るロボットシステムの構成例を概略的に示す図である。 図１のロボットシステムの制御系統の構成例を概略的に示すブロック図である。 図１のロボットシステムの動作例を示すフローチャートである。 図１のロボットシステムの動作例を示すフローチャートである。 図１のロボットシステムの動作例を示す図である。 図１のロボットシステムの動作例を示す図である。 図１のロボットシステムの動作例において、反力の経時変化を示す図である。 図１のロボットシステムの動作例において、緩和速度ベクトルの押圧方向成分の経時変化を示す図である。 図１のロボットシステムの動作例において、加減速度ベクトルの押圧方向成分の経時変化を示す図である。 本発明の実施の形態２に係るロボットシステムの制御系統の構成例を概略的に示すブロック図である。

以下、本発明の実施の形態について、図面を参照しながら説明する。なお、本実施の形態によって本発明が限定されるものではない。また、以下では、全ての図を通じて、同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。

（実施の形態１）
図１は、本発明の実施の形態１に係るロボットシステム１００の構成例を概略的に示す図である。図２は、ロボットシステム１００の制御系統の構成例を概略的に示すブロック図である。

ロボットシステム１００は、図１及び図２に示すように、スレーブアーム１１がマスタアーム２１の動きをなぞるように動作するマスタースレーブ方式のロボットを含むシステムである。ロボットシステム１００では、スレーブアーム１１の作業領域から離れた位置（作業領域外）にいる操作者がマスタアーム２１を動かして動作指令をロボットシステム１００に入力することで、スレーブアーム１１が該動作指令に対応した動作を行い、部品の組み付け作業などの特定の作業を行うことができるように構成されている。

更に、ロボットシステム１００は、バイラテラル制御方式のロボットシステムであり、制御部がスレーブアーム１１に加えてマスタアーム２１の動作を制御することで、スレーブアーム１１に作用する力をマスタアーム２１を介して操作者に提示するように構成されている。ロボットシステム１００は、スレーブアーム１１を含むスレーブユニット１と、マスタアーム２１を含むマスタユニット２と、システム制御部３とを備える。

［スレーブユニットの構成例］
スレーブユニット１は、産業用ロボットである。しかし、これに限られるものではない。スレーブユニット１は、基部１０と、スレーブアーム１１と、スレーブ側力検出部１２と、スレーブアーム駆動部１３（図２参照）と、スレーブ側制御部１４（図２参照）とを含む。

スレーブアーム１１は、例えば、垂直多関節型のロボットアームである。すなわち、基端部から先端部に向かう方向に順次連結される複数のリンクと、隣り合うリンクの一方に対して他方を回動可能に連結する一以上の関節を備える。そして、スレーブアーム１１の先端部が作業端１１ａを構成し、作業端１１ａにはハンド（エンドエフェクタ）１６が設けられている。基部１０は、例えば床に固定され、スレーブアーム１１を支えている。

ハンド１６は、ワークＷの保持を行う保持動作及び保持したワークＷの解放を行う解放動作を行うことができるように構成されている。ハンド１６は、保持動作及び解放動作を行うための図示しないハンド駆動部を含む。なお、本実施の形態においてハンド１６は、例えば部品の組み付け作業を実施することができるように保持動作及び解放動作を行うことができるように構成されているがこれに限られるものではない。これに代えて、例えば溶接作業、塗装作業を行うことができるように構成されていてもよい。

スレーブ側力検出部１２は、互いに直交する３つの軸方向に作用する力の大きさを検出するセンサであり、スレーブアーム１１の作業端１１ａに配設されている。スレーブ側力検出部１２は互いに直交する３つの軸方向に作用する分力の検出が可能な３軸力覚センサで構成される。これによって、スレーブ側力検出部１２は、スレーブアーム１１の作業端１１ａに保持したワークＷがワークＷを組み付ける対象物Ｔに接触したときに、作業端１１ａ又は作業端１１ａに保持したワークＷに作用する反力ｆ_ｓの方向及び大きさを検出するように構成されている。

スレーブアーム駆動部１３は、スレーブアーム１１を駆動する。すなわち、スレーブアーム駆動部１３は、スレーブアーム１１の各関節に設けられたアクチュエータを含み、アクチュエータの駆動によって各関節を動作させることにより、スレーブアーム１１の基端部に対して先端部（作業端１１ａ）及びハンド１６を所定の動作領域内で移動させる。本実施の形態において、例えば、スレーブアーム１１の各関節は回動関節であり、アクチュエータは減速機を備えるサーボモータである。

スレーブ側制御部１４は、作業端１１ａの移動方向を規定するスレーブ動作指令ｘ_ｓに基づきスレーブアーム駆動部１３を制御し、スレーブアーム１１を動作させる。スレーブ動作指令ｘ_ｓは、スレーブ座標系における位置指令である。スレーブ側制御部１４は、スレーブ動作指令ｘ_ｓに基づき、各関節のサーボモータの出力軸の回転角を算出し、スレーブアーム１１の各関節のサーボモータに供給する電流を制御してサーボモータの動作を制御し、スレーブアーム１１の姿勢を変更する。

［マスタユニットの構成例］
マスタユニット２は、作業領域外に設置され、スレーブアーム１１の動作を制御する。

マスタユニット２は、マスタアーム２１と、マスタ側力検出部２２と、マスタアーム駆動部２３と、マスタ側制御部２４とを含む。

マスタアーム２１は、操作者Ｐが触れて操作して操作者Ｐからスレーブアーム１１に対する動作指令を入力する装置である。操作者Ｐが触れて操作する部位が操作端２１ａを構成し、操作者Ｐは操作端２１ａに力を加えて、スレーブアーム１１に対する動作指令を入力する。

マスタ側力検出部２２は、互いに直交する３つの軸方向に作用する力の大きさを検出するセンサであり、マスタアーム２１の操作端２１ａに配設されている。マスタ側力検出部２２は互いに直交する３つの軸方向に作用する分力の検出が可能な３軸力覚センサで構成される。これによって、マスタ側力検出部２２は、操作者Ｐからスレーブアーム１１に対する動作指令、すなわちマスタアーム２１の操作端２１ａに操作者Ｐが加えた操作力ｆ_ｍの方向及び大きさを検出するように構成されている。

マスタアーム駆動部２３は、マスタアーム２１を駆動する。すなわち、マスタアーム駆動部２３は、マスタアーム２１の各関節に設けられたアクチュエータを含み、アクチュエータの駆動によって各関節を動作させることにより、マスタアーム２１の操作端２１ａを移動させる。本実施の形態において、例えば、アクチュエータは減速機を備えるサーボモータである。

マスタ側制御部２４は、操作端２１ａの移動方向を規定するマスタ動作指令ｘ_ｍに基づきマスタアーム駆動部２３を制御し、マスタアーム２１を動作させる。マスタ動作指令ｘ_ｍは、マスタ座標系における位置指令である。マスタ側制御部２４は、マスタ動作指令ｘ_ｍに基づき、マスタアーム２１の各関節のサーボモータの出力軸の回転角を算出し、マスタアーム２１の各関節のサーボモータに供給する電流を制御して各関節のサーボモータの動作を制御し、マスタアーム２１の姿勢を変更する。

［システム制御部の構成例］
システム制御部３は、並列型バイラテラル制御方式によって、スレーブユニット１及びマスタユニット２を制御する。すなわち、システム制御部３は、マスタ側力検出部２２が検出した操作力ｆ_ｍ及びスレーブ側力検出部１２が検出した反力ｆ_ｓに基づいて、位置指令であるスレーブ動作指令ｘ_ｓ及び位置指令であるマスタ動作指令ｘ_ｍを生成する。マスタ動作指令ｘ_ｍは、スレーブ動作指令ｘ_ｓの作業端１１ａの移動方向に対応する移動方向に操作端２１ａを移動させる動作指令である。すなわち、スレーブアーム１１のスレーブ座標系とマスタアーム２１のマスタ座標系とは対応づけられ、スレーブ動作指令ｘ_ｓのスレーブ座標系における作業端１１ａの移動方向と、マスタ動作指令ｘ_ｍのマスタ座標系における操作端２１ａの移動方向とが同じ方向となる（互いに一致する）ように構成されている。

これによって、スレーブアーム１１の作業端１１ａがマスタアーム２１の操作端２１ａの動きをなぞるように動作し、且つスレーブアーム１１の作業端１１ａに作用する力をマスタアーム２１の操作端２１ａを介して操作者に提示する。したがって、操作者は、作業端１１ａが環境に接触した際の力覚を認識して作業を行うことができるように構成されている。

システム制御部３は、換算部３１と、規制部３２と、指令生成部３３とを含む。換算部３１、規制部３２、指令生成部３３は、所定の制御プログラムを図示しない演算部が実行することにより実現される機能ブロックである。

スレーブ側制御部１４、マスタ側制御部２４及びシステム制御部３に係る上記の演算部は、例えばマイクロコントローラ、ＣＰＵ、ＡＳＩＣ、ＦＰＧＡ等のプログラマブルロジックデバイス（ＰＬＤ）などの演算器で構成される。演算部は、集中制御する単独の制御器で構成されていてもよく、互いに協働して分散制御する複数の制御器で構成されてもよい。また、システム制御部３は、各種プログラム及びデータを記憶する記憶装置（図示せず）を備えている。また、システム制御部３は、スレーブ側制御部１４及びマスタ側制御部２４を含む単独の制御器で構成されていてもよく、互いに協働して分散制御する複数の制御器で構成されてもよい。

換算部３１は、マスタ側力検出部２２が検出した操作力ｆ_ｍ及びスレーブ側力検出部１２が検出した反力ｆ_ｓに基づいて作業端１１ａの移動方向を算出する。すなわち、操作力ｆ_ｍ及び反力ｆ_ｓの差分に基づいて作業端１１ａの移動方向及び移動速度を規定する原速度ベクトルｖ_ｄを算出する。換算部３１は、原速度ベクトルｖ_ｄを、例えば以下の式（１）に基づいて算出する。

規制部３２は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが予め設定された許容値ｆ_ｌｉｍを超えるときは、スレーブアーム１１の対象物Ｔを押圧する方向への作業端１１ａの移動を規制するようにスレーブアーム１１の作業端１１ａの移動方向を修正する。本実施の形態において、規制部３２は、力緩和部４１と、力加減部４２とを含む。

力緩和部４１は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えるときは、スレーブアーム１１の作業端１１ａの対象物Ｔを押圧する方向と反対方向にスレーブアーム１１の作業端１１ａを移動させるようにスレーブアーム１１の作業端１１ａの移動方向を修正する。

本実施の形態において、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えるときは、例えば以下の式（２）〜（５）によって、原速度ベクトルｖ_ｄに基づいて緩和速度ベクトルｖ_ｄｍを算出する。

すなわち、力緩和部４１は、反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えるときは、許容値ｆ_ｌｉｍから反力ｆ_ｓの押圧方向成分ｆ_ｓｉを減算した値に基づいて算出した値を原速度ベクトルｖ_ｄの押圧方向成分ｖ_ｄｉに加算することにより、原速度ベクトルｖ_ｄを修正し、緩和速度ベクトルｖ_ｄｍを算出するように構成されている。すなわち、許容値ｆ_ｌｉｍから反力ｆ_ｓの押圧方向成分ｆ_ｓｉを減算した値（負の値）に比例ゲインＫ_ｐを乗算した調整比例値、及び許容値ｆ_ｌｉｍから反力ｆ_ｓの押圧方向成分ｆ_ｓｉを減算した値を積算した値（負の値）に積分ゲインＫ_ｉを乗算した調整積算値を原速度ベクトルｖ_ｄの押圧方向成分ｖ_ｄｉに加算することにより、原速度ベクトルｖ_ｄを修正し、緩和速度ベクトルｖ_ｄｍを算出する。すなわち、反力ｆ_ｓが許容値ｆ_ｌｉｍを超えるときは許容値ｆ_ｌｉｍからの乖離値に基づいて原速度ベクトルｖ_ｄの対象物Ｔを押圧する方向成分である押圧方向成分ｖ_ｄｉが小さくなるように原速度ベクトルｖ_ｄを修正して緩和速度ベクトルｖ_ｄｍを算出する。このように、調整積算値を押圧方向成分ｖ_ｄｉに加算するように構成されているので、ｖ_ｄｍｉの値の振動を抑えて許容値ｆ_ｌｉｍに適切に収束させることができる。

なお、力緩和部４１は、緩和速度ベクトルｖ_ｄｍの対象物を押圧する方向と直交する方向の成分（ｖ_ｄｍｊ成分及びｖ_ｄｍｋ成分）が原速度ベクトルｖ_ｄの対象物を押圧する方向と直交する方向の成分（ｖ_ｄｊ成分及びｖ_ｄｋ成分）と同じ値となるように原速度ベクトルｖ_ｄを修正して緩和速度ベクトルｖ_ｄｍを算出する。

また、力緩和部４１は、反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えた後で反力ｆ_ｓが許容値ｆ_ｌｉｍ内に収まると、調整積算値を０にする。

一方、力緩和部４１は、反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍ内に収まるときは、原速度ベクトルｖ_ｄを修正することなく緩和速度ベクトルｖ_ｄｍとする。

力加減部４２は、反力ｆ_ｓが許容値ｆ_ｌｉｍを超えるときはスレーブアーム１１の作業端１１ａが対象物Ｔを押圧する方向における作業端１１ａの位置を一時的に維持するように作業端１１ａの移動方向を修正する。すなわち、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超え、且つ緩和速度ベクトルｖ_ｄｍが対象物Ｔを押圧する方向に向かう押圧方向成分ｖ_ｄｍｉを含むときは、この押圧方向成分ｖ_ｄｍｉが０になるように緩和速度ベクトルｖ_ｄｍを修正して加減速度ベクトルｖ_ｄａを算出する。

本実施の形態において、力加減部４２は、例えば以下の式（６）〜（９）によって、緩和速度ベクトルｖ_ｄｍに基づいて加減速度ベクトルｖ_ｄａを算出する。

なお、力加減部４２は、加減速度ベクトルｖ_ｄａの対象物Ｔを押圧する方向と直交する方向の成分（ｖ_ｄａｊ成分及びｖ_ｄａｋ成分）が緩和速度ベクトルｖ_ｄｍの対象物Ｔを押圧する方向と直交する方向の成分（ｖ_ｄｍｊ成分及びｖ_ｄｍｋ成分）と同じ値となるように緩和速度ベクトルｖ_ｄｍを修正して加減速度ベクトルｖ_ｄａを算出する。

一方、力加減部４２は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍに収まるとき、又は緩和速度ベクトルｖ_ｄｍが対象物を押圧する方向に向かう押圧方向成分ｖ_ｄｍｉを含まないとき（押圧方向成分ｖ_ｄｍｉが０又は負の値をとるとき）は、緩和速度ベクトルｖ_ｄｍを修正することなく加減速度ベクトルｖ_ｄａとする。

指令生成部３３は、修正した作業端１１ａの移動方向に基づいてスレーブ動作指令ｘ_ｓ及びマスタ動作指令ｘ_ｍを生成する。すなわち、指令生成部３３は、緩和速度ベクトルｖ_ｄｍに基づいて算出された加減速度ベクトルｖ_ｄａに基づいてスレーブ動作指令ｘ_ｓ及びマスタ動作指令ｘ_ｍを生成する。指令生成部３３は、加減速度ベクトルｖ_ｄａに基づいて作業端１１ａの目標位置を規定するスレーブ動作指令ｘ_ｓを生成するスレーブ動作指令生成部５１と、加減速度ベクトルｖ_ｄａに基づいてマスタアーム２１の目標位置を規定するマスタ動作指令ｘ_ｍを生成するマスタ動作指令生成部５２とを含む。上述の通り、マスタ動作指令ｘ_ｍは、スレーブ動作指令ｘ_ｓの作業端１１ａの移動方向に対応する移動方向に操作端２１ａを移動させる動作指令である。

［動作例］
次に、ロボットシステム１００の動作例を説明する。

図３Ａ及び図３Ｂは、ロボットシステム１００の動作例を示すフローチャートである。図４Ａ及び図４Ｂは、ロボットシステム１００の動作例を示す図である。本動作例においては、円筒形に形成され貫通孔Ｗａを有するワークＷと、上下方向に伸延する円柱状のピンであって、貫通孔Ｗａと嵌合可能に形成されている対象物Ｔとを嵌合させて、ワークＷを対象物Ｔに組み付ける作業をロボットシステム１００を用いて行う。ワークＷはスレーブアーム１１のハンド１６に把持されており、対象物Ｔは例えば生産ライン上に位置している。

まず、マスタアーム２１が操作されておらず、且つスレーブアーム１１が対象物Ｔ等の環境に接触していない状態においては、操作力ｆ_ｍは０であり、反力ｆ_ｓも０となり、操作力ｆ_ｍと反力ｆ_ｓとの差分も０となる。したがって、換算部３１が算出する原速度ベクトルｖ_ｄ、力緩和部４１が算出する緩和速度ベクトルｖ_ｄｍ、及び力加減部４２が算出する加減速度ベクトルｖ_ｄａは、何れも０となり、スレーブ動作指令生成部５１及びマスタ動作指令生成部５２は、それぞれスレーブアーム１１及びマスタアーム２１の目標位置を更新しない。よって、スレーブアーム１１及びマスタアーム２１は、現在の姿勢を維持する。

次に、操作者ＰがワークＷを対象物Ｔに近づける方向、すなわち下方にマスタアーム２１の操作端２１ａに操作力ｆ_ｍを加えると、換算部３１は、操作力ｆ_ｍと反力ｆ_ｓとの差分に応じた大きさを有する下方向の原速度ベクトルｖ_ｄ、すなわち操作力ｆ_ｍに応じた大きさを有する下方向の原速度ベクトルｖ_ｄを算出する（ステップＳ１）。

次に、力緩和部４１は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えるか否かを判定する（ステップＳ２）。ワークＷが対象物Ｔに近づくように移動している状態においては、スレーブ側力検出部１２が検出した反力ｆ_ｓは０であり、許容値ｆ_ｌｉｍに収まるので（ステップＳ２においてＮｏ）、原速度ベクトルｖ_ｄを修正することなく緩和速度ベクトルｖ_ｄｍとする（ステップＳ３２）。

次に、力加減部４２は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超え、且つ緩和速度ベクトルｖ_ｄｍが対象物Ｔを押圧する方向に向かう押圧方向成分ｖ_ｄｍｉを含むか否かを判定する（ステップＳ４）。ワークＷが対象物Ｔに近づくように移動している状態においては、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉは０であるので（ステップＳ４においてＮｏ）、緩和速度ベクトルｖ_ｄｍを修正することなく加減速度ベクトルｖ_ｄａとする（ステップＳ５２）。

次に、スレーブ動作指令生成部５１は、加減速度ベクトルｖ_ｄａに基づいてスレーブ動作指令ｘ_ｓを生成し、スレーブ座標系におけるスレーブアーム１１の作業端１１ａの目標位置を現在位置の下方の位置に更新する。スレーブ動作指令ｘ_ｓは、スレーブ側制御部１４に入力され、スレーブ側制御部１４がスレーブアーム１１の作業端１１ａが下方に移動するようにスレーブアーム１１の姿勢を変化させる。また、マスタ動作指令生成部５２は、加減速度ベクトルｖ_ｄａに基づいてマスタ動作指令ｘ_ｍを生成し、マスタ座標系におけるマスタアーム２１の操作端２１ａの目標位置を現在位置の下方の位置に更新する。マスタ動作指令ｘ_ｍは、マスタ側制御部２４に入力され、マスタ側制御部２４がマスタアーム２１の操作端２１ａが下方に移動するようにマスタアーム２１の姿勢を変化させる（ステップＳ６）。そして、再度ステップＳ１が実行される。

このように、ステップＳ１〜Ｓ６が繰り返し実行されることにより、スレーブアーム１１の作業端１１ａ及びマスタアーム２１の操作端２１ａが同時に下方に移動するように構成されている。これによって、操作者Ｐは、あたかもスレーブアーム１１の作業端１１ａがマスタアーム２１の操作端２１ａの動きをなぞるように動作するような感覚を得られる。

そして、スレーブアーム１１の作業端１１ａが下方に移動することにより、図４Ａに示すように、ワークＷが対象物Ｔの上端に接触し、ワークＷが対象物Ｔに押し付けられると、反力がワークＷに作用し、この力の方向及び大きさに応じた反力ｆ_ｓがスレーブ側力検出部１２により検出される。図５Ａは、本動作例におけるスレーブ側力検出部１２により検出された反力ｆ_ｓの押圧方向成分ｆ_ｓｉの経時的な変化を示すグラフであり、ワークＷが対象物Ｔの上端に接触した時刻ｔ_０において反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えたことを示している。

そして、スレーブ側力検出部１２により検出された反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えると、ステップＳ２において、力緩和部４１は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えたと判定し（ステップＳ２においてＹｅｓ）、原速度ベクトルｖ_ｄを修正して緩和速度ベクトルｖ_ｄｍを算出する（ステップＳ３１）。上述の式（３）に示す通り、緩和速度ベクトルｖ_ｄｍの押圧方向成分ｖ_ｄｍｉは、正の値をとる原速度ベクトルの押圧方向成分ｖ_ｄｉに負の値をとる調整比例値及び調整積算値を加算することによって算出され、図５Ｂに示すように、押圧方向成分ｖ_ｄｍｉは、時間の経過に伴って値が小さくなるように変化する。

次に、力加減部４２は、ステップＳ４において、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超え、且つ緩和速度ベクトルｖ_ｄｍが対象物Ｔを押圧する方向に向かう押圧方向成分ｖ_ｄｍｉを含むと判定し（ステップＳ４においてＹｅｓ）、この押圧方向成分ｖ_ｄｍｉが０になるように緩和速度ベクトルｖ_ｄｍを修正して加減速度ベクトルｖ_ｄａを算出する（ステップＳ５１）。

すなわち、図５Ｂに示すように、力加減部４２は、押圧方向成分ｖ_ｄｍｉの値が正の値をとる時間帯Ｔ１においては、緩和速度ベクトルｖ_ｄｍの押圧方向成分ｖ_ｄｍｉの値を０にして、加減速度ベクトルｖ_ｄａを算出する。これによって、図５Ｃに示すように、時間帯Ｔ１において加減速度ベクトルｖ_ｄａの押圧方向成分ｖ_ｄａｉは、０の値をとる。この加減速度ベクトルｖ_ｄａに基づいて生成されるスレーブ動作指令ｘ_ｓ及びマスタ動作指令ｘ_ｍの押圧方向（ｉ方向）における目標位置は現在位置と同じ位置である。したがって、ワークＷが対象物Ｔの上端に接触した直後の時間帯Ｔ１において、スレーブアーム１１の作業端１１ａは、スレーブアーム１１の作業端１１ａが対象物Ｔを押圧する方向における作業端１１ａの位置を一時的に維持し、スレーブアーム１１の作業端１１ａは下方に移動しないように移動が規制される。これによって、過度な力がワークＷ及び対象物Ｔに作用し、ワークＷ及び対象物Ｔが破損することを防止することができる。また、ワークＷが対象物Ｔの上端に接触した時刻ｔ_０において、マスタアーム２１の操作端２１ａは下方に移動しないように移動が突然規制され、更にその直後の時間帯Ｔ１においてもマスタアーム２１の操作端２１ａは下方に移動しないように移動が規制される。これによって、操作者Ｐは、作業端１１ａが対象物Ｔに接触して衝撃を受けた感覚を得られる。なお、図５Ａに示すように、時間帯Ｔ１において、スレーブアーム１１の作業端１１ａは移動しないため、スレーブ側力検出部１２が検出する反力ｆ_ｓは変化しない。

そして、時間が経過するに従い原速度ベクトルｖ_ｄに加算される調整積算値の絶対値が増大し、力緩和部４１が緩和速度ベクトルｖ_ｄｍの押圧方向成分ｖ_ｄｍｉの値が小さくなるように修正し、図５Ｂに示すように、時刻ｔ_１において押圧方向成分ｖ_ｄｍｉが０以下になると、力加減部４２は、図５Ｂ及び図５Ｃに示すように、ステップＳ４において緩和速度ベクトルｖ_ｄｍが対象物Ｔを押圧する方向に向かう押圧方向成分ｖ_ｄｍｉを含まないと判定し（ステップＳ４においてＮｏ）、緩和速度ベクトルｖ_ｄｍを修正することなく加減速度ベクトルｖ_ｄａとする。この加減速度ベクトルｖ_ｄａに基づいて生成されるスレーブ動作指令ｘ_ｓ及びマスタ動作指令ｘ_ｍの押圧方向（ｉ方向）における目標位置は、現在位置よりもワークＷが対象物Ｔを押圧する方向と反対方向、すなわちワークＷが対象物Ｔから遠ざかる側の位置である。したがって、図５Ｃに示すように、反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍ内に収まる時刻ｔ_２までの時間帯Ｔ２において、スレーブアーム１１の作業端１１ａは上方に移動する。これによって、ワークＷが対象物Ｔを押圧する力を小さく又はキャンセルすることができ、ワークＷ及び対象物Ｔが破損することを効果的に防止することができる。また、操作者Ｐは、時間帯Ｔ１において受けた衝撃に係る感覚を覚えている時間帯Ｔ１に続く時間帯Ｔ２において、スレーブアーム１１の作業端１１ａを徐々に上方に移動させるように構成されているので、スレーブアーム１１及びマスタアーム２１に振動が発生することを防止することができ、また、操作者Ｐがマスタアーム２１の操作性に違和感を与えることを防止することができる。

そして、時間帯Ｔ２において、スレーブアーム１１の作業端１１ａが上方に移動すると、図５Ａに示すように、反力ｆ_ｓの押圧方向成分ｆ_ｓｉの値が減少する。そして、図３Ｂに示すように、時刻ｔ_０において反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えた後、時刻ｔ_２において反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍ内に収まると、式（３）における調整比例値及び調整積算値を０にリセットする。これによって、再び反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超えたときに、前回の調整比例値及び調整積算値が原速度ベクトルｖ_ｄの押圧方向成分ｖ_ｄｉに加算され、スレーブアーム１１及びマスタアーム２１の動作に異常をきたすことを防止することができる。

なお、本実施の形態において、時刻ｔ_０から時刻ｔ_２までの間隔が５０ミリ秒程度となるように式（３）の比例ゲインＫ_ｐ及び積分ゲインＫ_ｉの値が選択される。これによって、操作性を向上させることができる。

なお、式（２）〜（５）の通り、力緩和部４１は、緩和速度ベクトルｖ_ｄｍの押圧方向と直交する方向の成分が原速度ベクトルｖ_ｄの押圧方向と直交する方向の成分と同じ値となるように原速度ベクトルｖ_ｄを修正して緩和速度ベクトルｖ_ｄｍを算出する。また、力加減部４２は、加減速度ベクトルｖ_ｄａの押圧方向と直交する方向の成分が緩和速度ベクトルｖ_ｄｍの押圧方向と直交する方向の成分と同じ値となるように緩和速度ベクトルｖ_ｄｍを修正して加減速度ベクトルｖ_ｄａを算出する。すなわち、規制部３２は、換算部３１が算出した原速度ベクトルｖ_ｄの押圧方向と直交する方向の成分（原速度ベクトルｖ_ｄのｊ方向成分及びｋ方向成分）を修正せずに、加減速度ベクトルｖ_ｄａを算出する。よって、ワークＷが対象物Ｔに押付けられた状態におけるワークＷのｊ方向及びｋ方向への移動操作（ｊｋ平面上の移動操作）は、ワークＷが対象物Ｔに押付けられていない状態と変化しない。よって、図４Ｂに示すワークＷと対象物Ｔとを嵌合させる操作、すなわちワークＷの貫通孔Ｗａの軸と対象物Ｔの軸とが一致する位置を探る操作を円滑に行うことができる。

以上に説明したように、本発明のロボットシステム１００、規制部３２が対象物Ｔを押圧する方向への操作端２１ａの移動を規制するので、ワークＷが対象物Ｔに押付けられた状態で押圧方向と直交する平面方向に移動することによって、過大な摩擦力がワークＷと対象物Ｔとの間に作用し、ワークＷ及び対象物Ｔが破損することを防止することができる。

また、作業端１１ａに保持したワークＷが対象物Ｔを押圧している状態において、モード切替操作等の特別な操作を行うことなく、作業端１１ａが対象物Ｔから離れる方向と対応する方向に操作端２１ａに力を加えることによって、スレーブアーム１１の作業端１１ａを対象物Ｔから離れる方向に移動させることができ、操作性を向上させることができる。すなわち、作業端１１ａに保持したワークＷが対象物Ｔを押圧している状態においても、作業端１１ａが対象物Ｔから離れる方向に作業端１１ａを移動させる操作を、作業端１１ａに保持したワークＷが対象物Ｔと接触していないときと同様に行うことができる。

更に、本実施の形態において、スレーブアーム１１は、産業用ロボットであり、スレーブアーム駆動部１３の減速機には波動歯車装置等の減速比の大きい機構が用いられ、そのために、通常、サーボモータの制御は、位置制御が用いられる。しかし、位置制御において、作業端１１ａに保持したワークＷが対象物Ｔ等の環境に接触すると大きな反力が発生し、ワークＷや対象物Ｔを破損する虞があった。しかし、規制部３２が対象物Ｔを押圧する方向への操作端２１ａの移動を規制するので、位置制御を用いたロボットシステム１００において効果的にワークＷや対象物Ｔの破損を防止することができる。

＜実施の形態１の変形例＞
スレーブ側力検出部１２は、スレーブアーム１１の作業端１１ａに配設したセンサを用いて、スレーブアーム１１の作業端１１ａ又は作業端１１ａに保持したワークＷに作用する反力ｆ_ｓの方向及び大きさを検出したがこれに限られるものではない。これに代えて、スレーブ側力検出部１２は、スレーブアーム１１の各関節のサーボモータに供給する電流値に基づいて、スレーブアーム１１の作業端１１ａ又は作業端１１ａに保持したワークＷに作用する反力ｆ_ｓの方向及び大きさを検出してもよい。

また、マスタ側力検出部２２は、マスタアーム２１の操作端２１ａに配設したセンサを用いて、マスタアーム２１の操作端２１ａに操作者Ｐが加えた操作力ｆ_ｍの方向及び大きさを検出したがこれに限られるものではない。これに代えて、マスタ側力検出部２２は、マスタアーム２１の各関節のサーボモータに供給する電流値に基づいて、マスタアーム２１の操作端２１ａに操作者Ｐが加えた操作力ｆ_ｍの方向及び大きさを検出してもよい。

（実施の形態２）
以下では実施の形態２の構成、動作について、実施の形態１との相違点を中心に述べる。

図６は、本発明の実施の形態２に係るロボットシステムの制御系統の構成例を概略的に示すブロック図である。

本実施の形態２において、システム制御部３は、更に誇張表現部２５２を備える。

誇張表現部２５２は、スレーブ側力検出部１２が検出した反力ｆ_ｓに基づきワークＷと対象物Ｔとが接触状態にあるか否かを判定し、接触状態にあると判定した場合は、操作者Ｐに対し、ワークＷが対象物Ｔに接触した感覚を誇張して提示する。

操作者Ｐに対する感覚の提示は、誇張表現部２５２がスレーブアーム１１の反力ｆ_ｓに基づいて速度成分を生成し、生成した速度成分をマスタ動作指令生成部５２に入力される加減速度ベクトルｖ_ｄａに加算する。生成される速度成分は、例えば１周期分の三角波成分であり、これにより、操作端２１ａを操作する操作者Ｐに対し、スレーブアーム１１が固い対象物に当たった感覚を表現することができる。

（実施の形態３）
上記実施の形態１において、規制部３２は、力緩和部４１及び力加減部４２を含む形態を例示したがこれに限られるものではない。これに代えて、規制部３２は、力緩和部４１及び力加減部４２のうち力緩和部４１を含むよう構成してもよい。

この場合、指令生成部３３は、緩和速度ベクトルｖ_ｄｍに基づいてスレーブ動作指令ｘ_ｓ及びマスタ動作指令ｘ_ｍを生成する。

（実施の形態４）
上記実施の形態１において、規制部３２は、力緩和部４１及び力加減部４２を含む形態を例示したがこれに限られるものではない。これに代えて、規制部３２は、力緩和部４１及び力加減部４２のうち力加減部４２を含むよう構成してもよい。

この場合、力加減部４２は、スレーブ側力検出部１２が検出した反力ｆ_ｓの押圧方向成分ｆ_ｓｉが許容値ｆ_ｌｉｍを超え、且つ原速度ベクトルｖ_ｄが対象物Ｔを押圧する方向に向かう押圧方向成分ｖ_ｄｉを含むときは、この押圧方向成分ｖ_ｄｉが０になるように原速度ベクトルｖ_ｄを修正して加減速度ベクトルｖ_ｄａを算出する。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

Ｔ 対象物
Ｗ ワーク
１ スレーブユニット
２ マスタユニット
３ システム制御部
１１ スレーブアーム
１１ａ 作業端
１２ スレーブ側力検出部
１３ スレーブアーム駆動部
１４ スレーブ側制御部
２１ マスタアーム
２１ａ 操作端
２２ マスタ側力検出部
２３ マスタアーム駆動部
２４ マスタ側制御部
３１ 換算部
３２ 規制部
３３ 指令生成部
１００ ロボットシステム

Claims (13)

1. 作業端を有するスレーブアームと、前記作業端に保持したワークが該ワークを組み付ける対象物に接触したときに前記作業端又は前記作業端に保持した前記ワークに作用する反力の方向及び大きさを検出するスレーブ側力検出部と、前記スレーブアームを駆動するスレーブアーム駆動部と、前記作業端の移動方向を規定するスレーブ動作指令に基づき前記スレーブアーム駆動部を制御するスレーブ側制御部と、を含むスレーブユニットと、
   操作端を有するマスタアームと、前記操作端に操作者が加えた操作力の方向及び大きさを検出するマスタ側力検出部と、前記マスタアームを駆動するマスタアーム駆動部と、前記操作端の移動方向を規定するマスタ動作指令に基づき前記マスタアーム駆動部を制御するマスタ側制御部と、を含むマスタユニットと、
   前記操作力及び前記反力に基づいて、前記スレーブ動作指令と、該スレーブ動作指令の前記作業端の移動方向に対応する移動方向に前記操作端を移動させる前記マスタ動作指令とを生成するシステム制御部と、を有するロボットシステムであって、
   前記システム制御部は、
   前記操作力及び前記反力に基づいて前記作業端の移動方向を算出する換算部と、
   前記反力の押圧方向成分が予め設定された許容値を超えるときは前記対象物を押圧する方向への前記作業端の移動を規制するように前記作業端の前記移動方向を修正する規制部と、
   修正した前記作業端の前記移動方向に基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する指令生成部と、を有する、ロボットシステム。
2. 前記スレーブ動作指令は位置指令である、請求項１に記載のロボットシステム。
3. 前記規制部は力加減部を有し、
   前記力加減部は、前記反力の押圧方向成分が前記許容値を超えるときは前記作業端が前記対象物を押圧する方向における前記作業端の位置を一時的に維持するように前記作業端の前記移動方向を修正する、請求項１又は２に記載のロボットシステム。
4. 前記換算部は、前記操作力及び前記反力の差分に基づいて前記作業端の移動方向及び移動速度を規定する原速度ベクトルを算出し、
   前記規制部は力加減部を有し、
   前記力加減部は、前記反力の前記対象物を押圧する方向に向かう押圧方向成分が前記許容値を超え且つ前記原速度ベクトルが前記対象物を押圧する方向に向かう押圧方向成分を含むときは該押圧方向成分が０になるように前記原速度ベクトルを修正して加減速度ベクトルを算出し、
   前記指令生成部は、前記加減速度ベクトルに基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する、請求項１又は２に記載のロボットシステム。
5. 前記力加減部は、前記加減速度ベクトルの前記対象物を押圧する方向成分である押圧方向成分と直交する方向の成分が前記原速度ベクトルの前記対象物を押圧する方向と直交する方向の成分と同じ値となるように前記原速度ベクトルを修正して前記加減速度ベクトルを算出する、請求項４に記載のロボットシステム。
6. 前記規制部は、力緩和部を有し、
   前記力緩和部は、前記反力の押圧方向成分が前記許容値を超えるときは前記作業端が前記対象物を押圧する方向と反対方向に前記作業端を移動させるように前記作業端の移動方向を修正する、請求項１又は２に記載のロボットシステム。
7. 前記換算部は、前記操作力及び前記反力の差分に基づいて前記作業端の移動方向及び移動速度を規定する原速度ベクトルを算出し、
   前記力緩和部は、前記反力の押圧方向成分が前記許容値を超えるときは前記許容値からの乖離値に基づいて前記原速度ベクトルの前記対象物を押圧する方向成分である押圧方向成分が小さくなるように前記原速度ベクトルを修正して緩和速度ベクトルを算出し、
   前記指令生成部は、前記緩和速度ベクトルに基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する、請求項６に記載のロボットシステム。
8. 前記力緩和部は、前記許容値から前記反力の押圧方向成分を減算した値を積算した値に所定のゲインを乗算した調整積算値を前記原速度ベクトルの前記押圧方向成分に加算して前記原速度ベクトルを修正して前記緩和速度ベクトルを算出する、請求項７に記載のロボットシステム。
9. 前記力緩和部は、前記反力の押圧方向成分が前記許容値を超えた後で前記反力の押圧方向成分が許容値内に収まると、前記調整積算値を０にする、請求項８に記載のロボットシステム。
10. 前記規制部は力加減部を有し、
    前記力加減部は、前記反力の押圧方向成分が前記許容値を超えるときは前記作業端が前記対象物を押圧する方向における前記作業端の位置を一時的に維持するように前記作業端の前記移動方向を修正する、請求項６乃至９の何れかに記載のロボットシステム。
11. 前記規制部は力加減部を更に有し、
    前記力加減部は、前記反力の前記対象物を押圧する方向に向かう押圧方向成分が前記許容値を超え且つ前記緩和速度ベクトルが前記押圧方向成分を含むときは該押圧方向成分が０になるように前記緩和速度ベクトルを修正して加減速度ベクトルを算出し、
    前記指令生成部は、前記緩和速度ベクトルに基づいて算出した前記加減速度ベクトルに基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する、請求項７乃至９の何れかに記載のロボットシステム。
12. 前記力緩和部は、前記緩和速度ベクトルの前記対象物を押圧する方向成分である押圧方向成分と直交する方向の成分が前記原速度ベクトルの前記対象物を押圧する方向と直交する方向の成分と同じ値になるように前記原速度ベクトルを修正して前記緩和速度ベクトルを算出し、
    前記力加減部は、前記加減速度ベクトルの前記対象物を押圧する方向と直交する方向の成分が前記緩和速度ベクトルの前記対象物を押圧する方向と直交する方向の成分と同じ値となるように前記緩和速度ベクトルを修正して前記加減速度ベクトルを算出する、請求項１１に記載のロボットシステム。
13. 作業端を有するスレーブアームと、前記作業端に保持したワークが該ワークを組み付ける対象物に接触したときに前記作業端又は前記作業端に保持した前記ワークに作用する反力の方向及び大きさを検出するスレーブ側力検出部と、前記スレーブアームを駆動するスレーブアーム駆動部と、前記作業端の移動方向を規定するスレーブ動作指令に基づき前記スレーブアーム駆動部を制御するスレーブ側制御部と、を含むスレーブユニットと、
    操作端を有するマスタアームと、前記操作端に操作者が加えた操作力の方向及び大きさを検出するマスタ側力検出部と、前記マスタアームを駆動するマスタアーム駆動部と、前記操作端の移動方向を規定するマスタ動作指令に基づき前記マスタアーム駆動部を制御するマスタ側制御部と、を含むマスタユニットと、
    前記操作力及び前記反力に基づいて、前記スレーブ動作指令と、該スレーブ動作指令の前記作業端の移動方向に対応する移動方向に前記操作端を移動させる前記マスタ動作指令とを生成するシステム制御部と、を有するロボットシステムの制御方法であって、
    前記システム制御部が前記操作力及び前記反力に基づいて前記作業端の移動方向を算出する換算ステップと、
    前記システム制御部が前記反力の押圧方向成分が予め設定された許容値を超えるときは前記対象物を押圧する方向への前記作業端の移動を規制するように前記作業端の移動方向を修正する規制ステップと、
    前記システム制御部が修正した前記作業端の移動方向に基づいて前記スレーブ動作指令及び前記マスタ動作指令を生成する指令生成ステップと、を有する、ロボットシステムの制御方法。

Images