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

Description

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

製造等の作業現場では、組み立て等の作業にロボットを用いることで、人手で行っていた作業の自動化が図られてきた。ロボットによる組み立て作業では、ロボットアームの先端のハンド等で部品等の対象物を把持し、他の対象物に組み付ける動作がなされることがある。そのような動作には、例えば、被挿入物上に挿入物を接触させ、その接触させた状態のまま力制御により挿入物を被挿入物に挿入することがある。このような場合に、被挿入物上に挿入物を接触させる方法としては、例えば、特許文献１に記載されているように、テーブル等に載置された被挿入物である穴を有する対象物に対して、ロボットハンドにより把持した挿入物であるワークを移動させる方法が考えられる。

特開２０１０−１３７２９９号公報

特許文献１に記載の方法では、被挿入物である対象物の穴の位置を事前に把握する必要があるところ、穴の位置を把握する方法として、例えばカメラによって対象物を撮像し、その撮像画像から穴の位置を検出する方法が考えられる。しかし、例えば被挿入物が歯車である場合、歯車の穴の大きさが小さい場合があり、歯車の穴の位置を正確に把握することが困難な場合がある。その結果、歯車の穴から遠い位置に、ロボットハンドにより把持した挿入物である軸を接触させることになり、歯車の穴に軸を挿入するまでに時間が掛かってしまう。また、場合によれば、軸を歯車の穴に近づけることができず、歯車の穴に軸を挿入することができなくなってしまうおそれもある。

（１）本発明の一態様は、第１の対象物の開口部を第２の対象物の突起部に挿入するロボットであって、力センサーと、ハンドと、を含み、前記ハンドにより、前記第１の対象物を把持し、前記第１の対象物の前記開口部を前記第２の対象物の前記突起部に近づける、ロボットである。
この構成によれば、開口部を有する第１の対象物を突起部に近づけるところ、開口部の面積よりも突起部の表面積の方が大きいため突起物を画像等により検出しやすくなり、第１の対象物を第２の対象物に挿入しやすくなる。

（２）本発明の他の態様は、上述したロボットであって、前記第２の対象物は、載置されている。
この構成によれば、第２の対象物は載置されているところ、第２の対象物の突起部を目標位置とすることで、第１の対象物を第２の対象物に挿入しやすくなる。

（３）本発明の他の態様は、上述したロボットであって、前記第１の対象物の開口部を前記第２の対象物を目標として動かす際、前記第１の対象物が前記第２の対象物に初めて接触した位置である当初接触点から所定の方向に変位した位置である変位点を設定し、前記第１の対象物が前記第２の対象物に接触した位置である接触点が前記設定した変位点から前記当初接触点に近づくように前記第２の対象物に接触させながら前記第１の対象物を動かすように動作する。
この構成によれば、全体として第１の対象物の開口部に第２の対象物が含まれる位置を効率的に探索することができる。そのため、第１の対象物の開口部に第２の対象物を効率的に嵌め込むことができる。

（４）本発明の他の態様は、上述したロボット制御装置であって、前記当初接触点と前記変位点との間の距離は、前記第１の対象物の位置の誤差に基づく距離である。
この構成によれば、誤差に基づいて第１の対象物を近づける範囲が定められるため、第１の対象物の開口部に第２の対象物が含まれる位置としての可能性がある位置が探索される。

（５）本発明の他の態様は、上述したロボット制御装置であって、前記接触点が前記変位点と前記当初接触点との間を結ぶ線分から所定の幅を有する領域を網羅する経路上に沿うように前記第１の対象物を動かす。
この構成によれば、第１の対象物の開口部に第２の対象物が含まれる位置として可能性がある位置が漏れなく探索される。

（６）本発明の他の態様は、上述したロボット制御装置であって、前記領域内に前記第２の対象物が検出されなかった場合、前記当初接触点から前記所定の方向とは異なる方向に変位した位置に変位点を再設定し、前記接触点が再設定した変位点から前記当初接触点に近づくように前記第１の対象物を動かすようにロボットの動作を制御する。
この構成によれば、第１の対象物の開口部に第２の対象物が含まれる位置が確実に探索される。

（７）本発明の他の態様は、ロボットと、前記ロボットの動作を制御するロボット制御装置とを備えたロボットシステムであって、前記ロボットは、第１の対象物の開口部を第２の対象物の突起部に挿入するロボットであって、力センサーと、ハンドと、を含み、前記ハンドにより、前記第１の対象物を把持し、前記ロボット制御装置は、前記第１の対象物の前記開口部を前記第２の対象物の前記突起部に近づけるように前記ロボットの動作を制御するロボットシステムである。
この構成によれば、開口部を有する第１の対象物を突起部に近づけるところ、開口部の面積よりも突起部の表面積の方が大きいため突起物を画像等により検出しやすくなり、第１の対象物を第２の対象物に挿入しやすくなる。

（８）本発明の他の態様は、力センサーと、ハンドと、を含み、前記ハンドにより、第１の対象物を把持し、前記第１の対象物の開口部を第２の対象物の突起部に挿入するロボットの動作を、前記第１の対象物の前記開口部を前記第２の対象物の前記突起部に近づけるように制御するロボット制御装置である。
この構成によれば、開口部を有する第１の対象物を突起部に近づけるところ、開口部の面積よりも突起部の表面積の方が大きいため突起物を画像等により検出しやすくなり、第１の対象物を第２の対象物に挿入しやすくなる。

