JP2010060451A - ロボット装置及び物体の位置姿勢推定方法 - Google Patents

Abstract

【課題】物体の位置姿勢を少ない計算時間で、かつ、正確に推定すること。
【解決手段】本発明に係るロボット装置１は、作業環境内に存在する物体との距離を計測してレンジデータを生成する距離センサ１０１と、生成したレンジデータを形状データ１０３と照合することによって、物体の形状と位置姿勢を推定する初期照合部１０２と、物体を撮像した画像データを生成するカメラ部１０４と、画像データの特徴を抽出する特徴抽出部１０５と、初期照合部１０２により推定した物体の形状と位置姿勢を初期値として、特徴抽出部１０５で抽出した画像データの特徴と、物体の形状と位置姿勢との誤差を評価関数とした最小化問題を解くことで、物体の位置姿勢を推定する姿勢推定部１０６と、を備えるものである。
【選択図】図１

Description

本発明はロボット装置及び物体の位置姿勢推定方法に関する。

作業環境内に存在する物体に対してロボットに何らかの動作を行わせる場合には、動作に先立って、対象物体の形状や位置姿勢を認識する必要がある。対象物の位置姿勢を推定する技術としては、物体モデルとの照合により、対象物の位置姿勢を推定する技術が知られている。

特許文献１には、対象物が存在する範囲を画像特徴から確率的に求めた後に、詳細な照合を行うことで対象物の位置姿勢を推定する画像特徴抽出装置が開示されている。
特許３３０００９２号

しかしながら、特許文献１記載の技術では、対象物の存在領域を限定する処理に関して詳細な説明が開示されておらず、画像特徴のみから領域を限定する場合には、画像全体に対して存在確率を計算する必要があり、計算コストが高くつく。更に、背景が複雑になると、物体モデルが背景に当てはまりやすくなり、物体の存在確率が画像の様々なところで高くなることが予想される。

ところで、従来の技術では、ステレオカメラにより対象物体を撮像し、ステレオカメラからの画像エッジ・距離画像と、物体モデルとを照合することで、物体の位置姿勢を推定することができる。通常の照合方法では、距離画像と物体モデルとの距離をある評価関数で定義し、最小化問題を解くことにより対象物の位置姿勢を推定する。

しかし、物体モデルとの照合を行う際には、初期値が適切に設定されていない場合には、計算が収束せずに計算時間が増大し、さらには、解が極値に落ちるなどの問題がある。特に、テクスチャの少ない対象物の周囲に複雑なテクスチャを持つ物体が乱雑に存在する環境の場合には、この問題が顕著なものとなる。ロボットが家庭環境に入って作業する場合には、このような環境に遭遇する場合が想定される。即ち、家具（机、椅子、タンス等）をロボットに認識させる場合には、家具自体は比較的テクスチャが少ないものの、絨毯や絵画、ポスター等のテクスチャが複雑な物体が家具の周囲に存在する状況が容易に想定される。

また、ロボットの自己位置推定や環境認識等においては、レーザレンジファインダ（以下、ＬＲＦと称する。）等のアクティブ距離センサが良く利用されている。しかし、ＬＲＦが計測可能な対象物には制限がある。ＬＲＦの計測原理は、照射した光が対象物に当たって反射して戻ってくるまでの時間を計測して、対象物までの距離を計測するものである。例えば、対象物の色が黒い場合には照射光を吸収し、逆に鏡のような対象物の場合には全反射するおそれがある。このため、いずれの場合にも、対象物までの距離を正確に計測するのは困難となる。ロボットが家庭環境に入って作業する場合には、このような環境に遭遇する場合が想定される。即ち、黒い色の家具や、鏡、金属光沢のある家具などＬＲＦでは計測できない対象物が存在する状況が容易に想定される。

従って、本発明は、対象物体の初期値を効果的に推定することで、物体モデルとの照合時に生じる上記問題を解決し、物体の位置姿勢を少ない計算時間で、かつ、正確に推定可能なロボット装置及び物体の位置姿勢推定方法を提供することを目的とする。
さらに、本発明は、レーザレンジファインダ等のアクティブ距離センサを利用して物体の位置姿勢を推定する際に、初期照合時の頑健性を向上させることが可能なロボット装置及び物体の位置姿勢推定方法をも提供することを他の目的とする。

