WO2021117701A1

WO2021117701A1 - マスタスレーブシステム及び制御方法

Info

Publication number: WO2021117701A1
Application number: PCT/JP2020/045599
Authority: WO
Inventors: 掃部　雅幸; 大樹高橋; 開清水; 裕規高山; 藤森　潤; 博貴木下
Original assignee: 川崎重工業株式会社
Priority date: 2019-12-13
Filing date: 2020-12-08
Publication date: 2021-06-17
Also published as: JPWO2021117701A1; US20230038804A1; JP7288521B2; CN114829076A

Abstract

マスタスレーブシステム（１）は、操作端（４０１）と、前記操作端に力が加えられることで入力される操作情報を検出し出力する操作検出部（４０５）と、前記操作端に力を与える力付与部（４０４）とを備えるマスタ装置（４０）と、対象物に作用を加える作用部（１１）と前記作用部を移動させる動作部（１２）とを備えるスレーブ装置（１０）と、制御装置（２０）とを備え、前記制御装置は、前記作用部の所定の移動を規制する規制条件と前記操作情報とに従って、前記動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力する第１指令部（２０ｄ）と、前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を前記力付与部に与えさせるための指令を出力する第２指令部（２０ｅ）とを含む。

Description

マスタスレーブシステム及び制御方法

関連出願への相互参照

　本件出願は、２０１９年１２月１３日に日本特許庁に出願された特願２０１９－２２５６９６号の優先権を主張するものであり、その全体を参照することにより本件出願の一部となすものとして引用する。

　本開示は、マスタスレーブシステム及び制御方法に関する。

　従来、ロボットは、人の行為を代替するために用いられてきた。例えば、特許文献１は、スレーブアームの先端動作を予め制限した作業範囲内に拘束する多関節型マスタ・スレーブマニピュレータを開示している。上記マスタ・スレーブマニピュレータの運転制御装置は、マスタアームの位置及び姿勢の変化量に、不必要な方向の変化量を０とする拘束条件を付加する拘束制御を実施することで、スレーブアームの運動を拘束する。具体的には、運転制御装置は、マスタアームの位置及び姿勢の変化量ベクトルを拘束条件のベクトルに投影して得られるベクトルを、スレーブアームの位置及び姿勢の変化量ベクトルに決定する。

特開平６－２６２５４８号公報

　特許文献１では、スレーブアームは、マスタアームの位置及び姿勢の変化量に拘束条件を付加して得られるスレーブアームの位置及び姿勢の変化量に対応する動作を行う。このため、マスタアームの位置及び姿勢等の状態とスレーブアームの位置及び姿勢等の状態とが同様にならない場合がある。この場合、操作者がマスタアームの状態から感じるスレーブアームの状態と実際のスレーブアームの状態との間に差異が生じ、操作者は自身の意図通りにスレーブアームを操作できないおそれがある。

　本開示は、マスタ装置の状態とスレーブ装置の状態とを同様の状態に維持しつつスレーブ装置の動作を規制することができるマスタスレーブシステム及び制御方法を提供することを目的とする。

　上記目的を達成するために、本開示の一態様に係るマスタスレーブシステムは、操作端と、前記操作端に力が加えられることで入力される情報である操作情報を検出し出力する操作検出部と、前記操作端に力を与える力付与部とを備えるマスタ装置と、対象物に作用を加える作用部と前記作用部を移動させる動作部とを備えるスレーブ装置と、制御装置とを備え、前記制御装置は、前記作用部の所定の移動を規制する規制条件と前記操作情報とに従って、前記動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力する第１指令部と、前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を前記力付与部に与えさせるための指令を出力する第２指令部とを含む。

　本開示によれば、マスタ装置の状態とスレーブ装置の状態とを同様の状態に維持しつつスレーブ装置の動作を規制することが可能になる。

図１は、実施の形態に係るロボットシステムの一例を示す概略図である。図２は、実施の形態に係るエンドエフェクタの構成の一例を示す側面図である。図３は、図２のエンドエフェクタの力センサの検出軸の一例を示す側面図である。図４は、実施の形態に係る操作入力装置の操作装置の構成の一例を示す斜視図である。図５は、図４の操作装置の力センサの検出軸の一例を示す斜視図である。図６は、実施の形態に係るロボットシステムの機能的な構成の一例を示すブロック図である。図７は、実施の形態に係る制御装置のハードウェア構成の一例を示すブロック図である。図８は、実施の形態に係るロボットシステムの規制モードでの動作の一例を示すフローチャートである。図９は、実施の形態に係るロボットシステムのエンドエフェクタを基本姿勢に移行させる動作の一例を示すフローチャートである。図１０は、エンドエフェクタの基本姿勢への移行中における移行の支障の発生例を示す側面図である。図１１は、エンドエフェクタの基本姿勢への移行中における回避動作の例を示す側面図である。

　まず、本開示の態様例を説明する。本開示の一態様に係るマスタスレーブシステムは、操作端と、前記操作端に力が加えられることで入力される情報である操作情報を検出し出力する操作検出部と、前記操作端に力を与える力付与部とを備えるマスタ装置と、対象物に作用を加える作用部と前記作用部を移動させる動作部とを備えるスレーブ装置と、制御装置とを備え、前記制御装置は、前記作用部の所定の移動を規制する規制条件と前記操作情報とに従って、前記動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力する第１指令部と、前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を前記力付与部に与えさせるための指令を出力する第２指令部とを含む。

　上記態様によると、制御装置は、規制条件に従って作用部の所定の移動を規制しつつ操作情報に従って作用部を動作させる。さらに、制御装置は、操作端に力を与えることで、作用部の所定の移動を指令する操作端への入力を規制する。よって、マスタ装置の状態とスレーブ装置の状態とが同様の状態に維持されつつ、スレーブ装置が規制条件が反映された動作を行う。例えば、第１及び第２指令部は、指令として、動作指令及び制御指令等を出力してもよい。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記スレーブ装置は、前記動作部によって前記作用部の位置及び姿勢を移動させ、前記操作端は、位置及び姿勢を変えるように移動可能であり、前記規制条件は、前記所定の移動として、前記作用部の姿勢の移動を規制する条件であってもよい。上記態様によると、制御装置は、作用部の姿勢の移動を規制しつつ作用部を動作させる。さらに、制御装置は、規制される作用部の姿勢の移動に対応する操作端の姿勢の移動を規制する。よって、マスタ装置の姿勢とスレーブ装置の姿勢とが同様の姿勢に維持されつつ、スレーブ装置が規制条件が反映された動作を行う。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記操作検出部は、前記操作端に加えられる力の方向及び大きさを前記操作情報として検出してもよい。上記態様によると、操作端に加えられる力の方向及び大きさの指令は、作用部の移動方向、移動速度及び当該移動方向での作用力の指令に変換され得る。よって、操作者は、操作端に力を加えることで作用部の動作を制御することができる。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記操作検出部は、前記操作端に加えられる３軸方向の力の大きさ及び３軸周りの力のモーメントを前記操作情報として検出してもよい。上記態様によると、操作端に加えられる３軸方向の力の大きさの指令は、当該３軸方向の作用部の移動速度及び作用力の指令に変換され得る。操作端に加えられる３軸周りの力のモーメントの指令は、当該３軸周りの作用部の回転速度及び回転力の指令に変換され得る。よって、操作者は、操作端に力を加えることで作用部の位置及び姿勢を制御することができる。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記規制条件は、前記所定の移動として、前記操作情報の前記３軸周りの力のモーメントに対応する前記作用部の姿勢の移動を規制する条件であってもよい。上記態様によると、制御装置は、規制条件に従って操作情報に含まれる３軸周りの力のモーメントを規制し、規制後の操作情報を用いて指令を出力することで、作用部の姿勢の移動の規制が反映された動作をスレーブ装置にさせることができる。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記作用部は、研削装置を含み、前記動作部は、ロボットアームを含んでもよい。上記態様によると、研削作業では、研削装置の研削砥石の姿勢は研削対象物に対して所定の姿勢であることが好ましい。制御装置は、ロボットアームを用いて研削砥石の姿勢を自在に制御し、所定の姿勢にすることができる。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記制御装置は、前記作用部の姿勢を所定の姿勢に移行させるための移行指令を受け付ける受付部をさらに含み、前記受付部が前記移行指令を受け付けると、前記第１指令部は、前記作用部の姿勢を前記所定の姿勢にさせるための指令を出力してもよい。上記態様によると、制御装置は、移行指令に従って、作用部の姿勢を所定の姿勢に移行させる。例えば、所定の姿勢は作用部の好ましい姿勢又は基本的な姿勢等であってもよい。そして、上記のような作用部の姿勢の移行は、操作端を用いた操作を必要とせず、簡易に実行可能である。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記力付与部は、前記力付与部が与える力によって前記操作端を移動させることができるように前記操作端と接続され、前記受付部が前記移行指令を受け付けると、前記第２指令部は、前記操作端の姿勢を前記所定の姿勢に対応する姿勢にさせる前記操作端への力を前記力付与部に与えさせるための指令をさらに出力してもよい。上記態様によると、制御装置は、移行指令に従って、作用部の姿勢を所定の姿勢に移行させ、且つ、操作端の姿勢を作用部の所定の姿勢に対応する姿勢に移行させる。よって、作用部及び操作端の両方の姿勢を対応させつつ目的とする姿勢にすることができる。

　本開示の一態様に係るマスタスレーブシステムにおいて、前記スレーブ装置は、前記作用部が前記対象物から受ける力を検出する力検出部をさらに備え、前記第１指令部は、前記作用部の前記所定の姿勢への移行中、前記力検出部が力を検出すると、前記作用部の位置を移動させ、且つ前記作用部の姿勢を前記所定の姿勢にさせるための指令を出力してもよい。上記態様によると、制御装置は、作用部の姿勢の移行中、接触及び衝突等により力検出部で力が検出されると、例えば、接触及び衝突等を回避するように作用部の位置を移動させ、且つ、作用部の姿勢を所定に姿勢にすることができる。よって、確実な姿勢の移行が可能になる。なお、力の検出後において、制御装置は、作用部の位置の移動と作用部の姿勢の移行とを並行して行ってもよく、作用部の姿勢の移行を停止し、作用部の位置の移動後に作用部の姿勢の移行を行ってもよい。後者の場合、制御装置は、作用部の位置の移動完了後に作用部の姿勢の移行を開始してもよく、作用部の位置の移動中に作用部の姿勢の移行を開始し、位置の移動と作用部の姿勢の移行とを並行して行ってもよい。

