JP2020101437A - ３次元位置・姿勢認識装置及び方法 - Google Patents

Abstract

【課題】把持対象商品の位置姿勢並びに把持座標の姿勢推定の高速化、つまり、ＩＣＰ高速化を可能とする３次元位置・姿勢認識装置及び方法を提供する。【解決手段】３次元位置認識装置は、物体毎のモデルデータを記憶する外部メモリと、物体の画像と、物体との距離を測定するセンサ部と、外部メモリと、センサ部とに接続され、物体毎のモデルデータから、物体毎に設定された解像度の構造化モデルデータを作成し、センサ部からの物体の画像と物体との距離の情報から、複数の解像度の計測点群データを生成し、解像度の構造化モデルデータと、複数の解像度の計測点群データのそれぞれの解像度の計測点群データとでＫ近傍点探索を行い、Ｋ近傍点探索から得られた対応点情報に基づく回転並進推定により物体の３次元位置認識処理を行う処理部を有する。【選択図】 図１Ａ

Description

本発明は、３次元位置・姿勢認識の技術に関する。特に、自律制御ロボットにおいて、商品ピッキング作業のための商品の３次元位置・姿勢認識技術に関する。

労働力不足を背景に、ロボット導入による人手作業の自動化が進んでいる。これまでも、工場における部品切削や溶接作業など定型的な作業の自動化が進んできた。今後、ロボット導入はさらに拡大し、建設現場、物流倉庫、食品・化粧品・医療品などの三品産業分野への応用を向けた開発が活発化している。

一例として、物流倉庫における商品ピッキング作業自動化があげられる。ピッキング作業自動化ロボットでは、ロボットアーム先端に取り付けた深度およびイメージセンサにより物品棚の３次元画像を取得し、目標商品の有無を画像認識により検出し、予め登録された商品の３次元モデルデータと比較しながら把持座標を算出する。得られた把持座標に対して軌道を計算し、ピッキングを実行する。

このような物体の配置を認識する技術として、特許文献１がある。特許文献１には、物体までの距離を計測するセンサと、センサを移動させる移動機構と、センサと接続し、物体の少なくとも形状を格納する物体情報データベース、物体の配置パターンを格納する物体配置データベース、移動機構によるセンサの移動に伴うセンサからの、物体までの距離を計測したセンサデータとセンサデータを得たセンサの位置とを入力とし、センサの位置に応じてセンサデータを３次元空間内に統合した、物体の輪郭を示す統合データを出力するセンサデータ統合部、及び物体情報データベースに格納された物体の形状から物体モデルを作成し、物体配置データベースに格納された物体の配置パターンを参照して、作成した物体モデルと統合データとを比較して物体の実配置を示す物体実配置データを出力する物体比較演算部が開示されている。

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

特許文献１では、データ構造型近傍探索における演算処理の高速化については、言及されていない。例えば、把持対象商品の位置姿勢並びに把持座標はＩｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ （ＩＣＰ）と呼ばれる演算により算出される。ＩＣＰでは商品の３次元モデルデータを回転・並進させながら、物品棚の計測データに上手く嵌る回転・並進行列を計算する。計算が繰り返されるため演算時間が長く、並列化による高速化が困難となることが考えられ、ロボットによるピッキング作業効率を人間作業者に比べ劣化させる要因となることが考えられる。発明者は研究により、ピッキング作業のための演算処理では、画像認識、姿勢推定、軌道計算を行う必要があるが、この内、姿勢推定の計算が、全体の８割以上を占め、姿勢推定のための計算の高速化がピッキング作業の演算処理を高速化するために重要であることを見出した。

そこで、本発明の目的は、把持対象商品の位置姿勢並びに把持座標の姿勢推定の高速化、つまり、ＩＣＰ高速化を可能とする３次元位置・姿勢認識装置及び方法を提供することにある。

上記課題を解決する本発明の３次元位置認識装置の一例は、物体毎のモデルデータを記憶する外部メモリと、物体の画像と、物体との距離を測定するセンサ部と、外部メモリと、センサ部とに接続される処理部を有する。処理部は、物体毎のモデルデータから、物体毎に設定された解像度の構造化モデルデータを作成し、センサ部からの物体の画像と物体との距離の情報から、複数の解像度の計測点群データを生成し、解像度の構造化モデルデータと、複数の解像度の計測点群データのそれぞれの解像度の計測点群データとでＫ近傍点探索を行い、Ｋ近傍点探索から得られた対応点情報に基づく回転並進推定により物体の３次元位置認識処理を行う。