本実施形態に係るロボットシステムの概略図である。 本実施形態に係るロボットシステムの機能構成を示すブロック図である。 本実施形態に係るロボットシステムにおける制御の流れの一例を示すブロック図である。 本実施形態に係るロボット制御処理を示すフローチャートである。 探索領域の一例を示す図である。 探索領域の他の例を示す図である。 探索経路の一例を示す図である。 探索経路の他の例を示す図である。 探索経路のさらに他の例を示す図である。 探索経路のまたさらに他の例を示す図である。 本実施形態に係るロボット制御装置の機能構成の他の例を示す図である。

以下、本発明の実施形態について、図面を参照しながら説明する。
図１は、本実施形態に係るロボットシステム１の概略図である。
ロボットシステム１は、ロボット制御装置１０と、ロボット２０と、１個又は複数（この例では、２個）の撮像装置３０とを備える。ロボット制御装置１０、ロボット２０及び撮像装置３０は、互いに通信することができるように接続される。

ロボット制御装置１０は、ロボット２０の動作を制御する。ロボット制御装置１０は、ロボット２０に対象物として第１の対象物（例えば、歯車Ｗ）を把持させ、その位置及び姿勢を制御する。ここで、ロボット制御装置１０は、第１の対象物の開口部を第２の対象物の突起部（例えば、挿入ピンＡ）を目標として動かす際、前記第１の対象物が前記第２の対象物に初めて接触した位置である当初接触点から所定の方向に変位した位置である変位点を設定し、前記第１の対象物が前記第２の対象物に接触した位置である接触点が前記設定した変位点から前記当初接触点に近づくように前記第２の対象物に接触させながら前記第１の対象物を動かすようにロボットの動作を制御する。

ロボット制御装置１０は、ロボット２０の動作を制御する際、例えば、力制御を実行する。力制御は、ロボット２０の可動部２１（後述）に作用する力が目的とする作業に適合した値に近づくように制御する手法である。また、ロボット制御装置１０は、第１の対象物を第２の対象物に近づける際、その他の制御方式、例えば、ビジュアルサーボを用いてもよい。ビジュアルサーボとは、より具体的には、撮像装置３０で現在撮像された対象物を含む現在画像が示す対象物の位置が、目標画像が示す対象物の位置に近づくようにロボット２０の動作を制御する手法である。ここで、目標画像とは、目標位置に配置された対象物を目標物として含む画像である。

ロボット２０は、ロボット制御装置１０からの制御信号に従って動作し、所定の作業を行う。ロボット２０は、ロボット２０の作業内容は、例えば作業台Ｔ上で作業の対象となる対象物Ｗの開口面に挿入ピンＡの一端が挿入される状態となるように、対象物Ｗを組み付ける作業である。対象物は、ワークとも呼ばれる。
ロボット２０は、１もしくは複数（この例では、２本）のアーム２２と、アーム２２の先端部に設けられたハンド２６と、アーム２２の手首部分に設けられた力覚センサー２５と、を備える。アーム２２の手首部分とはアーム２２の先端部とハンド２６との間の部分である。アーム２２は、１もしくは複数（この例では、左右各６個）のジョイント２３及び１もしくは複数（この例では、左右各５個）のリンク２４を含んで構成される。

ロボット２０は、第１の対象物（例えば、対象物Ｗ）の開口部を第２の対象物の突起（例えば、挿入ピンＡ）部に挿入するロボットであって、力センサー（例えば、力覚センサー２５）と、ハンド（例えば、ハンド２６）と、を含み、ハンドにより、第１の対象物を把持し、第１の対象物の開口部を第２の対象物の突起部に近づける。
力覚センサー２５は、例えば、ハンド２６に作用する力や、モーメントを検出する。力覚センサー２５は、検出した力やモーメントを示すセンサー値をロボット制御装置１０に出力する。センサー値は、例えば、ロボット制御装置１０においてロボット２０の力制御に用いられる。力覚センサー２５は、例えば、６軸力覚センサーである。６軸力覚センサーは、並進３軸方向の力成分と、回転３軸まわりのモーメント成分との６成分を同時に検出することができるセンサーである。並進３軸とは、例えば、３次元直交座標系を形成する互いに直交する３つの座標軸（Ｘ軸、Ｙ軸、Ｚ軸）である。回転３軸とは、例えば、その３つの座標軸のそれぞれである。力覚センサー２５が検出する成分の個数は、６成分に限られず、例えば、３成分（３軸力覚センサー）であってもよい。なお、力覚センサー２５は、力検出部とも呼ばれる。

ハンド２６は、例えば、４個の指状の構成部を備える。指状の構成部を、単に指と呼ぶ。ハンド２６は、指を用いて対象物（例えば、対象物Ｗ）を把持することができる。ここで、ハンド２６は、対象物が有する面の１つを、いずれかの座標軸（例えば、Ｚ軸）に垂直になるように把持することができる。
ハンド２６は、アームの２２の先端部に対して着脱可能であってもよい。ハンド２６は、エンドエフェクターの一種ということができる。つまり、ロボット２０は、ハンド２６に代え、又はハンド２６とともにハンド２６以外の種類のエンドエフェクターを備えてもよい。エンドエフェクターとは、対象物を把持したり、持ち上げたり、吊り上げたり、吸着したり、加工する、等の操作を行う部材である。エンドエフェクターは、ハンドフック、吸盤、等、様々な形態をとることができる。また、エンドエフェクターは、１本のアーム２２について各１個に限らず、２個、又は２個よりも多く備えられてもよい。

