JP2014528057A - 物体の姿勢を推定するための方法 - Google Patents

Abstract

物体の姿勢は、最初に、幾何プリミティブの対として一連の対特徴を定義することによって推定され、幾何プリミティブは、方向付けされた表面ポイント、方向付けされた境界ポイントおよび境界線セグメントを含む。モデル対特徴は、物体のモデルに対する一連の対特徴に基づいて決定される。場面対特徴は、３Ｄセンサによって取得されたデータからの一連の対特徴に基づいて決定され、次いで、モデル対特徴を場面対特徴と一致させ、物体の姿勢を推定する。

Description

本発明は、概して、３Ｄ物体の姿勢を推定することに関し、より具体的には、３Ｄセンサによって取得されたデータから姿勢を推定することに関する。

物体の姿勢には、６自由度（６−ＤｏＦ）、すなわち、３Ｄ並進および３Ｄ回転がある。姿勢推定の問題は、基準座標系、通常、センサの座標系に対する物体の姿勢を見出すことを意味する。姿勢は、例えば、３Ｄモデルを用いた、２Ｄ画像、３Ｄポイントクラウドなどのセンサからの測定値を使用して取得することができる。姿勢推定は、容器の拾上げ、把持、位置測定、自律的ナビゲーションおよび３Ｄ再構成など、多くのロボット工学応用において主な役割を果たす。

最近まで、姿勢推定は主に２Ｄ画像を使用して行われたが、その理由は、カメラの費用効率が高く、高速画像取得が可能であるためである。２Ｄ画像に関する主な問題は、２Ｄ特徴と、それらに対応するモデルの３Ｄ特徴とを一致させることにある。このことは、カメラの様々な照明条件や異なる視点により、画像空間における回転およびスケールの変化が生じるため、難易度が高いものとなる。その上、物体のいくつかの見方は、理論的に、曖昧な姿勢につながる恐れがある。入力画像と画像のデータベースとの対応を決定するため、いくつかの不変特徴記述子が知られており、同記述子では、２Ｄキーポイントを３Ｄ座標と一致させる。

多くの工業用部品は、例えば、機械加工された金属部品または成形されたプラスチック部品など、構造が曖昧なもの（ｔｅｘｔｕｒｅｌｅｓｓ）である。したがって、画像の観察可能なエッジに大幅に依存しなければならない。物体の境界が使用される場合、物体の一連のエッジテンプレートは、前提として知られる場合が多く、テンプレートは、クエリエッジマップ（ｑｕｅｒｙ ｅｄｇｅ ｍａｐ）において探索される。エッジの向きまたは階層表現を組み込むいくつかの変形形態が知られている。強度ベースのエッジ検出は、あまりにも多くのエッジ画素を生じさせる場合が多く、その場合、ほんのわずかなエッジ画素のみが深度不連続に由来する有効エッジである。マルチフラッシュカメラを使用することで、複数のフラッシュ方向から影を投影することによって深度エッジを直接推定することができる。

一般に、３Ｄセンサで得られる３Ｄデータは、２Ｄデータとは対照的に、はるかに少ない変形形態を有する。主な課題は、センサ雑音、オクルージョンおよびクラッタの存在下で対応問題の解を求めることである。対応問題は、データの特徴とモデルの特徴との１対１の一致を見出すことを意味する。特徴は、通常、物体のサイズや形状を特徴付けるように構成される。表面ポイントおよび法線の分布を使用するいくつかの３Ｄ特徴記述子や、一致手順が知られている。それらの記述子は、一般に、剛体変換に対して不変であるが、雑音やオクルージョンに敏感である。その上、それらの特徴は、高密度のポイントクラウドを必要とし、高密度のポイントクラウドは、利用可能でない場合がある。