本発明に係るロボット装置は、作業環境内に存在する物体との距離を計測してレンジデータを生成する距離センサと、前記生成したレンジデータを形状データと照合することによって、前記物体の形状と位置姿勢を推定する初期照合部と、前記物体を撮像した画像データを生成するカメラ部と、前記画像データの特徴を抽出する特徴抽出部と、前記初期照合部により推定した前記物体の形状と位置姿勢を初期値として、前記特徴抽出部で抽出した前記画像データの特徴と、前記物体の形状と位置姿勢との誤差を評価関数とした最小化問題を解くことで、前記物体の位置姿勢を推定する姿勢推定部と、を備えるものである。

これにより、距離センサにより対象物体の初期値を効果的に推定することで、物体モデルとの照合時に生じる上記問題を解決し、物体の位置姿勢を少ない計算時間で、かつ、正確に推定することができる。

また、前記特徴抽出部は、前記カメラ部で生成した画像データから、エッジ処理により線分を抽出すると共に、ステレオ処理により距離画像を取得するようにしてもよい。

さらにまた、前記姿勢推定部は、前記初期照合部により推定した前記物体の形状と位置姿勢を中心としてパーティクルを複数個設定し、前記各パーティクルに対して前記物体の形状と位置姿勢を射影するモデル射影部と、前記特徴抽出部で抽出した前記画像データの特徴と、前記モデル射影部で設定した前記パーティクルに含まれる前記物体の形状と位置姿勢とを比較する比較部と、前記比較部による比較結果に応じて前記各パーティクルの重みを評価する評価部と、を備え、前記評価部による評価結果が収束するまでの間、前記パーティクルに対する処理を繰り返すようにしてもよい。

また、前記初期照合部は、前記生成したレンジデータから前記物体の形状が棒形状であるか又は辺形状であるかを識別する識別器と、前記物体の形状が棒形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定する棒形状初期位置推定器と、前記物体の形状が辺形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定する辺形状初期位置推定器と、を備えるようにしてもよい。

さらにまた、前記距離センサの走査面上であり、かつ、前記物体の表面上に、前記距離センサの照射光を反射する反射マーカを設けるようにしてもよい。また、前記反射マーカを、前記物体の角部に設けるようにしてもよい。これにより、レーザレンジファインダ等のアクティブ距離センサを利用して物体の位置姿勢を推定する際に、初期照合時の頑健性を向上させることができる。

また、前記距離センサは、前記作業環境内に存在する物体との距離と、前記距離センサの照射光の反射強度とを計測して前記レンジデータを生成するようにしてもよい。さらにまた、前記距離センサは、前記計測した作業環境内に存在する物体との距離に応じて、前記照射光の反射強度に対して所定の閾値を設定し、前記閾値以上の反射強度を有する照射光から前記レンジデータを生成するようにしてもよい。これにより、初期照合時の頑健性をより向上させることができる。

本発明に係る物体の位置姿勢推定方法は、距離センサによって作業環境内に存在する物体との距離を計測してレンジデータを生成し、前記生成したレンジデータを形状データと照合することによって、前記物体の初期形状と初期位置姿勢を推定し、カメラによって撮像した前記物体の画像データを生成し、前記生成した画像データの特徴を抽出し、前記推定した前記物体の初期形状と初期位置姿勢を初期値として、前記抽出した前記画像データの特徴と、前記物体の形状と位置姿勢との誤差を評価関数とした最小化問題を解くことで、前記物体の位置姿勢を推定するものである。

これにより、距離センサにより対象物体の初期値を効果的に推定することで、物体モデルとの照合時に生じる上記問題を解決し、物体の位置姿勢を少ない計算時間で、かつ、正確に推定することができる。

また、前記生成した画像データから、エッジ処理により線分を抽出すると共に、ステレオ処理により距離画像を取得するようにしてもよい。

さらにまた、前記推定した前記物体の初期形状と初期位置姿勢を中心としてパーティクルを複数個設定し、前記各パーティクルに対して前記物体の形状と位置姿勢を射影し、前記抽出した前記画像データの特徴と、前記設定した前記パーティクルに含まれる前記物体の形状と位置姿勢とを比較し、前記比較結果に応じて前記各パーティクルの重みを評価し、前記評価結果が収束するまでの間、前記パーティクルに対する処理を繰り返すようにしてもよい。

また、前記生成したレンジデータから前記物体の形状が棒形状であるか又は辺形状であるかを識別し、前記物体の形状が棒形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定し、前記物体の形状が辺形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定するようにしてもよい。