　本開示の一態様に係る制御方法は、マスタ装置によって操作されるスレーブ装置を制御する制御方法であって、前記マスタ装置の操作端に力が加えられることで入力される情報である操作情報と、前記スレーブ装置における対象物に作用を加える作用部の所定の移動を規制する規制条件とに従って、前記スレーブ装置の動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力することと、前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を、前記マスタ装置に生成させるための指令を出力することとを含む。上記態様によると、本開示の一態様に係るマスタスレーブシステムと同様の効果が得られる。

　（実施の形態）
　以下において、本開示の実施の形態を、図面を参照しつつ説明する。なお、以下で説明する実施の形態は、いずれも包括的又は具体的な例を示すものである。また、以下の実施の形態における構成要素のうち、最上位概念を示す独立請求項に記載されていない構成要素については、任意の構成要素として説明される。また、添付の図面における各図は、模式的な図であり、必ずしも厳密に図示されたものでない。さらに、各図において、実質的に同一の構成要素に対しては同一の符号を付しており、重複する説明は省略又は簡略化される場合がある。また、本明細書及び請求の範囲では、「装置」とは、１つの装置を意味し得るだけでなく、複数の装置からなるシステムも意味し得る。

　［ロボットシステムの構成］
　実施の形態に係るロボットシステム１の構成を説明する。図１は、実施の形態に係るロボットシステム１の一例を示す概略図である。図１に示すように、実施の形態に係るロボットシステム１は、ロボット１０と、制御装置２０と、撮像装置３０と、操作入力装置４０と、提示装置５０とを備える。ロボットシステム１は、マスタスレーブシステムを構成し、ロボット１０はスレーブ装置を構成し、操作入力装置４０はマスタ装置を構成する。制御装置２０は、ロボットシステム１の全体の動作を制御する。制御装置２０は、ロボット１０と操作入力装置４０との間でバイラテラル制御を行う。

　ロボット１０は、本実施の形態では産業用ロボットである。ロボット１０は、基台１３上に固定して配置されるが、搬送装置等の移動可能な装置に配置され、移動できるように構成されてもよい。ロボット１０は、処理の対象物に対して作用を加える作用部としてのエンドエフェクタ１１と、当該作用を実行するようにエンドエフェクタ１１を動かす動作部としてのロボットアーム１２とを備える。本実施の形態では、エンドエフェクタ１１は、対象物に対して研削の作用を加えるように構成されるとして、以下の説明を行う。このようなエンドエフェクタ１１は、対象物に対して研削を施す研削装置１１ａを備え、ロボットアーム１２の先端に取り付けられる。なお、エンドエフェクタ１１の作用は、研削に限定されず、いかなる作用であってもよい。例えば、エンドエフェクタ１１の作用は、作用対象に対して好ましいエンドエフェクタ１１の姿勢が存在する作用であってもよい。

　本明細書及び請求の範囲において、「研削」は、対象物の不要な部分を除去することで所要の寸法及び形状等にする加工である切削と、対象物の表面を削り取ることで所要の寸法、形状及び表面粗さ等にする加工である研削と、対象物の表面を滑らかにする加工である研磨とを含み得る。

　研削装置１１ａの例は、グラインダ、オービタルサンダ、ランダムオービットサンダ、デルタサンダ及びベルトサンダ等の電力又は空気圧を動力源とする研削装置であるが、これに限定されない。グラインダは、円盤状の研削砥石を回転させるタイプ、円錐状又は柱状の研削砥石を回転させるタイプ等のグラインダであってもよい。

　本実施の形態では、「研削」は、金属製の対象物Ｗの研削対象領域ＷＡ内の不要な部分を削り取り、研削対象領域ＷＡの表面を滑らかにする加工であるとし、研削装置１１ａは、円盤状の研削砥石１１ｂを備える電動ディスクグラインダであるとして以下の説明を行う。研削対象領域ＷＡ内の不要な部分の例は、対象物Ｗの溶接ビードのような溶接痕等である。研削装置１１ａは、回転する研削砥石１１ｂが研削対象領域ＷＡ内の溶接痕等に押し当てられることで、当該溶接痕及びその周辺を研削する。図１に示される対象物Ｗは、大型タンクの壁である。

　ロボットアーム１２は、その先端の研削装置１１ａの位置及び／又は姿勢を変更することができる構成を有すれば、特に限定されないが、本実施の形態では、垂直多関節型ロボットアームである。なお、ロボットアーム１２は、例えば、水平多関節型、極座標型、円筒座標型、直角座標型、又はその他の型式のロボットアームとして構成されてもよい。

　ロボットアーム１２は基台１３に固定される。ロボットアーム１２は、その基部から先端に向かって順に配置されたリンク１２ａ、１２ｂ、１２ｃ、１２ｄ及び１２ｆと、リンク１２ａ、１２ｂ、１２ｃ、１２ｄ及び１２ｆを順次回転可能に接続する関節ＪＴ１、ＪＴ２、ＪＴ３、ＪＴ４、ＪＴ５及びＪＴ６と、関節ＪＴ１、ＪＴ２、ＪＴ３、ＪＴ４、ＪＴ５及びＪＴ６それぞれを回転駆動するアーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６とを備える。リンク１２ａは関節ＪＴ１を介して基台１３に取り付けられる。リンク１２ｆの先端部はメカニカルインタフェースを構成し、エンドエフェクタ１１と接続される。アーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６の動作は制御装置２０によって制御される。アーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６はそれぞれ、電力を動力源とし、これらを駆動する電気モータとしてサーボモータＭａ（図６参照）を有するが、これに限定されない。なお、ロボットアーム１２の関節の数量は、６つに限定されず、７つ以上又は５つ以下であってもよい。

　図２は、実施の形態に係るエンドエフェクタ１１の構成の一例を示す側面図である。図３は、図２のエンドエフェクタ１１の力センサ１１ｅの検出軸の一例を示す側面図である。図２に示すように、エンドエフェクタ１１は、研削装置１１ａと、研削砥石１１ｂと、取付具１１ｃと、力センサ１１ｅとを備える。取付具１１ｃは、研削装置１１ａを支持し且つリンク１２ｆと接続されるように構成され、研削装置１１ａをリンク１２ｆに取り付ける。力センサ１１ｅは、取付具１１ｃとリンク１２ｆとの間に配置され、取付具１１ｃからリンク１２ｆに作用する力である反力を検出し制御装置２０に出力する。反力は、研削作業時に研削装置１１ａが対象物Ｗから受ける力である。力センサ１１ｅは力検出部の一例である。

　図３に示すように、本実施の形態では、力センサ１１ｅは、互いに直交するＸｅ軸、Ｙｅ軸及びＺｅ軸それぞれの軸方向の力Ｆｘｅ、Ｆｙｅ及びＦｚｅと、Ｘｅ軸、Ｙｅ軸及びＺｅ軸それぞれの軸周りの回転力であるモーメントＭｘｅ、Ｍｙｅ及びＭｚｅとの６軸の力を検出する。Ｚｅ軸は、リンク１２ｆの捻れ回転軸Ｓ６と同一の軸であり、Ｚｅ軸正方向は、リンク１２ｆからエンドエフェクタ１１に向かう方向である。リンク１２ｆのメカニカルインタフェースを構成するインタフェース面１２ｆａとＺｅ軸との交点が原点Ｏｅである。Ｘｅ軸及びＹｅ軸は、インタフェース面１２ｆａ上に沿って延びる軸である。なお、力センサ１１ｅは上記６軸の力を検出するセンサに限定されず、例えば、上記６軸の力のうちのいくつかを検出するセンサであってもよい。

　図１及び図２に示すように、撮像装置３０は、ロボットアーム１２のリンク１２ｅに配置される。撮像装置３０は、カメラ（図示略）を備え、被写体を照明するためのＬＥＤ（light emitting diode）及びストロボ等の光源をさらに備えてもよい。撮像装置３０のカメラは、捻れ回転軸Ｓ６の軸方向に沿い且つエンドエフェクタ１１に向かう方向に指向され、研削対象領域ＷＡ及びその周辺を撮像することができる。なお、撮像装置３０の位置は、研削対象領域ＷＡを撮像できる位置であればよく、リンク１２ｅ以外のロボットアーム１２上の位置、又は、ロボットアーム１２の外部の位置であってもよい。撮像装置３０は、制御装置２０の指令に従って、カメラに撮像動作をさせる。撮像装置３０は、カメラによって撮像された画像の信号等を制御装置２０に送る。なお、以下の説明において、カメラが撮像することを、撮像装置３０が撮像すると表現する場合がある。

　撮像装置３０のカメラは、被写体までの距離等の撮像装置３０に対する被写体の３次元空間内の位置である３次元位置等を検出するための画像を撮像する。例えば、当該カメラは、デジタル画像を撮像するカメラであり、ステレオカメラ、単眼カメラ、ＴＯＦカメラ（トフカメラ：Time-of-Flight-Camera）、縞投影等のパターン光投影カメラ、又は光切断法を用いたカメラ等の構成を有してもよい。本実施の形態では、撮像装置３０のカメラはステレオカメラである。

　図４は、実施の形態に係る操作入力装置４０の操作装置４００の構成の一例を示す斜視図である。図５は、図４の操作装置４００の力センサ４０５の検出軸の一例を示す斜視図である。図６は、実施の形態に係るロボットシステム１の機能的な構成の一例を示すブロック図である。図１、図４及び図６に示すように、操作入力装置４０は、ロボット１０から離れて配置され、ロボットシステム１を管理及び／又は操作するユーザＰによる指令、データ及び情報等の入力を受け付け、当該指令、データ及び情報等を制御装置２０に出力する。操作入力装置４０は制御装置２０と有線通信又は無線通信を介して接続される。有線通信及び無線通信の形式はいかなる形式であってもよい。