また、本発明の３次元位置認識装置の他の一例は、物体毎のモデルデータを記憶する外部メモリと、物体の２次元画像と、物体との距離情報を取得するセンサ部と、センサ部によって取得される物体の２次元画像に基づいて、物体種別と物体領域座標を出力する画像認識部と、画像認識部からの物体種別と物体領域座標と、物体毎の許容誤差に基づいて、物体毎の解像度を出力するパラメタ算出部と、パラメタ算出部からの解像度に基づいて、画像認識部の出力である物体種別に基づいて外部メモリから読み出した物体のモデルデータからデータを間引いて間引きデータを出力するデータ間引き部と、間引きデータからデータ構造化近傍探索のための構造化モデルデータを生成するデータ構造化部と、データ構造化部によって生成された構造化モデルデータを記憶する内部メモリと、センサ部からの２次元画像から計測点群データを生成する点群化部と、計測点群データを、解像度を含む複数の解像度に基づいて、解像度変換して所定の解像度の計測点群データとして出力する解像度変換部と、内部メモリに記憶された構造化モデルデータと、解像度変換部からの所定の計測点群データとでＫ近傍点探索を行うＫ近傍点探索部と、Ｋ近傍点探索部の出力と、内部メモリに記憶された構造化モデルデータとで回転・並進推定を行う回転・並進推定部と、内部メモリに格納された構造化モデルデータと解像度変換部の計測データとの誤差が一定値以下となるまで、Ｋ近傍点探索部と回転・並進推定部の処理を繰り返す第1のループと、解像度変換部からの複数の解像度のそれぞれについて、Ｋ近傍点探索部と回転・並進推定部の処理を繰り返す第２のループとを有する。

本発明によれば、把持対象商品の位置姿勢並びに把持座標の姿勢推定の高速化を図ることができる。また、データ構造型近傍点探索におけるデータ構造化演算時間を短縮化し、例えば、ロボットアームによる商品の把持位置の算出のための演算時間を短縮化することができる。

上記した以外の課題、構成および効果は、以下の発明を実施するための形態の説明により明らかにされる。

３次元位置・姿勢認識装置の概要図である。 ３次元位置・姿勢認識装置のハードウェア構成図である。 パラメタ算出部の処理を説明する図である。 Ｋ−Ｄ木構造化法における空間分割を説明する図である。 Ｋ−Ｄ木構造によるデータ構造化を説明する図である。 グラフ構造によるデータ構造化とデータフォーマットを示す図である。 階層グラフによるデータ階層構造化を示す図である。 アクセラレータを用いた３次元位置・姿勢認識装置を示す図である。 アクセラレータを用いた３次元位置・姿勢認識装置のハードウェア構成図である。 データ構造型近傍探索を行うアクセラレータの処理を説明する図である。 データ構造化計算における距離計算処理並びに並び替え処理部のハードウェア実装処理を示した詳細図である。

以下、図面を参照して本発明の実施形態を説明する。以下の記載および図面は、本発明を説明するための例示であって、説明の明確化のため、適宜、省略および簡略化がなされている。本発明は、他の種々の形態でも実施する事が可能である。特に限定しない限り、各構成要素は単数でも複数でも構わない。

以下の説明では、「テーブル」、「リスト」、「キュー」等の表現にて各種情報を説明することがあるが、各種情報は、これら以外のデータ構造で表現されていてもよい。データ構造に依存しないことを示すために「ＸＸテーブル」、「ＸＸリスト」等を「ＸＸ情報」と呼ぶことがある。識別情報について説明する際に、「識別情報」、「識別子」、「名」、「ＩＤ」、「番号」等の表現を用いるが、これらについてはお互いに置換が可能である。

同一あるいは同様な機能を有する構成要素が複数ある場合には、基本的に同一の符号を付して説明するが、機能が同じであっても機能を実現するための手段が異なる場合がある。

また、以下の説明では、プログラムを実行して行う処理を説明する場合があるが、プログラムは、中央処理部であるプロセッサ（例えばＣＰＵ）によって実行されることで、定められた処理を、適宜に記憶資源（例えばメモリ）および／またはインターフェースデバイス（例えば通信ポート）等を用いながら行うため、処理の主体がプロセッサとされてもよい。

プログラムは、プログラムソースから計算機のような装置にインストールされてもよい。プログラムソースは、例えば、プログラム配布サーバまたは計算機が読み取り可能な記憶メディアであってもよい。プログラムソースがプログラム配布サーバの場合、プログラム配布サーバはプロセッサと配布対象のプログラムを記憶する記憶資源を含み、プログラム配布サーバのプロセッサが配布対象のプログラムを他の計算機に配布してもよい。また、以下の説明において、２以上のプログラムが１つのプログラムとして実現されてもよいし、１つのプログラムが２以上のプログラムとして実現されてもよい。

