JP2015168040A - ロボット、ロボットシステム、制御装置、及び制御方法 - Google Patents

Abstract

【課題】良好な組み立て作業を行うことができるロボットを提供すること。【解決手段】ハンドと、ハンドを動作させる制御部と、を含み、制御部は、ハンドと第１物体とを含む撮像画像に基づいて、ハンドによって、第１物体の所定位置を中心に第１物体を回転させ、第１物体を第２物体に対して相対移動させる、ロボットである。【選択図】図４

Description

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

力センサーにより取得された情報であって、ロボットの把持部や、把持部に把持された操作対象に働いた力に関する情報と、撮像部により撮像された撮像画像とに基づいて、ロボットのコンプライアントモーション制御を行う技術が研究・開発されている。

これに関連し、例えば、ロボットに操作対象と組付対象を所定の状態に組み立てる組み立て作業をさせる場合、操作対象に対して定義されたコンプライアントモーション制御の座標軸の向きを撮像画像から逐次取得することで、作業中に操作対象の姿勢が変化しても、初期状態におけるコンプライアントモーション制御の各軸に対して設定されていた運動特性を保ち、良好な組み立て作業を行うようにロボットを制御する制御装置が知られている。そして、そのような制御装置において、操作対象に加わる回転モーメントに応じて、操作対象が回転運動する際の回転中心を操作対象の所定位置に設定することで、良好な組み立て作業を行うようにロボットを制御する方法が知られている（非特許文献１参照）。

Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.1477-1482, 2001.

しかしながら、従来の方法では、作業中に操作対象が組付対象と接触することで把持部と操作対象との相対位置とともに回転中心の位置が変化してしまうと、その回転中心の位置の変化を検出することができず、良好な組み立て作業が行えない場合があった。

そこで本発明は、上記従来の方法の問題に鑑みてなされたものであり、良好な組み立て作業を行うことができるロボット、ロボットシステム、制御装置、及び制御方法を提供する。

本発明の一態様は、ハンドと、前記ハンドを動作させる制御部と、を含み、前記制御部は、前記ハンドと第１物体とを含む撮像画像に基づいて、前記ハンドによって、前記第１物体の所定位置を中心に前記第１物体を回転させ、前記第１物体を第２物体に対して相対移動させる、ロボットである。
この構成により、ロボットは、ハンドと第１物体とを含む撮像画像に基づいて、ハンドによって、第１物体の所定位置を中心に第１物体を回転させ、第１物体を第２物体に対して相対移動させる。このため、ロボットは、良好な組み立て作業を行うことができる。

また、本発明の一態様は、ロボットにおいて、前記所定位置は、前記第１物体とともに動く座標原点であり、前記制御部は、前記第１物体を回転させることに加えて、前記第１物体を並進させる、構成が用いられてもよい。
この構成により、ロボットは、回転させる場合の回転中心を第１物体とともに動く座標原点として、回転と並進によって第１物体を第２物体に対して相対移動させる。このため、ロボットは、良好な組み立て作業を行うことができる。

また、本発明の一態様は、ロボットにおいて、前記制御部は、前記撮像画像に基づいて、ビジュアルサーボ制御を行う、構成が用いられてもよい。
この構成により、ロボットは、撮像画像に基づいて、ビジュアルサーボ制御を行う。このため、ロボットは、第１物体を第２物体に対して逐次相対移動させることができるため、その結果として、精度の高い良好な組み立て作業を行うことができる。

前記制御部は、前記所定位置及び各軸方向に設定された運動特性に従ってコンプライアントモーション制御を行う、構成が用いられてもよい。
この構成により、ロボットは、所定位置及び各軸方向に設定された運動特性に従ってコンプライアントモーション制御を行う。このため、ロボットは、第２物体を破壊することなく、第２物体に対して第１物体を組み付けることができる。

また、本発明の一態様は、ロボットにおいて、前記制御部は、前記撮像画像に基づいて、前記ハンドに設定された位置と前記第１物体に設定された位置との相対的な位置関係を導出し、前記導出された位置関係に基づいて、前記所定位置を更新する、構成が用いられてもよい。
この構成により、ロボットは、撮像画像に基づいて、ハンドに設定された位置と第１物体に設定された位置との相対的な位置関係を導出し、導出された位置関係に基づいて、所定位置を更新する。このため、ロボットは、外力によってハンドと所定位置の位置関係がずらされても、ずらされた所定位置を中心に第１物体を第２物体に対して相対移動させることができ、その結果、良好な組み立て作業を行うことができる。

また、本発明の一態様は、ロボットにおいて、前記制御部は、前記導出された位置関係と、前記第１物体に設定された位置と前記所定位置との相対的な位置関係とに基づいて、前記所定位置を更新する、構成が用いられてもよい。
この構成により、ロボットは、ハンドに設定された位置と第１物体に設定された位置との相対的な位置関係を導出し、導出された位置関係と、第１物体に設定された位置と所定位置との相対的な位置関係とに基づいて、所定位置を更新する。このため、ロボットは、外力によってハンドと所定位置の位置関係がずらされても、第１物体に設定された位置を介して、間接的にハンドに設定された位置と所定位置との相対的な位置関係から、ずらされた所定位置を認識することができ、その結果、ずらされた所定位置を中心に第１物体を第２物体に対して相対移動させることができる。

また、本発明の一態様は、ロボットは、前記ハンドに働いた外力を検出する力センサーの位置を示すマーカーを備え、前記撮像画像は、さらに、前記マーカーを含み、ロボットにおいて、前記ハンドに設定された位置は、前記力センサーの位置によって表され、前記制御部は、前記撮像画像から検出された前記マーカーの位置に基づいて、前記ハンドに設定された位置と前記所定位置の相対的な位置関係を導出し、前記導出された前記ハンドに設定された位置と前記所定位置の相対的な位置関係に基づいて、前記所定位置を更新する、構成が用いられてもよい。
この構成により、ロボットは、撮像画像から検出された力センサーの位置を示すマーカーの位置に基づいて、ハンドに設定された位置と所定位置の相対的な位置関係を導出し、導出されたハンドに設定された位置と所定位置の相対的な位置関係に基づいて、所定位置を更新する。このため、ロボットは、力センサーがロボットのアーム部の部材に覆われていた場合であっても、力センサーを示すマーカーの位置を目印として、第１物体の位置を認識することができ、その結果、良好な組み立て作業を行うことができる。

また、本発明の一態様は、ロボットは、前記ハンドに働いた外力を検出する力センサーを備え、前記ハンドに設定された位置は、前記力センサーの位置によって表され、ロボットにおいて、前記制御部は、前記力センサーの初期状態における位置及び姿勢に基づいて、前記ハンドに設定されたと前記所定位置の相対的な位置関係を導出し、前記導出された前記ハンドに設定された位置と前記所定位置の相対的な位置関係に基づいて、前記所定位置を更新する、構成が用いられてもよい。
この構成により、ロボットは、力センサーの初期状態における位置及び姿勢に基づいて、ハンドに設定されたと所定位置の相対的な位置関係を導出し、導出されたハンドに設定された位置と所定位置の相対的な位置関係に基づいて、所定位置を更新する。このため、ロボットは、撮像部の画角から、力センサーを含むロボットのアーム部が外れてしまう場合であっても、第１物体の位置を認識することができ、その結果、良好な組み立て作業を行うことができる。

また、本発明の一態様は、ハンドを備えるロボットと、前記ハンドと第１物体とを撮像する撮像部と、を含み、前記ロボットは、前記撮像部により撮像された前記ハンドと前記第１物体とを含む撮像画像に基づいて、前記第１物体の所定位置を中心に、前記第１物体を回転させて、前記第１物体を第２物体に対して相対移動させる、ロボットシステムである。
この構成により、ロボットシステムは、ハンドと第１物体とを含む撮像画像に基づいて、第１物体の所定位置を中心に、第１物体を回転させて、第１物体を第２物体に対して相対移動させる。このため、ロボットシステムは、ロボットによって良好な組み立て作業を行うことができる。

また、本発明の一態様は、ハンドを備えるロボットを動作させる制御装置であって、前記ハンドと第１物体とを含む撮像画像に基づいて、前記ロボットによって、前記第１物体の所定位置を中心に前記第１物体を回転させ、前記第１物体を第２物体に対して相対移動させる、制御装置である。
この構成により、制御装置は、ハンドと第１物体とを含む撮像画像に基づいて、ロボットによって、第１物体の所定位置を中心に第１物体を回転させ、第１物体を第２物体に対して相対移動させる。このため、制御装置は、ロボットによって良好な組み立て作業を行うことができる。

また、本発明の一態様は、ハンドを備えるロボットを動作させる制御方法であって、前記ハンドと第１物体とを含む撮像画像を取得することと、前記撮像画像に基づいて、前記第１物体の所定位置を中心に前記第１物体を回転させ、前記第１物体を第２物体に対して相対移動させることと、を含む制御方法である。
この構成により、制御方法は、撮像画像に基づいて、第１物体の所定位置を中心に第１物体を回転させ、第１物体を第２物体に対して相対移動させる。このため、制御方法は、良好な組み立て作業を行うことができる。

