JP6922605B2 - ３次元物体検出装置、ロボット、及びプログラム - Google Patents

Description

本発明は、３次元物体検出装置、ロボット及びプログラムに関する。

特許文献１には、３次元辞書データから、対象物に関して測定されない領域に該当するデータを除去することで、高精度・高速な照合・認識を実現する技術が記載されている。姿勢の推定を行い、３次元辞書データを概要姿勢に応じて姿勢変換して、「概要変換後の３次元辞書データＰ’」を作成する。続いて「概要変換後の３次元辞書データＰ’」の点群を、測定器から対象物への距離方向と垂直な仮想平面へ投影する。そして、仮想平面を多数の矩形領域に分割し、矩形領域毎に、その矩形領域に属する（投影された）複数の点のなかから測定器に最も近い点のみを選択し、選択された点のみから構成される３次元辞書データを再構築することにより、３次元辞書データから対象物に関して測定されない領域に該当するデータを除去する。

特許文献２には、ステレオ処理と点群間の３Ｄモデルマッチングにおける一手法であるＩＣＰ(Iterative Closest Point)アルゴリズムの組み合わせによる方法で、ＩＣＰに用いる点群モデルに計測対象衛星の全形状の点群を考慮したモデルを用いた場合の精度・信頼性に限界があるという問題を解決する技術が記載されている。計測対象の３次元形状を複数台のカメラ画像をステレオ処理して計測点群の形で求めるステップと、計測対象の既知の形状情報とステレオカメラの相対位置・姿勢とから可視部を予測するステップと、該予測した可視部の形状情報のみを用い前記計測点群の空間密度に合わせたモデル点群を決定するステップと、前記計測点群とモデル点群の間でＩＣＰアルゴリズムを用いてマッチングさせ、最も評価関数が小さなモデル群に対応する位置・姿勢を計測結果として採用するステップを実行する。

特開２０１３−５３９２０号公報 特開２００８−１４６９１号公報

上記技術においては、概略姿勢、または予測した可視部分に基づき、見えないモデルデータ部分については単純に削除する処理を実行しており、その予測には誤差があることの影響を考慮していない。予測に誤差がある場合、見えないモデルデータ部分にも誤差が生じるため、単純に削除する処理では不具合が生じ得る。

また、ＩＣＰアルゴリズムは、剛体変換（回転および移動）の関係にある２つの点群データ間の回転・移動量を求めるアルゴリズムであるが、比較する２つの点群データにノイズがあると、正確な回転・移動量を求めるのが難しいことが知られている。

本発明の目的は、３次元物体の実際の姿勢と推定姿勢との間に誤差が生じていても、３次元物体の実際の姿勢を検出し得る技術を提供することにある。

請求項１に記載の発明は、３次元物体の距離画像を取得するカメラと、前記距離画像を用いて前記３次元物体に対応する３次元モデルの姿勢を算出する３次元モデル算出部と、算出された前記３次元モデルを前記カメラの視点から平面に投影して得られる投影画像のエッジを抽出する抽出部と、抽出された前記エッジにおいて前記カメラに最も近い位置の点群から予め定めた距離内にある前記３次元モデルの点群をマッチング用点群として抽出し、抽出したマッチング点群と前記距離画像から得られたマッチング用点群を照合することで前記３次元物体の姿勢を検出する処理部とを備える３次元物体検出装置である。

請求項２に記載の発明は、前記処理部は、前記投影画像において前記エッジに予め定めたサイズのウィンドウを設定し、前記ウィンドウ内で前記カメラから最も近い位置の点群を抽出する請求項１に記載の３次元物体検出装置である。

請求項３に記載の発明は、前記ウィンドウの前記サイズは、前記３次元モデルに応じて可変設定される請求項２に記載の３次元物体検出装置である。

請求項４に記載の発明は、前記距離は、固定値である請求項１に記載の３次元物体検出装置である。

請求項５に記載の発明は、前記距離は、前記３次元モデルに応じて可変設定される請求項１に記載の３次元物体検出装置である。

請求項６に記載の発明は、前記距離は、前記エッジに対応する前記３次元モデルの段差部の段差に応じて設定される請求項１に記載の３次元物体検出装置である。

請求項７に記載の発明は、前記エッジは、前記３次元物体の段差である請求項１に記載の３次元物体検出装置である。

請求項８に記載の発明は、前記エッジは、前記３次元物体の周縁部である請求項１に記載の３次元物体検出装置である。