姿勢推定は、センサ３Ｄデータとモデルとの様々な種類の対応、すなわち、３ポイント対応、２線対応および３つ以上の平面に対する６ポイントで実行可能である。通常、それらの対応は、姿勢を決定するためのランダムサンプルコンセンサス（ＲＡＮＳＡＣ：ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）などの仮説検証フレームワークで使用される。あるいは、姿勢は、ハフ投票方式またはパラメータ空間でのクラスタリングのいずれかを使用することによって、仮定の姿勢分布のモードから検索することができる。それらの手法は、画像も他の事前情報もない状態で、３Ｄセンサデータのみが利用可能である場合、２つの問題に悩まされる。ポイント、線および平面は、それほど個別に区別可能なものではなく、組み合わせて一致させることができ、モデルに対して何の事前処理も行うことなく、高速演算を達成することは難しい。

対特徴は、物体の表面上の２つの方向付けされたポイント間の距離および相対的な向きによって定義することができる。物体は、一連の方向付けされたポイント対特徴によって表され、それらの対特徴は、高速検索のためにハッシュ表Ｈに格納される。ランダムな２つのポイントは、センサデータからサンプリングされ、そのような各対は、特定の姿勢に投票する。必要な姿勢は、最大投票数を有するものに相当する。画素とオフセット画素との深度差からなる、より簡単な対特徴は、ランダムフォレスト集合分類子を用いる人間の姿勢推定に使用される。

本発明の実施形態は、３Ｄセンサによって取得されたデータから物体の姿勢を推定するための投票ベースの方法を提供する。先行技術で使用される方向付けされた表面ポイント（法線を有する、物体の表面上のポイント）は、かなりの曲率変化がある物体に対して区別可能なものであり、それらの表面ポイントは、平面状の多くの工業用の物体および現実世界の物体に対して十分にコンパクトなものでも、区別可能なものでもない。

境界から情報を得るため、２Ｄ登録ではエッジが重要な役割を果たす一方で、３Ｄでは深度不連続が極めて重要である。実施形態は、この境界情報をより良く活用する姿勢推定方法ファミリを提供する。方向付けされた表面ポイントに加えて、方向付けされた境界ポイント（方向を有する、物体の境界上のポイント）および境界線セグメントを含む、他の２つの幾何プリミティブを使用する。

慎重に選択されたプリミティブは、より多くの情報をコンパクトに符号化し、それにより、幅広いクラスの工業用部品に対してより優れた精度を提供し、さらなる高速演算を可能にする。

本発明の実施形態を使用することができるロボットアセンブリ装置の概略図である。 本発明の実施形態による、投票ベースの方法を使用して３Ｄ物体の姿勢を決定するための方法のブロック図である。 投票ベースの方法を使用して３Ｄ物体の姿勢を決定するための方法を詳述するブロック図である。 本発明の実施形態による、投票ベースの姿勢推定のための対特徴の概略図である。 本発明の実施形態による、投票ベースの姿勢推定のための対特徴の概略図である。 本発明の実施形態による、投票ベースの姿勢推定のための対特徴の概略図である。 本発明の実施形態による、投票ベースの姿勢推定のための対特徴の概略図である。 本発明の実施形態による、センサデータから得られた線対特徴とモデルから得られた線対特徴との間の変換の概略図である。

図１は、物体の姿勢を推定するためのシステム１００を示す。システムは、グリッパ１２０を備えた６軸ロボットアーム１１０を含む。３Ｄセンサ１３０は、アーム上に配置される。グリッパは、物体の姿勢１０１に応じて、容器１５０から物体１４０を拾い上げる。容器は異なる物体を含み得ることに留意されたい。

３Ｄセンサの一例は、赤外レーザによって生成される構造化された光を使用する。また、他のセンサも可能である。センサは、６４０×４８０画素の深度マップとして３Ｄ「ポイントクラウド」１６０を取得する。３Ｄセンサは、ロボットアームに対して較正され、それにより、姿勢を使用して、物体を把持することおよび拾い上げることが可能になる。

３Ｄポイントクラウドは、プロセッサ１７０で実行される方法によって処理される。プロセッサは、当技術分野で知られているように、メモリおよび入力／出力インターフェースを含み得る。本方法は、姿勢を決定し、物体１４０を拾い上げるようにアームに指示するため、決定された姿勢を、コントローラ１８０にフィードバックすることができる。

図２Ａは、方法２００のブロック図を示し、次の工程、すなわち、場面の３Ｄ検知２１０と、投票ベースの姿勢推定２２０と、姿勢改善２３０と、その姿勢に応じた物体の把持２４０とを含む。