以上により、ロボット、ロボットシステム、制御装置、及び制御方法は、ハンドと第１物体とを含む撮像画像に基づいて、ハンドによって、第１物体の所定位置を中心に第１物体を回転させ、第１物体を第２物体に対して相対移動させる。このため、ロボット、ロボットシステム、制御装置、及び制御方法は、良好な組み立て作業を行うことができる。

第１の実施形態に係るロボットシステム１の一例を示す構成図である。 ロボットシステム１において使用する座標系の一例を示す図である。 制御装置３０のハードウェア構成の一例を示す図である。 制御装置３０の機能構成の一例を示す図である。 制御装置３０が、操作対象Ｍと組付対象Ｏとを組み付けるようにロボット２０を制御するための処理の流れの一例を示すフローチャートである。 第２の実施形態に係るロボットシステム２の一例を示す構成図である。 第３の実施形態に係るロボットシステム１によって行われる所定の作業の一例を模式的に示す図である。 制御装置３０の制御部３６が、レンチＭ１によりボルトＯ１を締めるようにロボット２０を動作させるための処理の流れの一例を示すフローチャートである。

＜第１の実施形態＞
以下、本発明の第１の実施形態について、図面を参照して説明する。図１は、第１の実施形態に係るロボットシステム１の一例を示す構成図である。ロボットシステム１は、撮像部１０と、ロボット２０と、制御装置３０を具備する。

撮像部１０は、例えば、集光された光を電気信号に変換する撮像素子であるＣＣＤ（Charge Coupled Device）やＣＭＯＳ（Complementary Metal Oxide Semiconductor）等を備えたカメラである。また、撮像部１０は、２台のカメラによって構成されるステレオカメラであるが、例えば、３台以上のカメラによって構成されてもよいし、１台のカメラにより二次元画像を撮像する構成であってもよい。

撮像部１０は、ケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ（Universal Serial Bus）等の規格によって行われる。なお、撮像部１０と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続される構成であってもよい。

撮像部１０は、ロボット２０と、ロボット２０の把持部ＨＮＤが把持している操作対象Ｍと、ロボット２０により操作対象Ｍが組み付けられる組付対象Ｏが含まれる範囲を撮像可能な位置に設置されている。操作対象Ｍは、例えば、組付対象Ｏへ所定の状態で組み付けられることで、１つの工業製品を形成する部材である。なお、以下の説明では、操作対象Ｍは、予めロボット２０の把持部ＨＮＤに把持されていることを前提とする。
組付対象Ｏは、図１に示したように、ロボット２０により操作対象Ｍを組み付けることが可能な範囲で、ロボット２０から離れた位置に治具等によって予め設置されている。

ロボット２０は、例えば、把持部ＨＮＤ（エンドエフェクター）と、力センサー２２と、アーム部ＡＲＭ（マニピュレーター）と、図示しない複数のアクチュエーターを備えた単腕ロボットである。なお、ロボットシステム１は、単腕ロボットを具備する構成に代えて、双腕ロボットを具備する構成であってもよい。ロボットシステム１が双腕ロボットを具備する構成の実施例については、第２の実施形態で説明する。

ロボット２０の腕は、６軸垂直多関節型となっており、支持台とアーム部ＡＲＭと把持部ＨＮＤとがアクチュエーターによる連携した動作よって６軸の自由度の動作を行うことができる。なお、ロボット２０の腕は、５自由度（５軸）以下で動作するものであってもよいし、７自由度（７軸）以上で動作するものであってもよい。以下では、把持部ＨＮＤ及びアーム部ＡＲＭを備えた腕によって行われるロボット２０の動作について説明する。なお、把持部ＨＮＤは、ハンドの一例である。

ロボット２０は、例えばケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。なお、ロボット２０と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続されてもよい。また、ロボットシステム１では、ロボット２０は、図１に示したようにロボット２０の外部に設置された制御装置３０と接続されている構成とするが、この構成に代えて、制御装置３０がロボット２０に内蔵されている構成であってもよい。

ロボット２０の把持部ＨＮＤは、物体を把持可能な爪部を備える。
力センサー２２は、ロボット２０の把持部ＨＮＤとアーム部ＡＲＭとの間に備えられており、把持部ＨＮＤ（あるいは、把持部ＨＮＤに把持された操作対象Ｍ）に作用した力やモーメントを検出する。力センサー２２は、検出した力やモーメントを示す情報を、通信により制御装置３０へ出力する。力センサー２２により検出された力やモーメントを示す情報は、例えば、制御装置３０によるロボット２０のコンプライアントモーション制御等に用いられる。

ロボット２０は、操作対象Ｍ、組付対象Ｏ、力センサー２２それぞれの相対的な位置関係に基づいた制御信号を制御装置３０から取得し、取得した制御信号に基づいて、操作対象Ｍに対して所定の作業を行う。所定の作業とは、例えば、ロボット２０の把持部ＨＮＤにより把持された操作対象Ｍを現在位置から移動させ、組付対象Ｏに組み付ける作業等である。

制御装置３０は、ロボット２０が所定の作業を行うように制御する。より具体的には、制御装置３０は、撮像部１０により撮像された撮像画像であって、ロボット２０と、ロボット２０の把持部ＨＮＤが把持している操作対象Ｍと、ロボット２０により操作対象Ｍが組み付けられる組付対象Ｏとが含まれる範囲が撮像された撮像画像に基づいて、操作対象Ｍと組付対象Ｏと力センサー２２の相対的な位置関係を導出する。

そして、制御装置３０は、導出された相対的な位置関係に基づいてロボット２０が所定の作業を行うようにロボット２０を制御する。すなわち、制御装置３０は、把持部ＨＮＤ及びアーム部ＡＲＭによって操作対象Ｍを組付対象Ｏに対して相対移動させることで、操作対象Ｍが組付対象Ｏに組み付けられるようにロボット２０を制御する。

また、制御装置３０は、操作対象Ｍを組付対象Ｏに対して相対移動させる際、ロボット２０が把持部ＨＮＤにより把持している操作対象Ｍを回転させるように、ロボット２０を制御する場合がある。この場合、制御装置３０は、操作対象Ｍを、操作対象Ｍに設定された所定位置（以下、回転中心位置と称する）を中心として回転させるようにロボット２０を制御する。なお、以下では、回転中心位置における操作対象Ｍの姿勢を、回転中心姿勢と称する。

この制御において、制御装置３０は、操作対象Ｍと力センサー２２の相対的な位置及び姿勢（相対的な位置関係）が変化した場合であっても、撮像部１０により撮像された撮像画像から相対的な位置関係が変化した後の回転中心位置及び回転中心姿勢を検出する。検出された回転中心位置及び回転中心姿勢に基づいて、制御装置３０は、常に操作対象Ｍに設定された回転中心位置を中心として、回転中心姿勢を基準に操作対象Ｍを回転させる。なお、回転中心位置は、ユーザーにより操作対象Ｍ上の任意の位置に設定される。また、回転中心姿勢は、回転中心位置における操作対象Ｍの姿勢としたが、操作対象Ｍの姿勢と対応付けられた姿勢であれば、操作対象Ｍの姿勢と一致していなくてもよい。

ここで、図２を参照して、ロボットシステム１において使用する座標系について説明する。図２は、ロボットシステム１において使用する座標系を例示する図である。なお、以下の説明では、「＿」の後に記載した文字が、「＿」の手前の文字の下付き添え字を表すとして説明する。ロボットシステム１の制御装置３０は、図２に示したように、７つの三次元直交座標系、すなわち、撮像部座標系Σ＿ｃと、作業座標系Σ＿ｗと、ツール座標系Σ＿ｔと、重心座標系Σ＿ｇと、操作対象座標系Σ＿ｍと、外力座標系Σ＿ｅと、組付対象座標系Σ＿ｏを用いて、ロボット２０が所定の作業を行うように制御するための処理を行う。これら７つの座標系の原点及び座標軸の方向は、ユーザーにより制御装置３０に設定（記憶又は登録）される。

これら７つの座標系の原点はいずれも、対象Ｘ（今の場合、撮像部１０、ロボット２０の支持台、力センサー２２、操作対象Ｍの重心、操作対象Ｍの回転中心、操作対象Ｍ、組付対象Ｏ）の位置を表すように、対象Ｘに付随して動くように設定される。また、これら７つの座標系の座標軸の方向はいずれも、対象Ｘの姿勢を表すように、対象Ｘの姿勢変化に付随して動くように設定される。なお、ユーザーは、対象Ｘの位置と座標系の原点の位置が対応付けることができ、さらに、対象Ｘの傾きと座標系の方向が対応付けることができる前提で、対象Ｘに対して設定する座標系の原点の位置と座標軸の方向を任意の位置と方向に設定してもよい。