請求項９に記載の発明は、請求項１〜８のいずれかに記載の３次元物体検出装置と、前記３次元物体検出装置で検出された前記３次元物体の姿勢を用いて前記３次元物体を把持するロボットアームとを備えるロボットである。

請求項１０に記載の発明は、コンピュータに、３次元物体の距離画像を取得するステップと、前記距離画像を用いて前記３次元物体に対応する３次元モデルの姿勢を算出するステップと、算出された前記３次元モデルを前記距離画像を取得したカメラの視点から平面に投影して得られる投影画像のエッジを抽出するステップと、抽出された前記エッジにおいて前記カメラに最も近い位置の点群から予め定めた距離内にある前記３次元モデルの点群をマッチング用点群として抽出し、抽出したマッチング点群と前記距離画像から得られたマッチング用点群を照合することで前記３次元物体の姿勢を検出するステップとを実行させるプログラムである。

請求項１，１０に記載の発明によれば、３次元物体の実際の姿勢と推定姿勢との間に誤差が生じていても、３次元物体の実際の姿勢を検出し得る。

請求項２，３に記載の発明によれば、さらに、カメラから最も近い位置の適当な点群が抽出される。

請求項４，５，６に記載の発明によれば、さらに、エッジに対応する適当な点群が抽出される。

請求項７に記載の発明によれば、さらに、３次元物体の段差において適当な点群が抽出される。

請求項８に記載の発明によれば、さらに、３次元物体の周辺部において適当な点群が抽出される。

請求項９に記載の発明によれば、さらに、検出された姿勢を用いて３次元物体がロボットアームにより把持され得る。

実施形態の機能ブロック図である。 実施形態の構成図である。 実施形態の３Ｄ物体の一例を示す図である。 実施形態の処理を示す説明図（その１）である。 実施形態の処理を示す説明図（その２）である。 実施形態のエッジ抽出説明図である。 実施形態のマッチング用点群の抽出説明図（その１）である。 実施形態のマッチング用点群の抽出説明図（その２）である。 実施形態のマッチング用点群の抽出説明図（その３）である。 実施形態の所定距離の説明図である。 実施形態の処理フローチャートである。 変形例１のマッチング用点群の抽出説明図である。 変形例２のマッチング用点群の抽出説明図である。 変形例３のロボットの構成図である。

以下、図面に基づき本発明の実施形態について説明する。

図１Ａは、本実施形態における３次元物体検出装置１０の機能ブロック図を示す。３次元物体検出装置１０は、３次元物体の位置及び姿勢を検出する装置であり、機能ブロックとして、距離画像取得部１００、姿勢認識部１０１、３次元（３Ｄ）モデル回転部１０２、３Ｄモデル投影画像化部１０３、投影画像エッジ抽出部１０４、マッチングモデル作成部１０５、マッチング処理部１０６、及び３Ｄモデル記憶部１０７を備える。

距離画像取得部１００は、距離画像カメラを備え、検出対象である３Ｄ物体を上方から撮影し、画素毎に距離情報が付与された距離画像を得る。距離画像は、カメラ位置を原点とした実距離情報が付与され、例えば、最もカメラ位置に近い、つまり最も高い位置にある物体表面ほど高い階調値を得るように撮影画像を取得する。距離画像カメラは公知であり、投光した光がターゲットに当たって戻る時間を画素毎にリアルタイムで測定することにより、距離画像を取得する。位相差法を用い、投光パルスを高速で点滅させ、反射光の位相遅れの程度を計測することで、距離計測を行ってもよい。

姿勢認識部１０１は、距離画像取得部１００で取得した画像を用い、公知の方法により３Ｄ物体の概略姿勢を認識する。一例として、３Ｄ物体の任意の姿勢を機械学習させておき、距離画像取得部１００で得られた画像を入力して３Ｄ物体の概略姿勢を出力する。得られた概略姿勢は推定姿勢であるから、３Ｄ物体の実際の姿勢との間に誤差が生じ得る。

３Ｄモデル回転部１０２は、３Ｄモデル算出部として機能し、姿勢認識部１０１で得られた３Ｄ物体の概略姿勢に基づいて当該３Ｄ物体に対応する３Ｄモデルを回転させる。３Ｄ物体の３Ｄモデルは、予め３Ｄモデル記憶部１０７に記憶されており、３Ｄモデル回転部１０２は、検出対象の３Ｄ物体に対応する３Ｄモデルを３Ｄモデル記憶部１０７から読み出して概略姿勢に一致するように回転させる。