ＩＣＰでは計測データとモデルデータが入力され、いずれも２次元画像の各ピクセルに距離情報が付加された点群データである。ＩＣＰでは（１）点同士の対応付け計算と、（２）回転及び並進行列推定の２種類の計算から構成される。工程（１）における点同士の対応付けは、近傍点探索法により算出される。実際には最近傍点1点のみではなく、Ｋ個の候補点が算出され、その中で重心が対応点として選ばれる。工程（２）では得られた点同士の対応付けに基づき、回転および並進行列を求める。工程（１）、（２）を繰り返し計算し、距離の合計が最小になるまで繰り返し計算を行い、モデルデータに対して計測データの姿勢推定を行う。例えば、物品を計測し、計測データとモデルデータに基づいて、ロボットによる物品の把持位置等を算出する。

本発明の代表的な実施形態について概要を説明する。図１Ａは、代表的な実施の形態による３次元位置・姿勢認識装置100の概要を示す図である。図１Ａに示したように、３次元位置・姿勢認識装置100は、センサ部150と処理部151と外部メモリ部152で構成される。

センサ部150では、イメージセンサ101により認識対象物体の複数のピクセルデータを含む２次元画像を取得する。また、深度センサ102により、認識対象物体表面の各点と深度センサ間の距離情報を取得する。深度センサ102にはTime of Flight(ToF)方式と呼ばれる、送信した基準信号と反射信号の位相変位量から距離を求める方式、またはパターン光をプロジェクタにより測定対象に照射し、パターン変位量をイメージセンサにより観測し距離を求めるStructured-Light方式がよく用いられる。

画像認識部103は、イメージセンサ101により得られた画像に対して物体認識を行い、物体の種別と物体の画像内における領域座標を出力する。対象となる物体が複数の場合、物体毎に物体種別と物体の画像内における領域座標を出力する。画像認識部103の画像認識には深層学習などの手法が用いられる。

処理部151は、画像認識部103から出力された物体の種別に基づいて、外部メモリ152内に保存された複数のモデルデータ104の中から物体種別に合致するデータを取り出し、処理部内の内部メモリ108に格納する。並行して処理部151では、画像認識部103から出力された物体の種別と物体の画像内における領域座標のデータをパラメタ算出部105に伝送する。

パラメタ算出部105では、物体の種別と領域座標のデータに基づき、データ間引き部106および計測データの解像度変換部111で使用する解像度、ループ処理114、115で使用するループ回数等が決定される。パラメタ算出部105の詳細は、図２を用いて説明する。

モデルデータ104は、点群からなるモデル点群データである。外部メモリ152から処理部151に伝送されたモデルデータ104は、データ間引き部106に伝送され、パラメタ算出部105により決定された解像度に基づいて、モデルデータからデータの間引きが実施される。間引かれたモデルデータはデータ構造化部107において、データ構造化近傍点探索のためのデータ構造化処理を行う。

データ構造化部107により、構造化された構造化モデルデータは処理部内の内部メモリ108に格納され、Ｋ近傍点探索部112および回転・並進行列推定部113において使用される。構造化モデルデータは、Ｋ近傍点探索部112および回転・並進行列推定部113により、頻繁に参照されるデータであるため、外部メモリ152におかず内部メモリ108に配置することで高速化される。

イメージセンサ101で得られたカラー画像（複数のピクセルデータを含む２次元画像）と、深度センサ102により得られた深度データは、処理部151の画像合わせ部109に送られ、互いの視野と位置座標を揃えたデータに変換される。画像合わせ部109の画像合わせ処理に用いられる変換行列は、光軸の校正や基準画像を基にした校正手法などにより精度よく求められる。

次に、点群化部110は、画像合わせ部109によって変換され、互いに位置座標を揃えたイメージセンサ101と深度センサ102のデータを点群データ（計測点群データ）に変換する。つまり、イメージセンサ101のカラー画像の各ピクセルに、深度センサ102深度データを付与した計測点群データに変換される。

点群化部110から出力される計測点群データは、解像度分割型ＩＣＰアルゴリズムのため、解像度変換部111において、パラメタ算出部105にて決定された解像度に基づき間引き処理が実施される。

次に、Ｋ近傍点探索部112では、データ構造化部107にて計算された構造化モデルデータにおいて、間引き処理された計測データの各点に対し近い距離にある点をＫ個計算する。続いてＫ個の点の重心位置を計算する。