システムは、３Ｄセンサを使用して物体の容器をスキャンし、本方法によって決定された姿勢に従って物体を検出して、物体を拾い上げる。容器内の異なる各物体の３Ｄ ＣＡＤモデルを考慮して、投票ベースの方法は、スキャンされた３Ｄポイントクラウド１６０を使用して、物体の検出および姿勢推定を実行する。この工程は、複数の粗姿勢を提供する。システムは、いくつかの最適な粗姿勢を選択し、反復最近点（ＩＣＰ：ｉｔｅｒａｔｉｖｅ−ｃｌｏｓｅｓｔ ｐｏｉｎｔ）手順を使用して、姿勢を個別に改善する。

一般に、ＩＣＰは、２つの３Ｄポイントクラウド間の差を最小化する。ＩＣＰは、異なる位置で取得された複数の部分的なスキャンから完全な３Ｄモデルを再構成し、３Ｄ環境モデルに対してロボットの位置測定を行うために使用される場合が多い。手順は、対応するポイント間の距離の最小化に必要な変換（並進および回転）を反復修正することによって、リアルタイムで実行することができる。入力は、２つの３Ｄポイントクラウド、変換の初期推定および終了基準である。出力は、改善された変換である。主な工程は、最近隣基準によって対応するポイントを見出す工程、費用関数として平均二乗誤差を使用して変換パラメータを推定する工程、推定されたパラメータを使用してポイントを変換する工程、および、反復する工程を含む。

本発明者らのシステムのＩＣＰ手順は、現在の姿勢推定を使用して物体のＣＡＤモデルをレンダリングし、レンダリングされたモデルの表面をサンプリングすることによって、モデルに対する３Ｄポイントクラウドを生成する。次いで、ＩＣＰは、モデルの各３Ｄポイントに対して、スキャンされたポイントクラウドの最も近い３Ｄポイントを決定し、３Ｄポイント対応を使用して姿勢推定を更新する。

ＩＣＰが収束した後、スキャンされたポイントクラウドの対応する３Ｄポイントとモデルの対応する３Ｄポイントとの間の平均距離として登録誤差が測定される。登録誤差は、ＩＣＰが不適正な姿勢に収束する場合、または、他の物体からのオクルージョンが原因で物体の一部が欠如する場合に高い可能性がある。したがって、登録誤差が高い場合は、本発明者らのシステムは、把持に対して推定された姿勢を使用しない。登録誤差に加えて、本発明者らのシステムは、物体を拾い上げることができるかどうかを決定するため、ロボットアームによってその姿勢の物体に到達できるかどうかもチェックする。

座標系
本説明の目的のため、３つの座標系が使用される。センサは、センサ（カメラ）座標系Ｃの観点から表現される。物体のモデルは、物体座標系Ｏの観点から表現される。場面の物体は、世界座標系Ｗを使用する。センサは、一般に、現実世界の公知の位置および向きにあるため、センサ座標系Ｓは、世界座標系Ｗに対して較正することができる。

姿勢推定方法は、センサ座標系と物体座標系との間の変換を決定する。次いで、センサ座標系と世界座標系との関係は、場面の物体操作に使用することができる。

投票ベースの姿勢推定
姿勢推定のための本発明者らの特徴は、方向付けされたポイントまたは線の対に基づき、これを対特徴と呼ぶ。ある方向付けされたポイント（基準）から別のポイント（関連）まで特徴ベクトルを非対称的に構成するため、方向付けされた表面ポイントの対からなる対特徴は、Ｓ２Ｓで示される。同様に、方向付けされた境界ポイントの対（Ｂ２Ｂ）、線セグメントの対（Ｌ２Ｌ）および異種対（方向付けされた表面ポイントと方向付けされた境界ポイント（Ｓ２ＢとＢ２Ｓ）を含む）に基づいて、他の４つの対特徴を有する（図３Ａ〜３Ｄを参照）。