３Ｄモデル投影画像化部１０３は、３Ｄモデル回転部１０２で回転させた３Ｄモデルを、距離画像取得部１００の距離画像カメラを基点として平面に投影することで投影画像を作成する。例えば、投影画像の階調値は、カメラに近いほど白く、カメラから遠いほど黒くなるようにカメラからの距離を表現する。

投影画像エッジ抽出部１０４は、抽出部として機能し、３Ｄモデル投影画像化部１０３で作成された投影画像からエッジを求め、強いエッジのみを検出する。強いエッジとは、所定サイズのウィンドウ内で値が大きく変化するエッジを意味する。

マッチングモデル作成部１０５は、処理部として機能し、まず、３Ｄモデル投影画像化部１０３で作成された投影画像中の各ピクセルに投影される点群中で、最もカメラに近い点群のみをマッチングモデル用の点群として抽出する。次に、投影画像中のエッジに所定サイズのウィンドウを設定し（例えば５ピクセル×５ピクセルのウィンドウ）、強いエッジを求める。そして、強いエッジの中で最もカメラに近い点群の位置（高さ）を求め、そのウィンドウに対応するすべての点群の中から最も高い位置にある点群から所定距離にある全ての点群をマッチングモデル用の点群のリストに付加する。また、マッチングモデル作成部１０５は、マッチングモデル用の点群リスト中に重複があれば、それを除去する。

マッチング処理部１０６は、処理部として機能し、距離画像取得部１００で得られた画像内の点群と、マッチングモデル作成部１０５で得られた点群との間で例えばＩＣＰアルゴリズムを実行し、３Ｄ物体の回転量と移動量を検出する。ＩＣＰアルゴリズムは、対象物の完全なモデル点群が分かっている条件下で、その対象物の部分形状情報が別に得られたとき、その部分形状（計測点群）と既知のモデル点群との位置関係（座標変換）を推定するための手法として開発されたものである。ＩＣＰアルゴリズムでは、当初両点群（計測点群とモデル点群）間の相対位置関係を推定される座標変換を用いて設定し、両点群の各点間の対応関係を「最も距離が近い」という規範に従って構築し、対応点毎の差を集計して最小となる適正座標変換を繰り返し計算で求める。点群の座標変換は、例えば３×３の回転行列、３×１の平行移動ベクトルに基づいてなされる。ＩＣＰアルゴリズムについては、特開２００８−１４６９１号公報等に開示されている。

図１Ａに示す機能ブロックは、具体的にはコンピュータで実現され得る。

図１Ｂは、３次元物体検出装置１０の具体的な構成を示す。３次元物体検出装置１０は、プロセッサ１００ａ、ＲＯＭ１００ｂ、ＲＡＭ１００ｃ、入出力インターフェイス（Ｉ／Ｆ）１００ｄ、通信Ｉ／Ｆ１００ｅ、記憶装置１００ｆ、及びカメラ１２を備える。

１又は複数のプロセッサ１００ａは、ＲＯＭ１００ｂやＨＤＤ、ＳＳＤ等に記憶された処理プログラムを読み出して実行し、ＲＡＭ１００ｃをワーキングメモリとして用いることで図１Ａの各部の機能、具体的には姿勢認識部１０１、３Ｄモデル回転部１０２、３Ｄモデル投影画像化部１０３、投影画像エッジ抽出部１０４、マッチングモデル作成部１０５、及びマッチング処理部１０６を実現する。プロセッサは、ＣＰＵ又はＧＰＵで構成され得る。

入出力Ｉ／Ｆ１００ｄは、カメラ１２で撮影して得られた画像、すなわち対象物の３Ｄ物体を上方から撮影して得られた画像を入力する。カメラ１２及び入出力Ｉ／Ｆ１００ｄは、距離画像取得部１００として機能する。

通信Ｉ／Ｆ１００ｅは、ネットワークを介して外部のサーバとデータを送受信する。プロセッサ１００ａは、外部のサーバにアクセスして３Ｄ物体の３Ｄモデルを取得してもよい。

記憶装置１００ｆは、ＨＤＤやＳＳＤ等で構成され、３Ｄ物体の３Ｄモデルを記憶する。記憶装置１００ｆは、３Ｄモデル記憶部１０７として機能する。

次に、実施形態の基本的処理について説明する。

