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

Description

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

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

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

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

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

特許文献４の「三次元位置決め方法及び装置並びに上記方法を実現するソフトウェアを記録した媒体」は、特許文献３の方法で検出した物体の位置姿勢を認識候補とし、各認識候補の位置姿勢に確認用モデルデータを移動させ、画像データと確認用モデルデータを、２次元画像平面上または三次元空間上でマッチングすることで、認識候補の成否を判断するものである。

特許第３３００６８２号明細書、「画像処理機能を持つロボット装置」 特許第３７３８４５６号明細書、「物品の位置検出方法およびその装置」 特開平９−２１２６４３号公報、「三次元物体認識方法及びその装置」 特許第３７６３２１５号明細書、「三次元位置決め方法及び装置並びに上記方法を実現するソフトウェアを記録した媒体」

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

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

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

特許文献４の手段は、あくまで認識候補の成否判定であり、高精度化が考慮されていない。

本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、輪郭情報に基づき検出する対象物の位置と姿勢を高精度に検出することができる三次元位置姿勢計測方法および装置を提供することにある。

本発明によれば、検出する対象物の輪郭を表すモデル輪郭点群を予め入力するモデル入力ステップと、
対象物を撮影して画像を取得する画像撮影ステップと、
前記画像から、前記モデル輪郭点群に対応する画像輪郭点群を抽出する輪郭抽出ステップと、
前記モデル輪郭点群と画像輪郭点群の相対距離が最小になるようにモデル輪郭点群の位置姿勢を変換する距離最小化ステップと、
前記モデル輪郭点群と画像輪郭点群との誤対応を除去する誤対応除去ステップと、を有し、
前記距離最小化ステップは、
前記モデル輪郭点群を初期位置姿勢に座標変換する初期位置設定ステップと、
前記モデル輪郭点群と画像上の画像輪郭点群の最近傍点同士を対応付けする対応点決定ステップと、
前記最近傍点間の距離に基づく評価値が最小になるようにモデル輪郭点群の座標変換式を算出する座標変換ステップと、を有し、
前記対応点決定ステップと座標変換ステップを、繰り返し終了条件を満たすまで繰り返す、ことを特徴とする三次元位置姿勢計測方法が提供される。

前記距離最小化ステップにおける繰り返し終了条件は、
前記最近傍点間の距離に基づく評価値が所定の閾値を下回るか、もしくは、所定の繰り返し回数を超過することで、繰り返しを終了する、ことが好ましい。

また、前記誤対応除去ステップは、
前記最近傍点間の距離が所定の閾値を超える場合に、その対応付けを除去する処理Ａと、
前記最近傍点間において、輝度勾配を表すベクトル間の角度差が所定の閾値を超える場合に、その対応付けを除去する処理Ｂのうち、少なくともいずれか一方を実施する。

また本発明によれば、検出する対象物を撮影して画像を取得する撮像装置と、
前記画像から対象物の三次元位置姿勢を決定する演算装置とを備え、
該演算装置は、予め入力された対象物の輪郭を表すモデル輪郭点群に対応する画像輪郭点群を抽出する輪郭抽出ステップと、
前記モデル輪郭点群と画像輪郭点群の相対距離が最小になるようにモデル輪郭点群の位置姿勢を変換する距離最小化ステップと、
前記モデル輪郭点群と画像輪郭点群との誤対応を除去する誤対応除去ステップと、を実行し、
前記距離最小化ステップは、
前記モデル輪郭点群を初期位置姿勢に座標変換する初期位置設定ステップと、
前記モデル輪郭点群と画像上の画像輪郭点群の最近傍点同士を対応付けする対応点決定ステップと、
前記最近傍点間の距離に基づく評価値が最小になるようにモデル輪郭点群の座標変換式を算出する座標変換ステップと、を有し、
前記対応点決定ステップと座標変換ステップを、繰り返し終了条件を満たすまで繰り返す、ことを特徴とする三次元位置姿勢計測装置が提供される。

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

また、本発明によれば、モデル輪郭点群と画像輪郭点群の相対距離が最小になるようにモデル輪郭点群の位置姿勢を変換するので、ずれは生じにくく、物体の位置姿勢を非常に高い精度で検出できる。
例えば、モデルの円の中心と画像の楕円の中心を位置合わせ用の制御点として使用する手段では、画像の楕円の中心はモデルの円の中心と真には一致しないため、わずかなずれが生じ、物体の位置姿勢の検出精度を低下させる。
また、円の中心やコーナーのような制御点は点数が少なくいため誤差が発生しやすいが、本発明では位置合わせ用の点を大量に得ることができるため、精度が向上する。

