JP2007319938A - ロボット装置及び物体の三次元形状の取得方法 - Google Patents

Abstract

【課題】物体把持を行うことが可能な腕部を有するロボットにおいて、把持対象物体の三次元形状認識の精度を向上させる。
【解決手段】ロボット１は、頭部１０及び胴体部１１よりなる体幹部、胴体部１１に連結された腕部１２、頭部１０に固定された頭部カメラ１０１、腕部１２に固定された手先カメラ１２５を備えている。また、ロボット１は、頭部カメラ１０１によって腕部１２を撮像した画像に基づいて、前記第２のカメラの三次元位置及び姿勢を算出し、手先カメラ１２５によって複数の位置から把持対象物体を撮像して得た画像系列と、算出した手先カメラ１２５の三次元位置及び姿勢とに基づいて把持対象物体の三次元形状を算出する。さらに、ロボット１は、算出した持対象物体の三次元形状に基づいて前記物体の把持動作を実行する。
【選択図】図３

Description

本発明は、腕部を有するロボットによる物体の三次元形状の取得方法に関する。

物体を把持可能な腕部を有するヒューマノイド型のロボットに物体の把持を行わせるためには、把持動作に先立って把持対象物体の三次元形状をロボットに認識させることが必要である。このとき、腕部によって物体を把持する間は、把持対象物体に対して静止した状態にあるロボットの胴体部や頭部などに固定されたカメラによって把持対象物体を観測することにより、把持対象物の三次元形状の認識することが行われている。しかしながら、このような物体認識方法は、把持対象物体の置かれている環境によっては、カメラによる把持対象物体の撮像を十分に行うことができないため、把持対象物体の全体的な形状を認識できない場合がある。このように把持対象物体の形状認識が不十分であると、把持対象物体の種類の判断の誤りや、適切な把持位置を選択できないことにより、物体把持に失敗する場合があった。

なお、特許文献１には、物体を把持可能な腕部を有し、腕部によって把持した物体を頭部に固定されたカメラの前に移動して観測することにより、把持した物体の三次元形状を認識するロボットが開示されている。しかしながら、当該ロボットは、物体の把持を行うための腕部の制御を、頭部に固定されたカメラによって物体を撮像することにより行っている。このため、当該ロボットは上述した物体把持の失敗という問題を内包しているものと考えられる。

また、特許文献２には、いわゆるｅｙｅ−ｉｎ−ｈａｎｄ構造のロボットアームが開示されている。具体的には、ロボットアーム搭載されたカメラによって三次元位置が既知の基準点を観測することでロボットアームと基準点の相対位置関係を認識することにより、ロボットアームを所定の三次元位置に位置決めするものである。しかしながら、このようなロボットアームに搭載されたカメラを用いたロボットアームの位置決め方法は、撮影する基準点の三次元位置が既知であるために可能な方法である。したがって、三次元位置、形状が未知の把持対象物体の形状認識にそのまま適用することは困難である。

なお、上述した問題は、腕部による物体把持を行うロボットに限らず存在するものである。つまり、何らかの動作に先立って物体の三次元形状を認識する必要があるロボットなど、少なくとも物体の三次元形状の認識を行うロボットにおいて生じる問題である。
特開２００３−３４６１５２号公報 特許第２７６７４１７号公報

本発明は、上述した事情を考慮してなされたものであり、腕部を有するロボットにおいて、物体の三次元形状認識の精度を向上させることを目的とする。

本発明にかかるロボット装置は、体幹部と、前記体幹部に連結された少なくとも１つの腕部と、前記体幹部に固定された第１のカメラと、前記腕部に固定された第２のカメラと、前記第１のカメラによって前記腕部又は前記第２のカメラの少なくとも一方を撮像した画像に基づいて、前記第２のカメラの三次元位置及び姿勢を算出する第１の算出手段と、前記第２のカメラによって複数の位置から物体を撮像して得た画像系列と、前記第１の算出手段によって算出された前記第２のカメラの三次元位置及び姿勢とに基づいて、前記物体の三次元形状を算出する第２の算出手段とを備える。

このような構成により、腕部を動作させることで、第２のカメラによって物体を様々な角度から撮影して画像系列を得ることができ、得られた画像系列を用いて物体の三次元形状を復元、物体認識を行うことができる。これにより、物体が未知であっても、その三次元形状を復元して物体認識を行うことができる。