撮像部座標系Σ＿ｃは、撮像部１０の位置（例えば、撮像素子上の予め決められた位置を原点とする）及び姿勢を表す座標系である。
作業座標系Σ＿ｗは、ロボット２０の支持台の位置及び姿勢を表す座標系である。
ツール座標系Σ＿ｔは、力センサー２２の位置（例えば、力センサー２２の重心や、力センサー２２の位置を示すマーカーの位置を原点とする）及び姿勢に設定された座標系である。なお、本実施形態において、ツール座標系Σ＿ｔは、説明を簡略化するため、力センサー２２の位置及び姿勢を表すとともに、把持部ＨＮＤの位置及び姿勢を表す。一般的には、力センサー２１の位置及び姿勢を表すセンサー座標系と、把持部ＨＮＤの位置及び姿勢を表す手先（把持部）座標系とは一致せず、制御装置３０は、作業座標系とセンサー座標系との相対的な位置関係と、センサー座標系と手先座標系との相対的な位置関係とを用いて、作業座標系と手先座標系との相対的な位置関係を算出し、算出された作業座標系と手先座標系との相対的な位置関係に基づいたロボット２０の制御を行う。

重心座標系Σ＿ｇは、操作対象Ｍの重心の位置及び姿勢を表す座標系である。
操作対象座標系Σ＿ｍは、操作対象Ｍ（例えば、初期状態において把持部ＨＮＤから最も離れた操作対象Ｍ上の位置等）の位置及び姿勢を表す座標系である。
外力座標系Σ＿ｅは、対象物に働く外力及び外モーメントを定義する座標系である。又、本明細では、コンプライアントモーション制御による運動を定義する座標系を外力座標系Σ＿ｅと一致させる。すなわち、コンプライアントモーション制御による回転運動は外力座標系Σ＿ｅの姿勢を基準として、外力座標系Σ＿ｅの原点周りの回転で表される。ただし、これらは一致させなくともよく、ユーザーが任意に配置することが可能である。なお、以下では、区別する必要が無い限り、対象物に働く外力（すなわち、力センサーによって検出される外力）を単に力と称し、外モーメント（すなわち、力センサーによって検出されるモーメント）をモーメントと称する。上述したように、回転中心位置は、ユーザーにより任意の位置に設定することができるが、本実施形態では、操作対象Ｍ上の所定位置に設定されたものとして説明する。
組付対象座標系Σ＿ｏは、組付対象Ｏ（例えば、操作対象Ｍに最も近い組付対象Ｏ上の位置）の位置及び姿勢を表す座標系である。

以下の説明では、座標系が対象Ｘに付随して動くため、座標系ａにおける座標系ｂの原点の位置を、座標系ｂが設定された対象Ｘの位置として説明する。例えば、作業座標系Σ＿ｗにおける操作対象座標系Σ＿ｍの原点の位置を、作業座標系Σ＿ｗにおける操作対象Ｍの位置と称する。これと同様に、座標系ａにおける座標系ｂの姿勢を、座標系ｂが設定された対象Ｘの姿勢として説明する。例えば、作業座標系Σ＿ｗにおける操作対象座標系Σ＿ｍの姿勢を、作業座標系Σ＿ｗにおける操作対象Ｍの姿勢と称する。

ここで、より具体的な説明を記載する前に、制御装置３０が行う処理について説明するために用いる数式の表記法を示す。まず、以下の説明では、「＾」の後に記載した文字が、「＾」の手前の文字の上付き添え字を表すとして説明する。また、「（→）」が付記された一番手前の文字をベクトルとして説明する。また、「（＾）」が付記された一番手前の文字を行列として説明する。

これらの表記法の下では、座標系ａにおいて、座標系ｂが設定された対象Ｘの位置を表すベクトルを、位置ベクトルｐ＿ｂ＾ａ（→）と表す。位置ベクトルｐ＿ｂ＾ａ（→）は、以下の式（１）に示したように、座標系ｂにおける対象Ｘのｘ座標ｘ＾ｂ、ｙ座標ｙ＾ｂ、ｚ座標ｚ＾ｂで定義される。

位置ベクトルの表記法と同様に、座標系ａにおいて座標系ｂが設定された対象Ｘの姿勢を表すベクトルを、姿勢ベクトルｏ＿ｂ＾ａ（→）と表す。姿勢ベクトルｏ＿ｂ＾ａ（→）は、以下の式（２）に示したように、その成分を、オイラー角（α＿ｂ＾ａ，β＿ｂ＾ａ，γ＿ｂ＾ａ）を用いて以下の式（２）のように表す。

ここで、オイラー角は座標系ａのｘ軸、ｙ軸及びｚ軸を、それぞれ座標系ｂのｘ軸、ｙ軸及びｚ軸に一致させるために、座標系ａのｚ軸、ｙ軸及びｘ軸周りに回転させる角度として定義され、それぞれγ＿ｂ＾ａ、β＿ｂ＾ａ及びα＿ｂ＾ａと表す。

座標系ｂの姿勢で表された対象Ｘの位置や姿勢を、座標系ａの姿勢で表された対象Ｘの位置や姿勢に回転させる時の回転行列を、回転行列Ｒ＿ｂ＾ａ（＾）と表す。尚、上述のオイラー角と回転行列の間には以下に示した式（３）の関係がある。

ここで、上記の式（１）〜（３）で示したベクトルは、以下の変換式（４）〜（６）で添え字の上下を入れ替えることができる。

ここで、［Ｒ＿ｂ＾ａ（＾）］＾Ｔは、Ｒ＿ａ＾ｂの転置行列を表す。すなわち、回転行列の添え字は、回転行列を転置することによって入れ替えることができ、ベクトルの添え字は、回転行列によって入れ替えることができる。なお、以下では、座標系ａにおいて座標系ｂが設定された対象Ｘの位置を示す位置ベクトルｐ＿ｂ＾ａ（→）を、必要がある場合を除いて、単に座標系ａにおける対象Ｘの位置と称する。例えば、撮像部座標系Σ＿ｃにおいて表された組付対象Ｏに設定された組付対象座標系Σ＿ｏの原点の位置を示す位置ベクトルｐ＿ｏ＾ｃ（→）を、単に撮像部座標系Σ＿ｃにおける組付対象Ｏの位置と称する。

位置の場合と同様に、以下では、座標系ａにおいて座標系ｂが設定された対象Ｘの姿勢を示す姿勢ベクトルｏ＿ｂ＾ａ（→）を、必要がある場合を除いて、単に座標系ａにおける対象Ｘの姿勢と称する。例えば、撮像部座標系Σ＿ｃにおいて表された組付対象Ｏに設定された組付対象座標系Σ＿ｏの姿勢を示す姿勢ベクトルｏ＿ｏ＾ｃ（→）を、単に撮像部座標系Σ＿ｃにおける組付対象Ｏの姿勢と称する。

次に、図３を参照して、制御装置３０のハードウェア構成について説明する。図３は、制御装置３０のハードウェア構成の一例を示す図である。制御装置３０は、例えば、ＣＰＵ（Central Processing Unit）３１と、記憶部３２と、入力受付部３３と、通信部３４を備え、通信部３４を介して撮像部１０、ロボット２０等と通信を行う。これらの構成要素は、バスＢｕｓを介して相互に通信可能に接続されている。ＣＰＵ３１は、記憶部３２に格納された各種プログラムを実行する。

記憶部３２は、例えば、ＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read−Only Memory）、ＲＯＭ（Read−Only Memory）、ＲＡＭ（Random Access Memory）などを含み、制御装置３０が処理する各種情報や画像、プログラムを格納する。なお、記憶部３２は、制御装置３０に内蔵されるものに代えて、ＵＳＢ等のデジタル入出力ポート等によって接続された外付け型の記憶装置でもよい。

入力受付部３３は、例えば、キーボードやマウス、タッチパッド、その他の入力装置である。なお、入力受付部３３は、表示部として機能してもよく、さらに、タッチパネルとして構成されてもよい。
通信部３４は、例えば、ＵＳＢ等のデジタル入出力ポートやイーサネットポート等を含んで構成される。

次に、図４を参照することで、制御装置３０の機能構成について説明する。図４は、制御装置３０の機能構成の一例を示す図である。制御装置３０は、記憶部３２と、入力受付部３３と、画像取得部３５と、制御部３６を備える。これらの機能部のうち一部又は全部は、例えば、ＣＰＵ３１が、記憶部３２に記憶された各種プログラムを実行することで実現される。また、これらの機能部のうち一部または全部は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）等のハードウェア機能部であってもよい。

制御装置３０は、ビジュアルサーボのループの中にコンプライアントモーション制御を組み込むことで、操作対象Ｍを破壊することなく、操作対象Ｍと組付対象Ｏとが組み付けられた状態になるように、操作対象Ｍを組付対象Ｏに対して相対移動させる。より具体的には、制御装置３０は、ビジュアルサーボ技術を用いて、操作対象Ｍを予め記憶部３２に記憶されたテンプレート画像（例えば、操作対象Ｍと組付対象Ｏとが組み付けられた状態のＣＡＤ（Computer Aided Design）画像）と同じ状態にするようにロボット２０を制御する。

この時、制御装置３０は、撮像部１０により撮像された撮像画像中の操作対象Ｍと力センサー２２の相対的な位置関係を検出し、検出された相対的な位置関係に基づいて、コンプライアントモーション制御によってロボット２０を制御する際に、その動作を規定する回転中心位置及び回転中心姿勢を逐次決定する。そして、制御装置３０は、ロボット２０に操作対象Ｍを回転させる動作を行わせる際、回転中心位置を中心として、回転中心姿勢を基準に操作対象Ｍを回転させるようにロボット２０を制御する。また、制御装置３０は、ロボット２０に操作対象Ｍを並進させる際、回転中心姿勢を基準に並進させるようにロボット２０を制御する。