アーム２２、力覚センサー２５、及びハンド２６を含むユニットは、可動部２１を形成する。可動部２１は、マニュピレーターとも呼ばれる。図１に示す例では、ロボット２０は、２本の可動部２１を備える。可動部２１は、ジョイント２３やハンド２６等の各部を動作させるための駆動部、例えば、アクチュエーター（図示せず）を備える。アクチュエーターは、例えば、サーボモーターや、エンコーダー、等を備える、エンコーダーは、自部で検出した変位をエンコード（符号化）してエンコーダー値に変換し、変換したエンコーダー値をロボット制御装置に出力する。エンコーダー値は、ロボット制御装置１０においてロボット２０に対するフィードバック制御、等に用いられる。

ロボット２０は、ロボット制御装置１０から入力された制御命令に従って、各ジョイント２３を連動して駆動する。これにより、ロボット制御装置１０は、アーム２２の先端部等に設定された注目位置を所定の可動範囲内で所望の位置に移動させたり、所望の方向に向けたりすることができる。この注目位置を、端点と呼ぶ。端点は、エンドポイントとも呼ばれる。また、ロボット制御装置１０は、ハンド２６を駆動することで、対象物等を把持したり、解放したりすることができる。
なお、端点は、アーム２２の先端部に限られず、例えば、ハンド２６等のエンドエフェクターの先端部であってもよい。

撮像装置３０は、ロボット２０の作業領域を表す画像を撮像し、撮像した画像を示す画像データを生成する。ロボット２０の作業領域は、図１に示す例では、作業台Ｔ上において可動部２１により作業可能な範囲である３次元空間であり、可動部２１の端点を含む領域である。作業台Ｔ上には、それぞれ異なる位置に配置された２台の撮像装置３０が、作業台Ｔ上に配置された挿入ピンＡの一端に向けられている。撮像装置３０は、例えば、可視光カメラ、赤外線カメラ、等である。撮像装置３０は、生成した画像データをロボット制御装置１０に出力する。

図１に示す例では、作業台Ｔ上に１本の挿入ピンＡが鉛直方向に直立している。挿入ピンＡは、それぞれ２段階の半径を有する２段の軸部材である。挿入ピンＡの一端と他端の中間には、半径が変化する段部が形成されている。ここでは、半径が小さい方の端面を一端と呼び、半径が大きい方の端面を他端と呼ぶ。挿入ピンＡは、その他端が作業台Ｔ上に支持され、一端が上方に向けられている。この配置では、一端、段部、他端は、水平である。

ここで、対象物Ｗは、その中心部に開口部を有する平板、例えば、歯車である。挿入ピンＡの小さい方の半径は、対象物Ｗの開口部の半径よりも予め定めた許容誤差だけ小さく、挿入ピンＡの大きい方の半径は、対象物Ｗの開口部の半径よりも大きい。従って、対象物Ｗの開口部が、挿入ピンＡの直上に配置されたとき、対象物Ｗを真下に移動させることによって、開口部に挿入ピンＡが挿入される。そのまま対象部Ｗに対して挿入ピンＡの一端から他端の方向に力が作用されると、対象物Ｗはその主面のうち下方に向けられた面（下面）が挿入ピンＡの段部に接触し、対象物Ｗが挿入ピンＡの段部に支持される。主面とは、平板を形成する平面である。なお、主面のうち上方に向けられた面を上面と呼ぶ。

なお、上述したロボットシステム１の構成は、本実施形態の概要を説明するための一例に過ぎない。即ち、上述したロボットシステム１の構成は、上述した構成に限られず、その他の構成を有していてもよい。
例えば、図１は、ジョイント２３の数（軸数）が左右各６個（６軸）である例を示すが、ジョイント２３の数は各６個よりも少なくてもよいし、各６個よりも多くてもよい。リンク２４の数は、ジョイント２３の数に応じて定められる。また、ジョイント２３、リンク２４、ハンド２６、等の部材の形状、大きさ、配置、構造を適宜変更してもよい。

また、撮像装置３０の設置位置は、図１に示すように作業台Ｔ上に限られず、天井や壁面上であってもよい。また、これらの撮像装置３０に代えて、又はこれに加えて、アーム２２の先端部、胴体部、頭部、等に撮像装置３０を設けてもよい。また、撮像装置３０は、ロボット制御装置１０に代えてロボット２０に接続されてもよい。その場合には、撮像装置３０が生成した画像データは、ロボット２０を介してロボット制御装置１０に入力される。

図２は、本実施形態に係るロボットシステム１の機能構成を示すブロック図である。
ロボット２０は、駆動制御部２００を備える。駆動制御部２００は、ロボット制御装置１０から入力された制御信号と、アクチュエーターで取得されたエンコーダー値及び力覚センサー２５で取得されたセンサー値、等に基づいて、端点の位置が制御信号で指示される目標位置となるように、アクチュエーターを駆動させる。なお、端点の現在位置は、例えば、アクチュエーターにおけるエンコーダー値等から求めることができる。

ロボット制御装置１０は、力制御部１２０と、画像取得部１４０とを備える。力制御部１２０は、センサー情報取得部１２２と、軌道生成部１２４とを備える。

力制御部（制御部）１２０は、ロボット２０の力覚センサー２５からのセンサー情報（力情報やモーメント情報を示すセンサー値）に基づいて、力制御（力覚制御ともいう）を行う。力制御は、ロボットの手先（ハンド２６等）に外から加えられた力を、目的とする作業に適合した値に設定するための位置と力の制御手法である。