図２Ａは、実施形態における３Ｄ物体の一例を示す。３Ｄ物体１４ｒは、略直方体の頭部と略直方体の脚部から構成される。脚部は図においてｘ−ｙ平面上に配置されており、頭部は脚部に対してｚ方向に配置されているものとする。以下の説明において、カメラ１２は３Ｄ物体１４ｒの上方から３Ｄ物体１４ｒを撮影するが、上方とは図において＋ｚ方向を意味し、＋ｚ方向から−ｚ方向に向けて撮影することを意味する。また、投影画像とは、３Ｄ物体１４ｒの３Ｄモデルをｘ−ｙ平面上に投影した画像を意味する。

図２Ｂは、実施形態の処理を模式的に示す。図２Ｂ（ａ）は、３Ｄ物体の実際の姿勢をカメラ１２で撮影して得られた画像から抽出したマッチング用の点群である。また、図２Ｂ（ｂ）は、３Ｄ物体１４ｒに対応する３Ｄモデル１４ｍを設定し、実際の３Ｄ物体１４ｒの姿勢を推定して３Ｄモデル１４ｍの姿勢に反映させ、姿勢を反映させた３Ｄモデル１４ｍをカメラ１２の視点から平面、つまり図２Ａにおけるｘ−ｙ平面に投影して得られた投影画像から抽出したマッチング用の点群を示す。

図２Ｂ（ａ）において、３Ｄ物体１４ｒはその姿勢が多少傾いた状態にあるものとし、このときに撮影された画像（計測画像）からマッチング用の点群が点群１６ｒとして抽出される。このとき、３Ｄ物体１４ｒの段差部分１８、つまり頭部と脚部の段差部分にも点群１８ｒが存在する。

図２Ｂ（ｂ）において、３Ｄ物体１４ｒに対応する３Ｄモデル１４ｍの推定姿勢には傾きがないものとすると、このときの投影画像からマッチング用の点群が点群１６ｍとして抽出されるが、３Ｄ物体１４の段差部分１８は、投影画像ではエッジとして存在するからマッチング用の点群１６ｍは存在しないことになる。

すると、図２Ｂ（ａ）で得られた点群１６ｒと、図２Ｂ（ｂ）で得られた点群１６ｍは一致せず、これらの点群１６ｒ、１６ｍを用いてマッチング処理を実行しても、高精度に３Ｄ物体１４ｒの姿勢を検出することはできない。このような点群１６ｒ、１６ｍの相違は、図２Ｂ（ｂ）に示す３Ｄモデル１４ｍの推定姿勢と、実際の３Ｄ物体１４ｒの姿勢との間の誤差に起因するものである。

そこで、実施形態では、３Ｄモデル１４ｍをカメラ１２の視点から投影して得られた投影画像１４ｍｐに含まれるエッジに着目し、このエッジに関連してマッチング用の点群を新たに追加生成する。

図３は、実施形態におけるマッチング用の点群の追加生成処理を模式的に示す。

３Ｄ物体１４ｒの３Ｄモデル１４ｍには傾きがないものとし（実際の３Ｄ物体１４ｒには傾きが存在する）、これをカメラ１２の視点から投影して得られた投影画像１４ｍｐ中の強いエッジ１５を検出する。

そして、この強いエッジ１５を基準として最もカメラ１２の位置に近い、つまりｚ方向において最も高い位置にある点群からｚ方向の所定距離内にある点群を新たにマッチング用の点群２０ｍとして追加する。図において、新たに追加される点群２０ｍを斜線で示す。これにより、３Ｄモデル１４ｍから抽出されるマッチング用の点群は、（点群１６ｍ＋点群２０ｍ）となり、これらの点群を図２Ｂ（ａ）に示す点群１６ｒとマッチング処理する。点群２０ｍによって、図２Ｂ（ｂ）で欠落している点群が補完されるといえる。

実施形態の処理をより詳細に説明する。

図４は、投影画像エッジ抽出部１０４でのエッジ抽出処理を示す。投影画像エッジ抽出部１０４は、３Ｄモデル投影画像化部１０３で作成された投影画像１４ｍｐからエッジを検出し、検出されたエッジの中から相対的に強いエッジを検出する。すなわち、投影画像１４ｍに所定サイズのウィンドウ２２を設定し、このウィンドウ２２内でエッジの境界においてカメラ１２との距離が閾値以上に大きく変化するエッジ１５を検出する。高い位置にある物体表面ほど高い階調値を得るように撮影画像を取得した場合、エッジの境界において階調値の差が閾値以上と大きいエッジを検出する。ウィンドウ２２のサイズは固定値でもよいが、３Ｄ物体１４ｒに応じた可変値でもよい。