続く回転・並進推定部113では、得られた重心位置を利用して特異値分解法に代表される計算法により、回転・並進行列を算出する。構造化モデルデータの重心位置と対応する計測点群データの各点との距離を求め、平均距離がパラメタ算出部105で設定した計算回数の条件を満たしているか確認を行う。計算回数の条件を満たすまでループ114によるループ処理を実行する。

ループ処理114が満足された場合、ループ115によるループ処理により、パラメタ算出部105で計算される次の解像度（低解像度から徐々に高い解像度を選択する）で同様の計算を実施する。解像度変換部111により、計測データはより詳細な解像度のデータに変換される。従来手法であれば、この計測データの解像度変更に伴い、構造化モデルデータの解像度も変更する必要があり、変更された解像度の構造化モデルデータを参照してデータ間引き部106の処理、およびデータ構造化部107の処理が繰り返し実行されていた。しかし、一般にデータ構造化部107の処理は多くの並べ替え処理が発生し、外部メモリ152への参照・データ入れ替えが多発するため、計算時間が増化する傾向にある。

そのため、実施例１では、計測データの解像度が変更されても、構造化モデルデータの再生成処理を不要とし、構造化モデルデータの再利用を可能とするため、パラメタ算出部105の処理の初めに必要最低限に細かい解像度で構造化モデルデータを作成するようにした。この処理は、図２において説明するが、パラメタ算出部105に物体の種別ごとに許される許容誤差を指定し、これに基づいて、構造化モデルデータを作成するための最低解像度を計算することによって実現している(最低解像度は、説明を分かりやすくするための用語であって、使用者からの許容誤差に基づいて算出される解像度であれば、最低である必要はない)。これによりデータ間引き部106の処理およびデータ構造化部107の処理の演算回数を最小である１回に抑えることで、演算時間を大幅に短縮化することができる。

パラメタ算出部105にて指定された解像度の全てで計算が終了した場合、ループ処理115が満足され演算が終了される。最後に目標物体の回転・並進行列推定部113から把持座標が算出され、出力部116に演算結果が出力される。

図１Ｂは、実施例１の３次元位置・姿勢認識装置のハードウェア構成図である。３次元位置・姿勢認識装置100は、外部装置との通信を行うためのインタフェース121、入出力装置(図示せず)、処理部（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）122、メモリ108、ＨＤＤやＳＳＤ等から構成される記憶装置152を含んで構成されている。

CPU122は、記憶装置152に格納された各種プログラム103〜113をメモリ108に読み込み、各種プログラムを実行することで、図１Ａで説明した各種機能を実現する。メモリ108は、CPU122で実行される各種プログラムが格納されたり、CPU122の処理結果が一時的に格納されたりする。

ここで、各種プログラムは、図１Ａで説明した、画像認識部103、パラメタ算出部105、データ間引き部106、データ構造化107、画像合わせ部109、点群化部110、解像度変換部111、K近傍点探索部112、回転・並進推定部113であり、これらプログラムをCPU122が実行することで、図１Ａで説明した各種処理を実現する。

図２は、パラメタ算出部105の処理を説明する図である。
ステップ204で、パラメタ算出部105には、以下の情報が入力される。
予め使用者によって設定されたパラメタである許容誤差(説明を分かりやすくするために最大許容誤差と称するが、必ずしも最大である必要はなく、使用者によって設定された値である)（E_MAX）、イメージセンサ101の解像度（G_CAM）、最小点数（F_NUM）、解像度分解数（Ｓ）。

E_MAXは物体（例えば、ロボットアーム160によって把持される商品）ごとに予め決められる最大許容誤差である。商品ごとにE_MAXを設定することで、商品の把持位置を求めるため、モデルデータから構造化モデルデータを作成する際の最低解像度を求めることができ、この最低解像度で作成された構造化モデルデータを参照することで、種々の解像度の計測点データとの比較を行うことができる。そのため、計測点データの解像度を変更しても、構造化モデルデータを各解像度で作成する必要がなくなり、商品の把持位置を高速に求めることができる。

F_NUMは最低限の演算精度を担保するために必要な最低限度の点数であり、通常数百点程度に規定される。Ｓはループ処理115を何回繰り返すかを決める値であり、通常数回程度に規定される。

画像認識103から出力された物体領域座標Ｚを取得し（S205）、計測画像内における物体面積Ｂが算出される（S206）。

イメージセンサの解像度G_CAMと物体面積Ｂを用いて、計測物体モデルの点群数P_NUMがP_NUM=Ｂ×G_CAMによって算出される（S207）。

ステップ（S208）において、G_MIN=G_CAM×P_NUM/F_NUMにより最低解像度（G_MIN）が算出される。

ステップ(S209)において、最高解像度（G_MAX）は、最大許容誤差（E_MAX）と同じ距離分解能になるように設定される。