ポイント対特徴記述子Ｆ_Ｓ２Ｓ、Ｆ_Ｂ２Ｂ、Ｆ_Ｓ２ＢおよびＦ_Ｂ２Ｓは、方向付けされたポイントの対（ｍ，ｎ）の相対位置ｆ_１ならびに向きｆ_２、ｆ_３およびｆ_４によって定義され、ポイントは、表面法線を有する表面ポイントまたは方向を有する境界ポイントのいずれかを示す。図３Ｄでは、線対特徴記述子Ｆ_Ｌ２Ｌは、２つの無限線間の最小距離ｆ_１、２つの線方向間の鋭角ｆ_２および２つの線セグメント間の最大距離ｆ_３によって定義される。

物体の姿勢を推定するため、場面やモデルから得られるような対特徴間の対応を確立することが必要とされる。対特徴から、その記述子として、様々な幾何制約を使用する。次いで、場面対特徴とモデル対特徴との対応は、それらの記述子を一致させることによって確立される。

対特徴
Ｓ２Ｓ−表面対特徴：
表面対特徴Ｓ２Ｓは、物体の表面上の２つのポイントと、２つのポイントの法線とを使用して定義される。場面からの方向付けされたポイントおよびモデルからの対応するプリミティブを考慮して、３Ｄ姿勢は、ポイント位置およびポイントの法線を位置合わせすることによって、平面回転まで回復させることができる。回転の曖昧性を解決し、完全な６−ＤｏＦ姿勢を回復させるため、少なくとも場面プリミティブとモデルプリミティブの２対間の対応が必要である。

｛（ｍ_ｒ，ｎ_ｒ），（ｍ_ｉ，ｎ_ｉ）｝は対特徴を示し、ｍ_ｒは物体の表面上の基準点であり、ｍ_ｉは物体の表面上の関連点であり、ｎ_ｒおよびｎ_ｉはそれぞれに対応する法線であるものとする。この対特徴を有する関連記述子は、以下の通りであり、

式中、ｄは、基準点から関連点へのベクトルであり、∠（ｖ_１，ｖ_２）∈［０，π）は、２つのベクトル間の角度を表す。

したがって、記述子の第１の成分は、ｆ_１＝｜｜ｍ_ｉ−ｍ_ｒ｜｜_２＝｜｜ｄ｜｜_２を表し、これは、２つの表面ポイント間のユークリッド距離である。第２の成分ｆ_２は、ベクトルｄと表面法線ベクトルｎ_ｒとの間の角度であり、第３の成分ｆ_３は、ベクトルｄと表面法線ベクトルｎ_ｉとの間の角度である。最後の成分ｆ_４は、２つの法線ベクトル間の角度である。Ｓ２Ｓ特徴は、図３Ａに示される。物体が広範囲の法線にまで広がれば、この特徴は、物体の優れた記述を提供する。

Ｂ２Ｂ−境界対特徴：
Ｓ２Ｓ特徴は、広範囲の表面法線にまで広がらない形状に対しては、優れた記述を提供することができない。残念ながら、ロボットアセンブリの間に使用される多くの工業用部品は、平面状であり、非常に小さなセットの法線方向を有する。それに加えて、３Ｄデータにおける雑音が原因で、表面上の高曲率領域の法線を正確に推定することは難しく、問題をさらに複雑にする。

物体境界上の２つのポイントに基づいて、新規のポイント対特徴Ｂ２Ｂを定義する。表面ポイントとは対照的に、境界ポイントは、良く定義された法線を有さない。境界ポイントの場合、線セグメントを合わせ、方向付けに対して線セグメントの方向を使用する。物体境界上の線セグメントを抽出するため、最初に、キャニーエッジ検出器を使用して、深度マップにおけるエッジを決定する。エッジは、エッジマップに格納される。エッジマップからのポイントは、ランダムにサンプリングされ、３Ｄ線は、ＲＡＮＳＡＣ手順を使用して、これらのポイントを中心に展開されるローカル領域上に合わせる。最大インライアを用いて線セグメントの反復位置付けおよび除去を行うことで、全ての線セグメントを回復させる。これらの線セグメントは、インライアに最小二乗を適用することによって、さらに改善される。線を合わせた後、３Ｄ線セグメント上の境界ポイントを一様にサンプリングする。