さらにまた、前記距離センサの走査面上であり、かつ、前記物体の表面上に、前記距離センサの照射光を反射する反射マーカを設けるようにしてもよい。また、前記反射マーカを、前記物体の角部に設けるようにしてもよい。これにより、レーザレンジファインダ等のアクティブ距離センサを利用して物体の位置姿勢を推定する際に、初期照合時の頑健性を向上させることができる。

また、前記距離センサにより、前記作業環境内に存在する物体との距離と、前記距離センサの照射光の反射強度とを計測して前記レンジデータを生成するようにしてもよい。さらにまた、前記距離センサにより、前記計測した作業環境内に存在する物体との距離に応じて、前記照射光の反射強度に対して所定の閾値を設定し、前記閾値以上の反射強度を有する照射光から前記レンジデータを生成するようにしてもよい。これにより、初期照合時の頑健性をより向上させることができる。

本発明によれば、物体の位置姿勢を少ない計算時間で、かつ、正確に推定可能なロボット装置及び物体の位置姿勢推定方法を提供することができる。
さらに、レーザレンジファインダ等のアクティブ距離センサを利用して物体の位置姿勢を推定する際に、初期照合時の頑健性を向上させることが可能なロボット装置及び物体の位置姿勢推定方法をも提供することができる。

発明の実施の形態１．
本実施の形態１にかかるロボット１の要部構成を図１に示す。ロボット１は、レーザレンジファインダ１０１及びカメラ部１０４の２つのセンサによって外界の情報を入力し、これら２つのセンサによって取得したレンジデータ及び画像データの夫々に基づいて物体の位置姿勢を推定することができる。物体の位置姿勢の推定に際しては、取得したレンジデータに基づく初期照合結果を用いて、画像データと形状データとの誤差を評価関数とした最小化問題を解くことで、物体の位置姿勢を少ない計算時間で、かつ、正確に推定することができる。以下、図１を参照してロボット１が有する各機能ブロックについて説明する。

距離センサとしてのレーザレンジファインダ１０１は、レーザ光を周囲に照射し、その反射光を観測することによって得たレンジデータを初期照合部１０２に出力する。即ち、レーザレンジファインダ１０１は、作業環境内に存在する物体との距離を計測してレンジデータを生成する。図２に、レーザレンジファインダ１０１により取得したレンジデータの一例を示す。図２において、スキャンしたデータを記号"＋"により示す。また、矢印は、選択した点集合の周辺の点集合までのベクトルを示す。

初期照合部１０２は、レーザレンジファインダ１０７によって得たレンジデータを、格納されている三次元形状データ１０３と照合して、物体の形状と位置姿勢を推定する。作業空間内に存在する物体の物体モデルを示す情報として、３次元の幾何形状データである３次元形状データ１０６が予め記憶されている。具体的には、ロボット位置から見た三次元形状データとレンジデータとの照合を行う。ここで、レンジデータと三次元形状データの誤差は、レンジデータと三次元形状データとを照合した際のレンジデータの対応点と三次元形状データの対応点との距離又は二乗距離により定義される。初期照合部１０２は、照合結果を物体の初期形状と初期位置姿勢として、姿勢推定部１０６に出力する。図３に、初期照合結果の概念図の一例を示す。図３において、矢印（ロボット１）の前方に物体（机）が存在する場合に、物体の形状と位置姿勢を、複数の線分により示す。尚、初期照合部１０２による初期照合処理の詳細については後述する。

カメラ部１０４は、２台のカメラ１０４ａ及び１０４ｂを有する。カメラ１０４ａ及び１０４ｂはそれぞれ、ＣＣＤイメージセンサ又はＣＭＯＳイメージセンサ等の撮像素子を備えており、外界を撮像して得たデジタル画像データを特徴抽出部１０５に出力する。即ち、カメラ部１０４は、物体を撮像した画像データを生成する。図４に、カメラ部１０４により取得した画像データの一例を示す。図４においては、ロボット１の正面に机と椅子が存在する例を示している。

特徴抽出部１０５は、カメラ部１０４によって得た画像データの特徴を抽出する。特徴抽出部１０５は、画像データの特徴として、例えば、エッジ処理により線分を抽出する。また、ステレオ処理により距離画像を取得する。特徴抽出部１０５は、抽出した線分の画像データと距離画像を姿勢推定部１０６へと出力する。図５に、エッジ処理により取得した線分を含む画像エッジの一例を示す。図５においては、物体のエッジを白い線分により示している。図６に、ステレオ処理により取得した距離画像（ステレオデータ）の一例を示す。図６においては、距離を画像の濃淡により示している。