また、制御装置３０は、図示しない計時部を備えており、計時部により計時されたタイミングで、画像取得部３５から撮像部１０により撮像された撮像画像を取得し、それと同じタイミングでロボット２０の力センサー２２から、力センサー２２により検出された力及びモーメントを示す情報を取得する。

画像取得部３５は、撮像部１０により撮像された撮像画像を取得する。画像取得部３５は、取得した撮像画像を制御部３６に出力する。
制御部３６は、目標導出部３９と、回転中心位置算出部４１と、外力算出部４３と、負荷緩和動作量算出部４５と、移動動作量算出部４６と、動作終了判定部４７と、ロボット制御部４９を備える。
目標導出部３９は、画像取得部３５により取得された撮像画像から、撮像部座標系Σ＿ｃにおける組付対象Ｏの位置及び姿勢を検出する。

また、目標導出部３９は、撮像部座標系Σ＿ｃにおける組付対象Ｏの位置及び姿勢に基づいて、組み付け完了後の撮像部座標系Σ＿ｃにおける操作対象Ｍの位置及び姿勢を導出する。目標導出部３９は、この導出を行う際、上述のテンプレート画像に基づいて、組み付け完了後の操作対象Ｍの位置及び姿勢を検出する。以下では、目標導出部３９が導出した組み付け完了後の撮像部座標系Σ＿ｃにおける操作対象Ｍの位置及び姿勢を、目標位置及び姿勢と称する。

回転中心位置算出部４１は、画像取得部３５により取得された撮像画像から、撮像部座標系Σ＿ｃにおける操作対象Ｍの現在の位置及び姿勢を検出する。また、回転中心位置算出部４１は、ツール座標系Σ＿ｔにおける撮像部１０の位置及び姿勢を検出する。また、回転中心位置算出部４１は、検出された撮像部座標系Σ＿ｃにおける操作対象Ｍの現在の位置及び姿勢と、ツール座標系Σ＿ｔにおける撮像部１０の位置及び姿勢と、ユーザーにより予め設定された操作対象座標系Σ＿ｍにおける回転中心位置及び回転中心姿勢に基づいて、ツール座標系Σ＿ｔにおける回転中心位置及び回転中心姿勢を算出する。

また、回転中心位置算出部４１は、作業座標系Σ＿ｗにおける力センサー２２の位置及び姿勢を順運動学から算出する。また、回転中心位置算出部４１は、算出されたツール座標系Σ＿ｔにおける回転中心位置及び回転中心姿勢と、作業座標系Σ＿ｗにおける力センサー２２の位置及び姿勢とに基づいて、外力座標系Σ＿ｅにおけるロボット２０の支持台の位置及び姿勢を算出する。

外力算出部４３は、力センサー２２から取得された把持部ＨＮＤに働いた力及びモーメントであって、ツール座標系Σ＿ｔにおいて表された力及びモーメントを、回転中心位置算出部４１により算出された外力座標系Σ＿ｅにおけるロボット２０の支持台の位置及び姿勢に基づいて、外力座標系Σ＿ｅにおいて表された力及びモーメントを算出する。

負荷緩和動作量算出部４５は、外力算出部４３により算出された力やモーメントであって、外力座標系Σ＿ｅにおいて表された力及びモーメントに基づいて、ロボット２０の把持部ＨＮＤにより把持された操作対象Ｍに働いた力を緩和させるために、操作対象Ｍを動かす緩和動作量であって、外力座標系Σ＿ｅにおける緩和動作量を算出する。ここで、緩和動作量とは、検出された力が働いている方向へ操作対象Ｍを並進させる微小移動量（以下、微小緩和移動量と称する）と、検出されたモーメントの方向へ操作対象Ｍを回転させる微小回転量（以下、微小緩和回転量と称する）である。負荷緩和動作量算出部４５は、算出された外力座標系Σ＿ｅにおける緩和動作量に基づいて、作業座標系Σ＿ｗにおける緩和動作量を算出する。

移動動作量算出部４６は、操作対象Ｍの位置及び姿勢が、目標導出部３９により算出された目標位置及び姿勢と一致するように、把持部ＨＮＤにより操作対象Ｍを移動させる目標動作量を、撮像部座標系Σ＿ｃにおいて算出する。目標動作量とは、操作対象Ｍを目標位置へ並進させるための微小移動量（以下、微小目標移動量と称する）と、操作対象Ｍを目標姿勢へ回転させるための微小回転量（以下、微小目標回転量と称する）である。移動動作量算出部４６は、算出された撮像部座標系Σ＿ｃにおける目標動作量に基づいて、作業座標系Σ＿ｗにおける目標動作量を算出する。

動作終了判定部４７は、負荷緩和動作量算出部４５により算出された作業座標系Σ＿ｗにおける緩和動作量と、移動動作量算出部４６により算出された作業座標系Σ＿ｗにおける目標動作量とに基づいて、ロボット２０が操作対象Ｍを組付対象Ｏに組み付ける作業が終了したか否かを判定する。なお、動作終了判定部４７は、移動動作量算出部４６により算出された作業座標系Σ＿ｗにおける目標動作量のみに基づいて、ロボット２０が操作対象Ｍを組付対象Ｏに組み付ける作業が終了したか否かを判定してもよい。

ロボット制御部４９は、負荷緩和動作量算出部４５により算出された作業座標系Σ＿ｗにおける緩和動作量と、移動動作量算出部４６により算出された作業座標系Σ＿ｗにおける目標動作量に基づいて、作業座標系Σ＿ｗにおける力センサー２２が移動すべき位置及び姿勢を算出する。そして、ロボット制御部４９は、力センサー２２の位置及び姿勢を、算出された作業座標系Σ＿ｗにおける力センサー２２が移動すべき位置及び姿勢に一致させるようにロボット２０を制御する。

以下、図５を参照することにより、制御装置３０が、操作対象Ｍと組付対象Ｏを組み付けるようにロボット２０を動作させるための処理について説明する。図５は、制御装置３０が、操作対象Ｍと組付対象Ｏを組み付けるようにロボット２０を動作させるための処理の流れの一例を示すフローチャートである。

まず、制御装置３０は、入力受付部３３を介してユーザーから入力された位置と姿勢であって、操作対象Ｍと組付対象Ｏの組み付けが完了した状態の操作対象座標系Σ＿ｍにおける組付対象Ｏの位置ｐ＿ｏ＾ｍ（→）と姿勢ｏ＿ｏ＾ｍ（→）を設定する（ステップＳ１００）。
次に、制御装置３０は、入力受付部３３を介してユーザーから入力された回転中心位置と回転中心姿勢であって、操作対象座標系Σ＿ｍにおける回転中心位置ｐ＿ｅ＾ｍ（→）と回転中心姿勢ｏ＿ｅ＾ｍ（→）を設定する（ステップＳ１１０）。

ステップＳ１００からステップＳ１１０までが、ユーザーにより行われる制御装置３０への初期設定である。ここから、ステップＳ１１０の後に、撮像部１０による撮像が開始され、制御部３６が画像取得部３５から撮像画像を取得したものとして説明する。
制御部３６が撮像画像を取得した後、制御装置３０の目標導出部３９は、画像取得部３５により取得された撮像画像に基づいて、撮像部座標系Σ＿ｃにおける組付対象Ｏの位置ｐ＿ｏ＾ｃ（→）と姿勢ｏ＿ｏ＾ｃ（→）を検出する（ステップＳ１２０）。

次に、目標導出部３９は、ステップＳ１００で設定された操作対象Ｍと組付対象Ｏとの組み付けが完了した状態の操作対象座標系Σ＿ｍにおける組付対象Ｏの位置ｐ＿ｏ＾ｍ（→）及び姿勢ｏ＿ｏ＾ｍ（→）と、ステップＳ１２０で検出された撮像部座標系Σ＿ｃにおける組付対象Ｏの位置ｐ＿ｏ＾ｃ（→）及び姿勢ｏ＿ｏ＾ｃ（→）に基づいて、以下に示した式（７）から、操作対象Ｍと組付対象Ｏの組み付けが完了した状態の操作対象Ｍの目標位置と姿勢であって、撮像部座標系Σ＿ｃにおける操作対象Ｍの目標位置ｐ＿ｍ＾ｃ（ｄ）（→）と姿勢ｏ＿ｍ＾ｃ（ｄ）（→）を算出する（ステップＳ１３０）。

ここで、撮像部座標系Σ＿ｃにおける操作対象Ｍの目標位置ｐ＿ｍ＾ｃ（ｄ）（→）及び姿勢ｏ＿ｍ＾ｃ（ｄ）（→）の「（ｄ）」は、ステップＳ１４０で回転中心位置算出部４１により検出される撮像部座標系Σ＿ｃにおける操作対象Ｍの位置ｐ＿ｍ＾ｃ（→）及び姿勢ｏ＿ｍ＾ｃ（→）と区別するために付記したラベルである。