　操作入力装置４０は、操作装置４００と、入力装置４１０と、操作制御装置４２０とを備える。操作制御装置４２０は、操作入力装置４０全体の動作を制御し、制御装置２０と情報、指令及びデータ等の送受信を行う。操作装置４００は、ロボット１０を手動運転で操作するための入力を受け付け、入力された情報である操作情報を操作制御装置４２０に出力する。また、操作装置４００は、操作制御装置４２０の制御によって、操作装置４００を操作しているユーザＰに、操作力に対する反力等の力を与える。

　入力装置４１０は、情報、指令及びデータ等の入力を受け付け、操作制御装置４２０に出力する。例えば、入力装置４１０は、レバー、ボタン、キー、タッチパネル、ジョイスティック、モーションキャプチャ等の既知の入力装置を備えてもよい。入力装置４１０は、撮像装置３０の動作、エンドエフェクタ１１を所定の姿勢にする指令などの様々な指令、制御モードの切り替え、研削の対象物の情報及び研削対象領域の情報等の入力を受け付ける。エンドエフェクタ１１の所定の姿勢は、研削対象領域の表面に対するエンドエフェクタ１１の所定の姿勢であってもよい。研削対象領域の表面に対するエンドエフェクタ１１の姿勢は、研削対象領域の表面に対する研削砥石１１ｂの姿勢である。研削対象領域の情報は、研削対象領域の数量、位置、姿勢、形状及び寸法等の情報を含んでもよい。

　図４に示すように、操作装置４００は、把持部４０１と、支持部４０２と、アーム４０３と、モータ４０４と、力センサ４０５とを備える。把持部４０１は、ユーザＰによって把持可能であるように構成される。本実施の形態では、把持部４０１は、研削装置１１ａであるグラインダと同様の形状を有するが、これに限定されない。把持部４０１は、ユーザＰが握ることができる２つのハンドル部４０１ａ及び４０１ｂを含む。ユーザＰは、研削装置１１ａを把持して実際に研削するようにハンドル部４０１ａ及び４０１ｂを握った状態で把持部４０１を移動させることによって、ロボット１０を操作し研削動作させる。また、把持部４０１は、研削装置１１ａの操作等のための押しボタン等の入力部４０１ｃを含む。把持部４０１は操作端の一例である。

　支持部４０２は、把持部４０１を支持する。支持部４０２は、６つのアーム４０３によって移動可能に支持されている。６つのアーム４０３は、３つのペアで構成される。３つのペアのアーム４０３は、支持部４０２から３方向に放射状に延びる。各アーム４０３は、関節４０３ａを有し、関節４０３ａを中心に屈曲可能である。各アーム４０３の一端は、ボールジョイント等の自在継手を介して、直交する３軸周りに回動可能に支持部４０２と接続される。各アーム４０３の他端は、支持部４０２の下方の支持台４０６上に配置されたモータ４０４の回転軸と、減速機等（図示略）を介して接続される。各アーム４０３の関節４０３ａは、当該アーム４０３を構成する２つの柱状部材を、ボールジョイント等の自在継手を介して、直交する３軸周りに回動可能に接続する。

　６つのモータ４０４が支持台４０６上に配置されている。６つのモータ４０４は、３つのペアで構成される。各ペアのモータ４０４は、それぞれの回転軸が同軸となるように配置され、１つのペアのアーム４０３と接続される。３つのペアのモータ４０４は、それぞれの回転軸が三角形の各辺を構成するように配置されている。各モータ４０４は、サーボモータ等で構成される。

　上述のような把持部４０１は、３次元空間内で様々な位置及び姿勢をとることができる。そして、把持部４０１の位置及び姿勢に対応して、各アーム４０３が動作し各モータ４０４を回転させる。把持部４０１の位置及び姿勢に対応する６つのモータ４０４の回転量つまり回転角は、一義的に定まる。

　力センサ４０５は、把持部４０１と支持部４０２との間に配置され、この間に作用する力を検出する。図５に示すように、本実施の形態では、力センサ４０５は、互いに直交するＸｃ軸、Ｙｃ軸及びＺｃ軸それぞれの軸方向の力Ｆｘｃ、Ｆｙｃ及びＦｚｃと、Ｘｃ軸、Ｙｃ軸及びＺｃ軸それぞれの軸周りの回転力であるモーメントＭｘｃ、Ｍｙｃ及びＭｚｃとの６軸の力を検出する。

　Ｚｃ軸は、把持部４０１における力センサ４０５との円形接触面４０１ｄの中心を通り且つ接触面４０１ｄと垂直な軸である。Ｚｃ軸正方向は、力センサ４０５から把持部４０１に向かう方向である。接触面４０１ｄとＺｃ軸との交点が原点Ｏｃである。Ｘｃ軸及びＹｃ軸は、接触面４０１ｄ上に沿って延びる軸である。また、これに限定されないが、本実施の形態では、Ｘｃ軸、Ｙｃ軸及びＺｃ軸はそれぞれ、エンドエフェクタ１１の力センサ１１ｅのＸｅ軸、Ｙｅ軸及びＺｅ軸と対応付けられている。例えば、Ｘｃ軸正方向はＸｅ軸負方向に対応し、Ｙｃ軸正方向はＹｅ軸正方向に対応し、且つＺｃ軸正方向はＺｅ軸負方向に対応してもよく、又は、Ｘｃ軸正方向はＸｅ軸正方向に対応し、Ｙｃ軸正方向はＹｅ軸正方向に対応し、且つＺｃ軸正方向はＺｅ軸正方向に対応してもよい。なお、力センサ４０５は上記６軸の力を検出するセンサに限定されず、例えば、上記６軸の力のうちのいくつかを検出するセンサであってもよい。力センサ４０５は操作検出部の一例であり、モータ４０４は力付与部の一例である。

　本実施の形態では、制御装置２０は、操作装置４００とロボット１０との間で位置、姿勢及び力の状態を対応させるようにバイラテラル方式での力覚制御を行う。これに限定されないが、各モータ４０４は、サーボモータの回転子の回転量を検出するエンコーダ等の回転センサ（図示略）と、サーボモータの駆動電流を検出する電流センサ（図示略）とを備える。操作制御装置４２０は、力センサ４０５の６軸の力の検出信号（以下、「力信号」とも呼ぶ）を含む操作情報を、位置、姿勢、位置及び姿勢の移動速度並びに力等を指令する操作指令として制御装置２０に出力する。制御装置２０は、各モータ４０４の回転センサ及び電流センサの検出信号（以下、「回転信号」及び「電流信号」とも呼ぶ）を、把持部４０１へ付与する力をモータ４０４に発生させるためのフィードバック情報として用いる。なお、制御装置２０は、サーボモータの駆動回路が当該サーボモータに供給する電流の指令値をフィードバック情報として用いてもよい。操作指令は、操作情報の一例である。

　制御装置２０は、操作指令を用いて後述する動作指令等を生成する。動作指令は、操作指令に従ったエンドエフェクタ１１の３次元の位置、姿勢、位置及び姿勢の移動速度並びに作用力等の指令を含む。さらに、制御装置２０は、エンドエフェクタ１１の力センサ１１ｅの検出信号が示す６軸の力データと操作装置４００のフィードバック情報とに基づいて、各モータ４０４の出力トルクを制御する。つまり、制御装置２０は６つのモータ４０４をサーボ制御する。制御装置２０は、把持部４０１に付与すべき力を発生させるように各モータ４０４の出力トルクを制御する。例えば、上記力は、ユーザＰによる把持部４０１の操作力に対する力センサ１１ｅの力データに対応した反力、及び、操作力に対して把持部４０１の位置及び／又は姿勢の移動を規制する力等である。

　操作装置４００の構成は、図４の構成に限定されず、例えば、ユーザＰによって操作される部位の移動を制限できる構成であればよい。例えば、操作装置４００の構成は、操作される部位に力を与えることで当該部位の移動を制限する構成であってもよく、当該部位の移動を機械的に制限する構成であってもよい。例えば、操作装置４００は、ロボットアーム１２と類似するマスタロボットで構成され、ロボット１０がスレーブロボットとして制御されるように構成されてもよい。又は、操作装置４００は、ジョイスティック等であってもよい。

　また、操作装置４００の構成は、ユーザＰによって把持部４０１に力を加える操作が行われると、当該力に対応する操作情報を検出する構成であるが、これに限定されない。例えば、操作装置４００において、力が加えられることによって移動する把持部４０１の位置及び姿勢の移動量が操作情報として検出されてもよい。上記移動量は各モータ４０４の回転量から検出されてもよい。さらに、操作情報として、把持部４０１に加えられる力が、各モータ４０４の負荷から検出されてもよい。

　提示装置５０は、制御装置２０から受け取るロボットシステム１を動作させるための画像及び音声等を、ロボットシステム１のユーザＰに提示する。提示装置５０の例は、液晶ディスプレイ（Liquid Crystal Display）及び有機又は無機ＥＬディスプレイ（Electro-Luminescence Display）等であるが、これらに限定されない。提示装置５０は、音声を発するスピーカを備えてもよい。例えば、提示装置５０は、撮像装置３０によって撮像された画像を、操作入力装置４０を操作するユーザＰに提示する。

　［制御装置のハードウェア構成］
　制御装置２０のハードウェア構成を説明する。図７は、実施の形態に係る制御装置２０のハードウェア構成の一例を示すブロック図である。図７に示すように、制御装置２０は、ＣＰＵ（Central Processing Unit）２０１と、ＲＯＭ（Read Only Memory）２０２と、ＲＡＭ（Random Access Memory）２０３と、メモリ２０４と、入出力Ｉ／Ｆ（インタフェース：Interface）２０５及び２０６と、駆動Ｉ／Ｆ２０７及び２０８と、撮像Ｉ／Ｆ２０９とを構成要素として含む。上記構成要素はそれぞれ、バス、有線通信又は無線通信を介して接続されている。なお、上記構成要素の全てが必須ではない。

　例えば、ＣＰＵ２０１はプロセッサであり、制御装置２０の動作の全体を制御する。ＲＯＭ２０２は不揮発性半導体メモリ等で構成され、ＣＰＵ２０１に動作を制御させるためのプログラム及びデータ等を格納する。ＲＡＭ２０３は揮発性半導体メモリ等で構成され、ＣＰＵ２０１で実行するプログラム及び処理途中又は処理済みのデータ等を一時的に格納する。メモリ２０４は、揮発性メモリ及び不揮発性メモリなどの半導体メモリ、ハードディスク（ＨＤＤ：Hard Disc Drive）及びＳＳＤ（Solid State Drive）等の記憶装置で構成され、種々の情報を記憶する。