なお、力制御を行うためには、ハンド２６等のエンドエフェクターに加わる力やモーメントを検出する必要があるが、エンドエフェクターに加わる力やモーメントを検出する方法は力覚センサーを用いるものに限られない。例えば、アーム２２の各軸トルク値からエンドエフェクターに及ぼす外力を推定することもできる。したがって、力制御を行うためには、直接または間接的にエンドエフェクターに加わる力を取得する手段を、アーム２２が有していればよい。

センサー情報取得部１２２は、ロボット２０の力覚センサー２５から出力されるセンサー情報（検出されたセンサー値など）を取得する。なお、センサー情報取得部１２２は、力検出部と呼ぶこともできる。

軌道生成部１２４は、力制御により端点の移動方向及び移動量を決定する。また、軌道生成部１２４は、端点の移動方向及び移動量に基づいて、各ジョイント２３に設けられた各アクチュエーターの目標角度を決定する。また、軌道生成部１２４は、目標角度だけアーム２２を移動させるような指令値を生成し、ロボット２０へ出力する。なお、軌道生成部１２４が行う軌道の生成処理、目標角度の決定処理、指令値の生成処理等は、一般的な様々な技術を用いることができるため、詳細な説明を省略する。

なお、関節を持つロボット２０では、各関節の角度を決定すると、フォワードキネマティクス処理により端点の位置は一意に決定される。つまり、Ｎ関節ロボットではＮ個の関節角度により１つの目標位置を表現できることになるから、当該Ｎ個の関節角度の組を１つの目標関節角度とすれば、端点の軌道を目標関節角度の集合と考えることができる。よって、軌道生成部１２４から出力される指令値は、位置に関する値（目標位置）であってもよいし、関節の角度に関する値（目標角度）であってもよい。

力制御部１２０は、入力されたセンサー値が、目標位置及び目標姿勢に応じた目標値となるように指令値（ここでは、目標角度）を生成する。
力制御部１２０が行う処理について、第１の対象物（例えば、対象物Ｗ）の開口部の位置を第２の対象物の突起部（例えば、挿入ピンＡ）の一端（例えば、先端）の位置に合わせ、第１の対象物の開口部に第２の対象物が挿入される状態にする場合を例にとって説明する。ここで、第２の対象物の突起部は、作業台Ｔに載置されることで、その位置及び姿勢が固定されているものとする。

力制御部１２０には、時間経過に応じて予め定めた方向、速度で第２の対象物の一端接近する目標位置に応じた目標値が与えられる。
但し、第１の対象物の開口部を第２の対象物の一端に接近させる場合には、ロボット制御装置１０は、例えば、画像取得部１４０で取得した画像データが示す第２の対象物の画像から、例えば、画像認識技術を用いて第２の対象物の軸を検出する制御部を備えてもよい。当該制御部は、検出した軸の位置に第１の対象物を接近させるようにロボット２０の動作を制御する。第１の対象物を接近させる際、ロボット制御装置１０は、ビジュアルサーボを用いてもよい。
第２の対象物の一端における中心の位置が、第１の対象物の開口部の中心の目標位置として設定された場合、第２の対象物の中心の位置が目標位置からずれるという誤差が生じることがある。第２の対象物の一端の位置から、第１の対象物の開口部の位置がずれることにより、第２の対象物の一端が、第１の対象物の主面のうち開口部が形成されていない部分（平面部）に接触することがある。

力制御部１２０は、第１の対象物の平面部が第２の対象物の一端に接触したか否かを判定する。力制御部１２０は、例えば、力覚センサー２５から入力されたセンサー値の単位時間当たりの変化が予め定めたセンサー値の変化よりも急激であるとき、接触したと判定する。その場合には、第１の対象部は、第２の対象物から反力を受けるからである。その後、力制御部１２０は、第１の対象物に対して力を作用させることを停止する。それ以外の場合には、力制御部１２０は、接触していないと判定する。

そして、力制御部１２０は、第１の対象物の開口部に、第２の対象物の一端が挿入したか否かを判定する。ここで、力制御部１２０は、例えば、第１の対象物に対して第２の対象物の一端から他端への方向に予め定めた大きさの力を作用させ、力覚センサー２５から入力されたセンサー値が示す、その方向への力の大きさが、予め定めた力の大きさの閾値よりも小さいとき、挿入したと判定する。この閾値は、例えば、第１の対象物に対して作用させた力よりも小さい値である。その場合には、第１の対象物は、第２の対象物から受けていた反力が緩和又は失われるからである。それ以外の場合には、力制御部１２０は、挿入していないと判定する。

第２の対象物の一端が挿入していないと判定された場合には、第１の対象物の平面部が第２の対象物の一端に接触している。ここで、第１の対象物の表面部において第２の対象物の一端の中心に接触した位置を接触点と呼ぶ。特に、第１の対象物の平面部が第２の対象物の一端に初めて接触した時点での接触点を当初接触点と呼ぶ。この場合、力制御部１２０は、接触点が当初接触点から予め定めた方向（オフセット方向）に予め定めた量（オフセット量）だけずれた位置（変位点）にずれるように、第１の対象物を並進移動させる。変位点は、当初接触点から予め定めたオフセット方向に予め定めたオフセット量だけずれた第１の対象物の表面部上の１点である。