ステップ（S210）において、ループ処理114におけるi番目の解像度（G_ｉ）は、最高解像度（G_ＭＡＸ）と最低解像度（G_ＭＩＮ）を用いて算出される（G_ｉ＝G_ＭＡＸ＋i×（G_ＭＡＸ−G_ＭＩＮ）/S）。K近傍点探索部112、回転・並進推定部113の処理を、はじめは低解像度の計測点群データから徐々に解像度を高くして行うためである。これにより、はじめは粗く、K近傍点探索部112、回転・並進推定部113の処理を行い、徐々に解像度を高めることで、例えば、商品の目標把持位置の算出を高速かつ高精度に行うことができる。

ステップ(S211)においてi番目のループ処理115における、ループ処理114のループ回数（Ｎ_ｉ）を算出する。１番目のループを数百回程度に設定し、iが大きいほどループ回数を削減するように算出する。決定されたパラメタは解像度変換部111、データ間引き部106に送られる。

つまり、出力パラメータの内、最高解像度G_ＭＡＸは、データ間引き部106と解像度変換部111に出力され、最低解像度G_ＭＩＮと中間解像度G_ｉは、解像度変換部111に出力され、計算回数Ｎ_ｉは、ループ処理114、115に出力される。

データ構造化部107においては、近傍点探索のためのデータ構造化が行われる。従来はデータ構造化にはＫ−Ｄ木構造がよく用いられる。図３Ａに示したように、Ｋ−Ｄ木構造化処理では、空間を、代表点を通る軸により分割しながら、図３Ｂに示す各データ点を木構造に登録する。代表点には、分割対象領域の中央値となるデータ点が選択される。近傍探索を行う場合は、データと各軸の代表点との大小関係を比較しながら木構造の末端に登録されたデータ点を探索する。このＫ−Ｄ木構造による近傍点探索は１個の近傍点探索は高速化されているものの、Ｋ個の近傍点算出にはＫ回演算を繰り返す必要があり、依然としてＩＣＰにおいては膨大な演算時間が必要となる。

そのため、実施例１では、図３Ｃ、図３Ｄに示すＫ個の近傍点を一括して算出可能な階層グラフ型近傍点探索を用いる。図３Ｃに示すように、階層グラフ型近傍点探索では、各データ点同士を頂点として互いに接続したグラフを構築する。各データ点同士の距離を計算し、グラフの各辺の長さとして保存する。グラフの辺は短い順にＫ個のみを選択し、保存する。したがって、構造化データに対して近傍点探索を実施し、1つ近傍点が探索されると、その近傍点に近いＫ個の近傍点も演算無しに同時に探索でき、演算を高速化できる。階層グラフのデータフォーマット304に示す。各データ点をまずQuery pointとして選択し、周辺の点との距離を計算する。距離が短い順に並び替え、上位複数個のみをNeighbor pointとして各Query pointに対してＩＤと距離を登録する。Query pointはデータ点すべてに対して実行する。図３Ｃでは、Query point[１]を選択し、周辺の点[2]、[3]、[4]に対し、Neighbor point ＩＤ[2][3][4]をそれぞれ付与している。そして、各Neighbor point ＩＤに対し、Query point[１]から各店までの距離[0.05][0.03][0.05]を登録している。

ここで、図３Ｄに示しように、グラフ構築の演算量を削減するため、Ｋ−Ｄ木同様データを分割し、分割された領域内だけでグラフを作成する。分割された領域は代表点だけに縮約化され、代表点だけでさらにグラフが構築され階層化される。図３ＤのＬａｙｅｒ０は、詳細なデータであり、Ｌａｙｅｒ２は、縮約化されたデータである。

回転・並進推定部113において条件を満たした回転・並進行列が与えられる。モデル点群データに指定された把持座標に対し、得られた回転・並進行列をかけることで、目標把持座標が算出され装置より出力される。

以上の動作を簡単に纏めると、ＩＣＰ計算のうち、計算の大部分を占める近傍点探索法を高速化するため、データ構造化近傍点探索を解像度分解型ＩＣＰに導入する。画像認識結果に基づいて呼び出された該当物体のモデルデータは、ある解像度に基づいて間引き処理が施され、階層グラフ化などのデータ構造化処理により実施される。解像度は、その物体で許容される演算誤差から必要最低限の値を求め適用する。

