JP5083715B2 - 三次元位置姿勢計測方法および装置 - Google Patents

Description

本発明は、特徴の配置に基づく物体（対象物）の三次元位置姿勢計測方法および装置に関する。

ハンドリングロボット、自走ロボット、ヒューマノイドロボット等において、ロボットの目として、物体を三次元計測し、その位置及び姿勢を計測することが必要となる。
この目的を達成するために、例えば特許文献１〜３が既に提案されている。

特許文献１の「画像処理機能を持つロボット装置」は、１台のカメラで様々な方向から物体を撮影することで、事前に物体の見え方を十分教示しておき、この教示画像群と現在の見え方とのパターンマッチングにより、物体の位置姿勢を推定するものである。

特許文献２の「物品の位置検出方法およびその装置」は、ステレオ計測によって物体の３次元位置を検出するものである。まず、左右の画像それぞれに対して２次元見え方モデルとの照合を行い、各画像上の物体の特徴部の位置を決定する。この左右の特徴部を対応付けたステレオ視を行うことで、物体の３次元位置を決定する。
２次元見え方モデルと画像の照合は、例えば、画像上で特徴部（例えば画像上の円中心）を抽出し、複数の特徴部を結んだ多角形の各頂点の角度差の総和が最小になる特徴部の組合せを決定する。

特許文献３の「三次元物体認識方法及びその装置」は、入力画像から得られる輪郭などを直線、円弧に近似したデータであるとともにスレテオ計測で得た３次元位置データを有する特徴データと、モデルデータのマッチングにより物体の三次元位置姿勢を得る。ここで、三次元位置姿勢を得るために必要な３点を形成できる特徴のグループである特徴グループを用いるものである。

特許第３３００６８２号明細書、「画像処理機能を持つロボット装置」 特許第３７３８４５６号明細書、「物品の位置検出方法およびその装置」 特開平９−２１２６４３号公報、「三次元物体認識方法及びその装置」

上述した特許文献１の装置は、物体ごとに、大量の画像教示が必要となる。そのため、マッチング処理時間が長くなる。また、姿勢の分解能は教示画像の枚数に依存し、精度が悪い問題点がある。

特許文献２の手段は、物品の見え方が予め特定されている必要がある。画面上に複数の物体がある場合や、背景が複雑な場合には対応できない。また最終的な位置姿勢検出には２台のカメラによるステレオ視が必要である。

特許文献３の手段は、単純なグループ（２つの線分など）ごとに照合するため、大量の誤検出が発生する。そのため、２台のカメラによるステレオ視による距離データで検証しながらグループを絞り込むことで誤検出を減らす必要がある。また、複雑な形状の物体や、画面上に複数の物体がある場合や、乱雑な背景には対応できない。

本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、物体（対象物）の画像教示なしに、１台のカメラで物体の位置と姿勢を正確に検出することができる三次元位置姿勢計測方法および装置を提供することにある。

本発明によれば、検出する対象物のモデルデータを予め記憶するモデル入力ステップと、
対象物を撮影して画像を取得する画像撮影ステップと、
前記画像から、前記モデルデータに対応する画像上の特徴データを抽出する特徴抽出ステップと、
前記モデルデータと画像上の特徴データとの幾何学的一致度を評価する一致度評価ステップと、
モデルデータと画像上の特徴データが一致するように、モデルデータを３次元的に回転、移動させ、その回転量と移動量の６自由度データを取得する座標変換ステップと、
前記６自由度データにより対象物の三次元位置姿勢を決定する位置姿勢決定ステップとを有し、
同一の画像上に複数の特徴データが存在する場合において、
前記特徴抽出ステップの後に、モデルデータと画像上の複数の特徴データとの組み合わせを作成する特徴対応作成ステップを有し、
前記一致度評価ステップにおいて、前記組み合わせについて幾何学的一致度を評価し、
次いで、前記組み合わせのうち幾何学的一致度が高いものをグループ化するグループ化ステップを有し、
前記座標変換ステップにおいて、前記グループごとに、６自由度データを取得する、ことを特徴とする三次元位置姿勢計測方法が提供される。

本発明の好ましい実施形態によれば、前記モデルデータは、少なくとも１つの円を含む３つ以上の特徴データから構成され、これらは同一平面上に位置する。