また、本発明にかかるロボット装置は、エンコーダや関節角センサ等の内界センサの計測情報ではなく、体幹部に固定された第１のカメラによって撮影された画像データを基準に腕部に固定された第２のカメラの位置及び姿勢を算出する。このため、第２のカメラの位置及び姿勢の推定を正確に行うことができ、物体の認識精度の低下を抑制することができる。

また、上述した本発明にかかるロボット装置は、前記物体の三次元形状に基づいて、前記第２のカメラが固定された前記腕部によって前記物体の把持動作を実行する制御手段をさらに備えてもよい。把持を行う腕部と物体認識を行うための第２のカメラを搭載した腕部を共通化することにより、ロボット装置の構成が簡素化される。

また、前記画像系列は、前記第１の算出手段によって算出された前記第２のカメラの三次元位置及び姿勢にある前記第２のカメラによって撮影した画像を先頭画像として生成される画像系列としてもよい。少なくとも最初の画像を、前記第１の算出手段によって算出された前記第２のカメラの三次元位置及び姿勢に基づいて生成すれば、以降の画像の撮影したカメラ位置及び姿勢は、画像系列において対応点を追跡することによって推定することが可能である。

また、上述した本発明にかかるロボット装置は、前記画像系列を得るために前記腕部の移動経路を算出する経路計画部をさらに備えるものとし、前記経路計画部は、前記物体と前記腕部とが予め定めた距離より接近していることを判定した場合に、前記物体と前記腕部との距離を予め定めた距離より大きくする経路を算出してもよい。これにより、腕部と物体との衝突や、腕部が物体に接近しすぎたために、必要な対応点を含む画像を撮影できない状況を回避することができる。

また、上述した本発明にかかるロボット装置は、前記腕部の状態量を計測する内界センサと、前記内界センサの計測情報を用いて算出した前記カメラの変動情報に基づいて、前記第２のカメラによる撮影画像のぶれを補正する補正部とをさらに備えるものとし、前記第２の算出手段は、前記補正部によって画像ぶれを補正した画像系列を用いて前記物体の三次元形状を算出してもよい。これにより、第２のカメラの移動による画像ぶれの影響を軽減した解像度の高い画像系列が得られるため、前記物体の三次元形状の復元精度を向上させることができる。

一方、本発明にかかる方法は、少なくとも１つの腕部を有するロボットが実行する物体の三次元形状の取得方法である。ここで、前記ロボットは、前記物体の三次元形状を取得する際の位置及び姿勢の変化が前記腕部に比べて小さい前記ロボットの部位に固定された第１のカメラと、前記腕部に固定された第２のカメラとを備えるものとする。三次元形状の取得の際は、まず、前記第１のカメラによって前記腕部又は前記第２のカメラの少なくとも一方を撮像した画像に基づいて、前記第２のカメラの三次元位置及び姿勢を算出する、次に、算出した前記第２のカメラの三次元位置及び姿勢を初期値として前記腕部を移動して、前記第２のカメラで複数の位置から前記物体を撮像する。続いて、前記第２のカメラで前記物体を撮像して得た画像系列に基づいて、前記物体の三次元形状を算出する。

これにより、腕部を動作させることで、第２のカメラによって物体を様々な角度から撮影して画像系列を得ることができ、得られた画像系列を用いて物体の三次元形状を復元することができる。また、物体の三次元形状を取得する際の位置及び姿勢の変化が第２のカメラに比べて小さい第１のカメラによって撮影された画像データを基準に、腕部に固定された第２のカメラの位置及び姿勢を算出している。このため、腕部のエンコーダや関節角センサ等のロボットが備える内界センサの計測情報を用いて第２のカメラの位置及び姿勢を推定する場合に比べて、第２のカメラの位置及び姿勢の推定を正確に行うことができ、物体の三次元形状の復元精度の低下を抑制することができる。

本発明により、物体の三次元形状認識を正確に行うことが可能なロボット及び、ロボットによる物体の三次元形状の取得方法を提供できる。

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。各図面において、同一要素には同一の符号が付されており、説明の明確化のため、必要に応じて重複説明は省略する。なお、以下に示す発明の実施の形態は、物体把持を行う腕部を備えたロボットに本発明を適用したものである。