ここで、オフセット量ｄは、第１の対象物の中心と第２の対象物の一端における中心の位置を目標位置とした場合における、第１の対象物の位置の誤差（精度）に基づいて定めておいてもよい。この誤差は、目標位置からの接触点のずれである。オフセット量は、この誤差の分布の大きさを示す統計量とした水平面方向の１つの座標軸方向の標準偏差σに基づいて、例えば、６σと定めておいてもよい。６σとは、その座標軸方向の誤差の確率が正規分布に従うと仮定したとき、約９９．７％の確率で生じる誤差の範囲（片側３σ）を示す。また、オフセット方向は、第１の対象物の表面部に平行な面内であれば、任意に設定しておいてもよい。
なお、力制御では、一般に１つの対象物を他の対象物に接触させながら移動させるときの精度は、接触させないで移動させるときの精度よりも高い。つまり、このオフセット量ｄは、力制御を用いて第２の対象物に接触させながら第１の対象物を移動させる制御による誤差よりも大きい。

その後、力制御部１２０は、第１の対象物の表面部を第２の対象物の一端に接触させながら、その接触点が変位点から当初接触点に近づくように第１の対象物を移動させる（探り動作）。ここで、変位点から当初接触点への方向は、オフセット方向とは逆方向である。接触点が当初接触点に近づくように第１の対象物を動かす際、力制御部１２０は、変位点と当初接触点との間を結ぶ線分から所定の幅を有する領域を網羅する経路上に動かす。これにより、平面視で第１の対象物の開口部の領域が第２の対象物の一端に含まれる、第１の対象物の位置が探索される。このような領域を探索領域と呼ぶ。探索領域の例については後述する。
変位点から当初接触点に近づくように第１の対象物を移動させる間、力制御部１２０は、第１の対象物の開口部に、第２の対象物の一端が挿入したか否かを判定する。この判定に係る処理は、上述した第１の対象物の開口部に第２の対象物の一端が挿入したか否かを判定する処理と同様であってもよい。

挿入したと判定された場合には、力制御部１２０は、第１の対象物の姿勢を調整して、第１の対象物の開口部の中心軸と第２の対象物の中心軸を一致させる。その後、第１の対象物を一端から他端の方向（例えば、下方）に移動させる（嵌め込み）。第１の対象物の開口部の中心軸と第２の対象物の中心軸が傾いたまま、第１の対象物を移動させることで第１の対象物や第２の対象物に無理な力を加え、損傷又は破損するおそれを回避する。第１の対象物の姿勢を調整する際には、力制御部１２０は、インピーダンス制御を用いてもよい。

第１の対象物の開口部に第２の対象物の一端が挿入せずに、接触点が当初接触点に到達した場合、つまり、探索領域内に第２の対象物の一端が検出されなかった場合がある。
この場合、力制御部１２０は、上述したオフセット方向を変更し、当初接触点から変更したオフセット方向に予め定めたオフセット量だけずれた位置に変位点を再設定してもよい。
その後、力制御部１２０は、接触点が当初接触点から再設定した変位点にずれるように第１の対象物を移動させ、その変位点から接触点が当初接触点に近づくように探索領域内で第１の対象物を移動させるという処理を繰り返す。そして、第１の対象物の開口部に第２の対象物の一端が挿入しないで、接触点が当初接触点に到達した場合には、変位点を再設定する処理を繰り返す。

なお、処理の繰り返し回数Ｎは、予め任意に設定しておくことができる。Ｎは、例えば、２、４である。但し、同一のオフセット方向が繰り返されないようには、各処理でのオフセット方向を繰り返し回数Ｎに応じて予め定めておく。例えば、ｋ（ｋは、１より大きくＮ以下の整数）回目の繰り返しのオフセット方向を、当初接触点からの初回のオフセット方向を基準として、時計回り又は反時計回りに（ｋ−１）・３６０°／Ｎと定めておいてもよい。

画像取得部１４０は、撮像装置３０が撮像した画像データを取得する。画像取得部１４０は、取得した画像データを、例えば上述した制御に用いる。

次に、本実施形態に係るロボットシステム１における制御の流れの一例について説明する。
図３は、本実施形態に係るロボットシステム１における制御の流れの一例を示すブロック図である。
力制御部１２０において、力覚センサー２５から入力されたセンサー値に基づいて、目標として設定された目標値で、ロボット２０の可動部２１の端点を物体に接触させるためのフィードバックループが回っている。軌道生成部１２４は、入力されたセンサー値が、目標値となるように軌道及び指令値（ここでは、目標角度）を生成する。

なお、ロボット制御装置１０は、ビジュアルサーボ制御を行う際、画像データに基づいて推定された対象物の位置及び姿勢が、目標位置及び目標姿勢に近づけるように軌道及び指令値を生成する。一般的に画像処理の負荷は高いため、ビジュアルサーボ制御で指令値を生成する間隔（例えば３０ミリ秒（ｍｓｅｃ）毎）は、力制御部１２０が指令値を出力する間隔（例えば、１ミリ秒（ｍｓｅｃ）毎）よりも長い。

駆動制御部２００には、ロボット制御装置１０から指令値（目標角度）が入力される。駆動制御部２００は、各ジョイント２３に設けられた各アクチュエーターのエンコーダー値等に基づいて現在角度を取得し、目標角度と現在角度の差分（偏差角度）を算出する。また、駆動制御部２００は、偏差角度に基づいてアーム２２の移動速度を算出し、可動部２１を算出した移動速度に応じた偏差角度だけ移動させる。

次に、本実施形態に係るロボット制御処理について説明する。
図４は、本実施形態に係るロボット制御処理を示すフローチャートである。
この例では、第１の歯車を中心軸Ａ１に嵌め込む作業をロボット２０に実行させる場合が仮定されている。この作業において、ロボット制御装置１０が可動部２１に第１の歯車を把持させ、初期において第１の歯車が中心軸Ａ１から所定の方向に所定の距離をもって離れ、力制御により第１の歯車の位置、姿勢を制御して第２の歯車に嵌合させる。

