JP6752615B2 - 情報処理装置、情報処理方法、ロボット制御装置及びロボットシステム - Google Patents

Description

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

従来、乱雑に山積みされた対象物体を１つ１つロボットが把持するための技術として、視覚センサを用いて対象物体の位置および姿勢を把握するロボット装置が知られている。特許文献１には、対象物体と同一形状の物体を複数の方向から撮像して教示モデルを作成しておき、教示モデルと山積みされた対象物体を撮像した画像データとのマッチングを行うことで、対象物体の位置および姿勢を認識することが開示されている。

特許第３３００６８２号公報

視覚センサを用いた計測では、撮像した物体の表面位置の情報しか取得することができない。そのため、上記特許文献１に記載の技術では、対象物体がビニール袋や緩衝材によって包まれている場合、画像データ内の対象物体が教示モデルとマッチングしない。したがって、この場合、ビニール袋や緩衝材に包まれた中身である対象物体の位置および姿勢を適切に認識できず、ロボットは対象物体を把持することができない。
そこで、本発明は、表面位置情報だけでは対象物体を正確に認識できない場合であっても、対象物体の位置および姿勢を適切に認識することを目的とする。

上記課題を解決するために、本発明に係る情報処理装置の一態様は、対象物体と周辺物体からなる物体を認識する情報処理装置であって、物体の表面位置情報を取得する取得手段と、前記取得手段により取得された前記表面位置情報に基づいて、前記物体を実測するための計測位置を決定する決定手段と、前記決定手段により決定された前記計測位置において前記物体に接触したときの接触位置情報を計測する計測手段と、前記計測手段により計測された前記接触位置情報に基づいて、前記物体の空間的な位置情報を示す地図情報を作成する作成手段と、前記作成手段により作成された前記地図情報に基づいて、前記物体の位置および姿勢を認識する認識手段と、を備え、前記計測手段は、接近した前記物体までの距離を取得する距離の取得手段を備え、前記物体に接触しているときに前記距離の取得手段によって取得された距離に基づいて、前記接触位置情報を計測する。

本発明によれば、表面位置情報だけでは対象物体を正確に認識できない場合であっても、対象物体の位置および姿勢を適切に認識することができる。

第一の実施形態のロボットシステムの構成例を示す図である。 情報処理装置のハードウェア構成例を示す図である。 第一の実施形態における情報処理装置の機能ブロック図である。 第一の実施形態における動作を説明するためのフローチャートである。 第一の実施形態における動作を説明するための図である。 第二の実施形態のロボットシステムの構成例を示す図である。 第二の実施形態における情報処理装置の機能ブロック図である。 第二の実施形態における動作を説明するためのフローチャートである。 第三の実施形態のロボットシステムの構成例を示す図である。

以下、添付図面を参照して、本発明を実施するための形態について詳細に説明する。
なお、以下に説明する実施の形態は、本発明の実現手段としての一例であり、本発明が適用される装置の構成や各種条件によって適宜修正又は変更されるべきものであり、本発明は以下の実施の形態に限定されるものではない。
（第一の実施形態）
第一の実施形態では、撮像装置により撮像して得られた物体（対象物体およびその周辺の周辺物体）の画像情報をもとに計測した物体の３次元位置情報と、力覚センサによる計測情報とを統合して、物体の地図情報を作成する。そして、作成した地図情報を利用して、対象物体の位置および姿勢を認識する。本実施形態において、対象物体とは、ロボットによる操作の対象となる物体であり、例えばビニール袋や緩衝材に包まれたトナーカートリッジのような部品を含む。また、力覚センサによる計測とは、力覚センサによってロボットが物体へ接触していることが検知されているときに、位置検出センサによってロボットによる接触位置を計測することをいう。さらに、地図情報とは、対象物体および周辺物体の空間的な位置情報である。

図１は、本実施形態における情報処理装置２０を備えるロボットシステム１００の構成例を示す図である。
ロボットシステム１００は、ロボット１０と、情報処理装置２０と、を備える。ロボット１０は、例えば多関節ロボットであり、ロボットアーム等のマニピュレータや、ロボットハンド等のエンドエフェクタ１１を備える。また、ロボット１０は、マニピュレータの各関節の角度を変更することでエンドエフェクタ１１の位置姿勢を変更可能な位置姿勢変更機構を備える。位置姿勢変更機構は、電動モータによって駆動されてもよいし、油圧や空気圧等の流体圧で作動するアクチュエータによって駆動されてもよい。この位置姿勢変更機構は、情報処理装置２０から出力される動作指示情報に従って駆動される。

また、エンドエフェクタ１１は、ロボット１０の対象物体の種類に応じた操作を実現するためのツールであり、モータ駆動可能なチャック機構を有し対象物体を把持可能なハンドや、空気圧で対象物体を吸着する吸着パッドを用いたハンドを用いることができる。なお、エンドエフェクタ１１は、ロボットアームに対して着脱可能に取り付けられており、ワークの種類に応じて交換可能である。また、ロボット１０は、多関節ロボットに限定されるものではなく、数値制御（Numerical Control：ＮＣ）可能な可動式の機械であってもよい。

ロボット１０は、情報処理装置２０によって決定された行動を実施し、対象物体４１を搬送したり把持したりする操作を実行する。本実施形態において、行動とは、対象物体４１を認識あるいは操作するためのロボット１０の動作のことである。
対象物体４１は、トナーカートリッジのような、ロボット１０によって把持され搬送されることが想定される部品であり、ビニール袋や緩衝材に包まれている場合もある。つまり、対象物体４１の周辺には、対象物体４１を包んでいるビニール袋や緩衝材といった周辺物体４２が存在する。なお、周辺物体４２は、対象物体４１の周辺に位置する対象物体４１以外の全ての物体であり、例えば、山積みされた対象物体４１を収容している容器や、その容器の周辺に置かれた物体も含む。

また、ロボットシステム１００は、撮像装置３１と、光源３２と、力覚センサ３３と、位置検出センサ３４とをさらに備える。
撮像装置３１は、カメラや、光を検出するセンサ、フォトダイオードなどで構成される視覚センサであり、対象物体４１および周辺物体４２の画像情報を取得する。撮像装置３１は、取得した画像情報を情報処理装置２０に出力する。光源３２は、例えばプロジェクタによって構成され、可視光を出射したり、レーザ光源から赤外光を出射したりすることで、対象物体４１および周辺物体４２に対して均一照明光やパターン光を投射する。撮像装置３１は、光源３２によって光が投射された対象物体４１および周辺物体４２の画像を撮像する。なお、撮像装置３１は、光源３２によって光が投射されていない状態で、対象物体４１および周辺物体４２の画像を撮像することもできる。撮像装置３１および光源３２は、図１に示すように、撮像対象空間の上方に固定配置してもよいし、ロボット１０に搭載したり、他の作業機械に搭載したりしてもよい。また、撮像装置３１は複数配置してもよい。

力覚センサ３３は、ロボット１０のエンドエフェクタ１１の基部に取り付けられている。この力覚センサ３３は、歪みゲージや圧電素子によって構成され、ロボット１０が対象物体４１あるいは周辺物体４２に接触しているときのロボット１０にかかる力（反力）を計測し、計測結果である力覚情報を情報処理装置２０へ出力する。力覚センサ３３は、６軸力覚センサ（Force/Torqueセンサ）であってもよいし、３軸力覚センサであってもよい。また、力覚センサ３３は、１次元の圧力センサであってもよいし、接触の有無を判別する接触センサであってもよい。

位置検出センサ３４は、ロボット１０の位置を検出し、検出結果である位置検出情報を情報処理装置２０へ出力する。位置検出センサ３４は、ロボット１０に取り付けられたエンコーダを含んで構成され、エンコーダから取得した角度情報をもとに順運動学を用いることで、ロボット１０の位置を検出することができる。この位置検出センサ３４は、力覚センサ３３と一緒に用いられることで、ロボット１０が対象物体４１や周辺物体４２に接触しているときのロボット１０の位置を検出することができる。なお、ロボット１０の位置を検出する方法は、上記の位置検出センサ３４を用いる方法に限定されない。