また本発明によれば、検出する対象物を撮影して画像を取得する撮像装置と、
前記画像から対象物の三次元位置姿勢を決定する演算装置とを備え、
該演算装置は、前記画像から、前記モデルデータに対応する画像上の特徴データを抽出する特徴抽出ステップと、
前記モデルデータと画像上の特徴データとの幾何学的一致度を評価する一致度評価ステップと、
モデルデータと画像上の特徴データが一致するように、モデルデータを３次元的に回転、移動させ、その回転量と移動量の６自由度データを取得する座標変換ステップと、
前記６自由度データにより対象物の三次元位置姿勢を決定する位置姿勢決定ステップとを有し、
同一の画像上に複数の特徴データが存在する場合において、
前記特徴抽出ステップの後に、モデルデータと画像上の複数の特徴データとの組み合わせを作成する特徴対応作成ステップを有し、
前記一致度評価ステップにおいて、前記組み合わせについて幾何学的一致度を評価し、
次いで、前記組み合わせのうち幾何学的一致度が高いものをグループ化するグループ化ステップを有し、
前記座標変換ステップにおいて、前記グループごとに、６自由度データを取得する、ことを特徴とする三次元位置姿勢計測装置が提供される。

本発明によれば、モデルデータに対応する画像上の特徴データを抽出し、モデルデータが画像上の特徴データの位置に移動するように座標変換式を算出するので、物体（対象物）の画像教示なしに、１台のカメラで物体の位置と姿勢を正確に検出することができる。

また、モデルデータは、円、線分、コーナーといった形状特徴から構成されるので、モデルデータはシンプルであり、大量の画像データの教示は不要である。よって多品種に容易に対応可能である。

また、モデルデータと画像上の特徴データとの幾何学的一致度を評価し、該幾何学的一致度が最も高い画像上の特徴データを抽出するので、物体の回転、拡大縮小に加え、カメラの視線方向に対する傾きにも対応できる。従って、物品の見え方が予め特定されている必要はない。

また、同一の画像上に複数の特徴データが存在する場合において、モデルデータと画像上の複数の特徴データとの幾何学的一致度を算出し、次いで、幾何学的一致度が高い順に複数のグループを検出するので、幾何学的一致度に基づくグループ化により誤った対応を除去できるため、画面上に複数の物体がある場合や、乱雑な背景にも対応可能である。

また、幾何学的一致度により画像上の特徴データを抽出するので、モデルデータに対応する全ての特徴データが検出できなくても、一部の特徴データに基づき位置姿勢を検出することが可能であり、物体の一部隠れに対応可能である。

以下、本発明の好ましい実施例を図面を参照して説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明による三次元位置姿勢計測装置の全体構成図である。
この図において、本発明の三次元位置姿勢計測装置１０は、検出する対象物１を撮影して画像２を取得する撮像装置１２と、画像２から対象物１の三次元位置姿勢を決定する演算装置１４とを備える。

撮像装置１２（この例でカメラ）は、ロボット３のハンド４に搭載されており、検出する対象物１を撮影して画像２を取得する。なお、これ以外に、カメラは天井などに固定してもよい。また、カメラの位置はロボットのハンドとは別に制御可能に構成してもよい。

演算装置１４は例えばコンピュータであり、この例ではロボット制御部１５、物体計測処理部１６、及びモデルデータベース部１７からなる。モデルデータベース部１７には、対象物１のモデルデータ５を予め記憶する。

演算装置１４は、後述する特徴抽出ステップＳ３、一致度評価ステップＳ５、座標変換ステップＳ７、および位置姿勢決定ステップＳ８を実行するようになっている。
特徴抽出ステップＳ３では、画像２から、物体計測処理部１６により、モデルデータ５に対応する画像上の特徴データ６を抽出する。
一致度評価ステップＳ５では、モデルデータ５と画像上の特徴データ６との幾何学的一致度を評価する。
座標変換ステップＳ７では、モデルデータ５が幾何学的一致度の高い画像上の特徴データ６の位置に移動するように座標変換式を算出する。
位置姿勢決定ステップＳ８では、座標変換式により対象物１の三次元位置姿勢を決定する。