発明の実施の形態１．
本実施の形態にかかるロボット１の外観を図１に示す。ロボット１の頭部１０には、外界を撮影可能な頭部カメラ１０１が設けられている。頭部１０は、胴体部１１に連結されている。また、胴体部１１には、腕部１２が連結されている。より詳細には、腕部１２に含まれる上腕部１２１が胴体部１１と肩関節機構（不図示）を介して連結され、上腕部１２１と前腕部１２３とが肘関節部１２２を介して連結され、前腕部１２３の先端に手部１２４が設けられている。さらに、外界を撮影可能な手先カメラ１２５が手部１２４に固定されている。また、胴体部１１にはロボット１の移動機構である車輪１３１及び１３２が設けられている。

ロボット１は、頭部カメラ１０１によって撮影した画像を用いて手先カメラ１２５の位置及び姿勢を推定し、手先カメラ１２５によって撮像した画像系列を用いて把持対象である物体５０の三次元形状を認識する。さらに、認識した物体５０に対応した把持動作を実行することにより、手部１２４による物体５０の把持を行うものである。

以下では、ロボット１による把持対象物体５０の三次元形状の認識処理について、図２を及び図３を用いて説明する。図２に示すブロック図は、三次元形状の認識処理に関係するロボット１の主要部の内部構成を示している。図２において、画像補正部１１１は、頭部カメラ１０１及び手先カメラ１２５から入力された画像の歪みを補正する。三次元復元部１１２は、頭部カメラ１０１によって撮影した画像を用いて手先カメラ１２５の位置及び姿勢を算出するとともに、手先カメラ１２５によって撮影された画像を用いて物体５０の三次元形状を算出する。なお、三次元復元部１１２による物体５０の三次元形状の算出処理の詳細については後述する。

経路計画部１１３は、物体５０を様々な視点から撮影するために、手先カメラ１２５の移動経路を計画し、手先カメラ１２５の移動経路に応じた腕部１２の動作情報を制御部１１５に出力する。

物体認識部１１４は、三次元復元部１１２が算出した物体５０の三次元形状を用いて、物体５０を認識する。具体的には、三次元復元部１１２が算出した物体５０の三次元形状を、予め記憶している把持対象物体の形状データと照合することにより、物体５０の種別を決定する。さらに、物体認識部１１４は、決定した物体５０の種別に応じた把持動作の実行を制御部１１５に通知する。

制御部１１５は、頭部１０、腕部１２、車輪１３２及び１３３の位置、角度、速度、角速度等を検出するためにロボット１が備えているエンコーダ（不図示）や関節角センサ（不図示）等の内界センサの計測情報を収集し、頭部１０、腕部１２、車輪１３２及び１３３を駆動するための制御信号を、これらを駆動する駆動部に対して出力する。さらに、制御部１１５は、経路計画部１１３及び物体認識部１１４が決定した動作計画に従って腕部１２を動作させるための制御信号をアーム駆動部１１６に出力する。なお、アーム駆動部１１６は、腕部１２のアクチュエータを動作させるための駆動回路である。

次に、頭部カメラ１０１及び手先カメラ１２５による撮影画像を用いた物体５０の認識手順を、図３のフローチャートを用いて説明する。まずステップＳ１０１では、頭部カメラ１０１によって、物体５０の近傍に配置された手部１２４を撮像する。得られた画像データは、画像補正部１１１による補正処理を行った後に三次元復元部１１２に入力される。なお、頭部カメラ１０１が生成する画像データは、単眼画像でもステレオ画像でもよい。

ステップＳ１０２では、三次元位置算出部１１２が、頭部カメラ１０１によって撮影された画像データを用いて手先カメラ１０１の三次元位置及び姿勢を推定する。具体的には、図４のフローチャートに示す手順で手先カメラ１０１の三次元位置及び姿勢を推定する。