　例えば、ＣＰＵ２０１が動作するためのプログラムは、ＲＯＭ２０２又はメモリ２０４に予め保持されている。ＣＰＵ２０１は、ＲＯＭ２０２又はメモリ２０４からプログラムをＲＡＭ２０３に読み出して展開する。ＣＰＵ２０１は、ＲＡＭ２０３に展開されたプログラム中のコード化された各命令を実行する。

　制御装置２０の各機能は、ＣＰＵ２０１、ＲＯＭ２０２及びＲＡＭ２０３等からなるコンピュータシステムにより実現されてもよく、電子回路又は集積回路等の専用のハードウェア回路により実現されてもよく、上記コンピュータシステム及びハードウェア回路の組み合わせにより実現されてもよい。

　このような制御装置２０は、例えば、マイクロコントローラ、ＭＰＵ（Micro Processing Unit）、ＬＳＩ（Large Scale Integration：大規模集積回路）、システムＬＳＩ、ＰＬＣ（Programmable Logic Controller）、論理回路等で構成されてもよい。制御装置２０の複数の機能は、個別に１チップ化されることで実現されてもよく、一部又は全てを含むように１チップ化されることで実現されてもよい。また、回路はそれぞれ、汎用的な回路でもよく、専用の回路でもよい。ＬＳＩとして、ＬＳＩ製造後にプログラムすることが可能なＦＰＧＡ（Field Programmable Gate Array）、ＬＳＩ内部の回路セルの接続及び／又は設定を再構成可能なリコンフィギュラブルプロセッサ、又は、特定用途向けに複数の機能の回路が１つにまとめられたＡＳＩＣ（Application Specific Integrated Circuit）等が利用されてもよい。

　撮像Ｉ／Ｆ２０９は、ＣＰＵ２０１の指令に従って、撮像装置３０のカメラの撮像素子（図示略）の駆動を制御する。撮像Ｉ／Ｆ２０９は、撮像装置３０によって撮像された画像の信号をＲＡＭ２０３又はメモリ２０４に取り込む。撮像装置３０の駆動のための回路等が、制御装置２０の内部又は外部における撮像Ｉ／Ｆ２０９と撮像装置３０との間に設けられてもよい。

　第１入出力Ｉ／Ｆ２０５は、操作入力装置４０と接続され、情報、データ及び指令等の信号を入出力する。信号を変換及び増幅等する回路等が、制御装置２０の内部又は外部における第１入出力Ｉ／Ｆ２０５と操作入力装置４０との間に設けられてもよい。

　第２入出力Ｉ／Ｆ２０６は、提示装置５０と接続され、画面データ、音声データ、情報及び指令等の信号を入出力する。信号を変換及び増幅等する回路等が、制御装置２０の内部又は外部における第２入出力Ｉ／Ｆ２０６と提示装置５０との間に設けられてもよい。

　第１駆動Ｉ／Ｆ２０７は、ロボット１０のアーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６と接続され、情報及び指令等の信号を入出力する。アーム駆動回路（図示略）が、制御装置２０の内部又は外部における第１駆動Ｉ／Ｆ２０７とアーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６との間に設けられる。アーム駆動回路は、ＣＰＵ２０１の指令に従って、アーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６のサーボモータに電力を供給し各サーボモータの駆動を制御する。

　第２駆動Ｉ／Ｆ２０８は、エンドエフェクタ１１の研削装置１１ａと接続され、情報及び指令等の信号を入出力する。研削駆動回路（図示略）が、制御装置２０の内部又は外部における第２駆動Ｉ／Ｆ２０８と研削装置１１ａとの間に設けられる。研削駆動回路は、ＣＰＵ２０１の指令に従って、研削装置１１ａに電力を供給し研削装置１１ａの駆動を制御する。

　［ロボットシステムの機能的構成］
　ロボットシステム１の機能的構成を説明する。図６に示すように、操作入力装置４０の操作制御装置４２０は、入力処理部４２１と動作制御部４２２とを機能的構成要素として含む。制御装置２０は、撮像制御部２０ａと、画像処理部２０ｂ及び２０ｃと、動作指令部２０ｄ及び２０ｅと、動作制御部２０ｆと、動作情報処理部２０ｇと、姿勢決定部２０ｈと、回避判定部２０ｉと、回避経路決定部２０ｊと、記憶部２０ｐとを機能的構成要素として含む。上記機能的構成要素の全てが必須ではない。

　操作制御装置４２０の入力処理部４２１及び動作制御部４２２の機能は、ＣＰＵ等のコンピュータシステム、ハードウェア回路、又は、コンピュータシステム及びハードウェア回路の組み合わせによって実現される。制御装置２０の記憶部２０ｐを除く機能的構成要素の機能は、ＣＰＵ２０１等によって実現され、記憶部２０ｐの機能は、メモリ２０４、ＲＯＭ２０２及び／又はＲＡＭ２０３によって実現される。

　操作制御装置４２０の入力処理部４２１は、入力装置４１０、力センサ４０５及びモータ４０４から受け取る情報、データ及び指令等を制御装置２０に出力する。例えば、入力処理部４２１は、入力装置４１０から、撮像装置３０の動作の指令、所定の姿勢への移行の指令、制御モードの切り替えの指令、研削の対象物の情報及び研削対象領域の情報等を受け取る。

　撮像装置３０の動作の指令は、撮像開始、撮像終了及び撮像タイミングの指令等を含む。所定の姿勢への移行の指令は、エンドエフェクタ１１及び研削装置１１ａの姿勢を予め設定された姿勢に移行させる指令である。所定の姿勢の例は、研削動作のための基本姿勢等である。制御モードの切り替えの指令の例は、エンドエフェクタ１１及び操作装置４００の把持部４０１の移動の少なくとも一部を規制する規制モードと、上記規制をしない非規制モードとを切り替える指令等である。

　また、入力処理部４２１は、力センサ４０５の力信号から６軸の力の検出値を検出する。さらに、入力処理部４２１は、６軸の力の検出値、又は、６軸の力の検出値の演算処理値を制御装置２０に出力する。例えば、入力処理部４２１は、６軸の力の検出値を演算処理することで、エンドエフェクタ１１に対する操作指令を生成し制御装置２０に出力する。操作指令は、エンドエフェクタ１１の位置の変化量、変化方向及び変化速度、並びに、姿勢の変化量、変化方向及び変化速度等を指令する操作位置指令と、エンドエフェクタ１１が対象物に加える作用力の大きさ及び方向等を指令する操作力指令とを含む。

　また、入力処理部４２１は、各モータ４０４から回転センサ及び電流センサの検出信号を受け取り、当該検出信号から各モータ４０４の回転量及び電流値を検出し、フィードバック情報として制御装置２０に出力する。

　動作制御部４２２は、各モータ４０４の駆動を制御する。具体的には、動作制御部４２２は、制御装置２０の第２動作指令部２０ｅから、操作駆動指令及び／又は操作規制指令を含む動作指令を受け取る。動作制御部４２２は、上記動作指令に従って、各モータ４０４に回転負荷（「負荷トルク」とも呼ばれる）等のトルクを発生させる。この回転負荷は、ユーザＰが把持部４０１に与える操作力に対抗する力として作用する。動作制御部４２２は、各モータ４０４で検出される回転量及び電流値をフィードバック情報として用いて、各モータ４０４のトルクを制御する。

　操作駆動指令は、エンドエフェクタ１１が研削の対象物から受ける反力の大きさ及び方向に対応する反力を把持部４０１に与えるための指令を含む。上記反力の大きさ及び方向は、力センサ１１ｅで検出される力の大きさ及び方向に対応する。また、操作駆動指令は、把持部４０１の位置及び姿勢をエンドエフェクタ１１の位置及び姿勢に対応させるように把持部４０１を移動させるための力を把持部４０１に与えるための指令を含む。動作制御部４２２は、操作駆動指令に従った大きさ及び方向の力を把持部４０１に与えるように各モータ４０４に駆動させるための制御指令を生成し、当該モータ４０４に出力する。これにより、ユーザＰは、例えば、対象物から反力を受けているように把持部４０１から反力を感じつつ把持部４０１を操作する。また、把持部４０１の位置及び姿勢がエンドエフェクタ１１の位置及び姿勢と対応するように、モータ４０４が把持部４０１を移動させる。

　操作規制指令は、規制モードにおいて把持部４０１の移動の少なくとも一部を規制する規制力を把持部４０１に与えるための指令である。上記規制力は、記憶部２０ｐに記憶される規制条件２０ｐｂに従った規制力であり、把持部４０１の位置の移動及び姿勢の移動の少なくとも一部を制限する力の大きさ及び方向を含む。例えば、上記規制力は、規制条件２０ｐｂで規制されるエンドエフェクタ１１の位置及び姿勢の移動に対応する把持部４０１の位置及び姿勢の移動量を０（零）にするための力の大きさ及び方向を含む。動作制御部４２２は、操作規制指令に従った大きさ及び方向の規制力を把持部４０１に与えるように各モータ４０４に駆動させるための制御指令を生成し、当該モータ４０４に出力する。これにより、ユーザＰは、例えば、上記移動をさせないような規制を把持部４０１に受けているように感じつつ、把持部４０１を操作する。さらに、規制力は、ユーザＰが把持部４０１に上記移動をさせる操作力を与えても把持部４０１に上記移動をさせないようにすることも可能である。

　なお、操作規制指令の規制力の大きさは、予め決められた大きさであってもよく、動作制御部４２２が、力センサ４０５の検出値に対応して上記規制力を調節してもよい。例えば、動作制御部４２２は、ユーザＰが把持部４０１に与える操作力に対応して、規制された移動方向への力センサ４０５の検出値以上の大きさに規制力の大きさを調節してもよい。