（ステップＳ１０１）力制御部１２０は、第１の対象物の開口部を第２の対象物の一端に接近させる。その後、ステップＳ１０２に進む。
（ステップＳ１０２）力制御部１２０は、第１の対象物の平面部が第２の対象物の一端に接触したか否かを判定する。接触したと判定された場合（ステップＳ１０２ ＹＥＳ）、ステップＳ１０３に進む。接触していないと判定された場合（ステップＳ１０２ ＮＯ）、ステップＳ１０１を繰り返す。

（ステップＳ１０３）力制御部１２０は、第１の対象物の開口部に、第２の対象物の一端が挿入したか否かを判定する。挿入したと判定された場合（ステップＳ１０３ ＹＥＳ）、力制御部１２０は、第１の対象物の中心軸の方向が第２の対象物の中心軸の方向に一致させるように第１の対象物の姿勢を調整する。そして、力制御部１２０は、第１の対象物を一端から他端に押し込み、図４に係る処理を終了する。挿入していないと判定された場合（ステップＳ１０３ ＮＯ）、ステップＳ１０４に進む。
（ステップＳ１０４）力制御部１２０は、当初接触点から予め定めたオフセット方向に予め定めたオフセット量だけずれた変位点にずれるように第１の対象物を並進移動させる。その後、ステップＳ１０５に進む。

（ステップＳ１０５）力制御部１２０は、第１の対象物の表面部を第２の対象物の一端に接触させながら、接触点が変位点から当初接触点に近づくように第１の対象物を移動させる。その後、ステップＳ１０６に進む。
（ステップＳ１０６）力制御部１２０は、第１の対象物の開口部に、第２の対象物の一端が挿入したか否かを判定する。挿入したと判定された場合（ステップＳ１０６ ＹＥＳ）、図４に係る処理を終了する。挿入していないと判定された場合（ステップＳ１０６ ＮＯ）、ステップＳ１０７に進む。

（ステップＳ１０７）力制御部１２０は、第１の対象物の開口部に第２の対象物の一端が挿入せずに、接触点が上述した変位点から当初接触点に到達したか否かを判定する。到達したと判定された場合（ステップＳ１０７ ＹＥＳ）、ステップＳ１０８に進む。到達していないと判定された場合（ステップＳ１０８ ＮＯ）、ステップＳ１０５に進む。
（ステップＳ１０８）力制御部１２０は、オフセット方向を変更し、当初接触点から変更したオフセット方向に予め定めたオフセット量だけずれた位置に変位点を再設定する。その後、ステップＳ１０４に進む。

次に、接触点が変位点から接触点に近づくように第１の対象物を移動させる際に用いられる探索領域の例について説明する。
図５は、探索領域の一例を示す図である。
図５において、探索領域が破線で示されている。この探索領域は、当初接触点Ｃと変位点Ｓ１に基づいて定められる正方形、つまり、当初接触点Ｃと変位点Ｓ１とを結ぶ線分と垂直な方向に、オフセット量ｄと等しい幅を有する領域である。当初接触点Ｃから変位点Ｓ１に向けられた矢印は、予め定めたオフセット方向を示す矢印で、Ｗ、Ａは、それぞれ第１の対象物、第１の対象物の開口部を示す。探索領域の大きさを上述した誤差に基づいて設定することで、その探索領域内で変位点Ｓ１から当初接触点Ｃに近づくように、力制御部１２０は、第１の対象物を移動させる。これにより、第１の対象物の開口部の領域が第２の対象物の一端に含まれるような第１の対象物の位置としての可能性がある位置が漏れなく探索される。また、変位点Ｓ１から当初接触点Ｃに近づくように、その第１の対象物の位置を探索することで、全体としてその位置を効率的に探索することができる。つまり、図４に示す処理が複数回繰り返す場合、ある回では、当初接触点Ｃの位置により早期に探索できないこともあるが、この複数回全体としては、ランダムに探索する場合よりも早期に探索できることを意味する。

図６は、探索領域の他の例を示す図である。
第１の対象物の開口部Ａの位置は、当初接触点Ｃから予め定めたオフセット方向に定めた変位点Ｓ１に基づいて定めた探索領域内にない場合もある。その場合には、ステップＳ１０７において、第１の対象物の開口部に第２の対象物の一端が挿入せずに、接触点が変位点Ｓ１から当初接触点Ｃに到達したと判定される。そして、ステップＳ１０８において、オフセット方向が、例えば、前回のオフセット方向とは逆方向に変更され新たに変位点Ｓ２が設定される。図６において破線で示された正方形は、当初接触点Ｃと変位点Ｓ２に基づいて新たに定められた探索領域を示す。その探索領域内で接触点が変位点Ｓ２から当初接触点Ｃに近づくように、力制御部１２０は、第１の対象物を移動させる。これにより、第１の対象物の開口部の領域が第２の対象物の一端に含まれるような接触点が確実に探索される。