図３Ｂでは、ポイントは、２つの３Ｄ線セグメント上の境界ポイントを示す。次いで、境界対特徴記述子Ｆ_Ｂ２Ｂ∈Ｒ^４が以下によって定義される。

この特徴は、

および

が３Ｄ線の方向であることを除いて、Ｆ_Ｓ２Ｓと同等である。方向は一意的に決定されないことに留意されたい。したがって、Ｂ２Ｂ特徴を使用する際は、２つの可能な方向：

および

を考慮する。

物体境界は非常に有益である。Ｓ２Ｓと比べると、Ｂ２Ｂは、表面ポイントと比べてより少ない境界ポイントを有するため、より簡潔なモデリングを提供する。それに加えて、ローカル線セグメントからの方向付けは、表面法線と比べて、雑音への回復力に富む。

Ｓ２ＢとＢ２Ｓ−表面と境界との対特徴：
境界対特徴は、物体の深度エッジと関連付けられる。境界ポイントのみに依存するそのような特徴は、高曲率表面を有する物体に対しては、最も良い選択ではない可能性がある。例えば、球体の物体の表面上のいかなるポイントも、姿勢に基づいて深度エッジになれる可能性がある一方で、多面体の物体上の深度エッジは、より安定しており、平面交差上に常に現れる。

平面状の物体と曲線状の物体の両方を一緒に効率的にモデル化するため、方向付けされた表面ポイントと方向付けされた境界ポイントを使用して定義される異種対特徴を提供する。表面上と境界上の方向付けされたポイントを考慮することで、この特徴は、Ｂ２Ｂ特徴と比べて、姿勢サンプリングを低減する。

図３Ｃに示されるように、方向付けされた表面ポイントを基準点として選択し、方向付けされた境界ポイントを関連点として選択することで、Ｓ２Ｂ特徴記述子Ｆ_Ｓ２Ｂ∈Ｒ^４は、以下によって定義される。

同様に、Ｂ２Ｓ特徴は、方向付けされた境界ポイントを基準点として選択し、方向付けされた表面ポイントを関連点として選択することで定義される。

Ｌ２Ｌ−線対特徴：
図３Ｄに示されるように、３Ｄ線セグメントに対する対特徴Ｌ２Ｌも提供する。２つの線セグメントに対する特徴を構成することは、方向付けされたポイント対に対する特徴を構成することよりも若干複雑である。本発明者らの方法では、ＲＡＮＳＡＣ手順を使用して線を合わせる。線セグメントは、通常、雑音、ランダム化された線合わせおよびオクルージョンが原因で、より小さないくつかのセグメントに分割される。したがって、線セグメントのエンドポイントは、｛ｌ_ｒ ^１，ｌ_ｒ ^２，ｌ_ｉ ^１，ｌ_ｉ ^２｝と示され、確実に決定することはできない。

したがって、ｃ_ｒおよびｃ_ｉで示される２つのポイントを考慮して、本発明者らの特徴記述子を定義する。図３Ｄに示されるように、ｃ_ｒおよびｃ_ｉは、互いに対して各線上で最も近いポイントである。方向ｖ_１およびｖ_２を有する２つの線セグメント間の鋭角は、以下の通りである。

式中、

である。

本発明者らのＬ２Ｌ特徴記述子Ｆ_Ｌ２Ｌ∈Ｒ^３は、以下の通りであり、

式中、

である。

第１の成分は、最も近いポイントｃ_ｒとｃ_ｉとの間の距離（すなわち、２つの無限線間の距離）であり、第２の成分は、２つの線セグメント間の鋭角である。最後の成分は、２つの線セグメント間の最大距離を表す。この最大距離は、ある線セグメントのエンドポイントと別の線セグメントのエンドポイントとの間の全ての可能な距離の最大値であり得る。この最大距離は、同様の距離および角度を有する２つの線セグメント間での誤一致を取り除く際に役立つ。例えば、同一平面上の直交線のいかなる対も、同じ距離および角度を有する。しかし、上記で説明されるように、この最大距離は、線セグメントの切断が原因で、確実なものではない。したがって、姿勢推定のこの成分に対して、より大規模な量子化工程を使用する。平行線セグメントの対に対するＬ２Ｌ特徴は、そのような対は姿勢の曖昧性につながるため、構成しない。