図５、図６及び図７は、マッチングモデル作成部１０５での作成処理を示す。

まず、図５において、第１ステップとして、３Ｄモデル投影画像化部１０３で作成された投影画像１４ｍｐの各ピクセルに投影される３Ｄモデル１４ｍの点群の中で、最もカメラ１２に近い点群のみを抽出する。

具体的には、投影画像１４ｍｐ中のピクセル２８ｍｐに投影される３Ｄモデル１４ｍの点群としては、点群２４ｍ及び点群２６ｍがあるが、点群２４ｍの方が点群２６ｍよりもカメラ１２に近い位置にあるので、点群２４ｍを抽出する。

次に、図６において、第２ステップとして、投影画像１４ｍに所定サイズ、例えば５ピクセル×５ピクセルサイズのウィンドウ２２を設定し、ウィンドウ２２内で強いエッジを求め、そのエッジに投影される点群の中で、最もカメラ１２に近い点群３０ｍを抽出する。
次に、図７において、第３ステップとして、設定されたウィンドウ２２に投影される点群の中で、最もカメラ１２に近い位置にある点群３０ｍからｚ方向の所定距離内に位置する３Ｄモデル１４ｍの全ての点群３２ｍを抽出する。点群３４ｍは、ウィンドウ２２内に投影される点群であるが、所定距離外にあるため抽出対象から除外される。

以上のようにして、３Ｄモデル１４ｍからマッチング用の点群として点群２４ｍ、３０ｍ、及び３２ｍが抽出される。

最後に、第４ステップとして、点群２４ｍ、３０ｍ、及び３２ｍの中で重複する点群があればそれを削除して重複のないようにする。例えば、点群２４ｍと点群３０ｍの中で重複する点があれば、いずれか一つの点を残して他の点を除外する。

図７に示すように、設定されたウィンドウ２２に投影される点群の中で、最もカメラ１２に近い位置にある点群からｚ方向の所定距離内に位置する３Ｄモデル１４ｍの全ての点群３２ｍを抽出しているが、所定距離は予め定めた距離であって必ずしも固定距離である必要はなく、可変距離であってもよい。所定距離は、３Ｄ物体１４ｒの形状から強いエッジを形成する面の裏側（カメラ１２が位置する側の反対側）の点群を除外するためであるから、かかる機能が奏され得る距離であればよい。

図８は、所定距離の他の例を示す。３Ｄモデル１４ｍが図７の場合と異なる場合の所定距離である。図８（ａ）は、３Ｄモデル１４ｍが図７の場合よりもその脚部がｚ方向に長い場合であり、これに応じて所定距離も図７の場合と比べて長く設定される。図７における所定距離を第１所定距離、図８（ａ）における所定距離を第２所定距離、３Ｄモデル１４ｒの最も高い位置から脚部の上面Ｓ１までの距離をＬ１、３Ｄモデル１４ｒの最も高い位置から脚部の下面Ｓ２までの距離をＬ２とすると、
Ｌ１＜第１所定距離＜第２所定距離＜Ｌ２
である。図８（ａ）においても、強いエッジを形成する面Ｓ１の裏側Ｓ２の点群が除外され得る。

図８（ｂ）は、３Ｄモデル１４ｍが図７の場合に比べて脚部の下面Ｓ２が複雑な形状の場合であり、これに応じて所定距離も下面Ｓ２が含まれないような値に設定される。図８（ｂ）における所定距離を第３所定距離、３Ｄモデル１４ｒの最も高い位置から脚部の下面Ｓ２のうち最も高い位置までの距離をＬ３とすると、
Ｌ１≦第３所定距離＜Ｌ３
である。図８（ｂ）においても、強いエッジを形成する面Ｓ１の裏側Ｓ２の点群が除外され得る。

図８（ｃ）は、３Ｄモデル１４ｍが図７の場合と同様であるが、所定距離をより短く設定した場合である。具体的には、段差のサイズとほぼ等しく設定した場合である。図８（ｃ）における所定距離を第４所定距離とすると、
Ｌ１＝第４所定距離＜第１所定距離＜Ｌ２
である。図８（ｃ）においても、強いエッジを形成する面Ｓ１の裏側Ｓ２の点群が除外され得る。所定距離は、３Ｄ物体１４ｒの形状に応じて設定され得ることが理解される。