　制御装置２０の記憶部２０ｐは、種々の情報を記憶し、記憶している情報の読み出しを可能にする。例えば、記憶部２０ｐは、操作入力装置４０の入力装置４１０を介して入力される情報等を記憶する。また、記憶部２０ｐは、画像処理に用いられる情報として、撮像装置３０のカメラの外部パラメタと内部パラメタと含むカメラパラメタを記憶する。外部パラメタの例は、カメラの位置（３次元位置）及び向き（光軸中心の向き）等を示すパラメタである。内部パラメタの例は、カメラのレンズの歪み、焦点距離、撮像素子の１画素のサイズ及び光軸中心の画素座標等を示すパラメタである。また、記憶部２０ｐは、ロボット１０によって研削処理される対象物と当該対象物の研削対象領域の位置、形状及び寸法等とを関連付けて記憶してもよい。また、記憶部２０ｐは、撮像装置３０によって撮像された画像データ、当該画像データの処理画像データ及び／又はプログラム等を記憶してもよい。また、記憶部２０ｐは、姿勢情報２０ｐａと、規制条件２０ｐｂと、回避情報２０ｐｃとを記憶する。

　規制条件２０ｐｂは、エンドエフェクタ１１の位置及び姿勢の移動の少なくとも一部を規制するための情報である。具体的には、規制条件２０ｐｂは、位置の移動方向及び姿勢の移動方向のうちの少なくとも１つの方向へエンドエフェクタ１１を移動させないように、エンドエフェクタ１１の移動を制限するための情報を含む。本実施の形態では、規制条件２０ｐｂは、エンドエフェクタ１１の姿勢の移動のみを規制するが、規制条件２０ｐｂは、位置の移動のみを規制してもよく、位置及び姿勢の両方の移動を規制してもよい。規制条件２０ｐｂは、操作制御装置４２０の操作指令の操作位置指令に対して制限を加えるための情報であってもよく、操作指令に基づき第１動作指令部２０ｄによって生成されるエンドエフェクタ１１の動作指令の位置指令に対して制限を加えるための情報であってもよい。

　ここで、エンドエフェクタ１１の動作指令は、位置指令及び力指令の少なくとも位置指令を含み、本実施の形態では、両方を含む。位置指令は、エンドエフェクタ１１の位置を表す。力指令は、エンドエフェクタ１１が対象物に加える力を表す。位置指令は、エンドエフェクタ１１の３次元位置及び３次元姿勢の指令を含んでもよい。さらに、位置指令は、３次元位置及び３次元姿勢の指令の実行時刻を含んでもよい。力指令は、力の大きさ及び力の方向の指令を含んでもよい。さらに、力指令は、力の大きさ及び方向の指令の実行時刻を含んでもよい。本明細書及び請求の範囲において、「力」とは、力の大きさ及び方向のうちの少なくとも力の大きさを含み、「位置」とは、３次元位置及び３次元姿勢のうちの少なくとも３次元位置を含み得ることを意味する。

　３次元位置及び３次元姿勢は、３次元空間内での位置及び姿勢である。３次元位置及び３次元姿勢は、ワールド座標系及びロボット座標系等を用いて定義されてもよい。ワールド座標系は、ロボット１０が配置される空間に設定される座標系である。ロボット座標系は、ロボット１０を基準とする座標系であり、例えば、ロボットアーム１２が据え付けられる基台１３を基準とする座標系であってもよい。本実施の形態では、ロボットアーム１２が固定されているため、ワールド座標系とロボット座標系とは同じであるとして設定される。

　例えば、エンドエフェクタ１１の姿勢の移動を規制する規制条件２０ｐｂは、操作位置指令及び位置指令に含まれる姿勢の変位及び移動速度等の指令に対して制限する情報を含んでもよい。例えば、規制条件２０ｐｂは、力センサ１１ｅ及び４０５の３軸周りの力のモーメントに対応する３軸周りの回転変位及び回転速度等を、０（零）等の制限値に制限する情報を含んでもよい。これに限定されないが、本実施の形態では、規制条件２０ｐｂは、力センサ１１ｅのＸｅ軸及びＹｅ軸の各軸周りの回転変位及び回転速度等、又は、Ｘｅ軸、Ｙｅ軸及びＺｅ軸の各軸周りの回転変位及び回転速度等を制限する。又は、規制条件２０ｐｂは、力センサ４０５のＸｃ軸及びＹｃ軸の各軸周りの回転変位及び回転速度等、又は、Ｘｃ軸、Ｙｃ軸及びＺｃ軸の各軸周りの回転変位及び回転速度等を制限する。

　例えば、Ｘｅ軸及びＸｃ軸周りの回転移動はそれぞれ、エンドエフェクタ１１及び把持部４０１のピッチング方向の移動に対応する。Ｙｅ軸及びＹｃ軸周りの回転移動はそれぞれ、エンドエフェクタ１１及び把持部４０１のローリング方向の移動に対応する。Ｚｅ軸及びＺｃ軸周りの回転移動はエンドエフェクタ１１及び把持部４０１のヨーイング方向の移動に対応する。

　例えば、エンドエフェクタ１１の位置の移動を規制する規制条件２０ｐｂは、操作位置指令及び位置指令に含まれる位置の変位及び移動速度等の指令に対して制限する情報を含んでもよい。例えば、規制条件２０ｐｂは、力センサ１１ｅ及び４０５の３軸方向の力に対応する３軸方向の位置の変位及び移動速度等を、０等の制限値に制限する情報を含んでもよい。例えば、エンドエフェクタ１１の移動方向に直交する方向のエンドエフェクタ１１の位置の移動が規制される場合、規制条件２０ｐｂは、３軸方向の位置の変位及び移動速度等のうち、上記移動方向に直交する方向の成分を０等の制限値に制限する情報を含んでもよい。なお、エンドエフェクタ１１の移動方向は、エンドエフェクタ１１の軌跡の近似直線の方向、及びエンドエフェクタ１１の軌跡の接線の方向等の、研削作業中にエンドエフェクタ１１が移動される方向であってもよい。

　姿勢情報２０ｐａは、エンドエフェクタ１１に設定された所定の姿勢の情報である。制御装置２０は、操作入力装置４０に入力される移行指令に対応して、エンドエフェクタ１１の姿勢を所定の姿勢に移行させる。本実施の形態では、所定の姿勢は、研削作業時のエンドエフェクタ１１の基本的な姿勢である基本姿勢として設定される。以下において、所定の姿勢を基本姿勢とも表記する。基本姿勢は、研削作業時のエンドエフェクタ１１の好ましい姿勢でもある。基本姿勢は、ロボット１０に対する姿勢、及び研削対象領域ＷＡの表面に対する姿勢等として設定されてもよい。本実施の形態では、基本姿勢は、研削対象領域ＷＡの表面に対する姿勢として設定される。

　回避情報２０ｐｃは、移行指令に従ったエンドエフェクタ１１の姿勢の移行に支障が生じたときにエンドエフェクタ１１の位置を移動させるための情報である。例えば、上記支障は、エンドエフェクタ１１が他の物体と接触又は衝突等することにより基本姿勢に移行できないことである。上記支障は、制御装置２０によって、移行中の力センサ１１ｅの検出値に基づき検出される。回避情報２０ｐｃは、力センサ１１ｅによって検出される反力の方向と反対の方向以外の方向への所定の距離での位置の移動を指令する情報であってもよい。又は、回避情報２０ｐｃは、例えばＺｅ軸負方向等の予め決められた方向及び距離での位置の移動を指令する情報であってもよい。

　撮像制御部２０ａは、撮像装置３０の動作を制御し、撮像装置３０によって撮像された画像データを取得する。撮像制御部２０ａは、ステレオカメラを備える撮像装置３０によって同時刻に撮像された２つの画像データを対応付けて画像処理部２０ｂ及び２０ｃ等に出力する。撮像装置３０によって画像を撮像すること、及び、撮像装置３０によって撮像された画像データを取得することは、撮像装置３０によって撮像された１つの静止画像データを取得すること、及び、撮像装置３０によって撮像された動画データから１フレームの静止画像データを取得することを含む。

　第１画像処理部２０ｂは、撮像制御部２０ａから取得された画像データを処理することで、当該画像データに写し出される被写体の各位置の３次元位置を検出し、各位置の３次元位置を表す画像データである３次元画像データを生成する。具体的には、第１画像処理部２０ｂは、撮像装置３０によって同時刻に撮像された２つの画像データそれぞれに写し出される研削対象領域ＷＡを特定する。例えば、第１画像処理部２０ｂは、記憶部２０ｐに記憶される研削対象領域ＷＡの形状とのパターンマッチング手法等で比較することにより、各画像データにおいて研削対象領域ＷＡのエッジを抽出してもよい。さらに、第１画像処理部２０ｂは、記憶部２０ｐに記憶されるカメラパラメタを用いたステレオマッチング手法等で画像処理することにより、当該２つの画像データの少なくとも一方の研削対象領域ＷＡ内において、少なくとも３つの画素に写し出される被写体と撮像装置３０との距離を検出する。さらに、第１画像処理部２０ｂは、距離が検出された各画素に写し出される被写体について、ロボットシステム１が存在する３次元空間内での３次元位置を検出する。

　さらに、第１画像処理部２０ｂは、研削対象領域ＷＡの少なくとも３つの画素の３次元位置を用いて、当該３次元位置が３次元空間内で形成する平面を研削対象領域ＷＡの表面として検出する。なお、上記平面の検出方法は、既知のいかなる方法であってもよい。例えば、第１画像処理部２０ｂは、少なくとも３つの画素の３次元位置を通る平面を研削対象領域ＷＡの表面として検出してもよい。又は、第１画像処理部２０ｂは、少なくとも３つの画素の３次元位置又はその近傍を通る平面、つまり、少なくとも３つの上記３次元位置に近似する平面を研削対象領域ＷＡの表面として検出してもよい。又は、第１画像処理部２０ｂは、少なくとも３つの画素の３次元位置を通る曲面を検出し、当該曲面の法線に垂直であり且つ当該曲面と交点を有する平面を研削対象領域ＷＡの表面として検出してもよい。第１画像処理部２０ｂは、研削対象領域ＷＡの表面の３次元位置及び３次元姿勢等の情報を姿勢決定部２０ｈ及び記憶部２０ｐ等に出力する。