上述した本発明の三次元位置姿勢計測装置１０を備えたロボット３は、以下のように作動する。
１．カメラキャリブレーション（カメラ内部パラメータ算出）を行う。
２．カメラ座標系とハンド座標系のキャリブレーションを行う
３．対象物１のモデルデータ５をモデルデータベース部１７から読み込む。モデルデータ５がない場合は別途新規に作成する。
４．ある位置にロボット３のハンド４を移動させ、画像２を撮影する。このときのロボット座標におけるハンド位置姿勢は記録しておく。
５．対象物１の三次元位置姿勢計測を演算装置１４により行い、カメラ座標における対象物１の位置姿勢を得る。
６．ロボット座標におけるハンド位置姿勢と、カメラ座標における対象物１の位置姿勢から、ロボット座標における対象物１の位置姿勢を算出する。
７．ロボット３は、上記の結果に基づき、対象物１の把持といったアクションを起こす。

図２は、本発明による三次元位置姿勢計測方法の全体フロー図である。
この図を用いて、最初に、同一の画像上に単一の特徴データが存在する場合を説明する。

図２に示すように本発明の三次元位置姿勢計測方法は、モデル入力ステップＳ１、画像撮影ステップＳ２、特徴抽出ステップＳ３、一致度評価ステップＳ５、座標変換ステップＳ７、位置姿勢決定ステップＳ８を有する。

モデル入力ステップＳ１では、対象物１のモデルデータ５を予めモデルデータベース部１７に記憶する。
このモデルデータ５は、少なくとも１つの円を含む３つ以上の特徴データから構成される。また、これらは同一平面上に位置するものとする。

画像撮影ステップＳ２では、撮像装置１２（カメラ）により、検出する対象物１を撮影して画像２を取得する。

特徴抽出ステップＳ３では、画像２から、モデルデータ５に対応する画像上の特徴データ６を抽出する。
一致度評価ステップＳ５では、モデルデータ５と画像上の特徴データ６との幾何学的一致度を評価する。

図３は、モデルデータ５の２つの特徴と画像上の特徴データ６の２つの特徴がいずれも円の場合の、幾何学的一致度の評価方法を示している。
幾何学的一致度評価値ｘは数１の式（１）で定義する。

図４は、モデルデータ５の２つの特徴と画像上の特徴データ６の２つの特徴が、１つの円と１つのコーナー、または、１つの円と１つの線分の場合の、幾何学的一致度の評価方法を示している。この図において（Ａ）は特徴点がコーナーである場合、（Ｂ）は特徴点が線分である場合である。
この場合、（Ａ）（Ｂ）とも、幾何学的一致度評価値ｘは数１の式（２）で定義する。

式（１）（２）による幾何学的一致度評価値ｘは、対象物１の位置姿勢の影響を受けにくい。また、幾何学的一致度評価値ｘが小さい値であるほど、モデルデータ５と特徴データ６が正確に対応し、「幾何学的一致度が高い」ことになる。

座標変換ステップＳ７では、モデルデータ５が幾何学的一致度の高い画像上の特徴データ６の位置に移動するように座標変換式を算出する。すなわち、モデルデータ５と画像上の特徴データ６が一致するように、モデルデータ５を３次元的に回転、移動させ、その回転量と移動量の６自由度データを取得する。

位置姿勢決定ステップＳ８では、算出した座標変換式、すなわち取得した６自由度データにより、対象物１の三次元位置姿勢を決定する。

次に、同一の画像２上に複数の特徴データ６が存在する場合を説明する。
図５は、この場合のモデルデータ５の例であり、３つの円ｍ１，ｍ２，ｍ３からなる。
図６は、この場合の画像上の特徴データ６の例であり、ｓ１〜ｓ９の９つの楕円からなる。

同一の画像２上に複数の特徴データ６が存在する場合には、図２に示すように、本発明の方法は、さらに、特徴対応作成ステップＳ４と、グループ化ステップＳ６を有する。