情報処理装置２０は、ロボット１０の動作を制御するロボット制御装置である。情報処理装置２０は、例えばパーソナルコンピュータ（ＰＣ）により構成されている。図２は、情報処理装置２０のハードウェア構成の一例である。情報処理装置２０は、ＣＰＵ２１と、ＲＯＭ２２と、ＲＡＭ２３と、外部メモリ２４と、入力部２５と、表示部２６と、通信Ｉ／Ｆ２７と、システムバス２８とを備える。
ＣＰＵ２１は、情報処理装置２０における動作を統括的に制御するものであり、システムバス２８を介して、各構成部（２２〜２７）を制御する。ＲＯＭ２２は、ＣＰＵ２１が処理を実行するために必要なプログラムを記憶する不揮発性メモリである。なお、当該プログラムは、外部メモリ２４や着脱可能な記憶媒体（不図示）に記憶されていてもよい。ＲＡＭ２３は、ＣＰＵ２１の主メモリ、ワークエリアとして機能する。すなわち、ＣＰＵ２１は、処理の実行に際してＲＯＭ２２から必要なプログラムをＲＡＭ２３にロードし、当該プログラムを実行することで各種の機能動作を実現する。

外部メモリ２４は、例えば、ＣＰＵ２１がプログラムを用いた処理を行う際に必要な各種データや各種情報を記憶している。また、外部メモリ２４には、例えば、ＣＰＵ２１がプログラムを用いた処理を行うことにより得られた各種データや各種情報が記憶される。入力部２５は、例えばキーボードやマウスのポインティングデバイスにより構成され、オペレータが入力部２５を介して当該情報処理装置２０に指示を与えることができるようになっている。表示部２６は、液晶ディスプレイ（ＬＣＤ）等のモニタで構成される。通信Ｉ／Ｆ２７は、外部機器と通信するためのインターフェースである。システムバス２８は、ＣＰＵ２１、ＲＯＭ２２、ＲＡＭ２３、外部メモリ２４、入力部２５、表示部２６及び通信Ｉ／Ｆ２７を通信可能に接続する。
このように、情報処理装置２０は、通信Ｉ／Ｆ２７を介して、外部機器であるロボット１０、撮像装置３１、光源３２、力覚センサ３３とそれぞれ通信可能に接続されており、これらの外部機器の動作を制御する。

図３は、情報処理装置２０の機能ブロック図である。
情報処理装置２０は、画像取得部２０１と、表面位置計測部２０２と、地図情報作成部２０３と、計測位置決定部２０４と、行動計画部２０５と、を備える。さらに、情報処理装置２０は、力覚情報取得部２０６と、力計測部２０７と、位置情報取得部２０８と、位置計測部２０９と、把持位置決定部２１０と、を備える。表面位置計測部２０２、地図情報作成部２０３、計測位置決定部２０４、力計測部２０７および位置計測部２０９によって、対象物体４１の位置および姿勢を認識する処理を行う認識部２２０を構成している。この認識部２２０は物体認識装置として動作することができる。
画像取得部２０１は、撮像装置３１から出力される画像情報を取得する。画像取得部２０１は、取得した画像情報を画像データに変換し、表面位置計測部２０２に出力する。画像取得部２０１は、例えばキャプチャボードやメモリ（ＲＡＭ）によって構成される。表面位置計測部２０２は、画像取得部２０１から取得した画像データに基づいて、対象物体４１および周辺物体４２の表面位置の情報（表面位置情報）を計測する。表面位置計測部２０２は、計測した表面位置情報を地図情報作成部２０３へ出力する。

地図情報作成部２０３は、表面位置計測部２０２によって計測された表面位置情報と、後述する力計測部２０７によって計測された力情報と、後述する位置計測部２０９によって計測された位置情報とに基づいて地図情報を作成する。なお、地図情報作成部２０３は、位置計測部２０９を介して力計測部２０７によって計測された力情報を取得する。地図情報とは、対象物体４１および周辺物体４２の空間的な位置情報であり、本実施形態では、３次元の空間内における対象物体４１および周辺物体４２の位置情報とする。地図情報作成部２０３は、まず、表面位置情報に基づいて地図情報を作成し、その後、力情報と位置情報とに基づいて、表面位置情報に基づいて作成した地図情報を更新することで、詳細な地図情報を作成する。

画像データをもとに計測された表面位置情報により、対象物体４１および周辺物体４２の表面位置を求めることができる。しかしながら、上述したように、対象物体４１がビニール袋に包まれたトナーカートリッジである場合、表面位置情報により求まる表面位置は、ビニール袋の表面位置でありトナーカートリッジの表面位置ではない。このように、画像データをもとに計測される表面位置情報からは、対象物体４１の正確な位置および姿勢を求めることができない場合がある。

そこで、本実施形態では、より正確に対象物体４１を認識するために、ロボット１０により対象物体４１に直接触れ、対象物体４１を計測する。具体的には、力覚センサ３３および位置検出センサ３４を用いて、ロボット１０が対象物体４１に接触したときのロボット１０の位置情報（接触位置情報）を計測する。そして、計測したロボット１０の位置情報を用いて地図情報を更新する。つまり、地図情報作成部２０３は、地図情報の作成にあたり、撮像装置３１を用いて計測した対象物体４１の表面位置に対して、力覚センサ３３および位置検出センサ３４を用いて実際に対象物体４１に触って計測した、対象物体４１の詳細位置の結果を加える。このようにして、対象物体４１の位置および姿勢をより詳細に計測する。地図情報作成部２０３は、作成した地図情報を、計測位置決定部２０４や把持位置決定部２１０に出力する。

計測位置決定部２０４は、地図情報作成部２０３から入力した地図情報に基づいて、対象物体４１の詳細な位置および姿勢を実測するための計測位置を決定する。この計測位置決定部２０４に入力される地図情報は、地図情報作成部２０３により表面位置情報に基づいて作成された地図情報である。計測位置決定部２０４は、入力された地図情報を参照し、対象物体４１の存在確率が高いと判断した位置を計測位置として決定する。計測位置決定部２０４は、決定した計測位置を行動計画部２０５に出力する。

行動計画部２０５は、計測位置決定部２０４から入力した計測位置に基づいて、ロボット１０の行動を計画する。具体的には、行動計画部２０５は、エンドエフェクタ１１を計測位置に移動するためのロボット１０の軌道と、計測位置における対象物体４１への接触動作を実現するためのロボット１０の軌道とを作成し、ロボット１０へ動作指示情報を出力する。また、行動計画部２０５は、後述する把持位置決定部２１０から対象物体４１の把持位置も入力可能である。行動計画部２０５は、把持位置を入力した場合、入力した把持位置に基づいてロボット１０の行動を計画する。具体的には、行動計画部２０５は、エンドエフェクタ１１を把持位置に移動するためのロボット１０の軌道と、把持位置における対象物体４１の把持動作を実現するためのロボット１０の軌道とを作成し、ロボット１０へ動作指示情報を出力する。

力覚情報取得部２０６は、力覚センサ３３から出力される力覚情報を取得し、取得した力覚情報を力計測部２０７へ出力する。力覚情報取得部２０６は、例えばメモリ（ＲＡＭ）によって構成される。力計測部２０７は、力覚情報取得部２０６から入力した力覚情報に基づいて、対象物体４１あるいは周辺物体４２にロボット１０のエンドエフェクタ１１が接触しているときのロボット１０にかかる力（反力）を計測する。力計測部２０７は、計測した力情報を位置計測部２０９に出力する。

位置情報取得部２０８は、位置検出センサ３４から出力される位置検出情報を取得し、取得した位置検出情報を位置計測部２０９へ出力する。位置情報取得部２０８は、例えばメモリ（ＲＡＭ）によって構成される。位置計測部２０９は、力計測部２０７から入力した力情報と、位置情報取得部２０８から入力した位置検出情報とに基づいて、対象物体４１あるいは周辺物体４２にロボット１０のエンドエフェクタ１１が接触しているときの接触点の位置を計測する。位置計測部２０９は、計測した位置情報（接触位置情報）を地図情報作成部２０３に出力する。

