(57)【特許請求の範囲】
【請求項１】
操作対象物を操作するロボットハンドと、
前記ロボットハンドの関節角度を検出し、検出した関節角度を示す関節角度情報を生成する関節角度取得部と、
前記操作対象物までの距離を検出し、検出した距離を２次元空間上の点列で示す２次元距離情報を生成する２次元距離センサと、
前記操作対象物を撮像し、撮像した操作対象物を画像で示す画像情報を生成する撮像センサと、
前記関節角度取得部が生成した関節角度情報が示す関節角度に基づいて、前記２次元距離センサが生成した２次元距離情報が示す点列から、前記操作対象物の点列を抽出するとともに、前記撮像センサが生成した画像情報が示す画像におけるエッジを検出し、検出したエッジのうち、前記抽出した点列を前記画像上に投影した場合における当該投影した点列との一致度が所定基準を満たすエッジを抽出する候補線分抽出部と、
前記抽出したエッジの形状変化に基づいて、前記操作対象物の位置姿勢変化を推定する位置姿勢変化推定部と、を備えた
操作対象物の状態推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、操作対象物の状態推定装置に関し、特に、ロボットハンドにより操作対象物を操作する技術に関する。
【背景技術】
【０００２】
ロボットが扉や引き出しを開ける等の家具操作を行う際、家具の操作量を常に把握し、操作の成否に応じて次の行動を決定していく必要がある。すなわち、ロボットは、操作に失敗した場合には、迅速なリカバリ動作を行う必要がある。例えば、ロボットは、扉や引き出しを開け損ねた場合、再度、扉や引き出しを開ける動作を行う必要がある。
【０００３】
特許文献１には、ワークの三次元位置姿勢及び形態を簡便かつ迅速に計測し、計測された情報によりロボットアーム先端部及びロボットハンドを迅速かつ精密に操作することを目的とした装置が開示されている。この装置は、ワークを撮影するカメラと、ハンド部を有するロボットアームと、カメラにより撮影された画像を処理する画像処理部と、処理された画像に基づいてワークの三次元位置姿勢や形態を検出する検出部と、検出された情報に基づいてアーム先端部とロボットハンドを操作する制御部とから構成される。そして、上記カメラは、ステレオカメラであり、ロボットアームの先端部に装着されている。
【０００４】
しかしながら、家具の一部（扉や引き出し等）が、どのように動いたのかを確認するために、上記特許文献１に開示の装置のように、ステレオカメラのみを使用すると（またＲＧＢ−Ｄセンサのみを使用した場合であっても）、家具の操作中はロボットと家具が近すぎるため、深度情報を取得することができないという問題がある。
【０００５】
より具体的には、ＲＧＢ−Ｄセンサで深度情報を取得する場合、赤外光パターン照射が用いられるが、照射角度の関係で近距離になると照射が難しくなる。また、ステレオカメラで深度情報を取得する場合、２つのカメラが共通に目印とできる箇所をできる限り多く見つける必要がある。そのためには、２つのカメラのそれぞれで得られる画像内に、多様な形や色が見られる必要があるが、家具に接近すればするほど、形状や色の変化が少ない画像しか得られなくなる。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−２４１２４７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
上述したように、特許文献１に開示の技術では、家具操作中に、ロボットから家具までの距離を正確に測定することができないため、実際の家具の操作量を正確に把握することができず、家具操作の成否（実際の家具の操作量が、ロボットが目標する操作量と一致しているか否か）を確認することができないという問題がある。
【０００８】
すなわち、特許文献１に開示の技術では、操作対象物（例えば家具）の位置姿勢変化を正確に把握することができないという問題がある。
【０００９】
本発明は、操作対象物の位置姿勢変化を、より正確に推定することができる操作対象物の状態推定装置を提供するものである。
【課題を解決するための手段】
【００１０】
本発明の態様に係る操作対象物の状態推定装置は、操作対象物を操作するロボットハンドと、前記ロボットハンドの関節角度を検出し、検出した関節角度を示す関節角度情報を生成する関節角度取得部と、前記操作対象物までの距離を検出し、検出した距離を２次元空間上の点列で示す２次元距離情報を生成する２次元距離センサと、前記操作対象物を撮像し、撮像した操作対象物を画像で示す画像情報を生成する撮像センサと、前記関節角度取得部が生成した関節角度情報が示す関節角度に基づいて、前記２次元距離センサが生成した２次元距離情報が示す点列から、前記操作対象物の点列を抽出するとともに、前記撮像センサが生成した画像情報が示す画像におけるエッジを検出し、検出したエッジのうち、前記抽出した点列を前記画像上に投影した場合における当該投影した点列との一致度が所定基準を満たすエッジを抽出する候補線分抽出部と、前記抽出したエッジの形状変化に基づいて、前記操作対象物の位置姿勢変化を推定する位置姿勢変化推定部と、を備えたものである。
【００１１】
これによれば、操作対象物と、それを操作する状態推定装置との位置が近く、状態推定装置から家具までの距離を２次元距離センサによって正確に測定することができない場合であっても、画像における操作対象物のエッジの形状変化に基づいて、操作対象物の位置姿勢変化を推定するようにしているため、操作対象物の位置姿勢変化を、より正確に推定することができる。
【発明の効果】
【００１２】
上述した本発明の態様によれば、操作対象物の位置姿勢変化を、より正確に推定することができる操作対象物の状態推定装置を提供することができる。
【図面の簡単な説明】
【００１３】
【図１】実施の形態に係るロボットの外部構成図である。
【図２】実施の形態に係るロボットの内部構成図である。
【図３】実施の形態に係るロボットの処理を示すフローチャートである。
【図４】実施の形態に係るロボットの処理を示すフローチャートである。
【図５】実施の形態に係るロボットの処理を説明するための図である。
【図６】実施の形態に係るロボットの処理を説明するための図である。
【発明を実施するための形態】
【００１４】
以下に図面を参照しながら、本発明の好適な実施の形態について説明する。以下の実施の形態に示す具体的な数値等は、発明の理解を容易とするための例示にすぎず、特に断る場合を除き、それに限定されるものではない。また、以下の記載及び図面では、説明の明確化のため、当業者にとって自明な事項等については、適宜、省略及び簡略化がなされている。
【００１５】
＜発明の実施の形態＞
まず、本実施の形態に係るロボット１について説明する。図１を参照して、本実施の形態に係るロボット１の外部構成について説明する。図１は、本実施の形態に係るロボット１の外部構成図である。
【００１６】
ロボット１は、本体１０と、移動台車２０と、アーム３０と、頭部４０と、把持部５０とを有している。ロボット１は、典型的には、家庭内において人が行う作業を、人に代わり実行するための生活支援ロボットである。ロボット１は、例えば、扉や引き出しを開けるといった家具の操作をする、任意の場所に移動し、物を拾い人に届ける等の作業を実行する。
【００１７】
移動台車２０は、その上方で本体１０を支持するように、その上部が本体１０の下部に連結されている。移動台車２０は、その下部に設けられた車輪（図示せず）を駆動することでロボット１を移動させる。
【００１８】
本体１０は、その前方を観測可能となるように、２次元レーダー測域センサ１１が取り付けられている。２次元レーダー測域センサ１１は、ロボット１（２次元レーダー測域センサ１１）から、ロボット１の前方の各物体（例えば家具）までの距離を計測し、計測した各物体までの距離を示す２次元距離情報を生成する。
【００１９】
ここで、２次元レーダー測域センサ１１は、近距離でも広角で観測が可能な２次元走査型の距離センサである。すなわち、２次元レーダー測域センサ１１は、ロボット１の周辺を、ロボット１の上下方向と直行する平面上で扇状に走査することで各物体表面を線状になぞり、その線上における各点までの距離を示す２次元距離情報を生成する。よって、２次元距離情報は、ロボット１（２次元レーダー測域センサ１１）から、各物体表面の各点までの距離を、２次元空間上の点列で示す情報となる。つまり、この点列は、物体表面の距離の差が少ない場合には、線形の連続的な点列となり、物体表面の距離の差が大きい場合には、非線形な点列となる。よって、壁の前に家具が置かれている場合には、壁と家具のそれぞれの表面における各点までの距離は、線形の連続的な点列で示され、壁と家具の境界を挟んで隣接する２つの点は、非線形の点列で示される。
【００２０】
アーム３０は、本体１０の前方に突出するように、本体１０の前部に接続されている。アーム３０は、多関節のアームである。アーム３０は、その先端部に、任意の物体を把持できる把持部５０を含む。これにより、ロボット１は、アーム３０の各関節の角度を調整することで、把持部５０を所望の位置に移動させ、その位置に存在する物体（例えば家具）を操作することができる。アーム３０及び把持部５０は、ロボットハンドとして機能する。
【００２１】
頭部４０は、本体１０の上方で本体１０に支持されるように、本体１０の上部に連結されている。頭部４０は、その前方を観測可能となるように、その前部にＲＧＢセンサ４１が取り付けられている。ＲＧＢセンサ４１は、ロボット１の前方の物体（例えば家具）を撮像し、撮像した物体をＲＧＢ画像で示すＲＧＢ画像情報を生成するセンサである。
【００２２】
ＲＧＢセンサ４１は、撮像可能な角度範囲（画角）が所定の角度範囲となっている。頭部４０と本体１０との連結部は、頭部４０を動作可能となるように関節構造となっている。ロボット１は、頭部４０を動作させることで、ＲＧＢセンサ４１による撮像範囲を上下又は左右方向に調整することができる。ＲＧＢセンサ４１は、例えば、単眼カメラ、ステレオカメラ、又は、ＲＧＢ−Ｄカメラ等である。
【００２３】
上述した構成により、ロボット１は、物体（例えば家具）を操作するにあたり、２次元レーダー測域センサ１１及びＲＧＢセンサ４１によってロボット１が操作対象とする物体を観測し、観測した結果に基づいて、実際の物体の操作量を算出する。そして、ロボット１は、算出した実際の物体の操作量と、目標する物体の操作量とを比較することで、操作対象の物体の操作の成否を認識することが可能となる。
【００２４】
続いて、図２を参照して、本実施の形態に係るロボット１の内部構成について説明する。図２は、本実施の形態に係るロボット１の内部構成図である。
【００２５】
ロボット１は、２次元レーダー測域センサ１１と、制御部１２と、アクチュエータ２１ａ、２１ｂと、車輪２２ａ、２２ｂと、アクチュエータ３１ａ、３１ｂと、関節角度取得部３２ａ、３２ｂと、ＲＧＢセンサ４１と、アクチュエータ４２と、関節角度取得部４３とを有する。２次元レーダー測域センサ１１及び制御部１２は、本体１０に含まれる。アクチュエータ２１ａ、２１ｂ及び車輪２２ａ、２２ｂは、移動台車２０に含まれる。アクチュエータ３１ａ、３１ｂ及び関節角度取得部３２ａ、３２ｂは、アーム３０に含まれる。ＲＧＢセンサ４１、アクチュエータ４２及び関節角度取得部４３は、頭部４０に含まれる。
【００２６】
２次元レーダー測域センサ１１は、上述した通りである。２次元レーダー測域センサ１１は、生成した２次元距離情報を制御部１２に出力する。
【００２７】
制御部１２は、ロボット１を統括的に制御する。制御部１２は、ＣＰＵ（Central Processing Unit）及び記憶部（図示せず）を有しており、そのＣＰＵが記憶部に格納されたプログラムを実行することで、制御部１２としての各種処理を実現する。すなわち、このプログラムは、制御部１２が有するＣＰＵに制御部１２としての各所処理を実行させるためのコードを含んでいる。記憶部は、上記プログラムや各種情報を格納可能な記憶装置の少なくとも１つを含む。記憶装置として、例えば、メモリ及びハードディスク等の少なくとも１つを任意に使用してよい。
【００２８】
アクチュエータ２１ａ、２１ｂは、ロボット１の車輪２２ａ、２２ｂを駆動する。アクチュエータ２１ａは、制御部１２からの制御によって駆動され、車輪２２ａを回転させる。アクチュエータ２１ｂは、制御部１２からの制御によって駆動され、車輪２２ｂを回転させる。
【００２９】
車輪２２ａ及び車輪２２ｂは、上述した移動台車２０の下部に取り付けられた車輪に該当する。すなわち、制御部１２は、アクチュエータ２１ａ、２１ｂを制御して、車輪２２ａ、２２ｂを回転させることでロボット２を移動させる。
【００３０】
アクチュエータ３１ａ、３１ｂは、ロボット１のアーム３０を駆動する。アクチュエータ３１ａ、３１ｂは、制御部１２からの制御によって駆動され、アーム３０を動作させるアクチュエータ３１ａ、３１ｂのそれぞれは、アーム３０の関節として設けられている。なお、
【００３１】
関節角度取得部３２ａ、３２ｂのそれぞれは、アーム３０の各関節の角度を検出する角度センサを含む。この角度センサは、例えば、ロータリエンコーダである。関節角度取得部３２ａは、アクチュエータ３１ａに取り付けられており、アクチュエータ３１ａの関節角度を検出する。関節角度取得部３２ａは、検出した関節角度を示す関節角度情報を制御部１２に出力する。関節角度取得部３２ｂは、アクチュエータ３１ｂに取り付けられており、アクチュエータ３１ｂの関節角度を検出する。関節角度取得部３２ｂは、検出した関節角度を示す関節角度情報を制御部１２に出力する。これにより、制御部１２は、関節角度取得部３２ａ、３２ｂのそれぞれから出力された関節角度情報が示す関節角度に基づいて、アーム３０の状態を認識することが可能である。
【００３２】
ここでは、アーム３０が、２つの関節（２つのアクチュエータ及び関節角度取得部の組）を有する例について説明するが、アーム３０の関節（アクチュエータ及び関節角度取得部の組）の数は、これに限られない。
【００３３】
アクチュエータ４２は、ロボット１の頭部４０を駆動する。アクチュエータ４２は、制御部１２からの制御によって駆動され、頭部４０を動作させる。アクチュエータ４２は、頭部４０の関節として設けられている。
【００３４】
ＲＧＢセンサ４１は、上述した通りである。ＲＧＢセンサ４１は、生成したＲＧＢ画像情報を制御部１２に出力する。
【００３５】
関節角度取得部４３は、頭部４０の各関節の角度を検出する角度センサを含む。この角度センサは、例えば、ロータリエンコーダである。関節角度取得部４３は、アクチュエータ４２に取り付けられており、アクチュエータ４２の関節角度を検出する。関節角度取得部４３は、検出した関節角度を示す関節角度情報を制御部１２に出力する。これにより、制御部１２は、関節角度取得部４３のそれぞれから出力された関節角度情報が示す関節角度に基づいて、頭部４０の状態を認識することが可能となる。
【００３６】
ここでは、頭部４０が、１つの関節（１つのアクチュエータ及び関節角度取得部の組）を有する例について説明するが、頭部４０の関節（アクチュエータ及び関節角度取得部の組））の数は、これに限られない。
【００３７】
ここで、制御部１２は、候補線分抽出部１２１と、位置姿勢変化推定部１２２として機能する。
【００３８】
候補線分抽出部１２１は、関節角度取得部３２ａ及び関節角度取得部３２ｂから出力された関節角度情報のそれぞれが示す関節角度（アクチュエータ３１ａ及びアクチュエータ３１ｂのそれぞれの関節角度）に基づいて、２次元レーザー測域センサ１１から出力された２次元距離情報が示す点列から、ロボット１が操作対象とする物体の点列を抽出する。より具体的には、候補線分抽出部１２１は、２次元距離情報が示す複数の線形の点列から、ロボット１が操作対象とする物体における線形の点列を抽出する。
【００３９】
また、候補線分抽出部１２１は、ＲＧＢセンサ４１から出力されたＲＧＢ画像情報が示すＲＧＢ画像におけるエッジを検出する。これにより、ロボット１が操作対象とする物体のエッジを含む複数のエッジが検出される。そして、候補線分抽出部１２１は、検出した複数のエッジのうち、抽出した点列をＲＧＢ画像上に投影した場合におけるその投影した点列との一致度合いが所定基準を満たす少なくとも１つのエッジを、ロボット１が操作対象とする物体におけるエッジの候補として抽出する。
【００４０】
位置姿勢変化推定部１２２は、抽出したエッジの形状変化に基づいて、ロボット１が操作対象とする物体の位置姿勢変化を推定する。すなわち、ロボット１が操作対象とする物体の位置姿勢変化として、実際のその物体の操作量が算出される。
【００４１】
続いて、図３〜図６を参照して、本実施の形態に係るロボット１の動作について説明する。図３及び図４は、本実施の形態に係るロボット１の動作を示し、図５は、図３に示す各動作の模式図を示し、図６は、図４に示す各動作の模式図を示している。以下、ロボット１が操作対象とする物体が、棚（家具）であり、ロボット１がその棚の扉を開ける動作をする例について説明する。
【００４２】
ロボット１は、家具の操作準備（棚の扉の取っ手の把持部５０による把持）を終えたときに、ＲＧＢセンサ４１によってその家具を映し込む（Ｓ１）。すなわち、制御部１２は、アクチュエータ４２を駆動し、ＲＧＢセンサ４１の画角内にその家具を収める。
【００４３】
ロボット１は、ＲＧＢ画像情報、２次元距離情報、及び、関節角度情報を取得する（Ｓ２）。すなわち、制御部１２は、ＲＧＢセンサ４１から出力されたＲＧＢ画像情報と、２次元レーザー測域センサ１１から出力された２次元距離情報と、関節角度取得部３２ａ、３２ｂ、４３のそれぞれから出力された関節角度情報とを取得する。
【００４４】
なお、この各情報の取得は、各センサ１１、４１、３２ａ、３２ｂ、４３が所定時間間隔毎に測定及び情報の生成を行うようにし、ステップＳ１の動作を終了したときに制御部１２が各センサ１１、４１、３２ａ、３２ｂ、４３から出力された各情報を取得するようにしてもよく、ステップＳ１の動作を終了したときに各センサ１１、４１、３２ａ、３２ｂ、４３が測定及び情報の生成を行うようにしてもよい。
【００４５】
制御部１２の候補線分抽出部１２１は、取得した各情報に基づいて、家具面の候補となる線分を示す線分情報を取得する（Ｓ３）。
【００４６】
より具体的には、候補線分抽出部１２１は、ロボット構成情報が示すロボット１の構成と、アーム３０の関節角度取得部３２ａ、３２ｂのそれぞれから取得した関節角度情報が示す関節角度とに基づいて、２次元レーザー測域センサ１１に対する手先（アーム３０の把持部５０）の相対位置を算出する。手先に家具面が近接していると考えられるため、候補線分抽出部１２１は、算出した２次元レーザー測域センサ１１と手先との相対位置に基づいて、取得した２次元距離情報が示す複数の線形の点列から、手先位置の近くに存在する最も長い点列を抽出する。例えば、候補線分抽出部１２１は、手先位置の近くに存在する点列として、手先位置から所定範囲内に存在する複数の点列を抽出し、その複数の点列のうち、最も長い点列を抽出すればよい。これにより、ロボット１が操作対象とする物体の点列が抽出される。
【００４７】
ここで、ロボット構成情報は、制御部１２の記憶部に予め格納されている。ロボット構成情報は、ロボット１の構成として、ロボット１の各部（本体１０、アーム３０の各リンク、頭部４０及び把持部５０等）の物理形状、関節構成、及び、センサ構成などを示している。ロボット構成情報は、例えば、ＵＲＤＦ（Unified Robot Description Format）のようなロボット記述言語によって記述された情報である。
【００４８】
また、候補線分抽出部１２１は、ロボット構成情報が示すロボット１の構成と、頭部４０の関節角度取得部４３から取得した関節角度情報が示す関節角度とに基づいて、２次元レーザー測域センサ１１に対するＲＧＢセンサ４１の相対位置姿勢を算出する。候補線分抽出部１２１は、算出した相対位置姿勢に基づいて、抽出した点列を、ＲＧＢセンサ４１から取得したＲＧＢ画像情報が示すＲＧＢ画像に投影する。すなわち、候補線分抽出部１２１は、２次元レーザー測域センサ１１に対するＲＧＢセンサ４１の相対位置姿勢から、ＲＧＢ画像内において、抽出した点列に対応する位置を特定する。この際に、候補線分抽出部１２１は、ＲＧＢ画像に投影する点列として、抽出した点列の近似線分となる線分を、ＲＧＢ画像に投影する。この近似線分の導出は、例えばＮＵＲＢＳ（Non-Uniform Rational Basis Spline）フィッティングなどの各種技術のうち、任意の技術を採用して実施するようにしてよい。
【００４９】
候補線分抽出部１２１は、ＲＧＢ画像を解析して、例えば図５に示すような、複数のエッジを検出する。候補線分抽出部１２１は、検出した複数のエッジのうち、ＲＧＢ画像に投影した点列（線分）をＲＧＢ画像内で並進させた場合に、その並進させた点列（線分）との一致度が所定閾値以上であるエッジの全てを、家具面候補の線分として抽出する。例えば、図５に示す例では、エッジ（Ａ）〜（Ｃ）が、家具面候補の線分として抽出される。なお、エッジの検出は、例えばＣａｎｎｙ法などの各種技術のうち、任意の技術を採用して実施するようにしてよい。
【００５０】
以下、位置姿勢変化推定部１２２は、ロボット１が家具操作を完了するまで、所定時間間隔毎に、新たに生成されるＲＧＢ画像情報、２次元距離情報、及び、関節角度情報を取得し、ステップＳ４、Ｓ５の処理を繰り返す。
【００５１】
制御部１２の位置姿勢変化推定部１２２は、家具操作を実行中、家具面候補として抽出した線分（エッジ）の変化を追跡し、追跡した線分（エッジ）の変化に基づいて、家具面の位置姿勢変化を推定する（Ｓ４）。
【００５２】
より具体的には、位置姿勢変化推定部１２２は、２次元レーダー測域センサ１１から新たに取得した２次元距離情報が示す周辺の各物体までの距離に基づいて、レーザーオドメトリによって、ステップＳ３の実行時からのロボット１の移動距離を算出する。位置姿勢変化推定部１２２は、ロボット構成情報が示すロボット１の構成と、頭部４０の関節角度取得部４３から新たに取得した関節角度情報が示す関節角度とに基づいて、２次元レーザー測域センサ１１に対するＲＧＢセンサ４１の相対位置姿勢を新たに算出する。すなわち、位置姿勢変化推定部１２２は、２次元レーザー測域センサ１１に対するＲＧＢセンサ４１の相対位置姿勢を更新する。
【００５３】
位置姿勢変化推定部１２２は、算出したロボット１の移動距離と、ステップＳ３で算出した相対位置姿勢に対する新たに算出した相対位置姿勢に基づいて、基準とする点列（基準点列）を更新する。すなわち、家具操作の実行中に、頭部４０の動作、及び、家具操作を実行するための車輪２２ａ、２２ｂの回転によるロボット１の移動が行われる場合があるため、ステップＳ３の実行時から今回のステップＳ４の実行までのロボット１の動作に応じて、基準となる点列（基準点列）を、ステップＳ３で抽出した点列を現在のロボット１の状態で測定した場合における点列となるように更新する。
【００５４】
位置姿勢変化推定部１２２は、ＲＧＢセンサ４１から新たに取得したＲＧＢ画像情報が示すＲＧＢ画像を解析して、例えば図６に示すような、複数のエッジを検出する。位置姿勢変化推定部１２２は、ステップＳ３で抽出した家具面候補の線分のそれぞれについて、ステップＳ３で抽出した家具面候補の線分との一致度が所定閾値よりも高いエッジのうち、その家具面候補の線分との距離が最小のエッジを抽出し、抽出したエッジを新たな家具面候補の線分とするように、家具面候補の線分を更新する。すなわち、例えば図６に示すように、ステップＳ３で予め抽出した家具面候補の線分（Ａ）〜（Ｃ）のそれぞれに対応する線分（Ａ）〜（Ｃ）が抽出される。
【００５５】
位置姿勢変化推定部１２２は、例えば図６に示すように、更新後の基準点列を、並進方向に一定量動かした場合、基準点列の端点を軸に点列全体を一定量回転させた場合における点列を、それらの一定量を変化させながら何通りか算出する。位置姿勢変化推定部１２２は、算出した点列のそれぞれを、新たに取得したＲＧＢ画像情報が示すＲＧＢ画像に投影する。この際、位置姿勢変化推定部１２２は、ステップＳ３での説明と同様に、ＲＧＢ画像に投影する点列として、算出した点列の近似線分となる線分を、ＲＧＢ画像に投影する。
【００５６】
位置姿勢変化推定部１２２は、新たな家具面候補の線分として抽出した複数の線分のそれぞれと、基準点列から算出した複数の点列のそれぞれとのフィッティング（マッチング）を行う。これにより、位置姿勢変化推定部１２２は、新たな家具面候補の線分のそれぞれについて、新たな家具面候補の線分と最も一致度が高い点列を取得する。
【００５７】
これによれば、位置姿勢変化推定部１２２は、基準点列に対する、取得した点列の位置姿勢変化を、家具面の位置姿勢変化として算出することができる。言い換えると、家具面候補の線分の形状変化に基づいて、家具面の位置姿勢変化が推定されることになる。
【００５８】
位置姿勢変化推定部１２２は、家具操作によって期待される家具面の位置姿勢変化と、ステップＳ４で推定した家具面の位置姿勢変化とを比較し、家具操作の成否を判定する（Ｓ５）。
【００５９】
ここで、家具面が手先に合せて移動することを考慮すると、期待される家具面の位置姿勢変化は、手先位置姿勢の変化から算出することができる。すなわち、位置姿勢変化推定部１２２は、ロボット構成情報が示すロボット１の構成と、ステップＳ３でアーム３０の関節角度取得部３２ａ、３２ｂのそれぞれから取得した関節角度情報が示す関節角度と、ステップＳ５でアーム３０の関節角度取得部３２ａ、３２ｂのそれぞれから取得した関節角度情報が示す関節角度とに基づいて、期待される家具面の位置姿勢変化として、ステップＳ３からステップＳ５までの手先位置姿勢の変化を算出する。
【００６０】
そして、位置姿勢変化推定部１２２は、期待される家具面の位置姿勢変化として算出した家具面の位置姿勢変化が、ステップＳ４で推定した家具面の位置姿勢変化と一致する場合、家具操作が成功していると判定する。
【００６１】
このように、ロボット１は、ロボットハンド機構を用いて、取っ手を把持する等の家具操作の準備を行った後、家具を映し込んだＲＧＢ画像を取得し、エッジを検出する。また、ロボット１の各関節の現在角度から分かる手先位置近くの２次元距離情報を抽出し、ＲＧＢ画像に投影する。この投影結果に基づいて、検出したエッジから家具面の候補となる線分を抽出する。さらに、この線分の形状変化を追跡することで、家具の位置姿勢変化を推定することができる。また、推定した家具の位置姿勢変化と、期待される家具の位置姿勢変化とを比較することで、家具操作の成否を判断することができる。
【００６２】
以上に説明したように、本実施の形態に係る操作対象物（家具に対応）の状態推定装置（ロボット１に対応）では、ロボットハンド（アーム３０及び把持部５０に対応）は、操作対象物を操作する。関節角度取得部３２ａ、３２ｂは、ロボットハンドの関節角度を検出し、検出した関節角度を示す関節角度情報を生成する。２次元距離センサ（２次元レーダー測域センサ１１に対応）は、操作対象物までの距離を検出し、検出した距離を２次元空間上の点列で示す２次元距離情報を生成する。撮像センサ（ＲＧＢセンサ４１に対応）は、操作対象物を撮像し、撮像した操作対象物を画像で示す画像情報を生成する。候補線分抽出部１２１は、関節角度取得部３２ａ、３２ｂが生成した関節角度情報が示す関節角度に基づいて、２次元距離センサが生成した２次元距離情報が示す点列から、操作対象物の点列を抽出するとともに、撮像センサが生成した画像情報が示す画像におけるエッジを検出し、検出したエッジのうち、抽出した点列を画像上に投影した場合における、その投影した点列との一致度が所定基準を満たすエッジを抽出する。位置姿勢変化推定部１２２は、抽出したエッジの形状変化に基づいて、操作対象物の位置姿勢変化を推定する。
【００６３】
これによれば、操作対象物と、それを操作する状態推定装置との位置が近く、状態推定装置から家具までの距離を２次元距離センサによって正確に測定することができない場合であっても、画像における操作対象物のエッジの形状変化に基づいて、操作対象物の位置姿勢変化を推定するようにしているため、操作対象物の位置姿勢変化を、より正確に推定することができる。
【００６４】
なお、本発明は上記の実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【００６５】
上述の実施の形態では、ロボット１が操作対象とする物体を撮像することで生成する画像が、ＲＧＢ画像である例について説明したが、ＲＧＢ画像以外の種類の画像を生成するようにしてもよい。すなわち、ロボット１の前方の物体を撮像し、撮像した物体を画像で示す撮像センサであれば、ロボット１に搭載される撮像センサは、ＲＧＢセンサ４１に限られない。
【符号の説明】
【００６６】
１  ロボット
１０  本体
１１  ２次元レーダー測域センサ
１２  制御部
２０  移動台車
２１ａ、２１ｂ  アクチュエータ
２２ａ、２２ｂ  車輪
３０  アーム
３１ａ、３１ｂ  アクチュエータ
３２ａ、３２ｂ  関節角度取得部
４０  頭部
４１  ＲＧＢセンサ
４２  アクチュエータ
４３  関節角度取得部
５０  把持部
１２１  候補線分抽出部
１２２  位置姿勢変化推定部
【図１】
0006604147.tif 000002
【図２】
0006604147.tif 000003
【図３】
0006604147.tif 000004
【図４】
0006604147.tif 000005
【図５】
0006604147.tif 000006
【図６】
0006604147.tif 000007