また、距離最小化の手段として、座標変換したモデル輪郭点群を二次元画像上に投影し、そのモデル輪郭点群と画像上の画像輪郭点群間の距離を最小化するような、二次元画像上での検証手段が適用できる。
さらに、座標変換したモデル輪郭点群と、画像上の画像輪郭点群との距離を最小化するように、三次元空間上での検証手段を導入することにより、二次元画像上では距離が同じに見えても、視点から遠い位置のほうが距離が長いような、奥行に応じた重み付けが可能となり、精度を向上させることができる。

さらに、モデル輪郭点群と、画像上の画像輪郭点群との対応付けの手段として、単純に最近傍の点同士を対応付ける手段に加え、前記最近傍点間の距離が所定の閾値を超える場合や、最近傍点間において、輝度勾配を表すベクトル間の角度差が所定の閾値を超える場合に、その対応付けを除去することにより、誤対応を防ぐことができる。

また、モデル輪郭点群はシンプル（物体の輪郭データのみ）であるため、大量の画像データの教示は不要であり、多品種に容易に対応できる。

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

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

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

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

演算装置１４は、後述する輪郭抽出ステップＳ３、座標変換ステップＳ４、および位置姿勢決定ステップＳ５を実行するようになっている。
輪郭抽出ステップＳ３では、画像２から、モデル輪郭点群５に対応する画像輪郭点群６を抽出する。

座標変換ステップＳ４では、モデル輪郭点群５が画像輪郭点群６にほぼ一致するように座標変換式を算出する。この座標変換ステップＳ４では、モデル輪郭点群５と画像輪郭点群６が、正確に一致する必要はなく、隣接する位置にほぼ一致すればよい。
位置姿勢決定ステップＳ５では、座標変換式により対象物１の三次元位置姿勢を決定する。

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

図２は、本発明による三次元位置姿勢計測方法の全体フロー図である。
この図に示すように本発明の三次元位置姿勢計測方法は、モデル入力ステップＳ１、画像撮影ステップＳ２、輪郭抽出ステップＳ３、座標変換ステップＳ４、位置姿勢決定ステップＳ５、距離最小化ステップＳ６、誤対応除去ステップＳ７を有する。

モデル入力ステップＳ１では、対象物１の輪郭を表すモデル輪郭点群５を予めモデルデータベース部１７に記憶する。
このモデル輪郭点群５は、対象物１の輪郭を表す複数の点からなる。

画像撮影ステップＳ２では、撮像装置１２（カメラ）により、検出する対象物１を撮影して画像２を取得する。
対象物１は、モデル輪郭点群５と同一の特徴点群、すなわち対象物１の輪郭を表す複数の点からなる。

輪郭抽出ステップＳ３では、画像２から、モデル輪郭点群５に対応する画像輪郭点群６を抽出する。
座標変換ステップＳ４では、モデル輪郭点群５が画像輪郭点群６にほぼ一致するように座標変換式を算出する。
位置姿勢決定ステップＳ５では、座標変換式により対象物１の三次元位置姿勢を決定する。すなわち、モデル輪郭点群５と画像上の画像輪郭点群６がほぼ一致するように、モデル輪郭点群５を３次元的に回転、移動させ、その回転量と移動量の６自由度データを取得する。

図３は、距離最小化ステップＳ６の具体的フロー図であり、図４は距離最小化ステップＳ６の説明図である。
距離最小化ステップＳ６は、図３に示すように、Ｓ６１〜Ｓ６９の各ステップからなる。

ステップＳ６１では、モデル輪郭点群５を作成する。次いで同一の画像上にあるモデル輪郭点群５に対応するグループ毎にモデル輪郭点群５を初期位置姿勢に座標変換する（Ｓ６２，Ｓ６３）。
図４（Ａ）は、ステップＳ６１〜Ｓ６３により、モデル輪郭点群５をあるグループの画像輪郭点群６（以下、「シーン」と呼ぶ）の近傍に座標変換した状態を示している。