次に、接触点を探索領域内で変位点から当初接触点に近づくように第１の対象物を移動させる際に用いられる経路（探索経路）の例について説明する。
図７は、探索経路の一例を示す図である。
図７に示される探索経路は、変位点Ｓ１と当初接触点Ｃを含むジグザグ状の経路である。この経路は、当初接触点Ｃと変位点Ｓ１とを結ぶ線分に直交する部分（直交線分）と、その１つの直交線分の終点から隣接した直交線分の起点とを結ぶ部分（斜行線分）とが繰り返して構成される。この例では、１つの直交線分の長さは、オフセット量ｄに等しい。直交線分間又は斜行線分間の間隔を、第１対象物の開口部の直径よりも小さい間隔に定めることで、この探索経路で探索領域が網羅される。この間隔は、例えば、第１の対象物の開口部の直径と第２の対象物の一端の直径との差である。また、この探索経路の起点、終点は、当初接触点Ｃ、変位点Ｓ１であってもよいが、図７に示されるように探索領域の頂点Ｏ１、Ｄ１であってもよい。

図８は、探索経路の他の例を示す図である。
図８に示される探索経路は、変位点Ｓ１と当初接触点Ｃを含む矩形の経路である。この経路は、当初接触点Ｃと変位点Ｓ１とを結ぶ線分に直交する部分（直交線分）と、当初接触点Ｃと変位点Ｓ１と平行な部分（平行線分）とが繰り返して構成される。この例でも、１つの直交線分の長さはオフセット量ｄに等しい。直交線分間の間隔は図７に示した例と同様に、第１対象物の開口部の直径よりも小さい間隔に定めておく。探索経路は、１つの直交線分の終点と１つの平行線分の起点が接続され、その平行線分の終点が隣接する直交線分の起点に接続されるという構成が繰り返されて構成される。また、この探索経路の起点、終点は、当初接触点Ｃ、変位点Ｓ１であってもよいが、図８に示されるように探索領域の頂点Ｏ２、Ｄ２であってもよい。

図９は、探索経路のさらに他の例を示す図である。
図９に示される探索経路は、変位点Ｓ１を起点とし、当初接触点Ｃを終点とする渦巻状の経路である。この経路は、当初接触点Ｃと変位点Ｓ１と直交する長径の長さがオフセット量ｄに等しい曲線が周期的に繰り返されて構成される。１周期当たりの間隔を第１対象物の開口部の直径よりも小さい間隔に定めておく。これにより、探索領域が網羅される。

図１０は、探索経路のまたさらに他の例を示す図である。
図１０に示される探索経路は、変位点Ｓ１を起点とし、変位点Ｓ１を中心に回転する点であって、回転に伴って中心からの半径が増加する軌跡で形成される螺旋状の経路である。この経路の終点は当初接触点Ｃを終点とし、１周当たりの半径の増加量を第１対象物の開口部の直径よりも小さい間隔に定めておく。これにより、探索領域として、ほぼ円形の領域が網羅される。

なお、探索経路は、探索領域を網羅し、変位点Ｓ１から当初接触点Ｃに近づく経路であれば、いかなる形状を有していてもよい。探索経路は、例えば、変位点Ｓ１と当初接触点Ｃとを通る正弦曲線、等である。探索経路は、上述した例に示されるように、その一部に当初接触点Ｃから遠ざかる部分が含まれてもよい。