把持位置決定部２１０は、地図情報作成部２０３から出力される地図情報をもとに、対象物体４１の把持位置を決定する。この把持位置決定部２１０に入力される地図情報は、地図情報作成部２０３により力情報および位置情報に基づいて作成された詳細な地図情報である。把持位置決定部２１０は、入力された地図情報を参照し、ロボット１０が対象物体４１を把持可能な位置を把持位置として決定する。把持位置決定部２１０は、決定した把持位置を行動計画部２０５に出力する。
図３に示す情報処理装置２０の各部の機能は、図２に示すＣＰＵ２１がＲＯＭ２２もしくは外部メモリ２４に記憶されたプログラムを実行することで実現され得る。

以下、ロボットシステム１００の動作について説明する。
図４は、情報処理装置２０が実行するロボット制御処理手順を示すフローチャートである。図４に示される処理は、情報処理装置２０のＣＰＵ２１が、ＲＯＭ２２もしくは外部メモリ２４に格納されたプログラムを読み出して実行することにより実現される。ただし、図４の処理の一部または全部が、専用のハードウェアにより実現されてもよい。図４の処理は、例えばオペレータがロボットシステム１００を起動したときに開始される。ただし、開始のタイミングは、ロボットシステム１００の起動時に限定されるものではない。

先ずＳ１において、ＣＰＵ２１は、システムの初期化処理を行う。すなわち、ＣＰＵ２１は、ＲＯＭ２２もしくは外部メモリ２４に格納されたプログラムをロードし、ＲＡＭ２３上に展開して実行可能な状態とする。また、情報処理装置２０に接続された各機器のパラメータの読み込みや初期位置への復帰を行い、使用可能な状態にする。
次にＳ２において、画像処理部２０１は、撮像装置３１が出力する対象物体４１および周辺物体４２の画像情報を取得し、Ｓ３に移行する。Ｓ３では、表面位置計測部２０２は、Ｓ２において画像処理部２０１が取得した画像情報に基づいて、対象物体４１および周辺物体４２の表面位置を計測する。撮像装置３１から得られる画像情報は、３次元点群の情報であるため、表面位置計測部２０２は、取得した画像情報に基づいて、対象物体４１および周辺物体４２の表面の凹凸を認識することができる。なお、予め外部メモリ２４等に格納された部品データベース（部品ＤＢ）を参照して、対象物体４１および周辺物体４２の表面位置を決定してもよい。部品ＤＢは、認識の対象である対象物体４１の部品種および形状の情報を保持している。部品ＤＢに格納される対象物体４１の形状の情報は、例えばＣＡＤデータやＣＧデータ（ポリゴンデータ）などの３次元モデルデータである。なお、部品ＤＢに格納される対象物体の形状の情報は、対象物体４１や周辺物体４２を多方向から観察した２次元画像の集合から構成されていてもよい。

Ｓ４では、地図情報作成部２０３は、Ｓ３において表面位置計測部２０２が計測した表面位置情報に基づいて地図情報を作成する。以下、地図情報の作成方法について具体的に説明する。地図情報を作成するためには、複数の異なるセンサから得られた情報を一つの空間的な地図情報に落としこむ必要がある。しかしながら、撮像装置３１の撮像装置座標系、力覚センサ３３の力覚センサ座標系、ロボット１０のロボット座標系は全て異なる。そのため、これらの座標系を統一する必要がある。

まず、作業空間内で基準となる座標系として、ワールド座標系Σｗを設定する。そして、ワールド座標系Σｗからロボット座標系Σｒまでの変位を（ＲＸ，ＲＹ，ＲＺ）とする。また、ロボット１０の姿勢を表す３×３の回転行列をＲＭとする。
ロボット座標系Σｒからロボット１０の先端座標系Σｆまでの変位は（ＦＸ，ＦＹ，ＦＺ）とする。また、ロボット１０の先端の姿勢を表す３×３の回転行列はＦＭとする。さらに、先端座標系Σｆからロボット１０のエンドエフェクタ１１先端の座標系Σｅまでの変位は（ＥＸ，ＥＹ，ＥＺ）とする。また、エンドエフェクタ１１先端の姿勢を表す３×３の回転行列はＥＭとする。なお、エンドエフェクタ１１先端とは、対象物体４１あるいは周辺物体４２に接触する部分である。

ロボット１０の先端座標系Σｆから力覚センサ座標系Σｔまでの変位は（ＴＸ，ＴＹ，ＴＺ）とする。また、力覚センサ３３の姿勢を表す３×３の回転行列はＴＭとする。さらに、ロボット座標系Σｒから撮像装置座標系Σｃまでの変位は（ＣＸ，ＣＹ，ＣＺ）とする。また、撮像装置３１の姿勢を表す３×３の回転行列はＣＭとする。さらに、撮像装置座標系Σｃから対象物体座標系Σｖまでの変位は（ＶＸ，ＶＹ，ＶＺ）とする。また、対象物体４１の姿勢を表す３×３の回転行列はＶＭとする。ここで、ワールド座標系Σｗから見た対象物体４１の変位を（ＷＸ，ＷＹ，ＷＺ）、姿勢を表す３×３の回転行列をＷＭとする。
ロボット１０が対象物体４１に接触しているとき、ロボット１０の先端に取り付けられたエンドエフェクタ１１先端の位置は、撮像装置３１により撮影された対象物体４１の位置と一致する。したがって、以下の（１）式および（２）式が成り立つ。

なお、ワールド座標系Σｗから見た、接触動作時にロボット１０にかかる力を４×４の行列ＷＴとおくと、行列ＷＴは以下の式で求まる。この（３）式を用いて、力の方向をワールド座標系Σｗで考慮することができる。

地図情報作成部２０３は、以上の定義の下で、地図情報を作成する。つまり、地図情報作成部２０３は、上記（１）〜（３）式を用いて、異なる複数のセンサの座標系を統一し、座標系が統一されたら、各センサから得られた情報を利用して地図情報を作成していく。
Ｓ４では、地図情報作成部２０３は、まず、ワールド座標系Σｗの作業空間に対してボクセルを配置する。ここで、ボクセルの存在する領域は、対象物体４１あるいは周辺物体４２が存在する可能性のある領域とする。また、ボクセルの大きさは、対象物体４１の把持動作に必要な精度に応じて適宜設定される。
次に、地図情報作成部２０３は、表面位置情報を用いて、対象物体４１と周辺物体４２との表面位置よりも外側に存在するボクセルを全て削る処理を行う。表面位置情報は３次元点群データであり、この表面位置情報は、撮像装置座標系Σｃの情報として求まるので、対象物体４１の表面位置は、上記（２）式を用いてワールド座標系Σｗに変換することができる。ワールド座標系Σｗに配置されたボクセル内に点群が存在する場合、そのボクセルは削られる対象となる。ボクセルが削られた領域は物体が存在しない部分として処理される。以上により、対象物体４１と周辺物体４２とがおおよそ存在する空間を示す地図情報が作成される。

Ｓ５では、計測位置決定部２０４は、Ｓ４において作成された地図情報に基づいて、対象物体４１の計測位置を決定する。具体的には、計測位置決定部２０４は、地図情報を参照し、対象物体４１および周辺物体４２の表面の凹凸に関する情報をもとに、表面が凸形状である部分を選択する。そして、選択した部分は対象物体４１の存在確率が高い部分であるとして、対象物体４１を詳細に計測するための計測位置として決定する。なお、部品ＤＢを利用して対象物体４１の位置を推測できている場合、推測した位置をもとに計測位置を決定してもよい。このように、部品ＤＢにより、対象物体４１のおおまかな位置および姿勢を推測できる場合には、推測した位置および姿勢が正しいか否かを直接触って確認するための計測位置を決定する。