始めに、制御部１１５から内界センサの計測情報を入力して手部１２４又は手先カメラ１２５のおおまかな位置を特定する（ステップＳ２０１）。次に、ステップＳ２０１でおおまかに特定した位置を頭部カメラの画像に投影することにより、頭部カメラ１０１で撮影された画像データから手部１２４が存在する画像領域を抽出する（ステップＳ２０２）。なお、ステップＳ２０２では、ロボット１の手部１２４の動きによる動き差分画像を利用してもよい。続いて、抽出した画像と、ロボット１が予め記憶している手部１２４の三次元形状データ又はテクスチャデータとを照合することにより、手先カメラの姿勢及び位置を算出する。（ステップＳ２０３及びＳ２０４））。ここで、抽出した画像と手部１２４の三次元形状データとの照合は、従来から知られているパターン認識の手法を採用して行えばよい。

なお、ステップＳ２０１及びＳ２０２を行わずに、頭部カメラ１０１によって得た画像データの全体を走査して、手部１２４の像を検出してもよい。しかしながら、内界センサの計測情報に基づいて手部１２４の存在する画像領域を抽出した後に走査を行うことによって走査領域を小さくできるため、演算量が削減できる点で有効である。

図３に戻り説明を続ける。ステップＳ１０３では、手先カメラ１２５によって物体５０を撮像する。得られた画像データは、画像補正部１１１による補正処理を行った後に三次元復元部１１２に入力される。

ステップＳ１０４では、手先カメラ１２５によって撮影された画像データを用いて、物体５０の三次元形状を復元する。具体的には、手先カメラ１２５を移動して撮影した２枚の画像から対応点を検出し、２枚の画像によるステレオ視によって対応点の３次元位置を復元する。ここで、２つの撮影画像に対する対応点の探索は、２つの撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。

いま、２つの画像が撮影された手先カメラ１２５の位置及び姿勢から決定した射影行列Ｐ_１及びＰ_２を用いれば、２つの画像における対応点の座標の組ｍ_１＝（ｕ_１、ｖ_１）、ｍ_２＝（ｕ_２、ｖ_２）と、対応点の三次元位置Ｍ＝（Ｘ，Ｙ，Ｚ）^Ｔとは、以下の（１）式により対応付けられる。

ここで、射影行列Ｐ_１及びＰ_２は、手先カメラ１２５の焦点距離や画素サイズ、画像中心等の内部パラメータを表す行列Ａと、手先カメラ１２５の位置及び姿勢を表すロボット座標に対する回転行列Ｒ_ｊ及び平行移動ベクトルＴ_ｊによって、以下の（２）式により与えられる。また、（２）式においてπ_１、π_２、π_３はそれぞれ、３×４行列である射影行列Ｐ_ｊの１行目、２行目及び３行目に相当する１×３行列である。

射影行列Ｐ_１及びＰ_２を決定するために必要な手先カメラ１２５の位置及び姿勢の初期値には、ステップＳ１０２において頭部カメラ１０１の撮影画像を用いて推定した手部１２４の位置及び姿勢から決定した手先カメラ１２５の位置及び姿勢を利用する。その後、物体５０の撮像を繰り返し実行する際には、過去の撮影画像と新たな撮影画像との間で物体５０の対応点を追跡することによって決定した手先カメラ１２５の位置及び姿勢を利用して、射影行列Ｐ_１及びＰ_２を決定すればよい。

上述した（１）式をｚ_１，ｚ_２，Ｘ，Ｙ，Ｚの連立方程式とみなし、ｚ_１及びｚ_２を消去することで、対応点の三次元位置Ｍ＝（Ｘ，Ｙ，Ｚ）^Ｔを算出することができる。なお、対応点の三次元位置の復元の精度を高めるために、得られた三次元位置Ｍ＝（Ｘ，Ｙ，Ｚ）^Ｔを手先カメラ１２５による撮影画像に再投影した点と、対応点ｍ_ｊ＝（ｕ_ｊ、ｖ_ｊ）との距離の二乗和によって定義される再投影誤差を目的関数とする最適化を実行し、再投影誤差が最小となるように対応点の三次元位置を決定してもよい。最適化の目的関数εは、以下の（３）式で与えられる。

（３）式において添字ｉは、異なる対応点を表す。つまり、異なる対応点の再投影誤差の和として定義された目的関数εを最小とするような、対応点の三次元位置の組｛Ｍ_ｉ｝を決定する。