　第２画像処理部２０ｃは、撮像制御部２０ａから取得された画像データを提示装置５０に出力し表示させる。第２画像処理部２０ｃは、第１画像処理部２０ｂから処理済みの画像データを取得し、研削対象領域ＷＡの３次元形状を表す画像データを提示装置５０に出力してもよい。例えば、上記画像データは、各画素の画素値が撮像装置３０から被写体までの距離値である距離画像等であってもよい。距離値は、画素の濃淡又は色で表されてもよい。

　姿勢決定部２０ｈは、操作入力装置４０に入力される姿勢の移行指令に対応して、エンドエフェクタ１１に移行させる基本姿勢を決定する。姿勢決定部２０ｈは、操作入力装置４０からエンドエフェクタ１１の姿勢の移行指令を受け取ると、第１画像処理部２０ｂに研削対象領域ＷＡの表面の情報を要求する。第１画像処理部２０ｂは、撮像制御部２０ａに研削対象領域ＷＡの撮像を要求し、撮像制御部２０ａから受け取る画像データを画像処理することで、研削対象領域ＷＡの表面の３次元位置及び３次元姿勢等の情報を検出し、姿勢決定部２０ｈに出力する。姿勢決定部２０ｈは、記憶部２０ｐから姿勢情報２０ｐａを読み出し、姿勢情報２０ｐａと研削対象領域ＷＡの表面の情報とを用いて、研削対象領域ＷＡの表面に対するエンドエフェクタ１１の基本姿勢の情報を検出する。姿勢決定部２０ｈは、各座標軸周りの姿勢角等の基本姿勢の情報を第１動作指令部２０ｄに出力する。

　なお、姿勢情報２０ｐａが研削対象領域ＷＡの表面に対する基本姿勢の情報ではなく、ロボット１０に対する基本姿勢の情報を含む場合、姿勢決定部２０ｈは、姿勢情報２０ｐａに含まれる基本姿勢の情報を、エンドエフェクタ１１の基本姿勢の情報に決定する。

　第１動作指令部２０ｄは、エンドエフェクタ１１に目的とする動作をさせるための動作指令を生成し、動作制御部２０ｆに出力する。動作指令は、研削装置１１ａを動作させるための指令を含んでもよい。第１動作指令部２０ｄは第１指令部の一例である。

　例えば、第１動作指令部２０ｄは、操作入力装置４０から受け取る操作指令に対応する動作をエンドエフェクタ１１にさせるための動作指令を生成する。さらに、第１動作指令部２０ｄは、規制モードでは、記憶部２０ｐの規制条件２０ｐｂを動作指令に適用し、規制条件が反映された動作指令を生成する。第１動作指令部２０ｄは、規制条件が反映された動作指令及び／又は規制条件２０ｐｂを、第２動作指令部２０ｅにも出力してもよい。また、第１動作指令部２０ｄは、操作入力装置４０から受け取る移行指令に従って基本姿勢にエンドエフェクタ１１の姿勢を移動させるための動作指令を生成する。また、第１動作指令部２０ｄは、回避経路決定部２０ｊから受け取る回避経路に沿ってエンドエフェクタ１１の位置を移動させるための動作指令を生成する。

　例えば、非規制モードでは、第１動作指令部２０ｄは、操作指令に含まれる操作位置指令及び操作力指令を、ロボット座標系での位置指令及び力指令に変換し、当該位置指令及び力指令を含む動作指令を生成する。規制モードでは、第１動作指令部２０ｄは、規制条件２０ｐｂを適用することで操作位置指令を処理し、処理後の操作位置指令と操作力指令とを用いて動作指令を生成し出力してもよい。又は、第１動作指令部２０ｄは、操作位置指令と操作力指令とを用いて動作指令を生成し、規制条件２０ｐｂを適用することで当該動作指令を処理し、処理後の動作指令を出力してもよい。

　動作制御部２０ｆは、第１動作指令部２０ｄから受け取る動作指令に従ってロボット１０の各部を動作させるための制御指令を生成し、ロボット１０に出力する。動作制御部２０ｆは、ロボット１０の各部の動作状態の情報を動作情報処理部２０ｇから取得し、当該情報をフィードバック情報として用いて制御指令を生成する。具体的には、動作制御部２０ｆは、ロボットアーム１２のアーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６のサーボモータＭａとエンドエフェクタ１１の研削装置１１ａのモータ（図示略）とを動作させるための制御指令を生成する。

　動作情報処理部２０ｇは、ロボット１０の動作情報を検出し処理する。動作情報処理部２０ｇは、研削装置１１ａの通電状態等に基づき研削装置１１ａのＯＮ状態及びＯＦＦ状態を動作情報として検出する。動作情報処理部２０ｇは、アーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６それぞれのサーボモータＭａの回転センサＭｂの検出値と、当該サーボモータＭａの電流センサＭｃの検出値と、エンドエフェクタ１１の力センサ１１ｅの検出値とを動作情報として取得する。なお、動作情報処理部２０ｇは、サーボモータＭａの駆動回路が当該サーボモータＭａに供給する電流の指令値を動作情報として取得してもよい。動作情報処理部２０ｇは、上記動作情報をフィードバック情報として動作制御部２０ｆ及び第２動作指令部２０ｅに出力する。例えば、動作情報において、アーム駆動装置Ｍ１、Ｍ２、Ｍ３、Ｍ４、Ｍ５及びＭ６のサーボモータＭａの回転センサＭｂの検出値、及び、サーボモータＭａの電流センサＭｃの検出値は、エンドエフェクタ１１の位置及び姿勢の変位及び変位速度を示す位置データであり、力センサ１１ｅの検出値は、エンドエフェクタ１１が受ける反力の大きさ及び方向を示す力データである。

　第２動作指令部２０ｅは、操作装置４００の把持部４０１への力の付与、及び、把持部４０１の位置及び姿勢の移動等のための動作指令であるモータ動作指令を生成し操作入力装置４０に出力する。モータ動作指令は、操作駆動指令及び／又は操作規制指令を含む。第２動作指令部２０ｅは第２指令部の一例である。

　例えば、第２動作指令部２０ｅは、動作情報処理部２０ｇから受け取る動作情報の力データに基づき、力センサ１１ｅによって検出された力の大きさ及び方向に対応する反力を把持部４０１に与えるための操作駆動指令を含むモータ動作指令を生成する。

　また、第２動作指令部２０ｅは、動作情報処理部２０ｇから受け取る動作情報の位置データと操作入力装置４０から受け取る把持部４０１の動作情報の位置データとを用いて、把持部４０１の位置及び姿勢をエンドエフェクタ１１の位置及び姿勢に対応させるように把持部４０１を移動させるための操作駆動指令を含むモータ動作指令を生成する。操作駆動指令は、把持部４０１に対する位置指令及び力指令の少なくとも１つを含んでもよい。これにより、操作装置４００のモータ４０４は、非規制モードで規制を受けずに動作するエンドエフェクタ１１の位置及び姿勢、規制モードでの規制を受けて動作するエンドエフェクタ１１の位置及び姿勢、移行指令に従って動作するエンドエフェクタ１１の位置及び姿勢、並びに、回避経路に沿って動作するエンドエフェクタ１１の位置及び姿勢等と、把持部４０１の位置及び姿勢とを対応させるように、把持部４０１を移動させることができる。

　例えば、把持部４０１の動作情報において、モータ４０４の回転センサ（図示略）の検出値は、把持部４０１の位置及び姿勢の変位及び変位速度を示す位置データであり、力センサ４０５の検出値は、把持部４０１に作用する力の大きさ及び方向を示す力データである。

　また、第２動作指令部２０ｅは、規制モードにおいて、規制条件２０ｐｂによって制限された方向へのエンドエフェクタ１１の移動を指令する把持部４０１への力の入力に対抗する力を把持部４０１に与えるための操作規制指令を含むモータ動作指令を生成する。例えば、上記の対抗する力は、規制条件２０ｐｂで規制されるエンドエフェクタ１１の位置及び姿勢の移動に対応する把持部４０１の位置及び姿勢の移動量を０（零）にするための力の大きさ及び方向を含んでもよい。操作規制指令は、把持部４０１に対する位置指令及び力指令の少なくとも１つを含んでもよい。

　第２動作指令部２０ｅは、操作入力装置４０から受け取る力センサ４０５の検出値と、規制条件２０ｐｂ及び／又は規制条件２０ｐｂが反映された動作指令とを用いて、上記モータ動作指令を生成してもよい。これにより、操作装置４００のモータ４０４は、規制条件２０ｐｂによって制限された方向への把持部４０１の移動を阻止する力を把持部４０１に与えることができる。よって、規制条件２０ｐｂに従って動作するエンドエフェクタ１１の位置及び姿勢と、把持部４０１の位置及び姿勢とが同様の挙動を示すことができる。

　回避判定部２０ｉは、エンドエフェクタ１１の基本姿勢への移行中、エンドエフェクタ１１の位置を移動させる回避が必要であるか否かを判定し、判定結果を回避経路決定部２０ｊに出力する。回避判定部２０ｉは、エンドエフェクタ１１の基本姿勢への移行中、力センサ１１ｅが閾値よりも大きい力、つまり閾値超の力の入力を検出すると、回避が必要であると決定する。この場合、回避判定部２０ｉは、回避を実行する指令と、力センサ１１ｅによって検出された閾値超の力の方向とを回避経路決定部２０ｊに出力する。閾値は、ロボットシステム１の作業環境、要求される力の検知感度及び対象物の種類等の様々な条件に対応して設定されてもよく、例えば、０以上の値で設定されてもよい。例えば、閾値は、エンドエフェクタ１１の基本姿勢への移行を妨げるような力の大きさであってもよい。例えば、閾値は、エンドエフェクタ１１と物体との接触を表すような力の大きさであってもよい。

　回避経路決定部２０ｊは、エンドエフェクタ１１を回避させるための回避経路を決定し、第１動作指令部２０ｄに出力する。具体的には、回避経路決定部２０ｊは、回避判定部２０ｉから回避の実行指令を受け取ると、記憶部２０ｐから回避情報２０ｐｃを読み出す。さらに、回避経路決定部２０ｊは、回避判定部２０ｉから受け取った力の方向を回避情報２０ｐｃに適用することによって、当該力の入力を回避するためのエンドエフェクタ１１の位置の移動方向及び移動距離を含む回避経路を決定する。