Ｓ６では、行動計画部２０５は、Ｓ５において計測位置決定部２０４が決定した計測位置に基づいて、計測位置による計測を実現するためのロボット１０の行動計画を決定する。次にＳ７では、行動計画部２０５は、Ｓ６において決定した行動計画に基づいて、ロボット１０へ動作指示情報を出力する。これにより、ロボット１０は、エンドエフェクタ１１を計測位置まで移動し、計測位置において対象物体４１に対する接触動作を行う。このときのロボット１０の動作は、単にエンドエフェクタ１１が表面位置計測部２０２で計測した表面位置まで移動するだけではなく、表面位置よりさらに深く移動しながら力覚情報の取得も行う動作である。図５（ａ）に示すように、対象物体４１がビニール袋や緩衝材といった周辺物体４２に包まれている場合、エンドエフェクタ１１は、図５（ｂ）に示すように周辺物体４２を押しつぶしながら移動する。

Ｓ８では、力覚情報取得部２０６は、力覚センサ３３が出力する力覚情報を取得する。また、位置情報取得部２０８は、力覚情報取得部２０６による力覚情報の取得に同期して、位置検出センサ３４が出力する位置検出情報を取得する。Ｓ９では、力計測部２０７は、Ｓ８において力覚情報取得部２０６が取得した力覚情報に基づいて、ロボット１０にかかる力を示す力情報を計測する。また、位置計測部２０９は、Ｓ８において位置情報取得部２０８が取得した位置検出情報に基づいて、ロボット１０の位置を示す位置情報を計測する。

力情報および位置情報は、ロボット１０が対象物体４１に対する接触動作を行っている間、繰り返し計測してもよい。その場合、図４に示すフローチャートでは、Ｓ７〜Ｓ９を一方向の流れとして書いているが、Ｓ７からＳ８、Ｓ８からＳ９、Ｓ９からＳ７と、Ｓ７〜Ｓ９の処理を高速に繰り返す。つまり、ロボット１０は移動しながら力覚情報・位置情報を取得し、力計測・位置計測を行う。さらに、Ｓ７〜Ｓ９に限らず、Ｓ１０の地図情報の更新も行ってもよい。その場合は、Ｓ９からＳ７に戻るのではなく、Ｓ７からＳ８、Ｓ８からＳ９、Ｓ９からＳ１０、Ｓ１０からＳ７という処理の流れを高速に繰り返す。ここで、上記接触動作は、ロボット１０にかかる力の大きさが、予め設定された上限値を超えない範囲で実施するものとする。ロボット１０にかかる力の大きさが上限値を超えたか否かは、Ｓ９において計測される力情報に基づいて判定する。接触動作を行う中で、ロボット１０にかかる力は、対象物体４１への接触力に応じて増加する。そこで、上記の力の上限値は、接触動作により対象物体４１に破損等の不具合が生じない程度の力に設定する。例えば、対象物体４１がビニール袋に包まれたトナーカートリッジである場合、ビニール袋を潰しトナーカートリッジを壊さない程度の力を上限値として設定する。

ロボット１０がビニール袋を潰してトナーカートリッジの表面に当接し、ロボット１０にかかる力が上限値に達すると、情報処理装置２０はロボット１０の接触動作を停止する。接触動作を停止した時点でのロボット１０の位置は、対象物体４１の実際の表面位置として認識することができる。
Ｓ１０では、地図情報作成部２０３は、Ｓ９において計測した力情報および位置情報に基づいて、Ｓ４において作成された地図情報を更新し、新たな地図情報を作成する。Ｓ１０では、地図情報作成部２０３は、Ｓ９において取得した位置情報を、計測位置において実際にロボット１０が動作した結果として利用する。そして、Ｓ４において作成した地図情報に対して、ロボット１０自身が動いた空間に該当するボクセルを削る処理を行う。

その際、地図情報作成部２０３は、予め保持されたロボット１０の３次元モデル情報を用い、関節角度データから順運動学をもとに各時刻においてロボット１０が専有している位置（専有位置）を計算する。エンドエフェクタ１１部分についても同様に、予め保持したエンドエフェクト１１の３次元モデル情報を用い、ロボット１０先端の位置および姿勢から専有位置を計算する。このように、上記専有位置は、ロボット１０に設置した位置検出センサ３４から得られる位置検出情報であるロボット１０の各関節角度のデータをもとに計算することができる。その際、前述の座標系の統一処理と同様に、リンク間長さから変位の並進ベクトルを求め、関節角度から３×３の回転行列を求め、順運動学を用いる。これにより、ロボット１０の３次元での位置および姿勢、つまりロボット座標系Σｒからロボット１０の先端座標系Σｆまでの変位（ＦＸ，ＦＹ，ＦＺ）と回転行列ＦＭとを求めることができる。

上記により求まる専有位置は、前述の座標系の統一によってワールド座標系Σｗにおける位置情報として計算される。そのため、計算された専有位置に該当するボクセルを削ることができる。ロボット１０が対象物体４１あるいは周辺物体４２に接触している場合でも、力計測部２０７で計測している力が上限値よりも小さい場合は、ロボット１０は動き続ける。したがって、この場合、地図情報作成部２０３は、ロボット１０が対象物体４１にはまだ完全に接触していないと判断し、ロボット１０が動き続けると同時に空間的な掃引を行い、ボクセルを削っていく。対象物体４１がビニール袋のようなもので包まれている場合、ビニール袋のみが存在する空間では、エンドエフェクタ１１がビニール袋を押し潰すことでロボット１０は動作を継続する。そのため、ビニール袋のみが存在する空間に対応するボクセル内の領域は削られることになる。

ロボット１０が対象物体４１に接触し、ロボット１０にかかる力が上限値に達すると、その時点でロボット１０は動作を止める。このとき、地図情報作成部２０３は、ボクセルを削る処理を止め、接触箇所までに掃引した空間は物体がなく、接触した位置は物体があると判断して、地図情報の作成を終了する。
なお、地図情報の作成にあたっては、ボクセルを配置しない手法を用いてもよい。例えば、３次元点群データである表面位置情報を面形式データに変換し、ポリゴンを用いて対象物体４１や周辺物体４２の表面位置を表現してもよい。この場合、ポリゴンにより作成された表面位置情報を初期値として、ロボット１０が掃引した空間に対する３次元の位置情報をもとに表面生成を行い、地図情報を作成してもよい。また２次元で地図情報を作成する場合には、ボクセルではなくピクセルを用いて、２次元平面上において上記と同様の手法を用いて地図情報を作成してもよい。

Ｓ１１では、把持位置決定部２１０は、Ｓ１０における地図情報の作成結果をもとに、ロボット１０のエンドエフェクタ１１によって対象物体４１を把持可能であるか否かを判定する。そして、把持が可能である場合には計測を終了すると判断し、把持が不可能である場合には計測を継続すると判断する。
具体的には、地図情報をもとに把持対象となる対象物体４１の位置および姿勢を推定し、推定した対象物体４１の位置および姿勢と、ロボット１０のエンドエフェクタ１１の形状とに基づいて把持可否を判断する。エンドエフェクタ１１が対象物体４１の側面を上方から挟んで把持する場合、エンドエフェクタ１１が対象物体４１の側面に入り込む必要がある。その場合、まず、地図情報をもとに、対象物体４１の上にロボット１０が移動できる空間があるか否かを判断する。空間の有無は、地図情報において、ボクセルが取り除かれているか否かで判断する。

空間がある場合、ロボット１０が対象物体４１の上まで移動できると判断する。すると、次に、エンドエフェクタ１１が対象物体４１の側面に入る空間があるか否かを、接触を伴う動作によって判断する。そして、対象物体４１の側面にエンドエフェクタ１１が入れる空間がある場合には、エンドエフェクタ１１が対象物体４１を把持可能であるため、計測を終了すると判断してＳ１２に移行する。一方、エンドエフェクタ１１が対象物体４１を把持できないと判断した場合には、Ｓ５に戻って計測を続行する。このように、把持位置決定部２１０は、周囲にロボット１０の把持部であるエンドエフェクタ１１を挿入可能な空間を有する位置を、ロボット１０が対象物体４１を把持可能な位置として決定する。