ステップＳ１２０からステップＳ１３０までの処理は、制御部３６が、撮像部１０と組付対象Ｏの相対的な位置及び姿勢と、操作対象Ｍと組付対象Ｏの組み付けが完了した状態の操作対象Ｍと組付対象Ｏの相対的な位置関係（位置及び姿勢）に基づいて、操作対象Ｍと組付対象Ｏの組み付けが完了した状態の操作対象Ｍと、撮像部１０の相対的な位置及び姿勢を間接的に算出するための処理である。

次に、回転中心位置算出部４１は、取得された撮像画像から、撮像部座標系Σ＿ｃにおける操作対象Ｍの位置ｐ＿ｍ＾ｃ（→）と姿勢ｏ＿ｍ＾ｃ（→）を検出する（ステップＳ１４０）。

次に、回転中心位置算出部４１は、取得された撮像画像から、ツール座標系Σ＿ｔにおける撮像部１０の位置ｐ＿ｃ＾ｔ（→）と姿勢ｏ＿ｃ＾ｔ（→）を検出する（ステップＳ１５０）。

次に、回転中心位置算出部４１は、ステップＳ１４０で算出された撮像部座標系Σ＿ｃにおける操作対象Ｍの位置ｐ＿ｍ＾ｃ（→）及び姿勢ｏ＿ｍ＾ｃ（→）と、ステップＳ１５０で算出されたツール座標系Σ＿ｔにおける撮像部１０の位置ｐ＿ｃ＾ｔ（→）及び姿勢ｏ＿ｃ＾ｔ（→）に基づいて、以下に示した式（８）から、ツール座標系Σ＿ｔにおける回転中心の位置ｐ＿ｅ＾ｔ（→）と姿勢ｏ＿ｅ＾ｔ（→）を算出する（ステップＳ１６０）。

ここで、ステップＳ１６０の処理は、力センサー２２と上述の回転中心位置及び回転中心姿勢の相対的な位置関係の変化を検出する処理である。ステップＳ１６０において算出されたツール座標系Σ＿ｔにおける回転中心の位置ｐ＿ｅ＾ｔ（→）と姿勢ｏ＿ｅ＾ｔ（→）が、前のルーチンで算出された値と異なることは、力センサー２２と回転中心位置及び回転中心姿勢の相対的な位置関係が、外力によって変化させられたことを示す。制御部３６は、この変化を検出することで、外力によって力センサー２２に対する回転中心位置及び回転中心姿勢が変化されたとしても、変化された後の力センサー２２に対する回転中心位置及び回転中心姿勢に基づいて、ロボット２０が常に回転中心位置を中心として、回転中心姿勢を基準に操作対象Ｍを回転するように制御することができる。

次に、回転中心位置算出部４１は、順運動学に基づいて、作業座標系Σ＿ｗにおける力センサー２２の位置ｐ＿ｔ＾ｗ（→）と姿勢ｏ＿ｔ＾ｗ（→）を算出する（ステップＳ１７０）。
次に、回転中心位置算出部４１は、ステップＳ１６０で算出されたツール座標系Σ＿ｔにおける回転中心位置ｐ＿ｅ＾ｔ（→）と回転中心姿勢ｏ＿ｅ＾ｔ（→）と、ステップＳ１７０で算出された作業座標系Σ＿ｗにおける力センサー２２の位置ｐ＿ｔ＾ｗ（→）及び姿勢ｏ＿ｔ＾ｗ（→）に基づいて、以下に示した式（９）から、外力座標系Σ＿ｅにおけるロボット２０の支持台の位置ｐ＿ｗ＾ｅ（→）と姿勢ｏ＿ｗ＾ｅ（→）を算出する（ステップＳ１８０）。

ここで、制御部３６は、ステップＳ１８０の処理によって、外力座標系Σ＿ｅにおけるロボット２０の支持台の位置ｐ＿ｗ＾ｅ（→）と姿勢ｏ＿ｗ＾ｅ（→）を算出することで、力センサー２２に働いた力及びモーメントにより回転中心位置に働いた力及びモーメントを、次のステップＳ１９０の処理で算出することができる。

次に、外力算出部４３は、力センサー２２により検出された力とモーメントであって、ツール座標系Σ＿ｔにおける力ｆ＾ｔ（→）とモーメントｍ＾ｔ（→）に基づいて、以下に示したニュートン・オイラー方程式（１０）から、外力座標系Σ＿ｅにおける力（すなわち、回転中心位置に働いた力）ｆ＾ｅ（→）とモーメント（すなわち、回転中心位置に生じたモーメント）ｍ＾ｅ（→）を算出する（ステップＳ１９０）。

ここで、上記の式（１０）における上段式は、操作対象Ｍの回転中心位置に働いた外力の成分ｆ＾ｅ（→）が、力センサー２２によって検出された力の成分ｆ＾ｔ（→）から、重力による成分ｍＥ（＾）ｇ（→）とロボット２０のアーム部ＡＲＭによる慣性運動による成分ｍＥ（＾）ｐ＿ｃ＾ｗ（→）（・・）を差し引いた成分で表されることを示している。なお、「（・・）」が付記された一番手前の文字は、時間について二回微分された変数を表している。また、行列Ｅ（＾）は、単位行列であり、ベクトルｇ（→）は、重力加速度を表すベクトルであり、スカラーｍは、操作対象Ｍの質量を表す。

前記の上段式と同様に、下段式は、操作対象Ｍに働いた外力により回転中心に生じたモーメントｍ＾ｅが、ねじりモーメントの成分Ｉ（＾）ｏ＿ｃ＾ｗ（→）（・・）と、コリオリの力によるモーメントの成分ｏ＿ｃ＾ｗ（→）（・）×Ｉ（＾）ｏ＿ｃ＾ｗ（→）（・）と、力センサー２２により検出されたモーメントｍ＾ｔ（→）と、力センサー２２によって検出された力の成分ｆ＾ｔ（→）によるモーメントの成分ｐ＿ｅ＾ｔ（→）×ｆ＾ｔ（→）を加算したものから、重力によるモーメントの成分ｐ＿ｇ＾ｔ（→）×ｍＲ＿ｗ＾ｔ（＾）ｇ（→）を差し引いた成分で表されることを示している。

次に、負荷緩和動作量算出部４５は、ステップＳ１９０で算出された外力座標系Σ＿ｅにおける力ｆ＾ｅ（→）及びモーメントｍ＾ｅ（→）に基づいて、以下に示した式（１１）から、外力座標系Σ＿ｅにおける操作対象Ｍの緩和動作量の微小緩和移動量Δｐ＾ｅ（→）と微小緩和回転量Δｏ＾ｅ（→）を算出する（ステップＳ２００）。

ここで、ステップＳ２００で用いられる式（１１）のΔＰ＾ｅ（→）及びΔＯ＾ｅ（→）は、それぞれΔｐ＾ｅ（→）及びΔｏ＾ｅ（→）をラプラス変換したものである。また、式（１１）は、操作対象Ｍに設定されている回転中心位置が外力を受けたときに、その外力を緩和させる方向に移動するべき動作量（微小移動量及び微小回転量）を決定する運動モデルに依って異なる。本実施形態では、上記の式（１１）に示したように、微小移動量に関する慣性質量行列Ｍ＿ｐ（＾）、ダンパー係数行列Ｄ＿ｐ（＾）、バネ乗数行列Ｋ＿ｐ（＾）と、微小回転量に関する慣性質量行列Ｍ＿ｏ（＾）、ダンパー係数行列Ｄ＿ｏ（＾）、バネ乗数行列Ｋ＿ｏ（＾）とによって表される運動モデルを採用している。このような運動モデルによって行われる制御は、コンプライアントモーション制御のうちインピーダンス制御と呼ばれる。なお、スカラーｓは、ラプラス変換に用いる変数である。コンプライアントモーション制御としては、インピーダンス制御に限定されるものではなく、例えばスティフネス制御やダンピング制御を適用しても良い。

次に、負荷緩和動作量算出部４５は、ステップＳ２００で算出された外力座標系Σ＿ｅにおける操作対象Ｍの緩和動作量に基づいて、以下に示した式（１２）から、作業座標系Σ＿ｗにおける力センサー２２の緩和動作量として微小緩和移動量Δｐ＿ｔ＾ｗ（ｉ）（→）と微小緩和回転量Δｏ＿ｔ＾ｗ（ｉ）（→）を算出する（ステップＳ２１０）。

ここで、緩和動作量である微小緩和移動量Δｐ＿ｔ＾ｗ（ｉ）（→）と微小緩和回転量Δｏ＿ｔ＾ｗ（ｉ）（→）における「（ｉ）」は、緩和動作量と目標動作量を区別するためのラベルである。制御部３６のロボット制御部４９は、ステップＳ２００からステップＳ２１０までの処理により負荷緩和動作量算出部４５が算出した作業座標系Σ＿ｗにおける緩和動作量に基づいて、回転中心位置に働いた力及びモーメントが緩和される位置まで、力センサー２２の位置及び姿勢を移動するようにロボット２０を制御することができる。