構造化データは内部メモリ108に格納され、Ｋ近傍点探索112、回転・並進推定113における演算結果の評価に利用される。解像度分解型ＩＣＰでは、ループ処理114により演算精度が一定の条件を満たすまで繰り返し計算され、その後ループ処理115により次の解像度での計算に移行する。解像度変換部111により、計測データはより詳細な解像度のデータに変換される。従来手法であれば計測データの解像度変更に伴い、構造化データも解像度が変更され再生成される。しかし、実施例１ではパラメタ算出部105により、処理の初めに必要最低限の細かい解像度で構造化データを作成しており、計測データの解像度に応じて構造化データは再生成することなく、同一の構造化モデルデータが再利用される。

以上実施例１によれば、データ構造型近傍点探索におけるデータ構造化演算時間を短縮化、例えば、ロボットアームによる商品の把持位置の算出のための演算時間を短縮化することができる。

また、実施例１によれば、Ｋ近傍点探索部112および回転・並進行列推定部113により、頻繁に参照される構造化モデルデータを、外部メモリ152におかず内部メモリ108に配置することで高速化され、例えば、ロボットアームによる商品の把持位置の算出のための演算時間を短縮化することができる。

実施例２では、演算処理高速化のために、アクセラレータを組み込んだ形態について概要を説明する。実施例２による３次元位置・姿勢認識装置は、図４Ａに示したように、センサ部150と外部メモリ152と、処理部を構成する汎用処理部141とアクセラレータＡ142とアクセラレータＢ143とを有する。センサ部150の構成等は実施例１と同様である。また近傍点探索には階層グラフ型近傍点探索手法を用いる。アクセラレータＡ142とアクセラレータＢ143は、例えばＦＰＧＡやＡＳＩＣによって構成される。

画像認識部103には、深層学習などの手法が用いられる。必要演算回数が多く、ＣＰＵに代表される汎用演算器では演算時間が長く消費電力も大きいという課題がある。そこで、ＧＰＵやＡｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ（ＡＳＩＣ）など、並列演算に特化したハードウェアをアクセラレータとして用いることで、演算時間を短縮することが出来る。

この場合、イメージセンサ101で取得された認識対象物体の複数のピクセルデータを含む２次元画像は、汎用処理部141を介してアクセラレータモジュールＡ142に伝送される。画像認識部103はアクセラレータモジュールＡ142上で実行される。認識結果は実施例１と同様に、物体種別は外部メモリに伝送されモデルデータ104の呼び出しに使用される。物体の領域座標データは汎用処理部141のパラメタ算出部105に伝送される。

データ構造型近傍点探索を用いたＩＣＰアルゴリズムは、近傍点探索におけるデータの並び替え処理、データアクセス処理時間が課題となって演算時間が長くなる。そこで、処理高速化のため、ＩＣＰアルゴリズム処理はアクセラレータモジュールＢ143において実施される。データ構造化計算408と、構造化モデルデータを頻繁に参照し計算するＫ近傍探索処理部112の処理、回転・並列推定部113の処理を同一アクセラレータモジュールＢに実装することで、構造化モデルデータを逐次外部メモリに格納せず内部メモリやレジスタに格納することで、データ移動最小化でき演算時間を短縮できる。

図４Ｂは、実施例２の３次元位置・姿勢認識装置のハードウェア構成図である。３次元位置・姿勢認識装置100は、外部装置との通信を行うためのインタフェース121、入出力装置(図示せず)、処理部（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）122、メモリ108、ＨＤＤやＳＳＤ等から構成される記憶装置152、アクセラレータＡ142、アクセラレータＢ143を含んで構成されている。

CPU122は、記憶装置152に格納された各種プログラム109〜110をメモリ108に読み込み、各種プログラムを実行することで、各種機能を実現する。メモリ108は、CPU122で実行される各種プログラムが格納されたり、CPU122の処理結果が一時的に格納されたりする。

ここで、各種プログラムは、図４Ａで説明した、画像合わせ部109、点群化部110、である。アクセラレータＡ142は、図１Ａで示した画像認識部103の処理をハードウェアで実現する。アクセラレータＢ143は、図１Ａで示したデータ間引き部106、データ構造化部107の処理をハードウェアで実現する。アクセラレータＢ143の詳細は、図５で説明する。

図５は、アクセラレータモジュールＢ143を説明する図である。
アクセラレータＡ142の演算回路501により、センサ部150のイメージセンサ101の画像から物体の種別を判定する。演算回路501は、物体の種別を元に外部メモリ152に向かってアドレスが伝送されモデルデータが呼び出される。モデルデータはインタフェース502を介してアクセラレータモジュールＢ143に伝送される。モデルデータは、解像度変換部111が解像度変換処理を実施したのち、構造データ用内部メモリ507に構造化モデルデータとして格納される。同時に図４における点群化部110で計測された計測データもアクセラレータモジュールＢ143に伝送され、計測点群データとして計測データ用内部メモリ506に格納される。