図９は、実施形態の処理フローチャートを示す。検出対象である３Ｄ物体は既知としているが、利用者が３Ｄ物体の名称等を入力して特定してもよい。

まず、距離画像取得部１００は、検出対象の３Ｄ物体１４ｒを上方（ｚ方向）から撮影して距離画像を取得する（Ｓ１０１）。取得した距離画像はＲＡＭ１００ｃ等のメモリに格納される。

次に、姿勢認識部１０１は、得られた距離画像から３Ｄ物体１４ｒの姿勢を認識する。姿勢認識部１０１は、前もって３Ｄ物体の姿勢とそのときの距離画像を学習データとして機械学習を行い、各種パラメータを最適化してメモリに記憶しておく。そして、学習して最適化されたパラメータを用いて、Ｓ１０１で得られた距離画像に対応する姿勢を認識する。

機械学習は公知であり、ディープニューラルネットワーク（多層ニューラルネットワーク）を用いてもよい。ディープニューラルネットワークの学習は、学習データを入力し、学習データに対応する目標値と出力値との差によってロスを算出する。算出されたロスをニューラルネットワークで逆伝播させてディープニューラルネットワークのパラメータ、すなわち重みベクトルを調整する。重みが調整されたディープニューラルネットワークに次の学習データを入力し、新しく出力された出力値と目標値との差により再びロスを算出する。再算出されたロスとニューラルネットワークで逆伝播させてディープニューラルネットワークの重みベクトルを再調整する。以上の処理を繰り返すことでディープニューラルネットワークの重みベクトルを最適化する。

次に、３Ｄモデル回転部１０２は、３Ｄ物体１４ｒに対応する３Ｄモデル１４ｍを３Ｄモデル記憶部１０７から読み出し、姿勢認識部１０１で認識された姿勢に従って当該３Ｄモデル１４ｍを回転させる（剛体変換）。

次に、３Ｄモデル投影画像化部１０３は、３Ｄモデル回転部１０２で回転して得られた３Ｄモデル１４ｍをカメラ１２の位置を基点としてｘ−ｙ平面上に投影することで投影画像１４ｍｐを作成する（Ｓ１０４）。

次に、投影画像エッジ抽出部１０４は、作成された投影画像１４ｍｐから強いエッジを抽出する（Ｓ１０５）。具体的には、図４に示すように投影画像１４ｍｐ内にウィンドウ２２を設定し、エッジの境界において階調値の差が閾値以上となる強いエッジを検出する。強いエッジは複数存在してもよい。

次に、マッチングモデル作成部１０５は、抽出されたエッジを用いてマッチングモデル、すなわちマッチング用の点群を作成する（Ｓ１０６）。この処理は既述したように４つのステップから構成される。すなわち、
第１ステップ：投影画像１４ｍｐの各ピクセルに投影される３Ｄモデル１４ｍの点群の中で、最もカメラ１２に近い点群を抽出する
第２ステップ：エッジに投影される点群の中で、最もカメラ１２に近い点群を抽出する
第３ステップ：設定されたウィンドウ２２に投影される点群の中で、最もカメラ１２に近い位置にある点群からｚ方向の所定距離内に位置する３Ｄモデル１４ｍの全ての点群を抽出する
第４ステップ：抽出された点群の中で重複する点群を削除する
である。

そして、作成された点群と、Ｓ１０１で取得された画像から抽出された点群とを用いて、例えばＩＣＰアルゴリズムに従ってマッチング処理を実行し、３Ｄ物体の姿勢を検出する（Ｓ１０７）。３物体の姿勢は、具体的には３Ｄ物体１４ｒの形状、言い換えれば３Ｄモデル１４ｍの形状に応じた適切な座標系で表現し得る。例えば、３Ｄ物体１４ｒがナットのような形状であれば、その中心に開口が存在するので、その開口の中心位置を原点とし、ナットの円盤面に参照点を２点決め、原点からこれら２点の参照点を用いて３Ｄ物体１４ｒのｘｙ平面を求めて姿勢とすればよい。また、３Ｄ物体１４ｒが図２Ａに示すような形状である場合、例えば脚部の底面の１頂点を原点とし、原点と底面の他の頂点を用いて底面のｘｙ平面を求めて姿勢とすればよい。