次に、移動動作量算出部４６は、ステップＳ１３０で算出された操作対象Ｍと組付対象Ｏの組み付けが完了した状態の撮像部座標系Σ＿ｃにおける操作対象Ｍの目標位置ｐ＿ｍ＾ｃ（ｄ）（→）及び姿勢ｏ＿ｍ＾ｃ（ｄ）（→）と、ステップＳ１４０で検出された撮像部座標系Σ＿ｃにおける操作対象Ｍの位置ｐ＿ｍ＾ｃ（→）と姿勢ｏ＿ｍ＾ｃ（→）に基づいて、以下に示した式（１３）から、操作対象Ｍを目標位置及び姿勢に移動させるための目標動作量であって、撮像部座標系Σ＿ｃにおける目標動作量として微小目標移動量Δｐ＿ｍ＾ｃ（→）と微小目標回転量Δｏ＿ｍ＾ｃ（→）を算出する（ステップＳ２２０）。

次に、移動動作量算出部４６は、ステップＳ２２０で算出された撮像部座標系Σ＿ｃにおける操作対象Ｍの目標動作量に基づいて、以下に示した式（１４）から、作業座標系Σ＿ｗにおける力センサー２２の目標動作量として微小目標移動量Δｐ＿ｔ＾ｗ（ｖ）（→）と微小目標回転量Δｏ＿ｔ＾ｗ（ｖ）（→）を算出する（ステップＳ２３０）。

ここで、目標動作量である微小目標移動量Δｐ＿ｔ＾ｗ（ｖ）（→）と微小目標回転量Δｏ＿ｔ＾ｗ（ｖ）（→）における「（ｖ）」は、目標動作量と緩和動作量を区別するためのラベルである。

次に、動作終了判定部４７は、ステップＳ２１０で算出された緩和動作量と、ステップＳ２３０で算出された目標動作量を、以下に示した式（１５）のように加算した加算微小移動量Δｐ＿ｔ＾ｗ（→）及び加算微小回転量Δｏ＿ｔ＾ｗ（→）を算出する（ステップＳ２４０）。

次に、動作終了判定部４７は、ステップＳ２４０で算出された加算微小移動量Δｐ＿ｔ＾ｗ（→）及び加算微小回転量Δｏ＿ｔ＾ｗ（→）の両方が、それぞれに対する所定の閾値未満か否かを判定する（ステップＳ２５０）。なお、前記のそれぞれに対する所定の閾値は、それぞれが別に設定される。動作終了判定部４７は、加算微小移動量Δｐ＿ｔ＾ｗ（→）及び加算微小回転量Δｏ＿ｔ＾ｗ（→）の両方が、それぞれに対する所定の閾値未満であると判定した場合（ステップＳ２５０−Ｙｅｓ）、操作対象Ｍをこれ以上動かす必要がないと判断して処理を終了する。

一方、動作終了判定部４７が加算微小移動量Δｐ＿ｔ＾ｗ（→）及び加算微小回転量Δｏ＿ｔ＾ｗ（→）のうちいずれか一方又は両方が、それぞれに対する所定の閾値未満ではないと判定した場合（ステップＳ２５０−Ｎｏ）、ロボット制御部４９は、ステップＳ２１０で算出された緩和動作量と、ステップＳ２３０で算出された目標動作量とに基づいて、以下に示した式（１６）から、作業座標系Σ＿ｗにおける力センサー２２の目標位置ｐ＿ｔ＾ｗ（ｄ）（→）及び姿勢ｏ＿ｔ＾ｗ（ｄ）（→）を算出する（ステップＳ２６０）。

なお、動作終了判定部４７は、ステップＳ２４０で算出された加算微小移動量Δｐ＿ｔ＾ｗ（→）及び加算微小回転量Δｏ＿ｔ＾ｗ（→）の両方が、それぞれに対する所定の閾値未満か否かを判定する構成に代えて、ステップＳ２３０で算出された微小目標移動量Δｐ＿ｔ＾ｗ（ｖ）（→）と微小目標回転量Δｏ＿ｔ＾ｗ（ｖ）（→）の両方が、それぞれに対する所定の閾値未満であるか否かを判定する構成であってもよい。また、動作終了判定部４７は、ロボット２０に行わせる所定の作業の性質に応じて、Δｐ＿ｔ＾ｗ（→）（ｉ）、Δｏ＿ｔ＾ｗ（ｉ）、Δｐ＿ｔ＾ｗ（→）（ｖ）及びΔｏ＿ｔ＾ｗ（ｖ）で構成される任意の関数を用いて、前記関数から得られる値が所定の閾値未満か否か（操作対象Ｍをこれ以上動かす必要があるか否か）を判定する構成であってもよい。

次に、ロボット制御部４９は、力センサー２２の位置及び姿勢が、ステップＳ２６０で算出された作業座標系Σ＿ｗにおける力センサー２２の目標位置ｐ＿ｔ＾ｗ（ｄ）（→）及び姿勢ｏ＿ｔ＾ｗ（ｄ）（→）と一致するように、力センサー２２を移動させるようにロボット２０を制御する（ステップＳ２７０）。制御部３６は、ステップＳ１２０〜ステップＳ２７０の処理を、ステップＳ２５０の判定によって操作対象Ｍをこれ以上動かす必要がないと判断されるまで繰り返すことで、所定の作業をロボット２０が行うように制御する。

なお、回転中心位置算出部４１は、ステップＳ１５０からステップＳ１８０までの処理において、例えば、力センサー２２がアーム部ＡＲＭの内部に備えられ、外部から見えず、撮像部１０により力センサー２２を撮像することができない場合、上述した力センサー２２の位置を示すマーカーを備える構成としてもよく、ユーザーにより設定された力センサー２２と回転中心位置及び回転中心姿勢の初期状態における相対的な位置関係に基づいて、以下に示した式（１７）〜式（１９）から、外力座標系Σ＿ｅにおける力センサー２２の位置ｐ＿ｔ＾ｅ（→）と姿勢ｏ＿ｔ＾ｅ（→）を逐次計算する構成としてもよい。

上記の式（１７）〜式（１９）は、撮像画像から検出される操作対象Ｍの移動量から、ロボット２０自身が動いた量を差し引くことで、力センサー２２と操作対象Ｍの相対的な位置関係がずれた量を算出する式である。なお、ロボット２０自身が動いた量は、制御部３６によりロボット２０の初期状態に基づいて算出される。ユーザーにより設定された力センサー２２と回転中心位置及び回転中心姿勢の初期状態における相対的な位置関係及び上記の式（１７）〜（１９）を用いる構成は、操作対象Ｍと力センサー２２が撮像部１０の撮像範囲に収まらない場合、すなわち、力センサー２２の位置を示すマーカーを用いていても力センサー２２の位置を検出できない場合であっても、操作対象Ｍの位置及び姿勢と力センサー２２の位置及び姿勢を追跡できるため、有用である。

回転中心位置算出部４１は、上記の式（１７）〜式（１９）を用いた逐次計算をすることによって、撮像部１０により力センサー２２を撮像することができない場合であっても、撮像部１０により力センサー２２を撮像できる場合と同様にステップＳ１５０からステップＳ１８０の処理を行うことができる。その結果として、制御部３６は、力センサー２２と操作対象Ｍに設定された所定位置との相対的な位置関係に基づいて、ロボット２０が所定の作業を行うようにロボット２０を制御することができる。なお、撮像部１０により力センサー２２が撮像することができない場合とは、例えば、力センサー２２がアーム部ＡＲＭの部材に覆われていた場合や、撮像部１０の画角から力センサー２２を含むアーム部ＡＲＭが外れてしまうような場合等である。

以上説明したように、第１の実施形態に係るロボットシステム１のロボット２０は、把持部ＨＮＤと操作対象Ｍとを含む撮像画像に基づいて、把持部ＨＮＤによって、操作対象Ｍの回転中心位置を中心に操作対象Ｍを回転させ、操作対象Ｍを組付対象Ｏに対して相対移動させる。このため、ロボットは、良好な組み立て作業を行うことができる。

また、ロボット２０は、回転させる場合の回転中心を操作対象Ｍとともに動く座標原点として、回転と並進によって操作対象Ｍを組付対象Ｏに対して相対移動させる。このため、ロボットは、良好な組み立て作業を行うことができる。
また、ロボット２０は、操作対象Ｍを組付対象Ｏに対して逐次相対移動させることができるため、その結果として、精度の高い良好な組み立て作業を行うことができる。

また、ロボット２０は、所定位置及び各軸方向に設定された運動特性に従ってコンプライアントモーション制御を行う。このため、ロボット２０は、組付対象Ｏを破壊することなく、組付対象Ｏに対して操作対象Ｍを組み付けることができる。
また、ロボット２０は、撮像画像に基づいて、把持部ＨＮＤに設定された位置と操作対象Ｍに設定された位置との相対的な位置関係を導出し、導出された位置関係に基づいて、回転中心位置を更新する。このため、ロボット２０は、外力によって把持部ＨＮＤと回転中心の位置関係がずらされても、ずらされた回転中心を中心に操作対象Ｍを組付対象Ｏに対して相対移動させることができ、その結果、良好な組み立て作業を行うことができる。