階層グラフ型近傍点探索手法では、図３に示したように各データ点同士を頂点として互いに接続したグラフを構築し、各データ点同士の距離を計算し辺の長さとして保存する。グラフの辺は短い順にＫ個のみ選択され、構造データ用内部メモリ507に保存される。汎用処理部で本処理を実現する場合、並び替え処理において外部メモリへの参照が多く発生し処理時間が延びてしまう。そこで、内部に十分な計測データ用内部メモリ506、構造データ用内部メモリ507を備え、かつ並び替え処理を外部メモリ152への参照なしに実行可能なデータフロー型の並び替え演算回路509を備えたアクセラレータモジュールＢ143で演算処理を行う。

図６は、グラフ構造化回路を示す図である。構造データ用内部メモリ507に格納されたモデルデータの中から複数個（Ｋ個）のデータを一括して読み出し、1つをQuery、そのほかをCandidateとして距離計算モジュール508に伝送する。距離計算モジュール508では、複数個の距離計算回路603が並列に配置されており、Query データはすべての距離計算回路に一括して伝送される。

例えば、図３Ｃに示す例では、Query Point [1]に対し、複数の候補（Candidate）[2][3][4]に対し、各距離計算回路603がそれぞれ距離を並列に計算する。

Candidateはそれぞれの距離計算回路に伝送される。計算された距離は並び替え回路509に伝送され、距離の短い上位Ｋ個のCandidateが算出される。選択されたＫ個のデータをNeighbor pointとして、構造データ用内部メモリ507に登録する。並び替え回路509はアクセラレータモジュールＢ143上では専用の演算回路として実現される。この時並び替えられた中間データは内部レジスタ605に保持される。従来汎用演算器では中間データも外部メモリへ保存されており、外部メモリへの読み書きに膨大な処理時間と電力が必要であった。実施例２では内部レジスタ605により外部メモリへのアクセスを最小化でき、演算時間が短縮化される。アクセラレータモジュールＢ143は、設計者が専用回路設計可能であり、内部メモリとレジスタを潤沢に持つＦＰＧＡなどにより実現される。

以上の処理により、回転・並進推定部113において条件を満たした回転・並進行列が計算される。構造データ用内部メモリ507内に保持された把持座標に対し、得られた回転・並進行列を乗じることで目標把持座標が算出され出力される。

実施例２によれば、データ構造型近傍点探索におけるデータ構造化演算時間を短縮化し、例えば、ロボットアームによる商品の把持位置の算出のための演算時間を短縮化することができる。

また、実施例２によれば、Ｋ近傍点探索部112および回転・並進行列推定部113により、頻繁に参照される構造化モデルデータを、外部メモリ152におかず構造データ内部メモリ507、計測データ内部メモリ506に配置することで高速化され、例えば、ロボットアームによる商品の把持位置の算出のための演算時間を短縮化することができる。

さらに、実施例２によれば、階層グラフ型にデータを効率的に構造化するハードウェア構成により、例えば、ロボットアームによる商品の把持位置の算出を、さらに高速化することができる。

１０１：イメージセンサ、１０２：深度センサ、１０３：画像認識部、１０４：モデルデータ群、１０５：パラメタ算出部、１０６：データ間引き部、１０７：データ構造化部、１０８：内部メモリ、１０９：画像合わせ部、１１０：点群化部、１１１：解像度変換部、１１２：Ｋ近傍点探索部、１１３：回転・並進推定部、１１４：繰り返しループ処理、１１５：繰り返しループ処理、１４２：アクセラレータＡ、１４３：アクセラレータＢ、１６０：ロボットアーム、５０６：計測データ用内部メモリ、５０７：構造データ用内部メモリ、５０８：距離計算モジュール、５０９：並び替え演算回路、６０５：並び替え回路内のレジスタ。

Claims (10)

1. 物体毎のモデルデータを記憶する外部メモリと、
   物体の画像と、前記物体との距離を測定するセンサ部と、
   前記外部メモリと、前記センサ部とに接続され、
   前記物体毎のモデルデータから、前記物体毎に設定された解像度の構造化モデルデータを作成し、
   前記センサ部からの前記物体の画像と前記物体との距離の情報から、複数の解像度の計測点群データを生成し、
   前記解像度の構造化モデルデータと、前記複数の解像度の計測点群データのそれぞれの解像度の計測点群データとでＫ近傍点探索を行い、
   前記Ｋ近傍点探索から得られた対応点情報に基づく回転並進推定により前記物体の３次元位置認識処理を行う処理部を有する
   ことを特徴とする３次元位置認識装置。