特徴対応作成ステップＳ４は、特徴抽出ステップＳ３の後に、モデルデータ５と画像上の複数の特徴データ６との組み合わせを作成する。
図７は、この組み合わせの例であり、モデルデータ５が３つの円ｍ１，ｍ２，ｍ３、特徴データ６がｓ１〜ｓ９の９つの楕円である場合に、その組み合わせは、ｃ１（ｍ１，ｓ１），ｃ２（ｍ１，ｓ２），ｃ３（ｍ１，ｓ３），…，ｃ２７（ｍ３，ｓ９）の２７通りとなる。

次に、一致度評価ステップＳ５において、前記組み合わせ（２７通り）について幾何学的一致度評価値ｘを評価する。
図８は、前記組み合わせ（２７通り）について、幾何学的一致度の一覧を示す図である。ｘｉｊで示す幾何学的一致度評価値ｘは、それぞれ式（１）（２）により正の値となる。

次いで、グループ化ステップＳ６において、モデルデータと画像上の特徴データの組み合わせのうち、幾何学的一致度が高いものをグループ化し、さらに、幾何学的一致度が高い順にグループを並び替える。
図９（Ａ）は、グループ化ステップＳ６により、図６の複数の特徴データ６から３つのグループを検出した状態を示している。３つのグループの幾何学的一致度ｘは、ａ，ｂ，ｃの順で高いものとする。
図９（Ｂ）に示すように、異なるグループに画像上同一の特徴データ６が含まれる場合には、幾何学的一致度ｘが低いグループの当該特徴を除去して、グループの再評価を行う。

次いで、座標変換ステップＳ７において、グループごとに、座標変換式を算出する。

図１０は、一致度評価ステップＳ５の具体的フロー図である。
この図において、一致度評価ステップＳ５は、Ｓ５１〜Ｓ５８の各ステップからなる。
この一致度評価ステップＳ５では、モデルデータ５と画像上の複数の特徴データ６に対して、モデルデータ５の特徴データ群のうち２つの特徴データＣｉと、画像上の特徴データ群のうち２つの特徴データＣｊの組み合わせの幾何学的一致度評価値ｘｉｊを算出する。
幾何学的一致度評価値ｘｉｊは、物体検出に必要な全ての組み合わせで算出する。

図１１は、グループ化ステップＳ６の具体的フロー図である。
この図において、グループ化ステップＳ６は、Ｓ６１〜Ｓ６９の各ステップからなる。
このグループ化ステップＳ６では、モデルデータ５と、画像上の特徴データ６の対応Ｃｉを抽出し、グループに最初の１個目として追加する。この対応Ｃｉに対して幾何学的一致度が高い別の対応Ｃｊをグループに追加する。その後、グループに含まれるいずれの対応に対しても幾何学的一致度が高い新たな対応があれば、その対応を順にグループに追加していくことで、全体的に幾何学的一致度が高いグループを得る。画面上に物体が複数あれば複数のグループが生成される。

上述した本発明によれば、モデルデータ５に対応する画像上の特徴データ６を抽出し、モデルデータ５が画像上の特徴データ６の位置に移動するように座標変換式を算出するので、物体（対象物１）の画像教示なしに、１台のカメラ（撮像装置１２）で物体の位置と姿勢を正確に検出することができる。

また、モデルデータ５は、円、線分、コーナーといった形状特徴から構成されるので、モデルデータはシンプルであり、大量の画像データの教示は不要である。よって多品種に容易に対応可能である。

また、モデルデータ５と画像上の特徴データ６との幾何学的一致度を評価し、該幾何学的一致度が最も高い画像上の特徴データ６を抽出するので、物体の回転、拡大縮小に加え、カメラの視線方向に対する傾きにも対応できる。従って、物品の見え方が予め特定されている必要はない。

また、同一の画像上に複数の特徴データ６が存在する場合において、モデルデータと画像上の複数の特徴データとの幾何学的一致度評価値ｘを算出し、次いで、幾何学的一致度が高い順に複数のグループを検出するので、幾何学的一致度に基づくグループ化により誤った対応を除去できるため、画面上に複数の物体がある場合や、乱雑な背景にも対応可能である。

また、幾何学的一致度により画像上の特徴データを抽出するので、モデルデータに対応する全ての特徴データが検出できなくても、一部の特徴データに基づき位置姿勢を検出することが可能であり、物体の一部隠れに対応可能である。

なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。