線セグメントの数は表面ポイントまたは境界ポイントの数より少ないため、線対特徴は、非常に効率的な一致を提供する。この特徴は、多面体の物体および長い境界線セグメントを有する物体に対して特に効率的である。

実用的な実装形態では、表面ポイントは、深度画像をスキャンし、サブサンプリングすることによって得られ、３Ｄ線は、３Ｄスキャンから３Ｄ線セグメントを推定する本発明者らのＲＡＮＳＡＣ手順を介して決定され、次いで、境界ポイントは、線セグメントに沿ってサブサンプリングすることによって得られる。

物体表現
最初に、Ｓ２Ｓの方向付けされた表面ポイント、Ｂ２Ｂの方向付けされた境界ポイント、Ｓ２ＢとＢ２Ｓの方向付けされた表面ポイントと方向付けされた境界ポイントの両方、および、Ｌ２Ｌの３Ｄ線セグメントなどの幾何プリミティブＭを得る。これらの幾何プリミティブは、センサと物体との間の公知の較正を伴う３Ｄスキャンデータまたは公知のＣＡＤモデルからレンダリングされる合成深度データのいずれかから決定することができる。

図２Ａに示されるように、これらのプリミティブＭを用いることで、一連のモデル対特徴２０１ Ｓ２Ｓ、Ｂ２Ｂ、Ｓ２Ｂ、Ｂ２ＳおよびＬ２Ｌを使用して、特定の各物体１４０を表す。効率的な特徴一致に対しては、ハッシュ表Ｈ２０５にモデル対特徴を格納する。最初に、Δ_ｄのステップサイズで距離が、そして、Δ_ａのステップサイズで角度がそれぞれ量子化される特徴対を離散化する。量子化レベルを適切に定義することが重要である。大きなステップサイズを使用すると、記述子の区別能力が低減されるが、小さなステップサイズを使用すると、方法を雑音に敏感にする。続いて、ハッシュ関数のキーとして、離散化された対特徴記述子を使用し、それに応じて容器に対特徴を挿入する。適切な量子化工程を用いることで、同様の対特徴が一緒にグループ化され、一定の時間内に一致および投票を行うことができる。

物体表現を構成する工程は、オフラインで実行できる前処理工程であり得る。図２Ａは、後に拾い上げるために、容器内でランダムに混合させることができる３つの異なる物体を示す。

オンラインプロセスでは、スキャンされた３Ｄポイントクラウド１６０から場面対特徴２０２を決定し、これらの特徴を一連のモデル対特徴と一致させる。方法の詳細については、図２Ｂに示されている。

Ｓ２Ｓ、Ｂ２Ｂ、Ｓ２ＢおよびＢ２Ｓの投票方式
図２Ｂに示されるように、モデルから特徴の対２０２を決定し、ハッシュ表Ｈ２０５を構成した後、場面対特徴と一連の対応するモデル対特徴との間の剛体変換を計算することによって、姿勢を決定する。最適な姿勢を見出すプロセスを効率的なものにするため、投票方式を使用する。単純な手法は、６−ＤｏＦ姿勢空間での投票を必要とし、これは非効率的である。

代わりに、中間座標変換を使用して投票空間を２Ｄ空間に低減する投票方式を使用する。最初に、ハッシュ表Ｈにおいて場面ポイント対（ｓ_ｒ，ｓ_ｉ）が探索され、次いで、対応するモデルポイント対（ｍ_ｒ，ｍ_ｉ）が見出される。次いで、対の基準点の位置が座標系の原点に対応し、対の基準点の法線がｘ軸と一直線になるように、対の基準点が中間座標系に変換される。対を完全に位置合わせするため、関連点ｍ_ｉおよびｓ_ｉは、角度αで物体モデルをｘ軸を軸として回転させることによって位置合わせされる。