また、Ｓ１１で把持可能でないと判断した場合、Ｓ５の計測位置決定の処理を再び行うが、このとき、ここまでの処理の結果を利用してもよい。例えば、Ｓ１１にて把持可能かの判断をするにあたり、ロボットの動作が行われているが、その計測位置決定を行う際、次の計測位置は別の計測位置から求めるといった方法である。具体的には、今回の計測位置決定にあたり、候補となる位置が複数あるとする。このとき、Ｓ５の説明で述べたように、表面の凹凸状態などを利用して、対象物体４１の存在確率が高い部分を計測位置として決定している。この存在確率を評価関数として利用する場合、今回の計測ではこの評価関数が最大値を示した位置を計測位置として決定しているが、次回の計測では評価関数が最大値の次に大きい値を示した位置を計測位置として決定する。これらの結果は作成した地図情報に含んでもよい。このように、単に同じ処理を繰り返すだけでなく、今回の処理の結果を地図情報に随時更新することで、次回の処理において利用してもよい。こうすることで、ロボット１０が把持可能でない位置へ何度も移動することがなくなる。また、ロボット１０は把持可能性の高い順に移動することができる。そのため、一回目の処理で対象物体４１の把持に失敗しても、二回目以降の処理で把持できる確率が上がる。

なお、Ｓ１１において、図４では「把持可能か？」と記載しているが、これは単に把持ができるかできないかだけでなく、把持動作を実行できる位置までロボット１０が移動できるかどうかという判断も含む。ロボット１０が移動する際、想定外の障害などで、把持動作を実行する予定の位置まで移動できない場合が生じる可能性もある。その場合は、把持可能ではないと判断し、Ｓ５の処理を行う。

Ｓ１２では、把持位置決定部２１０は、Ｓ１０において作成された地図情報に基づいて、対象物体４１の把持位置を決定する。把持位置決定部２１０は、Ｓ１１において把持可能であると判断された位置の中から対象物体４１の把持位置を決定する。
Ｓ１３では、行動計画部２０５は、Ｓ１２において決定された把持位置による対象物体４１の把持動作を実現するためのロボット１０の行動計画を決定する。行動計画を決定する際には、行動計画部２０５は、把持位置をもとに、予め外部メモリ２４等に格納された動作データベース（動作ＤＢ）を参照する。動作ＤＢは、部品種および作業手順の情報を保持している。なお、ロボットの行動計画は、一般的な経路探索アルゴリズムを利用して決定することができる。経路探索アルゴリズムとしては、例えば、ＲＲＴ（Rapidly-exploring Random Tree）やＰＲＭ（Probabilistic RoadMap）などのサンプリングベース探索法を用いることができる。その際、地図情報を利用して、どこに対象物体４１があるか、どこに障害物があるか、といった３次元の位置情報を入力する。このような手法を用いることで、行動計画部２０５は、障害物を回避しながら対象物体４１を把持できるロボット１０の行動計画を決定する。ただし、同様の結果が得られるのであれば、別の手法を用いてもよい。

次にＳ１４では、行動計画部２０５は、Ｓ１３において決定した行動計画に基づいて、ロボット１０へ動作指示情報を出力する。これにより、ロボット１０は、エンドエフェクタ１１を把持位置まで移動し、把持位置において対象物体４１に対する把持動作を行う。ロボット１０によって把持された対象物体４１は、動作ＤＢに蓄積された作業手順に則って所定の場所へ搬送される。
Ｓ１５では、ＣＰＵ２１は、画像取得部２０１が取得した画像情報をもとに、認識対象となる次の対象物体４１が存在するか否かを判定する。そして、ＣＰＵ２１は、対象物体４１が存在しない場合には認識処理を終了すると判断して図４に示す処理を終了し、対象物体４１が存在する場合には、認識処理を継続すると判断してＳ２に戻る。

このように、情報処理装置２０は、撮像装置３１から得られる画像情報をもとに計測した表面位置情報に基づいて、より詳しく対象物体４１の位置および姿勢を計測するための計測位置を決定し、決定した計測位置へロボット１０を移動させる。そして、計測位置においてロボット１０を対象物体４１に実際に接触させ、その接触動作中に力覚センサ３３および位置検出センサ３４を用いて計測された力情報および位置検出情報に基づいて、対象物体４１の詳細位置を計測する。対象物体４１の位置および姿勢は、地図情報を作成することで認識する。これにより、撮像装置３１のみでは位置および姿勢を正確に認識できないような、ビニール袋や緩衝材に包まれた対象物体４１であっても、適切に把持動作のための認識が可能となる。したがって、対象物体４１の把持できるケースが増え、作業の効率化が図れる。

なお、本実施形態においては、対象物体４１に対する接触動作は１回としているが、一度の接触動作によって対象物体４１を認識できない場合には、複数回触って計測を繰り返して地図情報を作成してもよい。これにより、認識の精度を向上させることができる。
以上のように、本実施形態では、表面位置計測部２０２が、物体（対象物体４１および周辺物体４２）の表面位置情報を計測（取得）し、計測位置決定部２０４が、表面位置情報に基づいて対象物体４１を実測するための計測位置を決定する。また、力計測部２０７および位置計測部２０９は、計測位置において対象物体４１に接触したときの接触位置情報を計測する。そして、地図情報作成部２０３は、表面位置情報と接触位置情報とに基づいて、対象物体４１の位置および姿勢を認識する。

このように、対象物体４１に実際に触って接触位置情報を計測するので、対象物体４１の詳細位置を確認することができる。これにより、ビニール袋や緩衝材に包まれた中身である対象物体４１の位置および姿勢を適切に認識することができる。つまり、表面位置情報からだけでは、対象物体４１の実際の位置および姿勢が認識できない場合であっても、対象物体４１の実際の位置および姿勢の認識が可能となる。したがって、ロボット１０により対象物体４１を把持できるケースが増え、作業の効率化が図れる。

また、地図情報作成部２０３は、表面位置情報と接触位置情報とに基づいて、対象物体４１の空間的な位置情報を示す地図情報を作成し、作成した地図情報に基づいて、対象物体４１の位置および姿勢を認識する。このように、対象物体４１に実際に触って確認した位置情報を考慮して地図情報を作成するので、適切に対象物体４１の位置および姿勢を認識することができる。
さらに、地図情報作成部２０３は、地図情報の作成に際し、まず、表面位置情報に基づいて地図情報を作成し、次いで、計測位置において計測された接触位置情報に基づいて、表面位置情報に基づいて作成された地図情報を更新する。このように、撮像装置３１による対象物体４１の表面位置計測と、力覚センサ３３および位置検出センサ３４による位置計測の結果とを統合して地図情報を作成する。これにより、表面位置情報に基づいて作成したおおまかな地図情報において不足する情報を適切に補って最終的な地図情報を作成することができる。したがって、撮像装置３１から取得される情報だけでは対象物体４１の位置および姿勢が求まらない場合であっても、対象物体４１の形状を適切に認識してロボット１０による操作が可能になる。

また、計測位置決定部２０４は、表面位置情報に基づいて、表面が凸形状である位置を検出し、検出した位置を計測位置として決定する。したがって、対象物体４１および周辺物体４２が混在する中から、対象物体４１の存在確率が高い部分を選択して実測することができる。
さらに、表面位置計測部２０２は、対象物体４１および周辺物体４２を撮像した画像情報に基づいて表面位置情報を取得する。そのため、容易に表面位置情報を取得することができる。