このようにして検出された３Ｄ物体１４ｒの姿勢は、例えばバラ積みされた３Ｄ物体１４ｒのロボットアームによる把持動作等に適用され得る。３Ｄ物体１４ｒを把持して所定箇所に搬送するためには、まず３Ｄ物体１４ｒの位置及びその姿勢を正確に検出する必要があるところ、実施形態の３Ｄ物体検出装置１０により３Ｄ物体１４ｒの姿勢を検出し、その姿勢検出信号に基づいてロボットアームが駆動される。実施形態の３Ｄ物体検出装置１０は、ロボットに組み込まれ得るが、３Ｄ物体検出装置１０とロボットを別個とし、両者が有線あるいは無線のネットワークで接続される構成でもよい。

以上、本発明の実施形態について説明したが、本発明はこれに限定されるものではなく、種々の変形が可能である。以下、変形例について説明する。

＜変形例１＞
実施形態では、３Ｄ物体の段差部分に着目してマッチング用の点群を追加抽出しているが、３Ｄ物体の段差部分に加え、その周縁部分に着目してマッチング用の点群を追加抽出してもよい。３Ｄ物体の周縁部分も段差部分同様にその投影画像にエッジが生じるからである。

図１０は、この場合の処理を模式的に示す。図１０において、マッチングモデル作成部１０５は、投影画像１４ｍｐのうち、周縁部に対応するエッジ近傍に所定サイズのウィンドウ５０、５２を設定し、エッジに投影される点群の中で、最もカメラ１２に近い点群を抽出するとともに、これらのウィンドウ内において最もカメラ１２に近い位置にある点群から所定距離内に位置する３Ｄモデル１４ｍの全ての点群を抽出する。エッジに投影される点群の中で、最もカメラ１２に近い点群として点群４０ｍが抽出される。また、ウィンドウ５０内において最もカメラ１２に近い位置にある点群から所定距離内に位置する３Ｄモデル１４ｍの全ての点群として点群４２ｍが抽出され、ウィンドウ５２内において最もカメラ１２に近い位置にある点群から所定距離内に位置する３Ｄモデル１４ｍの全ての点群として点群４６ｍが抽出される。点群４４ｍについては、所定距離外であるため抽出対象から除外される。

＜変形例２＞
実施形態では、３Ｄ物体の段差部分に着目してマッチング用の点群を追加抽出しているが、姿勢認識部１０１で認識した３Ｄ物体の姿勢認識と実際の姿勢との間の誤差によっては、追加抽出すると逆に不要な点群が追加されてしまう場合もあり得る。このような場合には、所定距離という制限に加え、さらに他の制限も課すことで不要な点群が除外され得る。

図１１は、この場合の処理を模式的に示す。図１１（ａ）は、３Ｄ物体の実際の姿勢をカメラ１２で撮影して得られた画像から抽出したマッチング用の点群であり、図１１（ｂ）は、３Ｄ物体に対応する３Ｄモデルの推定姿勢をカメラ１２の視点から平面に投影して得られた投影画像から抽出したマッチング用の点群を示す。

図１１（ａ）において、３Ｄ物体１４ｒはその姿勢が多少傾いた状態にあるものとし、このときに撮影された画像からマッチング用の点群が点群１６ｒとして抽出される。このとき、３Ｄ物体１４ｒの段差部分１８には点群は存在しない。

他方で、図１１（ｂ）において、３Ｄ物体１４に対応する３Ｄモデル１４ｍの推定姿勢に傾きがあるもののその傾きに誤差があると、このときの投影画像からマッチング用の点群が点群１６ｍとして抽出され、さらに実施形態の処理により段差部分から点群２０ｍが追加抽出されることになる。追加抽出された点群２０ｍのうちのいくつかは、図１１（ａ）で抽出された点群１６ｒには存在しない点群であり、不要な点群である。

そこで、抽出したエッジに投影される点群の中で、最もカメラ１２に近い点群（図における点群１７ｍ）を基準として所定サイズのウィンドウを設定し、点群２０ｍのうちこのウィンドウ内に存在しない点群を除外する。図において、追加抽出された点群２０ｍのうち、ウィンドウ内に存在しない点群２０ｍを白丸で示す。これにより、追加抽出された点群２０ｍのうちの不要な点群が除外され、その後のマッチング処理の精度が確保される。

なお、ウィンドウの幅は、姿勢認識部１０１における最大認識誤差に応じて設定され、具体的には最大認識誤差が大なるほどウィンドウ幅は大きく設定される。姿勢認識部１０１における最大認識誤差は、予め実験で検出される。

＜変形例３＞
実施形態では，図２Ａに示すような３Ｄ物体１４ｒを例示したが、本発明はこれに限らず任意の形状、例えばネジ等の部品や工具、家具等に適用し得る。