姿勢推定部１０６は、初期照合部１０２により得た初期照合結果と、特徴抽出部１０５により得た線分の画像データ及び距離画像とに基づいて、物体の位置姿勢を推定する。具体的には、初期照合結果を推定に用いる初期値として、画像データの特徴（線分の画像データ及び距離画像）と、３次元形状データ１０３との誤差を評価関数とした最小化問題を解くことで、物体の位置姿勢を推定する。図７に、推定した物体の位置姿勢の出力の一例を示す。図７においては、線分により推定した物体の位置姿勢を示している。尚、図７においては、図４に示した画像データに、推定結果を示す線分を重ねて表示している。尚、姿勢推定部１０６による姿勢推定処理の詳細については後述する。

続いて以下では、まず、図８を参照しながら、初期照合部１０２による初期照合処理の詳細を説明する。図８は、ロボット１が行う初期照合処理の手順を示すフローチャートである。

まず、初期照合部１０２は、距離センサ（レーザレンジファインダ１０１）の平面走査により、平面内に存在する物体までの距離データ（レンジデータ）を取得する（Ｓ１０１）。

次いで、初期照合部１０２は、距離センサから取得した距離データを、識別器を用いて、棒形状のクラスと辺形状のクラスの２クラスに分類する（ｓ１０２）。ここで、棒形状のクラスは、例えば、机、椅子の脚などの形状を含む。また、辺形状のクラスは、例えば、タンス、冷蔵庫などの形状を含む。尚、識別器による分類は公知の手法を用いて行えばよい。例えば分類方法としては、まず、取得した距離データをグリッドマップ上に投影し、投影した距離データを示す点群を２値画像と見なし、物体の存在位置情報を示す２値画像を生成する。そして、生成した２値画像内の連結情報に基づいてラベリング処理を実行する。ラベリング処理とは、生成した２値画像に対して、連結している領域を１つにまとめる処理をいう。ラベリング処理を行うことで、距離センサにより得られたデータ群を複数のグループにグループ化することができる。このようにして分割された領域（即ち、グループ化された点群）の真円度を計算することにより、棒形状とそれ以外の形状とを識別することができる。また、通常、レーザレンジファインダ１０１などの距離センサは、投光器を回転して測距を行っているため、測距で得た点列データの並びには規則性が存在する。そこで、隣接した測距点間の距離を観察し、この距離が所定の閾値以上離れているか否かに基づいて点列データをグループ化し、各グループ内で点の直線性を判定する方法もある。

次いで、初期照合部１０２は、距離データが棒形状のクラスであるか否かを判定する（Ｓ１０３）。棒形状のクラスである場合には、Ｓ１０４へと進む。一方で、棒形状のクラスでない場合には、Ｓ１０６へと進む。

距離データが棒形状のクラスである場合には、初期照合部１０２は、物体モデルを用いて、棒物体の初期位置姿勢を推定する（Ｓ１０４）。より具体的には、初期照合部１０２は、棒形状に分類された距離データから１つを選択し、選択したデータの周囲の棒形状の距離データから、さらに２つの距離データを選択する。そして、初期照合部１０２は、棒間の距離及び棒間がなす角度を計算し、物体モデル（形状データ１０６）と照合する。尚、照合は公知の照合方法を用いて行えばよい。例えば、距離センサの走査面の形状情報（物体の角部が作る２辺の長さと、そのなす角度）を物体モデルに予め付加しておき、初期照合部１０２は、計測データ（距離データ）と物体モデルの形状情報とを比較する。物体モデルと距離データとが合致する場合には、照合対象となった棒を次の照合対象から除外し、照合対象が無くなるまで、物体モデルとの照合を繰り返す。多数の物体モデルに適合する場合には、ここでは、適合した全ての結果を記録しておくものとする。そして、初期照合部１０２は、Ｓ１０４での照合結果（適合した物体モデルと物体の位置姿勢）を、初期照合結果として出力する（Ｓ１０５）。