対応点決定ステップＳ６４では、図４（Ｂ）に示すように、モデル輪郭点群５と画像上の画像輪郭点群６の最近傍点同士を対応付けする。
座標変換ステップＳ６５では、図４（Ｃ）に示すように、最近傍点間の距離に基づく評価値が最小になるようにモデル輪郭点群５の座標変換式を算出する。
ステップＳ６６では、座標変換式に基づき、モデル輪郭点群５の座標変換して、モデル輪郭点群５を画像上の画像輪郭点群６に近づける。

ステップＳ６７により、図４（Ｄ）に示すように、対応点決定ステップＳ６４と座標変換ステップＳ６５を、繰り返し終了条件を満たすまで繰り返す。
距離最小化ステップにおける繰り返し終了条件は、最近傍点間の距離に基づく評価値が所定の閾値を下回るか、もしくは、所定の繰り返し回数を超過することで、繰り返しを終了する。
すなわち、最近傍点間の距離が所定の閾値以下になるまで、対応点決定ステップＳ６４から座標変換ステップＳ６６を繰り返し、最近傍点間の距離が所定の閾値以下になったときに、モデルの最終位置姿勢を決定し（Ｓ６８）、そのグループは終了する（Ｓ６９）。
同一の画像上にモデル輪郭点群５に対応する複数のグループが存在する場合には、上述したステップＳ６２〜Ｓ６８をグループ毎に実行する。

上述した距離最小化ステップＳ６により、モデル輪郭点群５と画像輪郭点群６の相対距離が最小になるようにモデル輪郭点群５の位置姿勢を変換することができる。

図５〜図７は、モデル輪郭点群５が円であり、その画像輪郭点群６が楕円である場合の誤対応除去ステップＳ７の作動説明図である。なお、これらの図では、説明の都合上、モデル輪郭点群５は回転させずに示している。

図５は、誤対応が発生しない例であり、モデル輪郭点群５（Ａ）と画像輪郭点群６（Ｂ）は、距離最小化ステップＳ６で（Ｃ）のように重なり、最近傍点同士が正しく対応付けされている。

図６は、画像輪郭点群６（Ｂ）にモデル輪郭点群５と対応しないエラー点群７が存在する場合を示している。
この場合、（Ｃ）に示すように、モデル輪郭点群５と画像輪郭点群６（エラー点群７）との間に誤対応８が発生する場合がある。本発明の誤対応除去ステップＳ７では、このような場合に、最近傍点間の距離の閾値を予め設定し、最近傍点間の距離が所定の閾値を超える場合に、その対応付けを除去する（処理Ａ）。

図７は、モデル輪郭点群５と画像輪郭点群６に輝度勾配が存在する場合を示している。（Ｂ）（Ｃ）において、輝度勾配を矢印のベクトルで示す。すなわちこの例では、円及び楕円の内側は暗く、外側が明るい状態を示している。
この場合、（Ｃ）に示すように、モデル輪郭点群５と画像輪郭点群６（エラー点群７）との間に図のような誤対応８が発生しても、最近傍点間において、輝度勾配を表すベクトル間の角度差が所定の閾値を超える場合に、その対応付けを除去する（処理Ｂ）。
処理Ａと処理Ｂは、少なくともいずれか一方を実施する。

上述した誤対応除去ステップＳ７により、モデル輪郭点群５と画像輪郭点群６との誤対応を除去することができる。
なお、距離最小化ステップＳ６と誤対応除去ステップＳ７は、必要に応じて繰り返すのがよい。

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

また、本発明によれば、モデル輪郭点群５と画像輪郭点群６の相対距離が最小になるようにモデル輪郭点群５の位置姿勢を変換するので、ずれは生じにくく、物体の位置姿勢を非常に高い精度で検出できる。
例えば、モデルの円の中心と画像の楕円の中心を位置合わせ用の制御点として使用する手段では、画像の楕円の中心はモデルの円の中心と真には一致しないため、わずかなずれが生じ、物体の位置姿勢の検出精度を低下させる。
また、円の中心やコーナーのような制御点は点数が少なくいため誤差が発生しやすいが、本発明では位置合わせ用の点を大量に得ることができるため、精度が向上する。

また、距離最小化の手段として、座標変換したモデル輪郭点群５を二次元画像上に投影し、そのモデル輪郭点群５と画像上の画像輪郭点群６間の距離を最小化するような、二次元画像上での検証手段が適用できる。
さらに、座標変換したモデル輪郭点群５と、画像上の画像輪郭点群６との距離を最小化するように、三次元空間上での検証手段を導入することにより、二次元画像上では距離が同じに見えても、視点から遠い位置のほうが距離が長いような、奥行に応じた重み付けが可能となり、精度を向上させることができる。