また、実施形態では、検出された３Ｄ物体１４ｒの姿勢をロボットアームによる部品等の把持・搬送に適用する場合について説明したが、これに限らず、障害物の姿勢を検出して移動体を制御する自動運転、商品の姿勢を検出することによる商品の検品、介護者の姿勢を検出して介護者を介護する介護ロボット等にも適用し得る。

図１２は、ロボットアームによる部品把持・搬送に適用した場合のロボットの全体構成図を示す。なお、ケースからバラ積みされた部品を取り出す工程は一般にランダムピッキングと称され、このロボットは、ランダムピッキングを自動化するシステムに適用され得る。

ロボットは、３Ｄ物体検出装置１０と、３Ｄ物体検出装置１０からの検出信号に基づきその動きが制御されるロボットアーム２００を備える。３Ｄ物体１４ｒは、ケースにバラ積みされており、３Ｄ物体検出装置１０のカメラ１２は、バラ積み状態の３Ｄ物体１４ｒを撮影し、得られた距離画像を画像処理してその位置及び姿勢を検出する。検出された位置及び姿勢は、ロボットアーム２００の制御機構に供給される。

なお、３Ｄ物体検出装置１０で検出される位置及び姿勢は、距離画像カメラを基準とした座標系における位置及び姿勢であり、これをロボットアーム２００の制御機構に供給する際には、ロボットアーム２００の座標系に変換して供給する。座標系の変換は、例えば実空間内の任意の３点について距離画像カメラの座標とロボットアーム２００の座標系をそれぞれ計測し、両者の間の変換行列を算出し、得られた変換行列を用いて変換すればよい。

１０ ３次元（３Ｄ）物体検出装置、１２ カメラ、１４ｒ ３Ｄ物体、１４ｍ ３Ｄモデル、１４ｍｐ 投影画像、１５ エッジ、２２，５０，５２ ウィンドウ。

Claims (10)

1. ３次元物体の距離画像を取得するカメラと、
   前記距離画像を用いて前記３次元物体に対応する３次元モデルの姿勢を算出する３次元モデル算出部と、
   算出された前記３次元モデルを前記カメラの視点から平面に投影して得られる投影画像のエッジを抽出する抽出部と、
   抽出された前記エッジにおいて前記カメラに最も近い位置の点群から予め定めた距離内にある前記３次元モデルの点群をマッチング用点群として抽出し、抽出したマッチング点群と前記距離画像から得られたマッチング用点群を照合することで前記３次元物体の姿勢を検出する処理部と、
   を備える３次元物体検出装置。
2. 前記処理部は、前記投影画像において前記エッジに予め定めたサイズのウィンドウを設定し、前記ウィンドウ内で前記カメラから最も近い位置の点群を抽出する
   請求項１に記載の３次元物体検出装置。
3. 前記ウィンドウの前記サイズは、前記３次元モデルに応じて可変設定される
   請求項２に記載の３次元物体検出装置。
4. 前記距離は、固定値である
   請求項１に記載の３次元物体検出装置。
5. 前記距離は、前記３次元モデルに応じて可変設定される
   請求項１に記載の３次元物体検出装置。
6. 前記距離は、前記エッジに対応する前記３次元モデルの段差部の段差に応じて設定される
   請求項１に記載の３次元物体検出装置。
7. 前記エッジは、前記３次元物体の段差である
   請求項１に記載の３次元物体検出装置。
8. 前記エッジは、前記３次元物体の周縁部である
   請求項１に記載の３次元物体検出装置。
9. 請求項１〜８のいずれかに記載の３次元物体検出装置と、
   前記３次元物体検出装置で検出された前記３次元物体の姿勢を用いて前記３次元物体を把持するロボットアームと、
   を備えるロボット。
10. コンピュータに、
    ３次元物体の距離画像を取得するステップと、
    前記距離画像を用いて前記３次元物体に対応する３次元モデルの姿勢を算出するステップと、
    算出された前記３次元モデルを前記距離画像を取得したカメラの視点から平面に投影して得られる投影画像のエッジを抽出するステップと、
    抽出された前記エッジにおいて前記カメラに最も近い位置の点群から予め定めた距離内にある前記３次元モデルの点群をマッチング用点群として抽出し、抽出したマッチング点群と前記距離画像から得られたマッチング用点群を照合することで前記３次元物体の姿勢を検出するステップと、
    を実行させるプログラム。
    

Images