　［ロボットシステムの動作］
　［非規制モードでの操作入力装置の操作によるロボットの動作］
　非規制モードでの操作入力装置４０の操作装置４００の操作によるロボット１０の動作を説明する。図１、図４及び図６に示すように、例えば、ユーザＰは、把持部４０１のハンドル部４０１ａ及び４０１ｂを握り、エンドエフェクタ１１の研削装置１１ａの目的の位置への移動方向及び目的の姿勢への回動方向へ、把持部４０１を移動させ姿勢を変える。また、ユーザＰは把持部４０１の入力部４０１ｃへの入力を与えることで、研削装置１１ａを起動させる。

　支持部４０２は、把持部４０１と一緒に移動し且つ姿勢を変え、６つのアーム４０３それぞれに屈曲及び姿勢変化等の動作をさせ、当該アーム４０３と接続されたモータ４０４の回転軸を回転させる。操作制御装置４２０は、当該モータ４０４の回転センサの回転信号及び力センサ４０５の力信号に基づく操作指令を制御装置２０に出力する。

　制御装置２０は、操作指令に基づき動作指令を生成し、当該動作指令に従ってロボット１０を動作させる。制御装置２０は、エンドエフェクタ１１の位置の変化、姿勢の変化及び研削装置１１ａを介した対象物Ｗへの作用力が上記力信号を反映するように、ロボットアーム１２を動作させる。これにより、ユーザＰは、操作装置４００の把持部４０１を操作して、ロボット１０に意図する動作をさせることができる。

　さらに、制御装置２０は、エンドエフェクタ１１の力センサ１１ｅの検出信号に基づく力データに対応する反力を把持部４０１に与えるために、当該反力に対応する負荷トルクを各モータ４０４に発生させる。これにより、ユーザＰは、例えば、対象物Ｗから反力を受けているように把持部４０１から反力を感じつつ、把持部４０１の位置及び姿勢を操作することができる。

　把持部４０１の反力は、力センサ１１ｅによって検出される力の大きさ及び方向を反映する。このような把持部４０１の反力は、対象物Ｗの表面状態に応じて異なる研削中の研削装置１１ａの状態を、ユーザＰの手に感じさせることができる。例えば、把持部４０１の反力は、ユーザＰが研削装置１１ａを手持ちし研削する場合にユーザＰの手が受ける感触を、ユーザＰの手に感じさせることができる。

　また、力センサ４０５の力信号が示す位置の変化量、姿勢の変化量及び力の大きさ等の指令に対して、エンドエフェクタ１１の位置の変化量、姿勢の変化量及び作用力の大きさ等の指令は、割り増しされる。これにより、ロボットアーム１２は、把持部４０１の可動範囲を大きく超えてエンドエフェクタ１１の位置及び姿勢を変えることができる。さらに、エンドエフェクタ１１は、把持部４０１に加えられる力を大きく超えた作用力を発生させることができる。

　［規制モードでの動作］
　規制モードでのロボットシステム１の動作の一例を説明する。本例では、規制モードにおいて、エンドエフェクタ１１のＸｅ軸、Ｙｅ軸及びＺｅ軸の各軸周りの回転変位が制限され、操作装置４００の把持部４０１のＸｃ軸、Ｙｃ軸及びＺｃ軸の各軸周りの回転変位が制限されるものとする。図８は、実施の形態に係るロボットシステム１の規制モードでの動作の一例を示すフローチャートである。

　図８に示すように、ステップＳ１０１において、ユーザＰは、操作装置４００を操作することによって、エンドエフェクタ１１の研削装置１１ａの研削砥石１１ｂの姿勢を、研削対象領域ＷＡの表面に対して所望の姿勢に移行させる。ユーザＰによる所望の姿勢への移行は、研削作業開始前であってもよく、研削作業中であってもよい。所望の姿勢への移行は、移行指令に従った基本姿勢への移行であってもよい。なお、研削砥石１１ｂの姿勢が既に所望の姿勢である場合、本ステップの処理は省略されてもよい。

　次いで、ステップＳ１０２において、ユーザＰは、操作入力装置４０の入力装置４１０に、規制モードを実行する指令を入力し、操作入力装置４０の操作制御装置４２０は当該指令を受け付け制御装置２０に出力する。

　次いで、ステップＳ１０３において、制御装置２０は、規制モードでの制御を開始し実行する。具体的には、制御装置２０は、エンドエフェクタ１１の現状の姿勢である第１設定姿勢に対するＸｅ軸、Ｙｅ軸及びＺｅ軸の各軸周りのエンドエフェクタ１１の回転変位を制限し、把持部４０１の現状の姿勢である第２設定姿勢に対するＸｃ軸、Ｙｃ軸及びＺｃ軸の各軸周りの把持部４０１の回転変位を制限するように制御を行う。

　次いで、ステップＳ１０４において、ユーザＰによって操作装置４００の把持部４０１に操作が入力されると、操作制御装置４２０は、入力された操作に対応する力センサ４０５の力信号に基づき操作指令を生成し制御装置２０に出力する。

　次いで、ステップＳ１０５において、制御装置２０は、記憶部２０ｐの規制条件２０ｐｂと操作指令とを用いて、規制条件２０ｐｂが反映された動作指令する。例えば、制御装置２０は、操作指令においてＸｃ軸、Ｙｃ軸及びＺｃ軸の各軸周りのモーメントに対応する指令値を０にする処理、又は、操作指令を用いて生成された動作指令においてＸｅ軸、Ｙｅ軸及びＺｅ軸の各軸周りのモーメントに対応する指令値を０にする処理を行う。

　次いで、ステップＳ１０６において、制御装置２０は、動作指令に従ってロボット１０の各部を動作させるための制御指令を生成し、ロボット１０に出力する。次いで、ステップＳ１０７において、ロボットアーム１２は、第１設定姿勢からのエンドエフェクタ１１の姿勢の変化を制限しつつ、操作指令に従ってエンドエフェクタ１１の位置を移動させる。つまり、ロボットアーム１２は、規制条件２０ｐｂに従って制限された動作を行う。

　さらに、ステップＳ１０８において、制御装置２０は、規制条件２０ｐｂと力センサ４０５の検出値とに基づき、力センサ４０５の各軸周りのモーメントの検出値に対抗するモーメントを把持部４０１に与えるためのモータ動作指令を生成し、操作入力装置４０に出力する。モータ動作指令は、ユーザＰによって把持部４０１に入力される各軸周りのモーメントを打ち消すモーメントを把持部４０１に与えるように、操作入力装置４０のモータ４０４を駆動させるための指令である。つまり、制御装置２０は、規制条件２０ｐｂに従って把持部４０１の動作を制限するためのモータ動作指令を出力する。

　次いで、ステップＳ１０９において、操作制御装置４２０は、モータ動作指令に従って各モータ４０４を駆動させることで把持部４０１に力を付与し、力センサ４０５の各軸周りの把持部４０１の回転変位を制限する。つまり、操作制御装置４２０は、各モータ４０４の駆動力により把持部４０１の動作を制限する。

　次いで、ステップＳ１１０において、制御装置２０は、入力装置４１０を介して規制モードを終了する指令を受け付けたか否かを判定し、受け付けた場合（ステップＳ１１０でＹｅｓ）に一連の処理を終了し、受け付けていない場合（ステップＳ１１０でＮｏ）にステップＳ１０４に戻る。

　ステップＳ１０１からステップＳ１１０の処理により、制御装置２０は、エンドエフェクタ１１の姿勢と把持部４０１の姿勢とを同一の所望の姿勢に維持しつつ、把持部４０１に入力される操作に従ってエンドエフェクタ１１を動作させる。

　［エンドエフェクタの基本姿勢への移行動作］
　ロボットシステム１におけるエンドエフェクタ１１の姿勢を基本姿勢に移行させる動作の一例を説明する。図９は、実施の形態に係るロボットシステム１のエンドエフェクタ１１を基本姿勢に移行させる動作の一例を示すフローチャートである。

　図９に示すように、ステップＳ２０１において、ユーザＰは、操作入力装置４０の入力装置４１０に、エンドエフェクタ１１を基本姿勢へ移行する指令を入力し、操作制御装置４２０は当該指令を受け付け制御装置２０に出力する。

　次いで、ステップＳ２０２において、ユーザＰは、操作装置４００を操作することによって、エンドエフェクタ１１を研削対象領域ＷＡに対する所望の位置に移動させる。なお、エンドエフェクタ１１の位置が既に所望の位置である場合、本ステップの処理は省略されてもよい。

　次いで、ステップＳ２０３において、ユーザＰは、入力装置４１０に撮像実行の指令を入力し、操作制御装置４２０は当該指令を受け付け制御装置２０に出力する。

　次いで、ステップＳ２０４において、制御装置２０は、撮像装置３０に研削対象領域ＷＡを撮像させる。次いで、ステップＳ２０５において、制御装置２０は、研削対象領域ＷＡの画像データを処理することで、当該画像データの３次元画像データを生成する。次いで、ステップＳ２０６において、制御装置２０は、３次元画像データを用いて、研削対象領域ＷＡの表面の姿勢を検出する。次いで、ステップＳ２０７において、制御装置２０は、記憶部２０ｐの姿勢情報２０ｐａと研削対象領域ＷＡの表面の姿勢の情報とを用いて、エンドエフェクタ１１の基本姿勢を決定する。

　次いで、ステップＳ２０８において、制御装置２０は、エンドエフェクタ１１の姿勢を基本姿勢に移行させるように、ロボットアーム１２を動作させる。つまり、制御装置２０は基本姿勢への移行を実行する。

　次いで、ステップＳ２０９において、制御装置２０は、力センサ１１ｅによって閾値超の力が検出されたか否かを判定し、検出された場合（ステップＳ２０９でＹｅｓ）にステップＳ２１０に進み、検出されない場合（ステップＳ２０９でＮｏ）にステップＳ２１２に進む。例えば、図１０に示すように、閾値超の力が検出された場合、エンドエフェクタ１１が他の物体Ａと接触又は衝突することにより移行の支障が発生した可能性がある。図１０は、エンドエフェクタ１１の基本姿勢への移行中における移行の支障の発生例を示す側面図である。図１０では、実線で示されるように研削砥石１１ｂが鉛直面と平行であるエンドエフェクタ１１の姿勢が、研削砥石１１ｂが鉛直面に対し斜めに傾斜する基本姿勢に移行される途中で、破線で示されるようにエンドエフェクタ１１が物体Ａと衝突する。