2. 前記処理部は、
   前記構造化モデルデータを格納する内部メモリを有し、
   前記複数の解像度の計測点群データのそれぞれの解像度の計測点群データとでＫ近傍点探索を、前記内部メモリに格納された前記構造化モデルデータを参照して行うことを特徴とする請求項１に記載の３次元位置認識装置。
3. 前記処理部は、
   前記物体毎の許容誤差の入力を受け付け、
   前記許容誤差に基づいて、前記物体毎の解像度を算出するパラメタ算出部を有することを特徴とする請求項２記載の３次元位置認識装置。
4. 前記Ｋ近傍点探索と回転並進推定の処理を、専用ハードウェアで行うことを特徴とする請求項３記載の３次元位置認識装置。
5. 前記専用ハードウェアには、前記構造化モデルデータを格納する構造データ用内部メモリと、所定の解像度の計測点群データを格納する計測データ用内部メモリとを有することを特徴とする請求項４に記載の３次元位置認識装置。
6. 前記計測データ用内部メモリに格納された所定の解像度の計測点群データと一つのクエリー点に対し、前記構造データ用内部メモリに格納された構造化モデルデータの複数点との距離を並列に計算する距離計算モジュールを備えたことを特徴とする請求項５に記載の３次元位置認識装置。
7. 物体毎のモデルデータを記憶する外部メモリと、
   物体の２次元画像と、前記物体との距離情報を取得するセンサ部と、
   前記センサ部によって取得される前記物体の２次元画像に基づいて、物体種別と物体領域座標を出力する画像認識部と、
   前記画像認識部からの物体種別と物体領域座標と、物体毎の許容誤差に基づいて、物体毎の解像度を出力するパラメタ算出部と、
   前記パラメタ算出部からの解像度に基づいて、前記画像認識部の出力である物体種別に基づいて前記外部メモリから読み出した前記物体のモデルデータからデータを間引いて間引きデータを出力するデータ間引き部と、
   前記間引きデータからデータ構造化近傍探索のための構造化モデルデータを生成するデータ構造化部と、
   前記データ構造化部によって生成された構造化モデルデータを記憶する内部メモリと、
   前記センサ部からの２次元画像から計測点群データを生成する点群化部と、
   前記計測点群データを、前記解像度を含む複数の解像度に基づいて、解像度変換して所定の解像度の計測点群データとして出力する解像度変換部と、
   前記内部メモリに記憶された前記構造化モデルデータと、前記解像度変換部からの前記所定の計測点群データとでＫ近傍点探索を行うＫ近傍点探索部と、
   前記Ｋ近傍点探索部の出力と、前記内部メモリに記憶された構造化モデルデータとで回転・並進推定を行う回転・並進推定部と、
   前記内部メモリに格納された構造化モデルデータと前記解像度変換部の計測データの対応点情報の誤差が一定値以下となるまで、前記Ｋ近傍点探索部と前記回転・並進推定部の処理を繰り返す第1のループと、
   前記解像度変換部からの前記複数の解像度のそれぞれについて、前記Ｋ近傍点探索部と前記回転・並進推定部の処理を繰り返す第２のループとを
   有することを特徴とする物体の３次元位置認識装置。
8. ３次元位置認識装置における物体の３次元位置認識方法であって、
   外部メモリは、物体毎のモデルデータを記憶し、
   センサ部により、物体の画像と、前記物体との距離を測定し、
   前記外部メモリと、前記センサ部とに接続される処理部により、
   前記物体毎のモデルデータから、物体毎に設定された解像度の構造化モデルデータを作成し、
   前記センサ部からの前記物体の画像と前記物体との距離の情報から、複数の解像度の計測点群データを生成し、
   前記解像度の構造化モデルデータと、前記複数の解像度の計測点群データのそれぞれの解像度の計測点群データとでＫ近傍点探索を行い、
   前記Ｋ近傍点探索から得られた対応点情報に基づく回転並進推定により前記物体の３次元位置認識処理を行う
   ことを特徴とする３次元位置認識方法。
9. 前記処理部は、
   前記構造化モデルデータを格納する内部メモリを有し、
   前記複数の解像度の計測点群データのそれぞれの解像度の計測点群データとでＫ近傍点探索を、前記内部メモリに格納された前記構造化モデルデータを参照して行うことを特徴とする請求項８に記載の３次元位置認識方法。
10. 前記処理部は、
    前記物体毎の許容誤差の入力を受け付け、
    前記許容誤差に基づいて、前記物体毎の解像度を算出するパラメタ算出部を有することを特徴とする請求項９記載の３次元位置認識方法。

Images