上記の（ｍ_ｒ，ｍ_ｉ）から（ｓ_ｒ，ｓ_ｉ）への変換は、基準モデル点の対および平面回転角度（ｍ_ｒ，α）によってパラメータ化され、

によって得られ、式中、Ｒ_ｘ（α）は、角度αでのｘ軸を軸とする回転であり、Ｔ_ｓ→ｇは、場面座標系から中間座標系への変換であり、Ｔ_ｍ→ｇは、モデル座標系から中間座標系への変換である。

投票段階では、場面の基準点ｓ_ｒは、場面の他の点ｓ_ｉと対になり、場面対特徴（ｓ_ｒ，ｓ_ｉ）と同様のモデル対特徴（ｍ_ｒ，ｍ_ｉ）は、それらの対特徴の記述子に基づいて、ハッシュ表Ｈから得られる。あらゆる対に対し、回転角度αは、中間座標系で決定され、次いで、投票は、対（ｍ_ｒ，α）に投じられる。投票後、既定の閾値を超える要素が選択され、その要素から、式（１０）のように候補姿勢（モデル座標と場面座標との間の剛体変換）２０６が演算される。このプロセスは、場面の異なる基準点を選択することによって繰り返される。この投票方式は、Ｓ２Ｓ、Ｂ２Ｂ、Ｓ２ＢおよびＢ２Ｓ対特徴に適用可能である。

Ｌ２Ｌの投票方式
Ｌ２Ｌ特徴は線セグメントの対によって定義されるため、Ｌ２Ｌ特徴に対しては、異なる投票方式を使用する。ポイント対特徴の投票方式と比べて、Ｌ２Ｌ特徴の投票方式は、より難しい、しかし、基本的な考えは同じであり、図４に示されるように、中間座標系で２つの対特徴を位置合わせする。

最初に、線対を中間座標系に変換する。Ｔ_ｓ→ｇによって、場面からの基準線ｌ^ｓ _ｒは、ｘ軸と一直線になるようにし、基準中間点ｏ^ｍは、原点に移動させる。モデルからの基準線ｌ^ｍ _ｒも同様に、Ｔ_ｍ→ｇによって変換される。その後、基準場面線ｃ^ｓ _ｒの最も近いポイントと基準モデル線ｃ^ｍ _ｒの最も近いポイントは、ｘ軸に沿って基準線を並進することによって、原点で位置合わせされる。これらの並進間の差はτで示される。次いで、関連線ｌ^ｓ _ｉおよびｌ^ｍ _ｉは、角度αでモデルをｘ軸を軸として回転させることによって位置合わせされる。角度αは、ｄ^ｓおよびｄ^ｍによって容易に決定することができる。

したがって、Ｌ２Ｌ投票方式に対し、中間座標系を介する（ｌ^ｍ _ｒ，ｌ^ｍ _ｉ）から（ｌ^ｓ _ｒ，ｌ^ｓ _ｉ）への変換は、（ｌ^ｍ _ｒ，α，τ）によってパラメータ化され、

によって得られ、式中、Ｔ_ｘ（τ）は、ｘ軸に沿った並進である。

姿勢クラスタリング
上記で説明される投票方式では、候補姿勢２０６は、場面の各基準プリミティブ（ポイントまたは線）に対して得られる。物体は複数の特徴対によってモデル化されるため、異なる基準プリミティブ（ポイントｍ_ｒまたは線ｌ^ｍ _ｒ）上で複数の候補姿勢を有し、一貫した姿勢仮定をサポートすることが予想される。したがって、異なる場面プリミティブから同様の姿勢を集める。

リー群における平均シフトなどの３Ｄ剛体変換空間ＳＥ（３）でクラスタリングを行うためのいくつかの方法があるが、それらの方法は、通常、時間依存の応用に対して、演算コスト上法外なものである。

非常に効率的な凝集クラスタリング２０７手法を使用する。最初に、投票数の多いものから順に候補姿勢をソートする。最大投票数を有する候補姿勢から、新しいクラスタを作成する。次の候補姿勢が既存のクラスタの候補姿勢に近ければ、候補姿勢はクラスタに追加され、クラスタ内の候補姿勢の平均としてクラスタの中心が更新される。次の候補姿勢がクラスタのいずれとも近くなければ、新しいクラスタが生成される。近接テストは、並進および回転に対する固定閾値を用いて行われる。並進に対する距離演算および平均化は、３Ｄユークリッド空間で実行されるが、回転に対する距離演算および平均化は、回転行列の四元数表現を使用して実行される。