ステップＳ１０５では、物体５０の三次元形状認識に必要十分なデータが取得できたか否かを判定する。具体的には、物体５０の半周１８０度や全周３６０度に渡って対応点が得られたか否かによって判定すればよい。物体５０の三次元形状認識に必要十分なデータが取得できた場合には、ステップＳ１０８の三次元形状認識を行う。一方、物体５０の三次元形状認識に必要十分なデータが取得できていない場合には、ステップＳ１０６を行う。

ステップＳ１０６では、次の撮影位置に手先カメラ１２５を移動するための手部１２４の移動経路を決定する。次の撮影位置は、前回の撮影によって得た画像データに含まれる特徴点が撮影画像に含まれる位置とすればよい手先カメラ１２５の移動前後の２つの撮影画像に共通して含まれる特徴点を対応点とすることにより、手先カメラ１２５の位置及び姿勢の推定、並びに、対応点の三次元位置の復元が可能となるためである。

なお、このとき、物体５０と手部１２４とが予め定めた距離より接近した場合には、物体５０と手部１２４との距離を大きくするような経路を計画することが望ましい。手部１２４と物体５０との衝突や、手部１２４が物体５０に接近しすぎたために、必要な対応点を含む画像を撮影できない状況を回避できる点において有効である。具体的には、手部１２４と物体５０との距離に対する閾値ｄを予め定めておき、（４）式が成立する場合に、上述した回避動作を行う経路を計画すればよい。

（４）式において、Ｈは手部１２４の周囲の空間領域、Ｏは物体５０の周囲の空間領域である。また、回避動作を行う経路を生成する際には、公知の経路回避手法を利用することができる。

ステップＳ１０７では、経路計画に従って制御部１１５がアーム駆動部１１６を駆動することによって、手部１２４を目標位置まで移動する。ステップＳ１０７の後は、ステップＳ１０３に戻って物体５０の撮像を実行する。

ステップＳ１０５において物体５０の三次元形状認識に必要十分なデータの取得が終了したことを判定した場合は、ステップＳ１０８において、物体５０の三次元形状の認識を行う。具体的には、物体認識部１１４が、手先カメラ１２５による撮影画像を用いて算出した物体５０の三次元形状を、ロボット１が備える記憶部（不図示）に予め格納している把持対象物体の形状データと照合し、物体５０の種別を決定する。なお、物体５０の三次元形状だけでなく、手先カメラ１２５による撮影画像から得られる物体５０のテクスチャを併せて照合することとしてもよい。物体認識部１１４は、決定した物体５０の種別に応じた把持動作の実行を制御部１１５に通知し、物体５０の認識処理を終了する。

なお、上述したステップＳ１０４において、手先カメラ１２５によって最初に撮影した画像に対しては、対応点を検索するための過去の画像が存在しない。このため、手先カメラ１２５による最初の撮影画像に対しては、ステップＳ１０４をスキップし、２枚目の撮影画像からステップＳ１０４を行うものとする。なお、ステップＳ１０３における手先カメラ１２５による物体５０の撮像は、最初だけ２フレームの画像を撮影することとし、撮影した２フレームの画像を用いてステップＳ１０４を行ってもよい。さらにまた、手先カメラ１２５を２台のカメラとし、これら２台のカメラによって物体５０を撮像して得た画像データを用いてステップＳ１０４を行ってもよい。

また、図３のフローチャートでは、頭部カメラ１０１による画像入力（Ｓ１０１）、手先カメラ１２５の位置及び姿勢の推定（Ｓ１０２）、手先カメラ１２５による画像入力（Ｓ１０３）の順序で行うことを示した。しかしながら、これらの処理はステップＳ１０４に先立って行われていればよく、処理順序に必然性はない。このため、Ｓ１０１乃至Ｓ１０３の順序を入れ替えて実行してもよい。また、Ｓ１０３の処理を、Ｓ１０１又はＳ１０２の処理と並行して行っても良い。