また、力計測部２０７は、対象物体４１に接触したときの反力を計測（取得）し、位置計測部２０９は、対象物体４１との接触点の位置を計測（取得）する。そして、位置計測部２０９は、力計測部２０７によって計測された反力が所定の上限値に達したときに接触点の位置を計測することで、接触位置情報を計測する。したがって、対象物体４１がビニール袋のようなやわらかい部材で包まれている場合には、対象物体４１を包んでいる部材を潰して対象物体４１に直接接触し、接触位置情報を計測することができる。その際、ビニール袋や緩衝材を潰し中身である対象物体４１を壊さない力を上記上限値として設定すれば、正確に対象物体４１の位置および姿勢を認識することができる。

把持位置決定部２１０は、地図情報作成部２０３により作成された地図情報から認識される対象物体４１の位置および姿勢に基づいて、ロボット１０が対象物体４１を把持可能な把持位置を決定する。このように、把持位置決定部２１０は、対象物体４１に実際に触って確認した位置情報を考慮して作成された地図情報をもとに把持位置を決定する。したがって、高精度で把持可能な位置を決定することができ、ロボット１０による把持動作を高確率で成功させることができる。また、このとき、把持位置決定部２１０は、周囲にロボット１０の把持部を挿入可能な空間を有する位置を把持位置として決定するので、適切に把持位置を決定することができる。

（第二の実施形態）
次に、本発明の第二の実施形態について説明する。
第二の実施形態では、上述した第一の実施形態に対し、さらに把持動作時に把持力を計測する把持力計測センサを追加し、把持力計測センサによって計測された把持力情報を利用して、地図情報をより詳細に作成するよう構成したものである。具体的には、まず、上述した第一の実施形態と同様に、撮像装置３１を用いて対象物体４１および周辺物体４２の表面位置情報を計測する。次に、上述した第一の実施形態と同様に、表面位置情報に基づいて対象物体４１の計測位置を決定し、計測位置において対象物体４１に対する接触動作を行う。そして、接触動作時に、力覚センサ３３および位置検出センサ３４を用いて、ロボット１０が対象物体４１に接触しているときの位置情報を計測し、計測した位置情報に基づいて対象物体４１の把持位置を決定する。本実施形態では、把持位置における把持動作時に、把持力計測センサと位置検出センサ３４とを用いて対象物体４１の把持幅情報を計測する。そして、以上の表面位置情報、位置情報、および把持幅情報の計測結果を統合して地図情報を作成することにより、対象物体４１を把持するための形状を認識する。

第一の実施形態では、ロボット１０が対象物体４１を把持する前までの情報から対象物体４１を認識した。そのため、大きさの異なる複数種類の物体中に目標の対象物体４１が混在する場合、必ずしも目標の対象物体４１を把持できるとは限らない。そこで、第二の実施形態では、把持動作時に計測した把持幅を部品ＤＢと照合し、目標の対象物体４１の大きさ（幅）を参照することで、把持動作によりロボット１０が把持した物体が目標の対象物体４１であるか否かを判定する。また、把持動作時も含めて地図情報を作成するようにする。

図６は、第二の実施形態におけるロボットシステム１００の構成例である。この図６において、上述した第一の実施形態と同様の構成を有する部分には、図１と同一符号を付し、以下、構成の異なる部分を中心に説明する。
本実施形態におけるロボットシステム１００は、把持力計測センサ３５がロボット１０のエンドエフェクタ１１先端に取り付けられている点で図１とは異なる。把持力計測センサ３５は、歪みゲージや圧電素子により構成され、ロボット１０が対象物体４１を把持しているときの把持力を計測し、計測結果である把持力覚情報を情報処理装置２０´へ出力する。把持力計測センサ３５は、６軸力覚センサであってもよいし、３軸力覚センサであってもよい。また、把持力計測センサ３５は、１次元の圧力センサであってもよいし、接触の有無を判別する接触センサであってもよい。

図７は、本実施形態における情報処理装置２０´の機能ブロック図である。図７に示す情報処理装置２０´は、図２に示す情報処理装置２０に対して、把持力覚情報取得部２１１と、把持力計測部２１２とが追加されている点が異なる。
把持力覚情報取得部２１１は、把持力計測センサ３５から出力される把持力覚情報を取得し、取得した把持力覚情報を把持力計測部２１２へ出力する。把持力覚情報取得部２１１は、例えばメモリ（ＲＡＭ）によって構成される。把持力計測部２１２は、把持力覚情報取得部２１１から入力した把持力覚情報に基づいて、ロボット１０のエンドエフェクタ１１によって物体を把持したときのロボット１０にかかる力を計測する。把持力計測部２１２は、計測した把持力情報を位置情報計測部２０９に出力する。表面位置計測部２０２、地図情報作成部２０３、計測位置決定部２０４、力計測部２０７、位置計測部２０９および把持力計測部２１２によって、対象物体４１の位置および姿勢を認識する処理を行う認識部２２０´を構成している。

図８は、情報処理装置２０´が実行するロボット制御処理手順を示すフローチャートである。図８に示す処理は、図４のＳ１４とＳ１５との処理の間に、Ｓ２１〜Ｓ２６の処理が追加されていることを除いては、図４と同様の処理を行う。したがって、以下、処理の異なる部分を中心に説明する。
Ｓ２１では、把持力覚情報取得部２１１は、把持力計測センサ３５が出力する把持力覚情報を取得する。また、位置情報取得部２０８は、把持力覚情報取得部２１１による把持力覚情報の取得に同期して、位置検出センサ３４が出力する位置検出情報を取得する。次にＳ２２では、把持力計測部２１２は、Ｓ２１において把持力覚情報取得部２１１が取得した把持力覚情報に基づいて、ロボット１０が対象物体４１を把持したときの把持力情報を計測する。また、位置計測部２０９は、Ｓ２１において位置情報取得部２０８が取得した位置検出情報に基づいて、ロボット１０が対象物体４１を把持したときのエンドエフェクタ１１先端の位置を計測する。つまり、位置計測部２０９は、ロボット１０が把持した対象物体４１の把持幅情報を計測する。

把持力情報および把持幅情報は、ロボット１０の対象物体４１に対する把持動作を行っている間、繰り返し計測される。上記把持動作は、対象物体４１を把持したときにロボット１０にかかる力の大きさが、予め設定された上限値を超えない範囲で実施するものとする。ロボット１０にかかる力の大きさが上限値を超えたか否かは、Ｓ２２において計測される把持力情報に基づいて判定する。把持動作を行う中で、ロボット１０にかかる力は、対象物体４１への把持力に応じて増加する。そこで、上記の力の上限値は、把持動作により対象物体４１に破損等の不具合が生じない程度の力に設定する。例えば、対象物体４１がビニール袋に包まれたトナーカートリッジである場合、ビニール袋を潰しトナーカートリッジを壊さない程度の力を上限値として設定する。

ロボット１０がビニール袋を潰してトナーカートリッジの表面に当接し、ロボット１０にかかる力が上限値に達すると、情報処理装置２０´はロボット１０の把持動作を停止する。把持動作を停止した時点でのロボット１０のエンドエフェクタ１１による把持幅は、対象物体４１の実際の大きさとして認識することができる。
Ｓ２３では、地図情報作成部２０３は、Ｓ２２において計測した把持力情報および把持幅情報に基づいて、Ｓ１０において作成された地図情報を更新し、新たな地図情報を作成する。このＳ２３では、地図情報作成部２０３は、Ｓ２２において計測した把持幅を、対象物体４１の大きさを示す情報として利用する。このように、地図情報作成部２０３は、対象物体４１を実際に把持して得られた把持幅情報を加味して地図情報を作成し、対象物体４１を認識する。

地図情報作成部２０３は、まず各座標系の統一処理を行う。ロボット１０、撮像装置３１、力覚センサ３３の座標系の統一については第一の実施形態において述べたとおりである。したがって、以下、把持力計測センサ３５の座標系について述べる。本実施形態では、ロボット１０の先端座標系Σｆから把持力計測センサ座標系Σｇまでの変位を（ＧＸ，ＧＹ，ＧＺ）とする。また、把持力計測センサ３５の姿勢を表す３×３の回転行列をＧＭとする。ここで、ワールド座標系Σｗから見た、把持動作時にロボット１０にかかる把持力を４×４の行列ＧＴとすると、行列ＧＴは以下の（４）式で求まる。この（４）式を用いて、把持力の方向をワールド座標系Σｗで考慮することができる。