上述したロボットシステム１の構成は、本実施形態の特徴を説明するものであって、これには限られない。ロボット制御装置１０は、コンピューターで構成されていてもよい。
図１１は、本実施形態に係るロボット制御装置１０の機能構成の他の例を示す図である。
ロボット制御装置１０は、ＣＰＵ(Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、メモリー１２、外部記憶装置１３、通信装置１４、入力装置１５、出力装置１６、及びＩ／Ｆ（Ｉｎｔｅｒｆａｃｅ）１７を含んで構成される。
ＣＰＵ１１は、上述した処理に係る演算を行う。メモリー１２は、不揮発性の記憶装置であるＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）や揮発性の記憶装置であるＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）を含む。通信装置１４は、ロボット２０との間で通信を行う。入力装置１５は、ユーザーの操作を受け付け、受け付けた操作に応じた操作入力信号を入力する装置、例えば、マウス、キーボード、等である。出力装置１６は、画像データを視認可能に出力する装置、例えば、ディスプレイ、等である。Ｉ／Ｆ１７は、他の装置と接続し、データを入力又は出力する。例えば、Ｉ／Ｆ１７は、例えば撮像装置３０から画像データを入力する。

上述の各機能部は、例えば、ＣＰＵ１１がメモリー１２に格納された所定のプログラムを読み出して実行することにより実現される。なお、所定のプログラムは、例えば、予めメモリー１２にインストールされてもよいし、通信装置１４を介してネットワークからダウンロードされてインストール又は更新されてもよい。

ロボット制御装置１０とロボット２０は、上述のように互いに別体で構成されてもよいが、ロボット制御装置１０がロボット２０と一体化したロボット２０として構成されてもよい。一体化して構成される場合には、ロボット２０は、ロボット制御装置１０を内蔵してもよい。
また、撮像装置３０は、上述のようにロボット制御装置１０又はロボット２０と別体であってもよいが、一体化されてもよい。
また、本実施形態では、ロボット２０の一例として、２本の可動部２１を備える双腕ロボットを示したが、１本の可動部２１を備える単腕ロボット、３本以上の可動部２１を備えるロボットであってもよい。また、ロボット２０は、スカラーロボットでもよく、垂直多関節ロボットでもよい。また、ロボット２０は、移動を可能にするために、例えば、車輪や脚部を備えてもよい。

以上、この発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計等も含まれる。
上述したロボットシステム１の機能構成は、ロボットシステム１の構成について理解を容易にするために、主な処理内容に応じて分類したものである。構成要素の分類の仕方や名称によって、本願発明が制限されることはない。ロボットシステム１の構成は、処理内容に応じて、さらに多くの構成要素に分類することもできる。また、１つの構成要素がさらに多くの処理を実行するように分類することもできる。また、各構成要素の処理は、１つのハードウェアで実行されてもよいし、複数のハードウェアで実行されてもよい。
また、ビジュアルサーボ制御が行われない場合、その他、画像データが用いられない場合には、画像取得部１４０、撮像装置３０の少なくともいずれか一方が省略されてもよい。

また、ロボット制御装置１０及びロボット２０の機能及び処理の分担は、図示した例に限られない。例えば、ロボット制御装置１０の少なくとも一部の機能は、ロボット２０に含まれ、ロボット２０により実現されてもよい。また、例えば、ロボット２０の少なくとも一部の機能は、ロボット制御装置１０に含まれ、ロボット制御装置１０により実現されてもよい。

また、上述したフローチャートの各処理単位は、ロボット制御装置１０の処理を理解容易にするために、主な処理内容に応じて分割したものである。処理単位の分割の仕方や名称によって、本願発明が制限されることはない。ロボット制御装置１０の処理は、処理内容に応じて、さらに多くの処理単位に分割することもできる。また、１つの処理単位がさらに多くの処理を含むように分割することもできる。

以上、本発明について実施形態を用いて説明したが、本発明の技術的範囲は上記実施形態に記載の範囲には限定されない。上記実施形態に多様な変更または改良を加えることが可能であることが当業者には明らかである。また、そのような変更または改良を加えた形態も本発明の技術的範囲に含まれ得ることが、特許請求の範囲の記載から明らかである。

１…ロボットシステム、
１０…ロボット制御システム、１１…ＣＰＵ、１２…メモリー、１３…外部記憶装置、
１４…通信装置、１５…入力装置、１６…出力装置、１７…Ｉ／Ｆ、
１２０…力制御部、１２２…センサー情報取得部、１２４…軌道生成部、
１４０…画像取得部、
２０…ロボット、２１…可動部、２２…アーム、２３…ジョイント、２４…リンク、
２５…力覚センサー、２６…ハンド、２００…駆動制御部、
３０…撮像装置

Claims (7)

1. 第１の対象物の開口部を第２の対象物の突起部に挿入するロボットであって、
   力センサーと、
   ハンドと、を含み、
   前記ハンドにより、前記第１の対象物を把持し、
   前記第１の対象物の前記開口部を前記第２の対象物の前記突起部に近づけ、
   前記第１の対象物の前記開口部を前記第２の対象物を目標として動かす際、前記第１の対象物が前記第２の対象物に初めて接触した位置である当初接触点から所定の方向に変位した位置である変位点を設定し、前記第１の対象物が前記第２の対象物に接触した位置である接触点が前記設定した変位点から前記当初接触点に近づくように前記第２の対象物に接触させながら前記第１の対象物を動かすように動作する
   ロボット。
2. 前記第２の対象物は、載置されている請求項１に記載のロボット。
3. 前記当初接触点と前記変位点との間の距離は、前記第１の対象物の位置の誤差に基づく距離である請求項１又は請求項２のいずれか１項に記載のロボット。
4. 前記接触点が前記変位点と前記当初接触点との間を結ぶ線分から所定の幅を有する領域を網羅する経路上に沿うように前記第１の対象物を動かす請求項１から請求項３のいずれか１項に記載のロボット。
5. 前記領域内に前記第２の対象物が検出されなかった場合、前記当初接触点から前記所定の方向とは異なる方向に変位した位置に変位点を再設定し、前記接触点が再設定した変位点から前記当初接触点に近づくように前記第１の対象物を動かすようにロボットの動作を制御する請求項４に記載のロボット。
6. ロボットと、前記ロボットの動作を制御するロボット制御装置とを備えたロボットシステムであって、
   前記ロボットは、第１の対象物の開口部を第２の対象物の突起部に挿入するロボットであって、
   力センサーと、
   ハンドと、を含み、
   前記ハンドにより、前記第１の対象物を把持し、
   前記ロボット制御装置は、
   前記第１の対象物の前記開口部を前記第２の対象物の前記突起部に近づけるように前記ロボットの動作を制御し、
   前記第１の対象物の前記開口部を前記第２の対象物を目標として動かす際、前記第１の対象物が前記第２の対象物に初めて接触した位置である当初接触点から所定の方向に変位した位置である変位点を設定し、前記第１の対象物が前記第２の対象物に接触した位置である接触点が前記設定した変位点から前記当初接触点に近づくように前記第２の対象物に接触させながら前記第１の対象物を動かすように動作するように前記ロボットの動作を制御する
   ロボットシステム。
7. 力センサーと、
   ハンドと、を含み、
   前記ハンドにより、第１の対象物を把持し、
   前記第１の対象物の開口部を第２の対象物の突起部に挿入するロボットの動作を、
   前記第１の対象物の前記開口部を前記第２の対象物の前記突起部に近づけるように制御し、
   前記第１の対象物の前記開口部を前記第２の対象物を目標として動かす際、前記第１の対象物が前記第２の対象物に初めて接触した位置である当初接触点から所定の方向に変位した位置である変位点を設定し、前記第１の対象物が前記第２の対象物に接触した位置である接触点が前記設定した変位点から前記当初接触点に近づくように前記第２の対象物に接触させながら前記第１の対象物を動かすように動作するように前記ロボットの動作を制御する
   ロボット制御装置。

Images