(57)【特許請求の範囲】
【請求項１】
物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測装置であって、
前記物体の形状を示すモデル情報からモデルエッジと前記モデルエッジの幅とを取得する取得手段と、
前記物体の位置及び姿勢のうちの少なくとも何れか一方のパラメータに基づいて、前記物体が撮像された画像データに対して前記モデルエッジを投影する投影手段と、
前記モデルエッジが投影された位置の周囲の領域に、前記モデルエッジの幅に応じて決定される検出フィルタを適用することにより、前記画像データから前記投影されたモデルエッジの幅に対応する幅を有する画像エッジを検出する検出手段と、
前記投影したモデルエッジと前記検出した画像エッジとの距離を小さくするように前記パラメータを更新する更新手段とを有することを特徴とする計測装置。
【請求項２】
前記検出手段は、前記モデルエッジが投影された位置の周囲の領域に前記モデルエッジの幅に応じて決定される検出フィルタを適用して得られる相関に基づいて、前記投影されたモデルエッジの幅に対応する幅を有する画像エッジを検出することを特徴とする請求項１に記載の計測装置。
【請求項３】
前記更新手段は、前記モデルエッジの領域の法線又は位置に基づく重み係数を用いて、前記パラメータの更新量を算出することを特徴とする請求項１又は２に記載の計測装置。
【請求項４】
物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測装置が実行する計測方法であって、
前記物体の形状を示すモデル情報からモデルエッジと前記モデルエッジの幅とを取得する取得ステップと、
前記物体の位置及び姿勢のうちの少なくとも何れか一方のパラメータに基づいて、前記物体が撮像された画像データに対して前記モデルエッジを投影する投影ステップと、
前記モデルエッジが投影された位置の周囲の領域に、前記モデルエッジの幅に応じて決定される検出フィルタを適用することにより、前記画像データから前記投影されたモデルエッジの幅に対応する幅を有する画像エッジを検出する検出ステップと、
前記投影したモデルエッジと前記検出した画像エッジとの距離を小さくするように前記パラメータを更新する更新ステップとを含むことを特徴とする計測方法。
【請求項５】
コンピュータを、請求項１乃至３の何れか１項に記載の計測装置の各手段として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体の位置及び姿勢のうちの少なくとも何れか一方を計測する技術に関するものである。
【背景技術】
【０００２】
物体の位置及び姿勢を計測する技術は、マシンビジョンの技術の一つとして物体のハンドリングで利用される。物体の幾何学的な形状は、画像データ上ではエッジとして検出される。非特許文献１には、物体の幾何学的な形状を示す情報（以下、モデルと称す）を用いて当該物体の位置及び姿勢を推定する技術が開示されている。非特許文献１に開示される技術は、先ず、推定されている位置及び姿勢のパラメータを用いて、撮像された物体の画像データ上にモデルが投影される。次に、投影されたモデルとエッジとの対応付けが画像データ上において行われ、対応付けられたモデルとエッジとの距離を小さくするように位置及び姿勢のパラメータが更新される。以上の処理が繰り返されることにより、物体の位置及び姿勢が推定される。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】Tom Drummond, Roberto Cipolla, "Real-Time Visual Tracking of Complex Structures", IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol.24, No.7, pp.932-946, 2002.
【発明の概要】
【発明が解決しようとする課題】
【０００４】
非特許文献１において開示されている物体のモデルは、面の輪郭をエッジの線分として表現している。そのエッジには幅がなく、エッジの位置は固定されている。物体がモールド部品や組み付け部品である場合、面と面との稜線部の角を削り取った面取り部や、小さい曲率の曲面で構成されているフィレット部を有するものが多い。これら面取り部やフィレット部のエッジは、数画素の幅として画像データから検出される。
【０００５】
撮影距離が短い場合、面取り部やフィレット部は、画像データ上では幅のある濃度勾配となり、その濃度勾配により検出されるエッジの位置が変化する。また、フィレット部のような曲面で構成されている部分は、光源の位置により光沢反射のパターンが変化し、同じく検出されるエッジの位置が変化する。照明を工夫したとしても、山積み部品のように物体が様々な姿勢をとる場合、やはり検出されるエッジの位置が変化する。このように検出されるエッジの位置が変化すると、モデルとエッジとの距離を評価関数として位置及び姿勢のパラメータを算出する方法においては誤差要因となり、精度が低下するという問題があった。
【０００６】
そこで、本発明の目的は、フィレット部や面取り部等のエッジ幅のある形状を有する物体の位置及び姿勢のうちの少なくとも何れか一方を精度よく推定することにある。
【課題を解決するための手段】
【０００７】
本発明の計測装置は、物体の位置及び姿勢のうちの少なくとも何れか一方を計測する計測装置であって、前記物体の形状を示すモデル情報からモデルエッジと前記モデルエッジの幅とを取得する取得手段と、前記物体の位置及び姿勢のうちの少なくとも何れか一方のパラメータに基づいて、前記物体が撮像された画像データに対して前記モデルエッジを投影する投影手段と、前記モデルエッジが投影された位置の周囲の領域に、前記モデルエッジの幅に応じて決定される検出フィルタを適用することにより、前記画像データから前記投影されたモデルエッジの幅に対応する幅を有する画像エッジを検出する検出手段と、前記投影したモデルエッジと前記検出した画像エッジとの距離を小さくするように前記パラメータを更新する更新手段とを有することを特徴とする。
【発明の効果】
【０００８】
本発明によれば、フィレット部や面取り部等のエッジ幅のある形状を有する物体の位置及び姿勢のうちの少なくとも何れか一方を精度よく推定することができる。
【図面の簡単な説明】
【０００９】
【図１】本発明の実施形態に係る位置姿勢計測装置の構成を示す図である。
【図２】本発明の実施形態に係る位置姿勢計測装置の処理を示すフローチャートである。
【図３】モデルエッジの幅と検出フィルタとの関係を示す図である。
【発明を実施するための形態】
【００１０】
以下、本発明を適用した好適な実施形態を、添付図面を参照しながら詳細に説明する。
【００１１】
先ず、本発明の第１の実施形態について説明する。図１は、本発明の第１の実施形態に係る位置姿勢計測装置２００の構成を示す図である。図１に示すように、位置姿勢計測装置２００は、物体の概略的な位置及び姿勢を示す概略位置姿勢情報１１０と、物体の幾何学的な形状を示す情報であるモデル１２０と、撮像装置によって撮像された物体の画像データ１３０とを入力する。そして、位置姿勢計測装置２００は、物体の位置及び姿勢の推定結果（以下、位置姿勢推定結果と称す）１４０を出力する。
【００１２】
モデルエッジ抽出部２１０は、モデル１２０からエッジに相当する領域（以下、モデルエッジと称す）と当該モデルエッジの幅とを抽出する。モデルエッジ投影部２２０は、推定中の物体の位置及び姿勢のパラメータと、画像データ１３０の撮影時のパラメータとを用いて、モデルエッジ抽出部２１０により抽出されたモデルエッジを画像データ１３０に投影する。エッジ検出部２３０は、上記モデルエッジの幅に対応する検出フィルタを用いて、画像データ１３０から物体のエッジを検出する。
【００１３】
位置姿勢更新部２４０は、モデルエッジ抽出部２１０により抽出されたモデルエッジと、エッジ検出部２３０により検出された物体のエッジとの距離を小さくするように位置及び姿勢のパラメータの更新量を算出する。そして、位置姿勢更新部２４０は、算出した更新量で位置及び姿勢のパラメータを更新する。ここで、位置及び姿勢のパラメータの更新量が所定の値（規定値）以下である場合、位置姿勢更新部２４０は、そのときの位置及び姿勢のパラメータ（又は、位置及び姿勢のパラメータに基づく位置情報及び姿勢情報）を位置姿勢推定結果１４０として出力する。一方、位置及び姿勢のパラメータの更新量が所定の値（既定値）より大きい場合、モデルエッジ抽出部２１０からの処理に戻って、物体の位置及び姿勢の推定を繰り返す。なお、本実施形態では、物体の位置及び姿勢の双方について推定を行うが、本発明は、物体の位置及び姿勢のうちの何れか一方を推定する場合にも適用することができる。
【００１４】
以下、図２を参照しながら、本実施形態に係る位置姿勢計測装置２００の処理について説明する。図２は、本実施形態に係る位置姿勢計測装置２００の処理を示すフローチャートである。なお、図２に示す処理は、位置姿勢計測装置２００内における、不図示のＣＰＵがＲＯＭ等の記録媒体から必要なデータ及びプログラムを読み込み、実行することによって実現する処理である。
【００１５】
ステップＳ２０１において、モデルエッジ抽出部２１０は、物体の幾何学的な形状を示す情報であるモデル１２０から、モデルエッジと当該モデルエッジの幅とを抽出する。なお、モデルエッジ抽出部２１０に対して入力されるモデル１２０は、設計時の３ＤＣＡＤモデルでもよいし、リバースエンジニアリングにより再構成されたポリゴンモデルでもよい。フィレット部や面取り部を示す情報は、設計時に設定されているものがあれば、それを利用してもよい。また、面と面との交線から或る距離の稜線範囲を面取り部として抽出してもよい。モデルエッジの幅を抽出することができれば、何れの方法を使用しても構わない。例えば、実測により物体のモデルエッジの幅を測定してもよい。
【００１６】
また、モデルエッジの幅の表現方法としては、面取り部のエッジを円筒の円弧として近似したときの当該円筒の半径の値によってモデルエッジの幅を表現してもよい。或いは、面取り部を平面として近似したときの当該平面の法線と当該平面上のエッジ領域の長さとによりモデルエッジの幅を表現してもよい。また、面取り部の境界部の座標値でモデルエッジの幅を表現してもよい。即ち、実際の物体の物理的な長さに変換できるように表現できれば、何れも方法を用いてもよい。
【００１７】
なお、モデルエッジのうち、視線の反対側に位置するモデルエッジは観察されない。そこで、観察されるモデルエッジを視線に応じて選択しなければならない。具体的には、モデル１２０の対応する面に異なる描画色を設定し、グラフィックスプロセッサでモデル１２０を描画したときの描画色からモデルエッジを抽出することができる。また、モデル１２０を複数の視線から観測したときのモデルエッジを視線毎に事前に保持しておき、該当するモデルエッジを読み出すようにしてもよい。
【００１８】
ステップＳ２０２において、モデルエッジ投影部２２０は、推定中の物体の位置及び姿勢のパラメータと、画像データ１３０の撮影時のパラメータとを用いて、モデルエッジ抽出部２１０により抽出されたモデルエッジを画像データ１３０の座標系に対応付ける。これにより、モデルエッジが画像データ１３０に対して投影される。ここで、モデルエッジ上の１つのサンプル点に対応するモデルエッジの幅の投影方法について説明する。
【００１９】
モデルエッジ上のサンプル点は、モデル１２０の幾何学的な形状を定義する座標系上での値としてＭ（Ｘ，Ｙ，Ｚ）と表現される。このサンプル点は、式１に示すように、画像データ１３０の座標系上におけるモデル１２０の位置及び姿勢のパラメータである回転Ｒ及び並進ｔにより、Ｍ´(Ｘ´,Ｙ´,Ｚ´)に剛体変換される。
【００２０】
【数１】
0006045178.tif 000002
【００２１】
画像データ１３０の撮影時のパラメータとして、画像データ１３０の撮影時の焦点距離をｆとし、主点位置を（ｃｘ，ｃｙ）とすると、Ｍ´(Ｘ´,Ｙ´,Ｚ´)は、画像データ１３０上の投影位置（ｕ，ｖ）として、式２のように表すことができる。
【００２２】
【数２】
0006045178.tif 000003
【００２３】
ここで、モデルエッジのサンプル点に対応するモデルエッジの幅の表現方法として、（１）１つの幅で表現する方法、（２）平面として表現する方法、（３）３次元のモデルとして表現する方法、がある。
【００２４】
（１）１つの幅で表現する方法では、モデルエッジの幅を円筒の一部として考えた場合で、その値をｅ０とすると、式３に示すように、投影されたモデルエッジの幅ｅ０´は、サンプル点の奥行きＺ´に依存する。
【００２５】
【数３】
0006045178.tif 000004
【００２６】
ここでは、Ｚ´は各モデルエッジの奥行き値を用いているが、撮影距離に対してモデルエッジの奥行きが小さい場合、モデル１２０の平均奥行き値を各モデルエッジの奥行き値としても構わない。モデルエッジのエッジ方向と直交するエッジ探索方向のベクトルを（ｎｅｘ，ｎｅｙ，ｎｅｚ）とし、画像データ１３０の撮影時のパラメータを使用して、画像データ１３０にモデルエッジを投影すると、画像データ１３０上のエッジ投影方向は、式４のように（ｎｅｘ´，ｎｅｙ´）で表される。
【００２７】
【数４】
0006045178.tif 000005
【００２８】
また、モデルエッジの幅を含むエッジ位置（ｕ´，ｖ´）は、式５のように表すことができる。
【００２９】
【数５】
0006045178.tif 000006
【００３０】
（２）平面として表現する方法では、面取り部を平面で近似したときの当該平面の法線（ｐｎｘ，ｐｎｙ，ｐｎｚ）から、エッジ探索方向のベクトル（ｎｅｘ，ｎｅｙ，ｎｅｚ）が求められる。その際、エッジ方向を（ｅｘ，ｅｙ，ｅｚ）とすると、エッジ探索方向のベクトルは、外積から式６のように表される。
【００３１】
【数６】
0006045178.tif 000007
【００３２】
式６から式４及び式５を用いてエッジの範囲を決めることができる。エッジの面と視軸との関係から、傾いているときのエッジの範囲が調整される。
【００３３】
（３）３次元モデルエッジとして表現する方法では、面取り部と隣接する面との境界部の座標値をエッジの（Ｘ，Ｙ，Ｚ）との差分として保持しておき、式４を用いて投影計算することにより、より正確な投影計算をすることができる。
【００３４】
何れのエッジ幅の情報も、カメラ座標系におけるモデル１２０の奥行き方向の値に応じて、観察されるモデルエッジの幅が調整されればよいため、上述した方法以外の方法で定式化してもよい。
【００３５】
また、（１）〜（３）のうちから使用する表現方法を選択する方法としては、それぞれの表現方法によって表現されたモデルエッジの幅を保持しておき、位置及び姿勢の推定誤差が少ないものを選択してもよい。或いは、ユーザが事前に選択できるように設定する方法でもよい。なお、その設定方法についてはファイルで記述する方法でもよいし、ＧＵＩにより指示する方法でもよい。また、選択された表現方法による結果を画面に表示すれば、表現方法の選択の際における有用な情報を提供することができる。
【００３６】
ステップＳ２０３において、エッジ検出部２３０は、モデルエッジ投影部２２０において算出された各モデルエッジのエッジ探索方向から、モデルエッジの幅に対応する特徴（エッジ）を画像データ１３０から検出する。図３は、モデルエッジの幅と検出フィルタとの関係を示す図である。図３（ａ）は、モデルエッジの幅を示している。図３（ｂ）は、モデルエッジの幅に対応する検出フィルタとして、モデルエッジがある範囲を１とし、それ以外を−１とした単純なフィルタを示している。図３（ｃ）は、モデルエッジの幅に対応する広がりが設定されたＧａｂｏｒ特徴のフィルタを示している。本実施形態においては、投影されたモデルエッジの幅に対応するエッジを検出することができるように検出フィルタを構成することができればよいため、他の検出フィルタを用いてもよい。
【００３７】
画像データ１３０上のエッジモデルの投影位置（ｕ，ｖ）周りを、エッジ探索方向（ｎｅｘ´，ｎｅｙ´）に検出フィルタで検出したときの画素値は、相互相関係数を計算することで求めることができる。相互相関係数の値をＧとして、検出フィルタのエッジ探索方向の１次元の移動量をΦとし、フィルタ係数をｈとすると、相互相関係数Ｇは、式７のように表すことができる。
【００３８】
【数７】
0006045178.tif 000008
【００３９】
ここで、Ｉ（ｕ，ｖ）は画像データ１３０の輝度値である。Φをモデルエッジの投影位置（ｕ，ｖ）から左右にエッジ探索方向（ｎｅｘ´，ｎｅｙ´）に沿って移動することにより、Ｇ（Φ）のプロファイルが得られる。エッジ検出部２３０は、式８のように、Ｇ（Φ）の相関値が最大となるΦ´をエッジの位置として求める。
【００４０】
【数８】
0006045178.tif 000009
【００４１】
ここでは、Ｇ（Φ）を１つ選ぶ方法として、最大の値をとる方法を選択するが、実際には、Ｇ（Φ）が既定値より大きい複数の候補から、モデルエッジの投影位置（ｕ，ｖ）との距離が小さいものを選択してもよい。ここでは、幅のあるエッジを、投影されたモデルエッジの幅に応じて検出し、そのときのモデルエッジからの移動量であるΦ´の値が求められればよい。そのため、他の相関の式を用いても同様の移動量が求めることができれば、本質的には問題なく実施することができる。
【００４２】
ステップＳ２０４において、位置姿勢更新部２４０は、モデルエッジの投影位置（ｕ，ｖ）と、モデルエッジの幅に対応する検出フィルタを用いたエッジの位置（上述のΦ´）との距離を小さくする位置及び姿勢のパラメータを算出する。
【００４３】
位置及び姿勢のパラメータの算出方法は、非特許文献１において開示されている姿勢成分と並進成分とのヤコビアンを用いて算出する方法でも構わない。姿勢成分は非線形関数であるため、繰り返し最適化方法が用いる方法が一般的であるが、繰り返さない解法としては、モデルの点群から重心成分を引いて、姿勢成分を、特異値分解を用いて求める方法でも構わない。さらに、拡張カルマンフィルタを用いて更新量を推定しても構わない。ここでは、画像データ１３０のエッジと投影されたモデルエッジとの距離が小さくなるように位置姿勢を更新する方法であればよい。
【００４４】
ステップＳ２０５において、位置姿勢更新部２４０は、位置及び姿勢のパラメータの更新量が既定値以下であるか否かを判定する。位置及び姿勢のパラメータの更新量が規定値以下である場合、処理はステップＳ２０６に移行する。ステップＳ２０６において、位置姿勢更新部２４０は、そのときの位置及び姿勢のパラメータを位置姿勢推定結果１４０として出力する。なお、計算途中の残差や繰り返し回数等の精度に関係する情報も含めて出力することは、本処理における精度を定量的に判断する上で有用である。一方、位置及び姿勢のパラメータの更新量が規定値より大きい場合、処理はステップＳ２０１に戻る。概略位置姿勢情報１１０が実際の位置及び姿勢と異なっている場合、一連の処理で十分に推定値を更新することができない。そこで、本実施形態においては、位置及び姿勢のパラメータの更新量が既定値より大きい場合、位置姿勢更新部２４０は位置及び姿勢のパラメータを更新し、その後、ステップＳ２０１からの処理を再度実行することにより、位置姿勢推定結果１４０の精度を向上させることができる。また、誤差が含まれている場合、更新量が減少しない場合もある。そのため、既定された回数の更新処理が実行された場合、モデルエッジ抽出部２１０からの処理を繰り返すことを中断してもよい。
【００４５】
本実施形態に係る位置姿勢計測装置２００は、撮像装置によって撮像された計測対象となる物体の画像データ１３０を入力することにより、フィレット部や面取り部等のエッジ幅のある形状を有する当該物体の位置及び姿勢を精度よく推定することができる。
【００４６】
次に、本発明の第２の実施形態について説明する。第２の実施形態においては、エッジモデルの幅を抽出する方法として、エッジモデルの法線を利用してエッジの観察時のエッジモデルの傾きを計算する。エッジモデルの傾きは、エッジの特徴量の強度と相関がある。画像特徴としての検出の信頼性は、推定している位置及び姿勢のパラメータとエッジモデルの法線との関係として表現することができる。なお、第２の実施形態に係る位置姿勢計測装置の構成は、図１に示した第１の実施形態に係る位置姿勢計測装置２００の構成と同様であるため、以下の説明においても、図１の符号を用いるものとする。
【００４７】
モデル座標系におけるエッジモデルの法線を（ｐｎｘ，ｐｎｙ，ｐｎｚ）とし、モデル座標系をカメラ座標系に変換する際の姿勢成分Ｒを用いると、カメラ座標系におけるエッジモデルの法線（ｐｎｘ´，ｐｎｙ´，ｐｎｚ´）は、式９のように表すことができる。
【００４８】
【数９】
0006045178.tif 000010
【００４９】
カメラ座標系に変換されたエッジモデルの法線に対して重み係数ｗを計算する関数Ｗを規定すると、重み係数ｗは、式１０のように表すことができる。
【００５０】
【数１０】
0006045178.tif 000011
【００５１】
ここで、関数Ｗは、視線と正対している場合にエッジが強く出現するとした場合、エッジモデルの法線と視線との内積を計算して求めることができる。また、エッジモデルの法線が視線に対して直交する場合、そのエッジは、ジャンプエッジとして背景との境界のエッジとなっているものとして、エッジが検出されやすい状態とするために重みを大きくしても構わない。関数Ｗは、エッジモデルの法線を引数としているが、エッジモデルの位置を引数として利用しても構わない。ここでは、エッジが画像特徴として検出される場合に重みが大きく、エッジが画像特徴として検出されにくい場合に重みが小さくなるように算出するものであれば、何れの方法でも構わない。
【００５２】
本実施形態における位置姿勢更新部２４０は、各モデルエッジの重み係数ｗをヤコビアンに乗じた重み係数行列として表現し、重み付き最適化計算による更新量の推定値を算出する。これにより、推定している位置及び姿勢におけるモデルエッジの画像特徴の検出の信頼性を考慮した重み付き推定を行うことができ、物体の位置及び姿勢の推定精度を向上させることができる。
【００５３】
以上説明した実施形態によれば、ロボットビジョンシステムによる部品組み立てにおいて、部品がフィレット部や面取り部等の形状を有している場合においても、当該部品の位置及び姿勢を高精度に推定することができる。
【００５４】
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。
【符号の説明】
【００５５】
１１０：概略位置姿勢情報、１２０：モデル、１３０：画像データ、１４０：位置姿勢推定結果、２００：位置姿勢計測装置、２１０：モデルエッジ抽出部、２２０：モデルエッジ投影部、２３０：エッジ検出部、２４０：位置姿勢更新部
【図１】
0006045178.tif 000012
【図２】
0006045178.tif 000013
【図３】
0006045178.tif 000014