クラスタリングの後は、クラスタは、全投票数の多いものから順にソートされ、それにより、推定姿勢の信頼度が決定される。最大投票数を有するクラスタが最適な姿勢２０８に相当する。

発明の効果
方向付けされた表面ポイント、方向付けされた境界ポイントおよび境界線セグメントを使用する一連の対特徴は、様々な物体をモデル化するために提供される。正確で効率的な姿勢推定のため、投票フレームワークにおいて対特徴を使用する。

境界からの特徴は、よりコンパクトかつ有益であり、それにより、より高い精度およびより高速の演算をもたらす。

本発明の実施形態に従って実装される容器拾上げシステムは、９８％より高い拾上げ成功率と、並進に対しては０．３ｍｍ未満の姿勢推定誤差および回転に対しては０．３度未満の姿勢推定誤差とを有する。

Claims (15)

1. 物体の姿勢を推定するための方法であって、
   幾何プリミティブの対として一連の対特徴を定義する工程であって、前記幾何プリミティブは、方向付けされた表面ポイント、方向付けされた境界ポイントおよび境界線セグメントを含む、工程と、
   前記物体のモデルに対する前記一連の対特徴に基づいて、モデル対特徴を決定する工程と、
   ３Ｄセンサによって取得されたデータからの前記一連の対特徴に基づいて、場面対特徴を決定する工程と、
   前記物体の前記姿勢を推定するために、前記モデル対特徴と前記場面対特徴とを一致させる工程であって、プロセッサで実行される、工程と
   を含む、方法。
2. 前記物体は、他の物体とランダムに混合させた複数の異なる物体のうちの１つであり、各物体に対してモデル対特徴がある、請求項１に記載の方法。
3. 前記３Ｄセンサは、ロボットアーム上に装着され、前記ロボットアームは、前記姿勢に応じて前記物体を拾い上げるためのグリッパを含む、請求項１に記載の方法。
4. 前記３Ｄセンサは、赤外レーザによって生成される構造化された光を使用する、請求項１に記載の方法。
5. 各対特徴は、２つの方向付けされた表面ポイント、２つの方向付けされた境界ポイント、１つの方向付けされた表面ポイントと１つの方向付けされた境界ポイント、および、２つの境界線セグメントを使用して定義される、請求項１に記載の方法。
6. 各方向付けされた表面ポイントは、前記物体の表面上の３Ｄ位置と、３Ｄ法線ベクトルとを備える、請求項１に記載の方法。
7. 各方向付けされた境界ポイントは、前記物体の境界上の３Ｄ位置と、３Ｄ方向ベクトルとを備える、請求項１に記載の方法。
8. 前記３Ｄ方向ベクトルは、前記３Ｄ位置における前記境界の接線方向として定義される、請求項７に記載の方法。
9. 各境界線セグメントは、前記物体の境界上の３Ｄポイントに合わせた線セグメントである、請求項１に記載の方法。
10. 記述子は、各対特徴から決定される、請求項１に記載の方法。
11. 前記一致は、前記モデル対特徴の前記記述子と前記場面対特徴の前記記述子とを比較することによって実行される、請求項１０に記載の方法。
12. 前記対特徴の前記記述子を離散化する工程と、
    前記モデル対特徴の前記離散化された記述子をハッシュ表に格納する工程と、
    前記ハッシュ表を使用して前記一致を実行する工程と
    をさらに含む、請求項１１に記載の方法。
13. 前記物体の候補姿勢は、中間座標変換を介する投票に基づいて推定される、請求項１に記載の方法。
14. 前記候補姿勢をクラスタリングする工程をさらに含む、請求項１３に記載の方法。
15. 反復最近点手順を使用して前記姿勢を改善する工程をさらに含む、請求項１に記載の方法。

Images