一方で、距離データが棒形状のクラスでない場合には、初期照合部１０２は、距離データが辺形状のクラスであるか否かを判定する（Ｓ１０６）。辺形状のクラスである場合には、Ｓ１０７へと進む。一方で、辺形状のクラスでない場合には、Ｓ１０８へと進む。即ち、棒形状、及び、辺形状のいずれでもない距離データについては、物体モデルとの照合対象とはせずに、初期照合部１０２は、これらの距離データを除去する（Ｓ１０８）。

距離データが辺形状のクラスである場合には、初期照合部１０２は、物体モデルを用いて、辺物体の初期位置姿勢を推定する（Ｓ１０７）。より具体的には、初期照合部１０２は、辺形状に分類された距離データから１つを選択してその辺の長さを計算し、物体モデル（形状データ１０６）と照合する。尚、照合は公知の照合方法を用いて行えばよい。例えば、距離センサの走査面の形状情報（物体の各辺の長さ）を物体モデルに予め付加しておき、初期照合部１０２は、計測データ（距離データ）と物体モデルの形状情報とを比較する。物体モデルと距離データとが合致する場合には、照合対象となった辺を次の照合対象から除外し、照合対象が無くなるまで、物体モデルとの照合を繰り返す。多数の物体モデルに適合する場合には、ここでは、適合した全ての結果を記録しておくものとする。そして、初期照合部１０２は、Ｓ１０７での照合結果（適合した物体モデルと物体の位置姿勢）を、初期照合結果として出力する（Ｓ１０５）。

続いて以下では、図９を参照しながら、初期照合結果に基づいて、物体の詳細な位置姿勢を推定する処理の詳細を説明する。図９は、ロボット１が行う物体の姿勢推定処理の手順を示すフローチャートである。

まず、上述したようにして、特徴抽出部１０５は、カメラ部１０４によって得た画像データに対してエッジ処理を行い、線分を抽出する（Ｓ２０１）。また、特徴抽出部１０５は、ステレオ処理により距離画像（デプスマップ）を取得する（Ｓ２０２）。

以下、姿勢推定部１０６は、初期照合部１０２により得た初期照合結果と、特徴抽出部１０５により得た線分の画像データ及び距離画像とに基づいて、物体の位置姿勢を推定する。画像データの特徴に基づいて物体モデルとの照合を行う方法としては、公知の手法を採用することができる。ここでは、パーティクルフィルタによる物体の位置姿勢の推定方法を用いて、照合を行うものとして説明する。

ここでは、各パーティクルを、物体の位置姿勢を示す変数により構成する。例えば、テーブルのように床面上に存在する物体の場合には、３つの変数（平面移動の２自由度、及び、床面垂直軸周りの回転１自由度）により構成する。初期照合の結果を用いることで、パーティクルの個数を少量に限定することができるため、位置姿勢推定処理の高速化が可能となる。

まず、姿勢推定部１０６は、初期照合の結果を用いて、パーティクルを設定する（Ｓ２０３）。具体的には、初期照合結果として得られた物体の初期形状と初期位置姿勢を中心として、その初期結果の近傍に複数のパーティクルを設定する。

次いで、姿勢推定部１０６は、物体モデルを各パーティクルに射影する（Ｓ２０４）。即ち、設定した各パーティクルに対して、物体モデル（形状データ１０６）を射影する。

次いで、姿勢推定部１０６は、各パーティクルについて、射影した物体モデルと、画像エッジ・距離画像とのデータ比較処理を行う（Ｓ２０４）。即ち、姿勢推定部１０６は、各パーティクルに射影された物体モデルと、画像データから抽出した特徴との整合性を評価する。より具体的には、画像データの特徴（線分の画像データ及び距離画像）と、３次元形状データ１０３との誤差を評価する。

次いで、姿勢推定部１０６は、データ比較処理の結果に応じて、各パーティクルの重み付け処理を行う（Ｓ２０６）。例えば、姿勢推定部１０６は、誤差の小さなパーティクルほど、大きな重みを設定する。

次いで、姿勢推定部１０６は、パーティクルの変数及び重みが収束したか否かを判定する（Ｓ２０７）。判定の結果、収束していない場合には、Ｓ２０６で設定した重みに基づいて、パーティクルの再サンプリングを行い、Ｓ２０５へと進む。そして、Ｓ２０５以降の処理をパーティクルが収束するまでの間繰り返す。

一方で、判定の結果、収束した場合には、姿勢推定部１０６は、最も重みの大きなパーティクルの値を、物体の形状と位置姿勢の推定値として出力する。