また、ステップＳ１０２において頭部カメラ１０１による撮影画像を用いた手先カメラ１２５の位置及び姿勢の推定を行う場合にも、ステップＳ１０４で示した再投影誤差の最小化による三次元位置の算出を行うことにより、位置推定精度を向上させてもよい。具体的には、頭部カメラ１０１による撮影画像を用いて復元した手先カメラ１２５の特徴点の三次元位置を撮影画像に再投影したときの再投影誤差を最小化するように、手先カメラ１２５の三次元位置を決定すればよい。このとき手先カメラ１２５の全ての特徴点に対する再投影誤差の最適化を行うと、計算量が膨大になる場合がある。この場合には、手先カメラ１２５の全ての特徴点に対する最適化を行う必要はなく、ＳＩＦＴ（Scale Invariant Feature Transform）特徴点であるコーナー特徴点などを選択して最適化を行えばよい。

上述したように、本実施の形態にかかるロボット１は、腕部１２の先端部分に手先カメラ１２５を設けている。このため、腕部１２を動作させることで、手先カメラ１２５によって物体５０を様々な角度から撮影して画像系列を得ることができる。物体５０を様々な角度から撮影した画像系列が得られれば、この画像系列を用いて物体５０の三次元形状を復元し、復元した三次元形状を用いて物体５０が如何なる種別の物体であるかを認識することが可能となる。これにより、把持対象物体が未知であっても、その三次元形状を復元して物体認識を行うことができ、把持対象物体に応じた把持動作を確実に行うことができる。また、把持対象物体に近接させることが可能な手先カメラ１２５で把持対象物体の撮像を行うため、把持対象物体が小さい場合も形状復元を正確に行うことができる。

さらに、ロボット１は、頭部カメラ１０１によって手部１２４を撮像し、得られた画像データを用いて手部１２４に固定された手先カメラ１２５の位置及び姿勢を算出することとしている。腕部１２は、物体把持を行う場合、その準備動作として物体認識を行う場合に大きく動作する部位であるため位置及び姿勢の変化量が大きいうえに、関節数が多いことから、内界センサの情報に基づいて手先カメラ１２５の位置及び姿勢を推定すると、推定誤差が比較的大きくなるという問題がある。手先カメラ１２５の位置及び姿勢の推定誤差が大きいと、復元した物体５０の三次元形状の誤差が大きくなり、物体５０の認識の精度を低下させるおそれがある。これに対してロボット１は、内界センサの情報ではなく、頭部カメラ１０１によって撮影された画像データを基準に手先カメラ１２５の位置及び姿勢を算出するため、手先カメラ１２５の位置及び姿勢の推定を正確に行うことができ、物体５０の認識精度の低下を抑制することができる。

なお、上述したロボット１は、頭部１０に設けた頭部カメラ１０１によって手部１２４を撮像することとした。しかしながら、手先カメラ１２５の位置及び姿勢の推定に使用される画像データを得るためのカメラは、例えば胴体部１１に設けてもよい。上述したように、腕部１２は、把持対象物体の認識を行う場合に大きく動作する部位であるために位置及び姿勢の変化量が大きいうえ、関節数が多いことから、内界センサの情報に基づく位置、姿勢の推定誤差が比較的大きくなる点に問題がある。このため、把持対象物体の認識を行う場合の位置及び姿勢の変化が腕部１２に比較して小さい頭部１０や胴体部１１などの体幹部に、手先カメラ１２５の位置及び姿勢の推定に用いるカメラを設けることが望ましい。

発明の実施の形態２．
本実施の形態にかかるロボット２は、発明の実施の形態１にかかるロボット１の構成に、手先カメラ１２５で撮影した画像の画像ぶれを補正する処理部を追加したものである。ロボット２の外観構成は、図１に示したロボット１の外観と同様であるため、詳細な説明を省略するとともに、以下の説明においてロボット２の各部に言及する際は、ロボット１の各部の符号を準用することとする。

ロボット２の主要部の構成を図５に示す。図５において、画像ぶれ補正部２１７は、制御部１１５から手部１２４の位置、速度等を示す内界センサの計測情報を入力し、これらの計測情報と手先カメラ１２５のシャッタ速度を用いて、手先カメラ１２５による撮像時における手先カメラ１２５の変位ベクトルを算出する。ここで、変位ベクトルとは、三次元空間での手先カメラ１２５の変位量及び変位方向を示すベクトルである。さらに、画像ぶれ補正部２１７は、手先カメラ１２５の変位ベクトルを用いて、手先カメラ１２５の移動による画像ぶれを表す画像ブレ行列Ｈを算出する。ここで、画像ぶれ行列Ｈは、手先カメラ１２５の移動によって画像ブレを生じた撮影画像の各画素と、手先カメラ１２５の移動による画像ブレがない状態の各画素との変換を示す行列である。