さらに、モデル輪郭点群５と、画像上の画像輪郭点群６との対応付けの手段として、単純に最近傍の点同士を対応付ける手段に加え、前記最近傍点間の距離が所定の閾値を超える場合や、最近傍点間において、輝度勾配を表すベクトル間の角度差が所定の閾値を超える場合に、その対応付けを除去することにより、誤対応を防ぐことができる。

また、モデル輪郭点群はシンプル（物体の輪郭データのみ）であるため、大量の画像データの教示は不要であり、多品種に容易に対応できる。

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

本発明による三次元位置姿勢計測装置の全体構成図である。 本発明による三次元位置姿勢計測方法の全体フロー図である。 距離最小化ステップＳ６の具体的フロー図である。 距離最小化ステップＳ６の説明図である。 モデル輪郭点群５が円であり、その画像輪郭点群６が楕円である場合の誤対応が発生しない例を示す図である。 画像輪郭点群６にモデル輪郭点群５と対応しないエラー点群７が存在する場合を示す誤対応除去ステップＳ７の作動説明図である。 モデル輪郭点群５と画像輪郭点群６に輝度勾配が存在する場合を示す誤対応除去ステップＳ７の作動説明図である。

符号の説明

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

Claims (4)

1. 検出する対象物の輪郭を表すモデル輪郭点群を予め入力するモデル入力ステップと、
   対象物を撮影して画像を取得する画像撮影ステップと、
   前記画像から、前記モデル輪郭点群に対応する画像輪郭点群を抽出する輪郭抽出ステップと、
   前記モデル輪郭点群と画像輪郭点群の相対距離が最小になるようにモデル輪郭点群の位置姿勢を変換する距離最小化ステップと、
   前記モデル輪郭点群と画像輪郭点群との誤対応を除去する誤対応除去ステップと、を有し、
   前記距離最小化ステップは、
   前記モデル輪郭点群を初期位置姿勢に座標変換する初期位置設定ステップと、
   前記モデル輪郭点群と画像上の画像輪郭点群の最近傍点同士を対応付けする対応点決定ステップと、
   前記最近傍点間の距離に基づく評価値が最小になるようにモデル輪郭点群の座標変換式を算出する座標変換ステップと、を有し、
   前記対応点決定ステップと座標変換ステップを、繰り返し終了条件を満たすまで繰り返す、ことを特徴とする三次元位置姿勢計測方法。
2. 前記距離最小化ステップにおける繰り返し終了条件は、
   前記最近傍点間の距離に基づく評価値が所定の閾値を下回るか、もしくは、所定の繰り返し回数を超過することで、繰り返しを終了することを特徴とする請求項１に記載の三次元位置姿勢計測方法。
3. 前記誤対応除去ステップは、
   前記最近傍点間の距離が所定の閾値を超える場合に、その対応付けを除去する処理Ａと、
   前記最近傍点間において、輝度勾配を表すベクトル間の角度差が所定の閾値を超える場合に、その対応付けを除去する処理Ｂのうち、少なくともいずれか一方を実施する、ことを特徴とする請求項１に記載の三次元位置姿勢計測方法。
4. 検出する対象物を撮影して画像を取得する撮像装置と、
   前記画像から対象物の三次元位置姿勢を決定する演算装置とを備え、
   該演算装置は、予め入力された対象物の輪郭を表すモデル輪郭点群に対応する画像輪郭点群を抽出する輪郭抽出ステップと、
   前記モデル輪郭点群と画像輪郭点群の相対距離が最小になるようにモデル輪郭点群の位置姿勢を変換する距離最小化ステップと、
   前記モデル輪郭点群と画像輪郭点群との誤対応を除去する誤対応除去ステップと、を実行し、
   前記距離最小化ステップは、
   前記モデル輪郭点群を初期位置姿勢に座標変換する初期位置設定ステップと、
   前記モデル輪郭点群と画像上の画像輪郭点群の最近傍点同士を対応付けする対応点決定ステップと、
   前記最近傍点間の距離に基づく評価値が最小になるようにモデル輪郭点群の座標変換式を算出する座標変換ステップと、を有し、
   前記対応点決定ステップと座標変換ステップを、繰り返し終了条件を満たすまで繰り返す、ことを特徴とする三次元位置姿勢計測装置。
   

Images