このように、距離センサによる初期位置姿勢の推定結果を物体の位置姿勢の推定に用いることで、検索範囲を限定し、計算速度と認識精度を向上させることができる。特に、テクスチャの少ない対象物の周囲に複雑なテクスチャを持つ物体が乱雑に存在する環境の場合においても、距離センサにより対象物体の初期値を効果的に推定することで、物体の位置姿勢を少ない計算時間で、かつ、正確に推定することができる。

その他の実施の形態．
上述した実施の形態１で位置姿勢の推定対象とする物体について、距離センサの走査面上であり、かつ、物体の表面上に、距離センサの照射光を反射する反射マーカを設けるようにしてもよい。さらに、反射マーカを、物体の角部に設けるようにしてもよい。これにより、レーザレンジファインダ等のアクティブ距離センサを利用して物体の位置姿勢を推定する際に、初期照合時の頑健性を向上させることができる。例えば図１０（ａ）に示すように、物体２の角部の表面上に反射マーカ４ａ、４ｂ、４ｃ、４ｄを設ける。また、反射マーカ４ａ、４ｂ、４ｃ、４ｄは、距離センサの走査面（距離センサの照射光の走査線３上の平面）に設ける。図１０（ｂ）は、距離センサにより取得したレンジデータを示す概念図であり、反射マーカ４ａ、４ｂ、４ｃ、４ｄを設けることで、物体２の形状や位置姿勢の推定精度を向上させることができる。

また、距離センサから、作業環境内に存在する物体との距離情報に加えて、距離センサの照射光の反射強度情報をも計測するようにしてもよい。さらには、距離センサの計測情報について、計測した作業環境内に存在する物体との距離に応じて、照射光の反射強度に対して所定の閾値を設定し、閾値以上の反射強度を有する照射光のみからレンジデータを生成するようにしてもよい。これにより、初期照合時の頑健性をより向上させることができる。

尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態１に係るロボットの要部構成を示す全体図である。 本発明の実施の形態１に係る距離センサにより取得したレンジデータの一例を示す図である。 本発明の実施の形態１に係る初期照合結果の概念図の一例を示す図である。 本発明の実施の形態１に係るカメラ部により取得した画像データの一例を示す図である。 本発明の実施の形態１に係るエッジ処理により取得した線分を含む画像エッジの一例を示す図である。 本発明の実施の形態１に係るステレオ処理により取得した距離画像（ステレオデータ）の一例を示す図である。 本発明の実施の形態１に係る物体の位置姿勢の推定結果の一例を示す図である。 本発明の実施の形態１に係るロボットが行う初期照合処理の手順を示すフローチャート図である。 本発明の実施の形態１に係るロボットが行う物体の姿勢推定処理の手順を示すフローチャート図である。 本発明のその他の実施の形態に係る物体に設けた反射マーカを説明するための図である。

符号の説明

１ ロボット、 ２ 物体、 ３ 走査線、 ４ａ、４ｂ、４ｃ 反射マーカ、
１０１ レーザレンジファインダ、 １０２ 初期照合部、
１０３ 形状データ、 １０４ カメラ部、１０４ａ、１０４ｂ カメラ、
１０５ 特徴抽出部、 １０６ 姿勢推定部、

Claims (16)

1. 作業環境内に存在する物体との距離を計測してレンジデータを生成する距離センサと、
   前記生成したレンジデータを形状データと照合することによって、前記物体の形状と位置姿勢を推定する初期照合部と、
   前記物体を撮像した画像データを生成するカメラ部と、
   前記画像データの特徴を抽出する特徴抽出部と、
   前記初期照合部により推定した前記物体の形状と位置姿勢を初期値として、前記特徴抽出部で抽出した前記画像データの特徴と、前記物体の形状と位置姿勢との誤差を評価関数とした最小化問題を解くことで、前記物体の位置姿勢を推定する姿勢推定部と、
   を備えるロボット装置。
2. 前記特徴抽出部は、
   前記カメラ部で生成した画像データから、エッジ処理により線分を抽出すると共に、ステレオ処理により距離画像を取得する
   ことを特徴とする請求項１記載のロボット装置。