具体的には、以下の（５）式に示す残差Ｃを最小化する復元画像の画像ベクトルｘを最急降下法や共役勾配法等の最適化手法によって決定すれば良い。（５）式においてｙは撮影画像の画像ベクトルである。ここで画像ベクトルとは、画素値を成分とするベクトルである。

図６は、ロボット２による物体５０の認識手順を示すフローチャートである。上述した画像ぶれ補正部２１７による画像ブレ補正はステップＳ３０１において行われる。なお、ステップＳ３０１を除く他の処理は、発明の実施の形態１において説明した図３のフローチャートの各ステップと同様であるため詳細な説明を省略する。

このように手先カメラ１２５による撮影画像の画像ぶれを補正することにより、手先カメラ１２５の移動による画像ぶれの影響を軽減した解像度の高い画像が得られるため、対応点の検出精度を向上させることができる。

その他の実施の形態．
上述した発明の実施の形態において、頭部カメラ１０１をステレオカメラとし、得られたステレオ画像から距離データを生成してもよい。この場合、得られた距離画像と、手部１２４の三次元形状データとを照合することにより、手部１２４の位置及び姿勢を算出することができる。

上述した発明の実施の形態にかかるロボット１及び２は、手部１２４に手先カメラ１２５を固定することとしたが、前腕部１２３に固定してもよい。要するに、手先カメラ１２５は、物体５０の周囲に手先カメラ１２５を移動させることができるように、腕部１２に固定されていればよい。

上述した発明の実施の形態にかかるロボット１及び２は、頭部カメラ１０１による撮影画像と、ロボット１が予め記憶している手部１２４の三次元形状データ又はテクスチャデータとを照合することにより、手先カメラ１２５の姿勢及び位置を算出することとした。しかしながら、頭部カメラ１０１で物体５０を撮像して得た画像から物体５０の特徴点の三次元位置を算出し、これらの特徴点の三次元位置を基準として手先カメラ１２５の三次元位置を算出することとしてもよい。具体的には、三次元位置が特定された物体５０の特徴点を手先カメラ１２５によって撮像し、得られた撮影画像を用いて物体５０の特徴点に対する手先カメラ１２５の相対位置を求めることにより、手先カメラ１２５の姿勢及び位置を算出することができる。

上述した発明の実施の形態にかかるロボット１及び２は、手部１２４の位置及び姿勢を特定した上で、手先カメラ１２５の位置及び姿勢を決定することとした。しかしながら、頭部カメラ１０１による撮影画像を手先カメラ１２５の形状データと照合することにより、直接的に手先カメラ１２５の位置及び姿勢を決定してもよい。

上述した発明の実施の形態にかかるロボット１及び２は、復元した物体５０の三次元形状を、予め記憶している把持対象物体の形状データと照合することにより、物体５０の種別を決定し、決定した物体種別に応じた把持動作を実行するものとして説明した。しかしながら、復元した物体５０の三次元形状に適した把持位置を算出し、物体５０の三次元形状及び把持位置に応じた腕部１２の動作を決定して把持動作を開始してもよい。

上述した発明の実施の形態にかかるロボット１及び２は、１つの腕部１２を備えるものとして説明した。しかしながら、ロボット１及び２を複数の腕部１２を備えてもよく、手先カメラ１２５を備える腕部と物体５０の把持を行う腕部とは別個のものでもよい。

上述した発明の実施の形態にかかるロボット１及び２は、頭部カメラ１０１による撮影画像から算出した手先カメラ１２５の位置及び姿勢を初期値として利用し、その後の手先カメラ１２５の位置及び姿勢は、手先カメラ１２５により得た画像系列から検出した対応点を追跡することによって算出するものとして説明した。しかしながら、頭部カメラ１０１によって手部１２４の撮像が可能である間は、頭部カメラ１０１による撮影画像から手先カメラ１２５の位置及び姿勢を算出し、物体５０の特徴点の三次元復元に利用してもよい。