本発明による三次元位置姿勢計測装置の全体構成図である。 本発明による三次元位置姿勢計測方法の全体フロー図である。 モデルデータ５と特徴データ６が２つの円の場合を示す図である。 モデルデータ５と特徴データ６が１つの円と２以上の特徴点の場合を示図である。 同一の画像２上に複数の特徴データ６が存在する場合のモデルデータ５の例である。 同一の画像２上に複数の特徴データ６が存在する場合の画像上の特徴データ６の例である。 モデルデータ５と画像上の複数の特徴データ６との全ての組み合わせの例である。 全ての組み合わせ（２７通り）について、幾何学的一致度の一覧を示す図である。 グループ化ステップＳ６において、幾何学的一致度が高い順に複数のグループを検出する例である。 一致度評価ステップＳ５の具体的フロー図である。 グループ化ステップＳ６の具体的フロー図である。

符号の説明

１ 対象物、２ 画像、３ ロボット、４ ハンド、
５ モデルデータ、６ 画像上の特徴データ、
１０ 三次元位置姿勢計測装置、１４ 演算装置（コンピュータ）、
１５ ロボット制御部、１６ 物体計測処理部、
１７ モデルデータベース部

Claims (4)

1. 検出する対象物のモデルデータを予め記憶するモデル入力ステップと、
   対象物を撮影して画像を取得する画像撮影ステップと、
   前記画像から、前記モデルデータに対応する画像上の特徴データを抽出する特徴抽出ステップと、
   前記モデルデータと画像上の特徴データとの幾何学的一致度を評価する一致度評価ステップと、
   モデルデータと画像上の特徴データが一致するように、モデルデータを３次元的に回転、移動させ、その回転量と移動量の６自由度データを取得する座標変換ステップと、
   前記６自由度データにより対象物の三次元位置姿勢を決定する位置姿勢決定ステップとを有し、
   同一の画像上に複数の特徴データが存在する場合において、
   前記特徴抽出ステップの後に、モデルデータと画像上の複数の特徴データとの組み合わせを作成する特徴対応作成ステップを有し、
   前記一致度評価ステップにおいて、前記組み合わせについて幾何学的一致度を評価し、
   次いで、前記組み合わせのうち幾何学的一致度が高いものをグループ化するグループ化ステップを有し、
   前記座標変換ステップにおいて、前記グループごとに、６自由度データを取得する、ことを特徴とする三次元位置姿勢計測方法。
2. 前記モデルデータは、少なくとも１つの円を含む３つ以上の特徴データから構成され、これらは同一平面上に位置する、ことを特徴とする請求項１に記載の三次元位置姿勢計測方法。
3. 前記グループ化ステップにおいて、グループを幾何学的一致度が高い順に並び替え、複数のグループに画像上同一の特徴データが含まれる場合、幾何学的一致度が低いグループの当該特徴を除去して、グループの再評価を行う、ことを特徴とする請求項１に記載の三次元位置姿勢計測方法。
4. 検出する対象物を撮影して画像を取得する撮像装置と、
   前記画像から対象物の三次元位置姿勢を決定する演算装置とを備え、
   該演算装置は、前記画像から、前記モデルデータに対応する画像上の特徴データを抽出する特徴抽出ステップと、
   前記モデルデータと画像上の特徴データとの幾何学的一致度を評価する一致度評価ステップと、
   モデルデータと画像上の特徴データが一致するように、モデルデータを３次元的に回転、移動させ、その回転量と移動量の６自由度データを取得する座標変換ステップと、
   前記６自由度データにより対象物の三次元位置姿勢を決定する位置姿勢決定ステップとを有し、
   同一の画像上に複数の特徴データが存在する場合において、
   前記特徴抽出ステップの後に、モデルデータと画像上の複数の特徴データとの組み合わせを作成する特徴対応作成ステップを有し、
   前記一致度評価ステップにおいて、前記組み合わせについて幾何学的一致度を評価し、
   次いで、前記組み合わせのうち幾何学的一致度が高いものをグループ化するグループ化ステップを有し、
   前記座標変換ステップにおいて、前記グループごとに、６自由度データを取得する、ことを特徴とする三次元位置姿勢計測装置。
   

Images