地図情報作成部２０３は、地図情報を作成する際には、上記（４）式と、上述した（１）〜（３）式とを用いて、異なる複数のセンサの座標系を統一する。そして、地図情報作成部２０３は、座標系が統一されたら、各センサから得られた情報を利用して地図情報を作成していく。このＳ２３では、地図情報作成部２０３は、Ｓ１０において作成した地図情報に対して、実際に対象物体４１を把持した結果を付加する処理を行う。
Ｓ２４では、ＣＰＵ２１は、Ｓ２３において作成された地図情報と、部品ＤＢを参照して得られる対象物体４１の形状情報とをもとに、ロボット１０が把持している物体が目標の対象物体４１であるか否かを判定する。判定の際には、対象物体４１を包んでいる周辺物体４２の変形具合を考慮し、対象物体４１の取りうる幅を推定する。そして、対象物体４１の把持幅が、推定した取りうる幅以内である場合には、ＣＰＵ２１は、ロボット１０が把持している物体が対象物体４１であると判断する。把持している物体が対象物体４１であると判断した場合には、動作を継続すると判断してＳ２５へ移行する。一方、把持している物体が対象物体４１ではないと判断した場合にはＳ１２へ戻り、再度、把持位置を決定し、地図情報の作成を繰り返す。

Ｓ２５では、行動計画部２０５は、把持した対象物体４１に対する操作を実現するためのロボット１０の行動計画を決定する。行動計画を決定する際には、行動計画部２０５は、把持位置をもとに、動作ＤＢに蓄積されている部品種と作業手順の情報を参照する。
次にＳ２６では、行動計画部２０５は、Ｓ２５において決定した行動計画に基づいて、ロボット１０へ動作指示情報を出力する。これにより、ロボット１０は、エンドエフェクタ１１によって把持した対象物体４１を、動作ＤＢに蓄積された作業手順に則って所定の場所へ搬送する。
このように、本実施形態では、上述した第一の実施形態と同様の手法により対象物体４１を認識し、対象物体４１を把持したとき、把持力計測センサ３５を用いて把持した物体の把持幅を計測する。そして、計測した把持幅に基づいて、把持した物体が対象物体４１であるか否かの判定を行い、地図情報をより詳細に作成する。これにより、大きさの異なる複数種類の対象物体４１が存在する場合であっても、それぞれ高精度で認識することができる。そのため、異なる種類の対象物体４１を正確に把持できるケースが増え、作業を継続できる可能性を拡大することができる。

具体的には、把持力計測部２１２は、把持位置において対象物体４１を把持したときの把持幅情報を計測し、地図情報作成部２０３は、把持幅情報をさらに用いて対象物体４１の位置および姿勢を認識する。したがって、より詳細に対象物体４１を把持するための形状を認識することができる。例えば、対象物体４１が大きさの異なる複数種類である場合にも、より高い精度で対象物体４１の位置および姿勢を認識し、ロボット１０による作業を継続できる可能性を拡大することができる。また、実際に対象物体４１を把持したときの把持幅を計測するので、対象物体４１の大きさを精度良く計測することができる。したがって、把持した物体が目標の対象物体４１であるか否かを適切に判断することができる。

（第三の実施形態）
次に、第三の実施形態について説明する。
第三の実施例では、上述した第一、第二の実施例に対し、オクルージョンのある領域にエンドエフェクタ１１を挿入したときに、作成した地図情報を利用することにより、周辺物体４２と干渉することのないロボット１０の行動計画を行うことができる。ここで、ロボットの制御の処理手順は図４に示すフローチャートと同じであるが、Ｓ１３での処理が異なる。
オクルージョンのある領域にエンドエフェクタ１１を挿入する場合とは、例えば図９（ａ）のように、撮像装置３１が撮像できる領域Ａ０に対して、対象物体４１が存在する領域が周辺物体４２などにより遮蔽されている状態にある場合である。この場合、図９（ｂ）のように、対象物体４１を認識できる領域Ａ１と、撮像装置３１が対象物体４１を撮像しようとしても対象物体４１を認識できない領域Ａ２とが発生してしまう。この状態にあるとき、対象物体４１を把持できたとしても、対象物体４１を搬送して取り出すときに、撮像装置３１によって撮像できない周辺物体４２に干渉してしまうことで、把持している対象物体４１を落下させてしまうおそれがある。

そこで、本実施形態では、Ｓ１３の処理において、Ｓ４、Ｓ１０で作成・更新された地図情報を利用する。対象物体４１を把持するまでに作成した地図情報は、対象物体４１に限らず、周辺物体４２に関する情報も含んでいる。そのため、ロボット１０が対象物体４１を把持してから搬送し、取り出すときの行動計画においては、周辺物体４２の存在する空間的な情報を考慮し、ロボット１０が移動可能な空間を限定する。
周辺物体４２の位置情報取得にあたっては、力覚センサ３３に代えて、接近した物体までの距離を検出する近接覚センサを用いてもよい。この場合、計測位置において、対象物体４１に接触しているときに近接覚センサにより対象物体４１までの距離情報を直接取得することで接触位置情報を計測し、地図情報を作成することができる。近接覚センサが図１や図６に示す力覚センサ３３と近い位置に設置されている場合、近接覚センサは、エンドエフェクタ１１の基部からロボット１０の対象物体４１に対する接触点の位置までの距離情報を取得することができる。さらに、近接覚センサを複数箇所に配置しておけば、一度に複数箇所の距離情報を計測して地図情報を作成することができる。そのため、撮像装置３１による計測ではオクルージョンがある場合、対象物体４１把持後の取出し動作において、対象物体４１の周辺の周辺物体４２に干渉することのないロボット１０の動作をより短時間に計画することができる。

また、力覚センサ３３と近接覚センサとを両方用いて計測してもよい。力覚センサ３３と近接覚センサとを両方利用することで、力覚センサ３３による計測により高い精度で位置計測を行いながら、近接覚センサにより複数箇所を一度に計測して地図情報を作成することができる。これにより、より短時間に対象物体４１と周辺物体４２とを認識し、ロボット１０の行動を適切に計画することができる。なお、近接覚センサは赤外線センサであってもよいし、磁気タイプや超音波タイプのセンサを用いてもよい。
こうすることによって、ロボット１０が把持している対象物体４１を搬送する際、搬送中に対象物体４１が周辺物体４２と干渉する可能性が低くなるので、搬送中に対象物体４１が落下する恐れが低くなり、作業効率を向上することができる。

（変形例）
上記各実施形態においては、ロボット１０が把持動作に失敗した場合、把持位置決定部２１０は、把持動作に失敗した把持位置を次に把持動作を行う把持位置候補から除外し、別の場所から把持位置を決定するようにしてもよい。このように、地図情報を作成することにより、ロボット動作における同じ失敗を防ぐための処理が可能となる。これにより、ロボット１０による作業時間を短縮することができる。

また、上記各実施形態においては、ロボット１０が対象物体４１に接触したときの力覚情報から対象物体４１の材質を認識し、認識した情報を利用して対象物体４１を把持するようにしてもよい。例えば、上記各実施形態のように、対象物体４１がビニール袋に包まれている場合、ロボット１０がビニール袋に接触したとき、わずかにロボット１０に力がかかる。その場合、ロボット１０にかかった力から、ビニール袋が存在する空間が分かるので、ビニール袋を把持して対象物体４１を操作（搬送）するようにしてもよい。これにより、対象物体４１そのものを把持できない場合であっても、対象物体４１を操作することができ、作業を継続できる可能性を向上させることができる。