また、ロボット２０は、把持部ＨＮＤに設定された位置と操作対象Ｍに設定された位置との相対的な位置関係を導出し、導出された位置関係と、操作対象Ｍに設定された位置と回転中心位置との相対的な位置関係とに基づいて、回転中心位置を更新する。このため、ロボット２０は、外力によって把持部ＨＮＤと回転中心位置の位置関係がずらされても、操作対象Ｍに設定された位置を介して、間接的に把持部ＨＮＤに設定された位置と回転中心位置との相対的な位置関係から、ずらされた回転中心位置を認識することができ、その結果、ずらされた回転中心位置を中心に操作対象Ｍを組付対象Ｏに対して相対移動させることができる。

また、ロボット２０は、撮像画像から検出された力センサー２２の位置を示すマーカーの位置に基づいて、把持部ＨＮＤに設定された位置と回転中心位置の相対的な位置関係を導出し、導出された把持部ＨＮＤに設定された位置と回転中心位置の相対的な位置関係に基づいて、回転中心位置を更新する。このため、ロボット２０は、力センサー２２がロボット２０のアーム部ＡＲＭの部材に覆われていた場合であっても、力センサー２２を示すマーカーの位置を目印として、操作対象Ｍの位置を認識することができ、その結果、良好な組み立て作業を行うことができる。

また、ロボット２０は、力センサー２２の初期状態における位置及び姿勢に基づいて、把持部ＨＮＤに設定されたと回転中心位置の相対的な位置関係を導出し、導出された把持部ＨＮＤに設定された位置と回転中心位置の相対的な位置関係に基づいて、回転中心位置を更新する。このため、ロボット２０は、撮像部１０の画角から、力センサー２２を含むロボット２０のアーム部ＡＲＭが外れてしまう場合であっても、操作対象Ｍの位置を認識することができ、その結果、良好な組み立て作業を行うことができる。

＜第２の実施形態＞
以下、本発明の第２の実施形態について、図面を参照して説明する。第２の実施形態に係るロボットシステム２は、ロボット２０として単腕ロボットを具備する構成に代えて、ロボット２５として双腕ロボットを具備する構成である。なお、第２の実施形態では、第１の実施形態と同様な構成部には、同じ符号を付してある。

図６は、第２の実施形態に係るロボットシステム２の一例を示す構成図である。ロボットシステム２は、撮像部１０と、ロボット２５と、制御装置３０とを具備する。
第２の実施形態において、組付対象Ｏは、テーブル等の台の上に治具等によって設置されており、双腕ロボットであるロボット２５のいずれか一方の腕によって、第１の実施形態で説明した所定の作業により操作対象Ｍが組み付けられる。なお、組付対象Ｏは、ロボット２５の把持部ＨＮＤ２によって把持され、把持部ＨＮＤ１によって第１の実施形態で説明した所定の作業が行われるとしてもよい。また、この場合、把持部ＨＮＤ１と把持部ＨＮＤ２の役割が逆であってもよい。

ロボット２５は、例えば、把持部ＨＮＤ１と、把持部ＨＮＤ２と、力センサー２２と、アーム部ＡＲＭ１と、アーム部ＡＲＭ２と、図示しない複数のアクチュエーターとを、図６に示したように、それぞれの腕に備えた双腕ロボットである。

ロボット２５の各腕は、６軸垂直多関節型となっており、一方の腕が支持台とアーム部ＡＲＭ１と把持部ＨＮＤ１とがアクチュエーターによる連携した動作よって６軸の自由度の動作を行うことができ、他方の腕が支持台とアーム部ＡＲＭ２と把持部ＨＮＤ２とがアクチュエーターによる連携した動作よって６軸の自由度の動作を行うことができる。なお、ロボット２０の各腕は、５自由度（５軸）以下で動作するものであってもよいし、７自由度（７軸）以上で動作するものであってもよい。

ロボット２５は、第１の実施形態において説明した制御装置３０によって制御される動作を、把持部ＨＮＤ１及びアーム部ＡＲＭ１を備えた腕によって行うとするが、把持部ＨＮＤ２及びアーム部ＡＲＭ２を備えた腕によって同様の動作が行われてもよい。なお、把持部ＨＮＤ１、把持部ＨＮＤ２は、それぞれハンドの一例である。ロボット２５は、例えばケーブルによって制御装置３０と通信可能に接続されている。ケーブルを介した有線通信は、例えば、イーサネット（登録商標）やＵＳＢ等の規格によって行われる。なお、ロボット２５と制御装置３０とは、Ｗｉ−Ｆｉ（登録商標）等の通信規格により行われる無線通信によって接続されてもよい。

また、ロボット２５は、図６に示したようにロボット２５がロボット２５の内部に搭載された制御装置３０によって制御される構成とするが、この構成に代えて、制御装置３０がロボット２５の外部に設置されている構成であってもよい。

以上説明したように、第２の実施形態に係るロボットシステム２のロボット２５は、双腕ロボットであり、双腕ロボットの２つの腕のうちいずれか一方又は両方によって、第１の実施形態で説明した所定の作業を行うため、第１の実施形態と同様の効果を得ることができる。

＜第３の実施形態＞
以下、本発明の第３の実施形態について、図面を参照して説明する。なお、第３の実施形態では、第１の実施形態と同様な構成部には、同じ符号を付してある。第３の実施形態に係るロボットシステム１は、例えば、所定の作業として、把持部ＨＮＤにより把持されたレンチ（操作対象Ｍの一例）をボルト（組付対象Ｏの一例）に組み付け、その後、把持されたレンチによってボルトを締める。

ここで、図７を参照して、第３の実施形態に係るロボットシステム１が行う所定の作業を説明する。図７は、第３の実施形態に係るロボットシステム１によって行われる所定の作業の一例を模式的に示す図である。図７（Ａ）には、ロボット２０により把持されたレンチＭ１がボルトＯ１に組み付けられる前の所定の初期位置の状態を示す。ロボットシステム１は、図７（Ａ）における一点鎖線で示したレンチＶＭ１の位置（ボルトＯ１の直前の位置）までレンチＭ１を移動させる。ここで、ロボットシステム１には、レンチＶＭ１の状態を示す画像が予め設定されており、ロボットシステム１は、その状態を実現するように第１の実施形態において図５を参照して説明した処理によって把持部ＨＮＤを動かす。図７（Ｂ）には、ロボット２０によりレンチＭ１がボルトＯ１に組み付けられた状態を示す。ロボットシステム１は、前記の図５を参照して説明した処理によって、図７（Ａ）に示したレンチＶＭ１の状態から、図７（Ｂ）により示されるレンチＭ１とボルトＯ１が組み付けられた状態まで把持部ＨＮＤを動かす。ここで、ロボットシステム１には、図７（Ｂ）に示した状態のレンチＭ１とボルトＯ１を示す画像が予め設定されており、ロボットシステム１は、その状態を実現するように第１の実施形態において図５を参照して説明した処理によって把持部ＨＮＤを動かす。

図７（Ｃ）には、ロボット２０に把持されたレンチＭ１によりボルトＯ１が６０°回転させられる様子を示す。ロボットシステム１には、回転させた状態（例えば、図７（Ｃ）において一点鎖線で示したレンチＶＭ２とボルトＶＯ１の状態）を示す画像が予め設定されており、ロボットシステム１は、その状態を実現するように把持部ＨＮＤを前記の図５を参照して説明した処理に基づいて動かす。なお、図７（Ｂ）において、ボルトＯ１とボルトＶＯ１は、正六角形のため、重なって示されている。

図７（Ｄ）には、ロボット２０に把持されたレンチＭ１によりボルトＯ１が回転させられた状態を示す。ロボットシステム１は、図７（Ｄ）に示したレンチＭ１によりボルトＯ１が回転させられた状態になった時、レンチＭ１をボルトＯ１から所定の距離だけ離れた位置に離す。ここで、ロボットシステム１には、レンチＭ１がボルトＯ１から所定の距離だけ離れた位置に移動した状態を示す画像が予め設定されており、ロボットシステム１は、その状態を実現するように第１の実施形態において図５を参照して説明した処理によって把持部ＨＮＤを動かす。そして、ロボットシステム１は、レンチＭ１がボルトＯ１から所定の距離だけ離れた位置から初期位置まで移動した状態となるまで把持部ＨＮＤを動かす。ロボットシステム１は、図７（Ａ）〜図７（Ｄ）に示した動作を行う処理を繰り返すことで、レンチＭ１によってボルトＯ１を締める。

以下、図８を参照することにより、制御装置３０の制御部３６が、レンチＭ１によりボルトＯ１を締めるようにロボット２０を動作させるための処理について説明する。図８は、制御装置３０の制御部３６が、レンチＭ１によりボルトＯ１を締めるようにロボット２０を動作させるための処理の流れの一例を示すフローチャートである。

以下では、ロボットシステム１は、図５に示したステップＳ１００からステップＳ１１０までの処理をすでに行ったものとして説明する。まず、制御部３６は、初期配置処理によって、把持部ＨＮＤに把持されたレンチＭ１が前述の所定の初期位置に配置されるまでロボット２０の把持部ＨＮＤを動かす（ステップＳ３００）。ここで、初期配置処理とは、図５に示したステップＳ１２０からステップＳ２７０までの処理を示す。