3. 前記姿勢推定部は、
   前記初期照合部により推定した前記物体の形状と位置姿勢を中心としてパーティクルを複数個設定し、前記各パーティクルに対して前記物体の形状と位置姿勢を射影するモデル射影部と、
   前記特徴抽出部で抽出した前記画像データの特徴と、前記モデル射影部で設定した前記パーティクルに含まれる前記物体の形状と位置姿勢とを比較する比較部と、
   前記比較部による比較結果に応じて前記各パーティクルの重みを評価する評価部と、を備え、
   前記評価部による評価結果が収束するまでの間、前記パーティクルに対する処理を繰り返す
   ことを特徴とする請求項１又は２記載のロボット装置。
4. 前記初期照合部は、
   前記生成したレンジデータから前記物体の形状が棒形状であるか又は辺形状であるかを識別する識別器と、
   前記物体の形状が棒形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定する棒形状初期位置推定器と、
   前記物体の形状が辺形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定する辺形状初期位置推定器と、を備える
   ことを特徴とする請求項１乃至３いずれか１項記載のロボット装置。
5. 前記距離センサの走査面上であり、かつ、前記物体の表面上に、前記距離センサの照射光を反射する反射マーカを設ける
   ことを特徴とする請求項１乃至４いずれか１項記載のロボット装置。
6. 前記反射マーカを、前記物体の角部に設ける
   ことを特徴とする請求項５記載のロボット装置。
7. 前記距離センサは、
   前記作業環境内に存在する物体との距離と、前記距離センサの照射光の反射強度とを計測して前記レンジデータを生成する
   ことを特徴とする請求項１乃至６いずれか１項記載のロボット装置。
8. 前記距離センサは、
   前記計測した作業環境内に存在する物体との距離に応じて、前記照射光の反射強度に対して所定の閾値を設定し、前記閾値以上の反射強度を有する照射光から前記レンジデータを生成する
   ことを特徴とする請求項７記載のロボット装置。
9. 距離センサによって作業環境内に存在する物体との距離を計測してレンジデータを生成し、
   前記生成したレンジデータを形状データと照合することによって、前記物体の初期形状と初期位置姿勢を推定し、
   カメラによって撮像した前記物体の画像データを生成し、
   前記生成した画像データの特徴を抽出し、
   前記推定した前記物体の初期形状と初期位置姿勢を初期値として、前記抽出した前記画像データの特徴と、前記物体の形状と位置姿勢との誤差を評価関数とした最小化問題を解くことで、前記物体の位置姿勢を推定する
   物体の位置姿勢推定方法。
10. 前記生成した画像データから、エッジ処理により線分を抽出すると共に、ステレオ処理により距離画像を取得する
    ことを特徴とする請求項９記載の物体の位置姿勢推定方法。
11. 前記推定した前記物体の初期形状と初期位置姿勢を中心としてパーティクルを複数個設定し、前記各パーティクルに対して前記物体の形状と位置姿勢を射影し、
    前記抽出した前記画像データの特徴と、前記設定した前記パーティクルに含まれる前記物体の形状と位置姿勢とを比較し、
    前記比較結果に応じて前記各パーティクルの重みを評価し、
    前記評価結果が収束するまでの間、前記パーティクルに対する処理を繰り返す
    ことを特徴とする請求項９又は１０記載の物体の位置姿勢推定方法。
12. 前記生成したレンジデータから前記物体の形状が棒形状であるか又は辺形状であるかを識別し、
    前記物体の形状が棒形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定し、
    前記物体の形状が辺形状である場合に、前記物体を前記形状データと照合することによって、前記物体の形状と位置姿勢を推定する
    ことを特徴とする請求項９乃至１１いずれか１項記載の物体の位置姿勢推定方法。
13. 前記距離センサの走査面上であり、かつ、前記物体の表面上に、前記距離センサの照射光を反射する反射マーカを設ける
    ことを特徴とする請求項９乃至１２いずれか１項記載の物体の位置姿勢推定方法。
14. 前記反射マーカを、前記物体の角部に設ける
    ことを特徴とする請求項１３記載の物体の位置姿勢推定方法。
15. 前記距離センサにより、
    前記作業環境内に存在する物体との距離と、前記距離センサの照射光の反射強度とを計測して前記レンジデータを生成する
    ことを特徴とする請求項９乃至１４いずれか１項記載の物体の位置姿勢推定方法。
16. 前記距離センサにより、
    前記計測した作業環境内に存在する物体との距離に応じて、前記照射光の反射強度に対して所定の閾値を設定し、前記閾値以上の反射強度を有する照射光から前記レンジデータを生成する
    ことを特徴とする請求項１５記載の物体の位置姿勢推定方法。