さらに、上記各実施形態においては、力覚センサ３３を用いた対象物体４１の詳細位置計測の後に、撮像装置３１による対象物体４１の表面位置計測を再度行ってもよい。力覚センサ３３により複数回の計測が行われると、センサドリフトによりズレが蓄積されてしまう場合がある。したがって、適宜、撮像装置３１による表面位置計測を行うことで、上記ズレを補正して地図情報を作成することができる。これにより、センサドリフトのズレによる影響を抑制し、より正確に地図情報を作成できるので、作業を正確に行える可能性を向上させることができる。

また、上記各実施形態においては、撮像装置３１による計測を最初に複数回行い、表面位置情報をより詳細に計測してもよい。これにより、その後の力覚センサ３３や近接覚センサによる計測の回数を少なくすることができる。その結果、より短時間で対象物体４１を認識し、ロボット１０の行動を計画することができる。
さらに、上記各実施形態においては、位置および姿勢が認識された対象物体４１を、ロボットによって把持し搬送する場合について説明した。しかしながら、対象物体４１の位置および姿勢の認識が目的である場合、把持や搬送といったロボットによる操作は行わなくてもよい。本発明の効果は、表面位置情報だけでは対象物体４１を正確に認識できない場合であっても、対象物体４１の位置および姿勢を適切に認識できることである。そのため、認識された対象物体４１の位置および姿勢に関する情報を作業者に提示（表示）したり、認識された対象物体４１の位置および姿勢をもとに、対象物体４１の検査を行ったりしてもよい。ここで、対象物体４１の検査とは、対象物体４１の形状を確認したり、望ましい位置にあるか確認したりすることである。

また、認識された対象物体４１の位置および姿勢をもとに、ロボットが対象物体４１に対して把持を伴わない操作を行ってもよい。ここで、把持を伴わない操作とは、把持せずに対象物体４１に接触し、対象物体４１の位置を変化させる操作である。このように、対象物体４１に対してロボットが操作を実行する場合、その操作は、把持を伴う操作であってもよいし把持を伴わない操作であってもよい。
さらに、表面位置情報や接触位置情報を取得するためのロボットは、１台であってもよいし複数台であってもよい。また、表面位置情報や接触位置情報を取得するためのロボットと、対象物体４１に対して把持や搬送といった操作を実行するロボットとは、同じであってもよいし違っていてもよい。

また、上記各実施形態においては、ロボット１０は、組立型ロボットである場合について説明したが、組立型ロボットに限定されるものではなく、移動ロボットであってもよい。
さらに、上記各実施形態においては、地図情報は３次元の情報である場合について説明したが、地図情報は２次元の情報でもよい。また、力計測部２０７により計測された力情報と位置計測部２０９により計測された位置情報との２つの情報のみに基づいて、地図情報を作成してもよい。
また、上記各実施形態においては、撮像装置３１と光源３２とは、レーザーレンジファインダ装置であってもよい。さらに、光源３２を用いず、撮像装置３１を２つ用いてステレオ計測を行ってもよいし、光源３２を用いず撮像装置３１を１つだけ用いてもよい。つまり、対象物体４１と周辺物体４２との表面位置を認識するための表面位置情報を計測できる構成であればよい。

（その他の実施形態）
本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１０…ロボット、１１…エンドエフェクタ、２０，２０´…情報処理装置、３１…撮像装置、３２…光源、３３…力覚センサ、３４…位置検出センサ、３５…把持力計測センサ、４１…対象物体、４２…周辺物体、１００…ロボットシステム

Claims (16)

1. 対象物体と周辺物体からなる物体を認識する情報処理装置であって、
   物体の表面位置情報を取得する取得手段と、
   前記取得手段により取得された前記表面位置情報に基づいて、前記物体を実測するための計測位置を決定する決定手段と、
   前記決定手段により決定された前記計測位置において前記物体に接触したときの接触位置情報を計測する計測手段と、
   前記計測手段により計測された前記接触位置情報に基づいて、前記物体の空間的な位置情報を示す地図情報を作成する作成手段と、
   前記作成手段により作成された前記地図情報に基づいて、前記物体の位置および姿勢を認識する認識手段と、を備え、
   前記計測手段は、接近した前記物体までの距離を取得する距離の取得手段を備え、
   前記物体に接触しているときに前記距離の取得手段によって取得された距離に基づいて、前記接触位置情報を計測することを特徴とする情報処理装置。
2. 前記作成手段は、
   前記取得手段により取得された前記表面位置情報に基づいて前記地図情報を作成する第一の作成手段と、
   前記計測手段により計測された前記接触位置情報に基づいて、前記第一の作成手段により作成された前記地図情報を更新する第二の作成手段を備えることを特徴とする請求項１に記載の情報処理装置。
3. 前記決定手段は、
   前記取得手段により取得された前記表面位置情報に基づいて、前記物体の表面が凸形状である位置を検出し、検出した位置を前記計測位置として決定することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記取得手段は、
   前記物体を撮像した画像情報に基づいて、前記表面位置情報を取得することを特徴とする請求項１から３のいずれか１項に記載の情報処理装置。
5. 前記計測手段は、
   前記物体に接触したときの反力を取得する反力の取得手段と、
   前記物体との接触点の位置を取得する位置の取得手段と、をさらに備え、
   前記反力の取得手段によって取得された反力が所定の上限値に達したときに前記位置の取得手段によって取得された前記接触点の位置を、前記接触位置情報として計測することを特徴とする請求項１から４のいずれか１項に記載の情報処理装置。
6. 前記認識手段により認識された前記物体の位置および姿勢に基づいて、ロボットが前記物体を把持可能な把持位置を決定する第二の決定手段をさらに備えることを特徴とする請求項１から５のいずれか１項に記載の情報処理装置。
7. 前記第二の決定手段は、
   周囲に前記ロボットの把持部が挿入可能な空間を有する位置を、前記把持位置として決定することを特徴とする請求項６に記載の情報処理装置。
8. 前記第二の決定手段により決定された前記把持位置において前記物体を把持したときの把持幅情報を計測する第二の計測手段をさらに備え、
   前記認識手段は、
   前記第二の計測手段により計測された前記把持幅情報をさらに用いて、前記物体の位置および姿勢を認識することを特徴とする請求項６または７に記載の情報処理装置。
9. 前記請求項１から８のいずれか１項に記載の情報処理装置に、前記物体に対して作業を実行するロボットの動作を制御する制御手段を備えることを特徴とするロボット制御装置。
10. 前記請求項９に記載のロボット制御装置に、センサおよび前記ロボットの少なくとも一方を備えることを特徴とするロボットシステム。
11. 前記センサは、前記物体を撮像し画像情報を取得する視覚センサを含むことを特徴とする請求項１０に記載のロボットシステム。
12. 前記センサは、前記物体に接触したときの反力を取得する力覚センサを含むことを特徴とする請求項１０または１１に記載のロボットシステム。
13. 前記センサは、前記ロボットの位置および姿勢を取得する位置検出センサを含むことを特徴とする請求項１０から１２のいずれか１項に記載のロボットシステム。
14. 前記センサは、前記物体を把持したときの反力を取得する力覚センサを含むことを特徴とする請求項１０から１３のいずれか１項に記載のロボットシステム。
15. 対象物体と周辺物体からなる物体の表面位置情報を取得するステップと、
    取得された前記表面位置情報に基づいて、前記物体を実測するための計測位置を決定するステップと、
    決定された前記計測位置において前記物体に接触したときの接触位置情報を計測するステップと、
    計測された前記接触位置情報に基づいて、前記物体の空間的な位置情報を示す地図情報を作成するステップと、
    作成された前記地図情報に基づいて、前記物体の位置および姿勢を認識するステップと、を含み、
    前記接触位置情報を計測するステップは、接近した前記物体までの距離を取得するステップを含み、
    前記物体に接触しているときに前記距離を取得するステップにおいて取得された距離に基づいて、前記接触位置情報を計測することを特徴とする情報処理方法。
16. コンピュータを、前記請求項１から８のいずれか１項に記載の情報処理装置の各手段として機能させるためのプログラム。

Images