　ステップＳ２１０において、制御装置２０は、姿勢の移行を停止し、さらに、記憶部２０ｐの回避情報と力センサ１１ｅによって検出された閾値超の力の方向とを用いて、エンドエフェクタ１１の回避経路を決定する。本例では、回避経路は、閾値超の力の方向への所定の距離の経路である。図１０では、回避経路は、力センサ１１ｅによって検出される閾値超の力Ｆｒの方向への所定の距離の経路である。

　次いで、ステップＳ２１１において、制御装置２０は、回避経路に沿ってエンドエフェクタ１１を移動させるように、ロボットアーム１２を動作させ、ステップＳ２０９に進む。つまり、制御装置２０はエンドエフェクタ１１の回避を実行する。例えば、図１１に示すように、制御装置２０は、エンドエフェクタ１１を力Ｆｒの方向へ所定の距離だけ移動させる。図１１は、エンドエフェクタ１１の基本姿勢への移行中における回避動作の例を示す側面図である。図１１では、実線で示される図１０の状態で停止したエンドエフェクタ１１が、破線で示されるように回避経路に沿って移動される。

　ステップＳ２１２において、制御装置２０は、基本姿勢への移行が完了したか否かを判定し、完了済みの場合（ステップＳ２１２でＹｅｓ）に一連の処理を終了し、未完了の場合（ステップＳ２１２でＮｏ）にステップＳ２０８に戻る。例えば、図１１では、ステップＳ２１１の回避完了後、基本姿勢への移行が完了していないため、ステップＳ２０８以降の基本姿勢への移行の処理が繰り返される。

　ステップＳ２０１からステップＳ２１２の処理により、制御装置２０は、他の物体との接触及び衝突等の支障の発生に対応して、エンドエフェクタ１１を基本姿勢に移行させることができる。さらに、制御装置２０は、回避経路に沿ったエンドエフェクタ１１の移動中に力センサ１１ｅによって閾値超の力が検出されると、当該力の方向に対応して回避経路を再設定することができる。また、制御装置２０は、研削対象領域ＷＡの表面の姿勢を検出することで、研削対象領域ＷＡの表面の様々な姿勢に対応した基本姿勢への移行を可能にする。

　なお、ステップＳ２０１からステップＳ２１２の処理では、制御装置２０は、回避経路に沿ったエンドエフェクタ１１の移動と、基本姿勢へのエンドエフェクタ１１の姿勢の移行とを別々に実行する。具体的には、制御装置２０は、回避経路に沿ったエンドエフェクタ１１の移動完了後に、基本姿勢へのエンドエフェクタ１１の姿勢の移行を再開する。しかしながら、エンドエフェクタ１１の回避動作は上記に限定されない。例えば、制御装置２０は、回避経路に沿ったエンドエフェクタ１１の移動中に、基本姿勢へのエンドエフェクタ１１の姿勢の移行を再開し、２つの動作を並行して行ってもよい。又は、制御装置２０は、回避経路に沿ったエンドエフェクタ１１の移動と、基本姿勢へのエンドエフェクタ１１の姿勢の移行の再開とを同時に開始し、２つの動作を並行して行ってもよい。さらに、制御装置２０は、並行した動作中に力センサ１１ｅによって閾値超の力が検出されると、当該力の方向に対応して回避経路を再設定してもよい。

　（その他の実施の形態）
　以上、本開示の実施の形態の例について説明したが、本開示は、上記実施の形態に限定されない。すなわち、本開示の範囲内で種々の変形及び改良が可能である。例えば、各種変形を実施の形態に施したもの、及び、異なる実施の形態における構成要素を組み合わせて構築される形態も、本開示の範囲内に含まれる。

　例えば、実施の形態において、制御装置２０は、撮像装置３０によって撮像された画像を処理することで、基本姿勢を決定するための研削対象領域ＷＡの表面の姿勢を検出するが、これに限定されない。上記表面の姿勢は、対象物までの距離を検出することができる装置を用いて検出することができる。例えば、このような装置は、光波、レーザ又は超音波等を用いて距離を検出するセンサであってもよい。

　また、実施の形態では、本開示の技術が適用可能である機械装置として、産業用ロボットであるロボット１０が例示されているが、本開示の技術が適用可能である機械装置は、産業用ロボット以外の機械装置であってもよい。例えば、当該機械装置は、サービスロボット、建設機械、トンネル掘削機、クレーン、荷役搬送車、及びヒューマノイド等であってもよい。サービスロボットは、介護、医療、清掃、警備、案内、救助、調理、商品提供等の様々なサービス業で使用されるロボットである。

　また、本開示の技術は、制御方法であってもよい。例えば、本開示の一態様に係る制御方法は、マスタ装置によって操作されるスレーブ装置を制御する制御方法であって、前記マスタ装置の操作端に力が加えられることで入力される情報である操作情報と、前記スレーブ装置における対象物に作用を加える作用部の所定の移動を規制する規制条件とに従って、前記スレーブ装置の動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力することと、前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を、前記マスタ装置に生成させるための指令を出力することとを含む。上記制御方法は、ＣＰＵ、ＬＳＩなどの回路、ＩＣカード又は単体のモジュール等によって、実現されてもよい。

　また、本開示の技術は、上記制御方法を実行するためのプログラムであってもよく、上記プログラムが記録された非一時的なコンピュータ読み取り可能な記録媒体であってもよい。また、上記プログラムは、インターネット等の伝送媒体を介して流通させることができるのは言うまでもない。

　また、上記で用いた序数、数量等の数字は、全て本開示の技術を具体的に説明するために例示するものであり、本開示は例示された数字に制限されない。また、構成要素間の接続関係は、本開示の技術を具体的に説明するために例示するものであり、本開示の機能を実現する接続関係はこれに限定されない。

　また、機能ブロック図におけるブロックの分割は一例であり、複数のブロックを一つのブロックとして実現する、一つのブロックを複数に分割する、及び／又は、一部の機能を他のブロックに移してもよい。また、類似する機能を有する複数のブロックの機能を単一のハードウェア又はソフトウェアが並列又は時分割に処理してもよい。

１　ロボットシステム（マスタスレーブシステム）
１０　ロボット（スレーブ装置）
１１　エンドエフェクタ（作用部）
１１ａ　研削装置
１２　ロボットアーム（動作部）
２０　制御装置
２０ｄ　第１動作指令部（第１指令部）
２０ｅ　第２動作指令部（第２指令部）
４０　操作入力装置（マスタ装置）
４００　操作装置
４０１　把持部（操作端）
４０４　モータ（力付与部）
４０５　力センサ（操作検出部）

Claims

　操作端と、前記操作端に力が加えられることで入力される情報である操作情報を検出し出力する操作検出部と、前記操作端に力を与える力付与部とを備えるマスタ装置と、
　対象物に作用を加える作用部と前記作用部を移動させる動作部とを備えるスレーブ装置と、
　制御装置とを備え、
　前記制御装置は、
　前記作用部の所定の移動を規制する規制条件と前記操作情報とに従って、前記動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力する第１指令部と、
　前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を前記力付与部に与えさせるための指令を出力する第２指令部とを含む
　マスタスレーブシステム。
　前記スレーブ装置は、前記動作部によって前記作用部の位置及び姿勢を移動させ、
　前記操作端は、位置及び姿勢を変えるように移動可能であり、
　前記規制条件は、前記所定の移動として、前記作用部の姿勢の移動を規制する条件である
　請求項１に記載のマスタスレーブシステム。
　前記操作検出部は、前記操作端に加えられる力の方向及び大きさを前記操作情報として検出する
　請求項１または請求項２に記載のマスタスレーブシステム。
　前記操作検出部は、前記操作端に加えられる３軸方向の力の大きさ及び３軸周りの力のモーメントを前記操作情報として検出する
　請求項３に記載のマスタスレーブシステム。
　前記規制条件は、前記所定の移動として、前記操作情報の前記３軸周りの力のモーメントに対応する前記作用部の姿勢の移動を規制する条件である
　請求項４に記載のマスタスレーブシステム。
　前記作用部は、研削装置を含み、
　前記動作部は、ロボットアームを含む
　請求項１から請求項５までのいずれか一項に記載のマスタスレーブシステム。
　前記制御装置は、前記作用部の姿勢を所定の姿勢に移行させるための移行指令を受け付ける受付部をさらに含み、
　前記受付部が前記移行指令を受け付けると、
　前記第１指令部は、前記作用部の姿勢を前記所定の姿勢にさせるための指令を出力する
　請求項１から請求項６までのいずれか一項に記載のマスタスレーブシステム。
　前記力付与部は、前記力付与部が与える力によって前記操作端を移動させることができるように前記操作端と接続され、
　前記受付部が前記移行指令を受け付けると、
　前記第２指令部は、前記操作端の姿勢を前記所定の姿勢に対応する姿勢にさせる前記操作端への力を前記力付与部に与えさせるための指令をさらに出力する
　請求項７に記載のマスタスレーブシステム。
　前記スレーブ装置は、前記作用部が前記対象物から受ける力を検出する力検出部をさらに備え、
　前記第１指令部は、前記作用部の前記所定の姿勢への移行中、前記力検出部が力を検出すると、前記作用部の位置を移動させ、且つ前記作用部の姿勢を前記所定の姿勢にさせるための指令を出力する
　請求項７または請求項８に記載のマスタスレーブシステム。
　マスタ装置によって操作されるスレーブ装置を制御する制御方法であって、
　前記マスタ装置の操作端に力が加えられることで入力される情報である操作情報と、前記スレーブ装置における対象物に作用を加える作用部の所定の移動を規制する規制条件とに従って、前記スレーブ装置の動作部によって前記作用部に前記規制条件が反映された動作をさせるための指令を出力することと、
　前記規制条件に従って、前記作用部の前記所定の移動を指令する前記操作端への入力に対抗する前記操作端への力を、前記マスタ装置に生成させるための指令を出力することとを含む
　制御方法。