次に、制御部３６は、組み付け処理によってレンチＭ１とボルトＯ１を組み付ける（ステップＳ３１０）。ここで、組み付け処理とは、図５に示したステップＳ１２０からステップＳ２７０までの処理によって、レンチＭ１とボルトＯ１が図７（Ｂ）に示した状態となるまで、把持部ＨＮＤを動かしてレンチＭ１とボルトＯ１を組み付ける処理を示す。

次に、制御部３６は、ボルト締付準備処理によって、レンチＭ１とボルトＯ１が図７（Ｄ）に示した状態となるまでレンチＭ１を把持している把持部ＨＮＤを動かす準備を行う（ステップＳ３２０）。ここで、ボルト締付準備処理とは、図５に示したステップＳ１２０からステップＳ２４０までの処理によって把持部ＨＮＤを動かしてレンチＭ１によりボルトＯ１を回転させるための加算微小移動量と加算微小回転量を算出する処理を示す。

次に、制御部３６は、ステップＳ３２０において上記の式（１０）から算出したモーメント（締め付けトルクに相当）が所定値以上か否かを判定することで、ボルトＯ１の締め付けが完了したか否かを判定する（ステップＳ３３０）。制御部３６は、算出したモーメントが所定値以上と判定した場合（ステップＳ３３０−Ｙｅｓ）、ボルトＯ１の締め付けが完了したと判定し、処理を終了する。一方、制御部３６は、算出したモーメントが所定値未満と判定した場合（ステップＳ３３０−Ｎｏ）、ステップＳ３２０において上記の式（１５）から算出した加算微小移動量及び加算微小回転量の両方が、それぞれに対する所定の閾値未満か否かを判定する（ステップＳ３４０）。

加算微小移動量及び加算微小回転量の両方が、それぞれに対する所定の閾値以上と判定した場合（ステップＳ３４０−Ｎｏ）、制御部３６は、レンチＭ１とボルトＯ１が図７（Ｄ）に示した状態に達していないと判定し、ステップＳ３２０で算出された緩和動作量及び目標動作量とに基づいて、作業座標系Σ＿ｗにおける力センサー２２の目標位置及び姿勢を算出する（ステップＳ３６０）。次に、制御部３６は、力センサー２２の位置及び姿勢が、ステップＳ３６０で算出された作業座標系Σ＿ｗにおける力センサー２２の目標位置及び姿勢と一致するように、力センサー２２を移動させるようにロボット２０を制御することで、ボルトＯ１をレンチＭ１により更に回転させる（ステップＳ３７０）。なお、ステップＳ３６０は、ステップＳ２６０と同様の処理であり、ステップＳ３７０は、ステップＳ２７０と同様の処理であるため、これらの詳細な説明を省略する。一方、加算微小移動量及び加算微小回転量の両方が、それぞれに対する所定の閾値未満と判定した場合（ステップＳ３４０−Ｙｅｓ）、制御部３６は、レンチＭ１とボルトＯ１が図７（Ｄ）に示した状態に達したと判定し、レンチＭ１をボルトＯ１から離し（ステップＳ３５０）、その後、ステップＳ３００の処理を行う。

以上説明したように、第３の実施形態に係るロボットシステム１は、第１の実施形態において図５で説明した処理と、締め付けトルクに相当するモーメントに応じたステップＳ３３０の処理とに基づいて、ビジュアルサーボによる制御と、力センサーによるコンプライアントモーション制御を組み合わせ、把持部ＨＮＤにより把持したレンチＭ１によってボルトＯ１を確実に締めることができる。

なお、以上に説明した装置（例えば、ロボットシステム１、２）における任意の構成部の機能を実現するためのプログラムを、コンピューター読み取り可能な記録媒体に記録し、そのプログラムをコンピューターシステムに読み込ませて実行するようにしてもよい。なお、ここでいう「コンピューターシステム」とは、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）や周辺機器等のハードウェアを含むものとする。また、「コンピューター読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｋ）−ＲＯＭ等の可搬媒体、コンピューターシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピューター読み取り可能な記録媒体」とは、インターネット等のネットワークや電話回線等の通信回線を介してプログラムが送信された場合のサーバーやクライアントとなるコンピューターシステム内部の揮発性メモリー（ＲＡＭ：Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）のように、一定時間プログラムを保持しているものも含むものとする。

また、上記のプログラムは、このプログラムを記憶装置等に格納したコンピューターシステムから、伝送媒体を介して、あるいは、伝送媒体中の伝送波により他のコンピューターシステムに伝送されてもよい。ここで、プログラムを伝送する「伝送媒体」は、インターネット等のネットワーク（通信網）や電話回線等の通信回線（通信線）のように情報を伝送する機能を有する媒体のことをいう。
また、上記のプログラムは、前述した機能の一部を実現するためのものであってもよい。さらに、上記のプログラムは、前述した機能をコンピューターシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

１、２ ロボットシステム、１０ 撮像部、２０、２５ ロボット、２２ 力センサー、３０ 制御装置、３１ ＣＰＵ、３２ 記憶部、３３ 入力受付部、３４ 通信部、３５ 画像取得部、３６ 制御部、３９ 目標導出部、４１ 回転中心位置算出部、４３ 外力算出部、４５ 負荷緩和動作導出部、４６ 移動動作量算出部、４７ 動作終了判定部、４９ ロボット制御部

Claims (12)

1. ハンドと、
   前記ハンドを動作させる制御部と、
   を含み、
   前記制御部は、
   前記ハンドと第１物体とを含む撮像画像に基づいて、前記ハンドによって、前記第１物体の所定位置を中心に前記第１物体を回転させ、前記第１物体を第２物体に対して相対移動させる、
   ロボット。
2. 請求項１に記載のロボットであって、
   前記所定位置は、前記第１物体とともに動く座標原点であり、
   前記制御部は、前記第１物体を回転させることに加えて、前記第１物体を並進させる、
   ロボット。
3. 請求項１又は２に記載のロボットであって、
   前記制御部は、前記撮像画像に基づいて、ビジュアルサーボ制御を行う、
   ロボット。
4. 請求項１から３のうちいずれか一項に記載のロボットであって、
   前記制御部は、前記所定位置及び各軸方向に設定された運動特性に従ってコンプライアントモーション制御を行う、
   ロボット。
5. 請求項１から４のうちいずれか一項に記載のロボットであって、
   前記制御部は、前記撮像画像に基づいて、前記ハンドに設定された位置と前記第１物体に設定された位置との相対的な位置関係を導出し、前記導出された位置関係に基づいて、前記所定位置を更新する、
   ロボット。
6. 請求項５に記載のロボットであって、
   前記制御部は、前記導出された位置関係と、前記第１物体に設定された位置と前記所定位置との相対的な位置関係とに基づいて、前記所定位置を更新する、
   ロボット。
7. 請求項１から６のうちいずれか一項に記載のロボットであって、
   前記ハンドに設定された位置を示すマーカーを備え、
   前記撮像画像は、さらに、前記マーカーを含み、
   前記制御部は、前記撮像画像から検出された前記マーカーの位置に基づいて、前記ハンドに設定された位置と前記所定位置の相対的な位置関係を導出し、前記導出された前記ハンドに設定された位置と前記所定位置の相対的な位置関係に基づいて、前記所定位置を更新する、
   ロボット。
8. 請求項７に記載のロボットであって、
   前記制御部は、前記マーカー及び前記第１物体の位置及び姿勢を検出する際に、前記マーカーが撮像された第１撮像画像と、前記第１物体が撮像された第２撮像画像と、前記第１撮像画像を撮像した第１カメラと前記第２撮像画像を撮像した第２カメラとの間の相対的な位置関係とに基づいて、前記所定位置を更新する、
   ロボット。
9. 請求項１から６のうちいずれか一項に記載のロボットであって、
   前記制御部は、
   前記ハンドに設定された初期状態における位置に基づいて、前記ハンドに設定された位置と前記所定位置の相対的な位置関係を導出し、前記導出された前記ハンドに設定された位置と前記所定位置の相対的な位置関係に基づいて、前記所定位置を更新する、
   ロボット。
10. ハンドを備えるロボットと、
    前記ハンドと第１物体とを撮像する撮像部と、
    を含み、
    前記ロボットは、
    前記撮像部により撮像された前記ハンドと前記第１物体とを含む撮像画像に基づいて、前記第１物体の所定位置を中心に、前記第１物体を回転させて、前記第１物体を第２物体に対して相対移動させる、
    ロボットシステム。
11. ハンドを備えるロボットを動作させる制御装置であって、
    前記ハンドと第１物体とを含む撮像画像に基づいて、前記ロボットによって、前記第１物体の所定位置を中心に前記第１物体を回転させ、前記第１物体を第２物体に対して相対移動させる、
    制御装置。
12. ハンドを備えるロボットを動作させる制御方法であって、
    前記ハンドと第１物体とを含む撮像画像を取得することと、
    前記撮像画像に基づいて、前記第１物体の所定位置を中心に前記第１物体を回転させ、前記第１物体を第２物体に対して相対移動させることと、
    を含む制御方法。

Images