上述した発明の実施の形態では、物体の三次元形状を認識し、当該物体の把持を行うことを目的とするロボットについて説明した。しかしながら、本発明は、三次元形状を認識した後に把持以外の動作を実行するロボット、又は物体の三次元形状の認識のみを目的とするロボット等にも適用可能である。

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

発明の実施の形態１にかかるロボットの外観を示す図である。 発明の実施の形態１にかかるロボットの内部構成を示すブロック図である。 発明の実施の形態１にかかるロボットが行う物体認識処理の手順を示すフローチャートである。 発明の実施の形態１にかかるロボットが行う手先カメラの三次元位置及び姿勢を推定する処理の手順をフローチャートである。 発明の実施の形態２にかかるロボットの内部構成を示すブロック図である。 発明の実施の形態２にかかるロボットが行う物体認識処理の手順を示すフローチャートである。

符号の説明

１、２ ロボット
１０ 頭部
１０１ 頭部カメラ
１１ 胴体部
１２ 腕部
１２１ 上腕部
１２２ 肘関節機構
１２３ 前腕部
１２４ 手部
１２５ 手先カメラ
１３１、１３２ 車輪
１１１ 画像補正部
１１２ 三次元復元部
１１３ 経路計画部
１１４ 物体認識部
１１５ 制御部
１１６ アーム駆動部
２１７ 画像ぶれ補正部
５０ 把持対象物体

Claims (7)

1. 体幹部と、
   前記体幹部に連結された少なくとも１つの腕部と、
   前記体幹部に固定された第１のカメラと、
   前記腕部に固定された第２のカメラと、
   前記第１のカメラによって前記腕部又は前記第２のカメラの少なくとも一方を撮像した画像に基づいて、前記第２のカメラの三次元位置及び姿勢を算出する第１の算出手段と、
   前記第２のカメラによって複数の位置から物体を撮像して得た画像系列と、前記第１の算出手段によって算出された前記第２のカメラの三次元位置及び姿勢とに基づいて、前記物体の三次元形状を算出する第２の算出手段と、
   を備えるロボット装置。
2. 前記物体の三次元形状に基づいて、前記第２のカメラが固定された前記腕部によって前記物体の把持動作を実行する制御手段をさらに備える請求項１に記載のロボット装置。
3. 前記画像系列は、前記第１の算出手段によって算出された前記第２のカメラの三次元位置及び姿勢にある前記第２のカメラによって撮影した画像を先頭画像として生成される画像系列である請求項１に記載のロボット装置。
4. 前記画像系列を得るために前記腕部の移動経路を算出する経路計画部を備え、
   前記経路計画部は、前記物体と前記腕部とが予め定めた距離より接近していることを判定した場合に、前記物体と前記腕部との距離を予め定めた距離より大きくする経路を算出する請求項１に記載のロボット装置。
5. 前記腕部の状態量を計測する内界センサと、
   前記内界センサの計測情報を用いて算出した前記カメラの変動情報に基づいて、前記第２のカメラによる撮影画像のぶれを補正する補正部とをさらに備え、
   前記第２の算出手段は、前記補正部によって画像ぶれを補正した画像系列を用いて前記物体の三次元形状を算出する請求項１に記載のロボット装置。
6. 少なくとも１つの腕部を有するロボットが実行する物体の三次元形状の取得方法であって、
   前記ロボットは、
   前記物体の三次元形状を取得する際の位置及び姿勢の変化が前記腕部に比べて小さい前記ロボットの部位に固定された第１のカメラと、
   前記腕部に固定された第２のカメラとを備えており、
   前記第１のカメラによって前記腕部又は前記第２のカメラの少なくとも一方を撮像した画像に基づいて、前記第２のカメラの三次元位置及び姿勢を算出し、
   算出した前記第２のカメラの三次元位置及び姿勢を初期値として前記腕部を移動して、前記第２のカメラで複数の位置から前記物体を撮像し、
   前記第２のカメラで前記物体を撮像して得た画像系列に基づいて、前記物体の三次元形状を算出する方法。
7. 前記物体を撮像する処理において、
   前記物体と前記腕部とが予め定めた距離より接近しているか否かを判定し、
   前記物体と前記腕部とが接近していることを判定した場合は、前記物体と前記腕部との距離を予め定めた距離より大きくするよう前記腕部を移動する請求項６に記載の方法。