位置姿勢推定方法及びその装置
【課題】 標準的な形状の三次元形状モデルに比して、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供する。
【解決手段】 物体の三次元標準形状を表す三次元モデルを構成する特徴のずれの統計量を推定し、特徴ごとに信頼度を決定する。撮像装置によって取得した観測データから抽出した特徴と三次元モデル中の特徴とのずれ量を算出する。三次元モデルから抽出した各特徴に関するずれ量と信頼度とに基づいて、物体の三次元位置姿勢を推定する。
【解決手段】 物体の三次元標準形状を表す三次元モデルを構成する特徴のずれの統計量を推定し、特徴ごとに信頼度を決定する。撮像装置によって取得した観測データから抽出した特徴と三次元モデル中の特徴とのずれ量を算出する。三次元モデルから抽出した各特徴に関するずれ量と信頼度とに基づいて、物体の三次元位置姿勢を推定する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、三次元形状が既知である物体の位置姿勢を推定する技術に関する。
【背景技術】
【0002】
近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット(ハンド)との間の相対的な位置姿勢を高精度に推定する必要がある。このような位置姿勢の推定は、ロボットが部品を把持する場合だけでなく、ロボットが自律移動するための自己位置推定や、拡張現実感における現実空間と仮想物体の位置合わせなど様々な目的に応用される。
【0003】
位置姿勢を推定する方法として、カメラが撮像する二次元画像や距離センサから得られる距離画像を用いて行う方法がある。その中でも、撮像画像から抽出される画像特徴や距離画像から得られる点群データに対して物体の三次元形状モデルをあてはめるモデルフィッティングによる推定が一般的である。例えば、濃淡画像上で検出されるエッジに物体のワイヤフレームモデルの投影像があてはまるように物体の位置姿勢を推定する手法がある。また、距離画像から得られる点群データに対してメッシュモデルなどの三次元形状モデルをあてはめることにより物体の位置姿勢を推定する手法がある。
【0004】
工作精度やコストの問題から、大量生産される工業部品の形状やサイズには一般的にばらつきがある。このような部品について個体ごとに三次元形状モデルを作ることは現実的ではないため、通常は部品の標準的な形状を表す1つの三次元形状モデルを用いて位置姿勢の推定する。すなわち、三次元形状モデルと実部品の形状は必ずしも一致しない。この部品に対してモデルフィッティングによる位置姿勢を行う場合、モデルと実部品の形状の差が大きい場合、高精度な推定ができないという課題がある。
【0005】
特許文献1には、モデルを利用して物体の位置認識を行う際に物体の形状のばらつきを吸収する方法が開示されている。この方法では、物体を表す標準モデルと画像(計測データ)に基づいて物体の位置認識を行い、計測データを統計学的に処理し、標準モデルを逐次更新する。
【0006】
一方、非特許文献1には、顔の位置姿勢を推定する際に、顔の三次元モデルに対する個人差を吸収する方法が開示されている。この方法は、個体ごとの特徴点のずれを、予め求めておいたずれの分布と実際の計測データから求め、このずれを付与した特徴点に対してモデルフィッティングを行う。個体ごとにモデル生成を行うことで、形状のばらつきに左右されることなく位置姿勢を推定できる。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開平9−81744
【非特許文献】
【0008】
【非特許文献1】X.Chai,Shan,L.Qing,and W.Gao,”Pose estimation based on Gaussian error models,”International Conference on Biometrics(ICB 2006),LNCS 3832,pp.136−143,2005.
【非特許文献2】Y.Chen and G.Medioni,”Object modeling by registration of multiple range images,”Proc.1991 International Conference on Robotics and Automation(ICRA’91),pp.2724−2729,1991.
【非特許文献3】M.D.Wheeler,Y.Sato,and K.Ikeuchi,”Consensus surfaces for modeling 3D objects from multiple range images,”Proc.6th International Conference on Computer Vision(ICCV’98),pp.917−924,1998.
【非特許文献4】R.Hartley and A.Zisserman,”Multiple View Geometry in Computer Vision”,Cambridge University Press,2000.
【非特許文献5】Y.Liu,T.S.Huang,and O.D.Faugeras,”Determination of Camera Location from 2−D to 3−D Line and Point Correspondences,”IEEE Transactions on Pattern Analysis and Machine Intelligence,vol.12,no.1,pp.28−37,1990.
【発明の概要】
【発明が解決しようとする課題】
【0009】
特許文献1には、物体の形状のばらつきを吸収し、物体の認識率を向上させる。しかし、この方法は、実物体のばらつきを吸収するように標準モデルを更新するため、物体の認識や概略の位置姿勢推定には適しているが、高精度な位置姿勢推定には適さないという課題があった。
【0010】
非特許文献1には、この方法はずれを明示的に算出する方法であり、計測データからの特徴の誤検出などによって求めたずれ自体に誤差を含む可能性があるため、高精度な位置姿勢推定には適さないという課題があった。
【0011】
上記事情に鑑み、本発明では、標準的な形状の三次元形状モデルに比して、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供することを目的とする。
【課題を解決するための手段】
【0012】
上記の目的は以下の方法によって達成される。
【0013】
物体の位置姿勢を求める位置姿勢推定方法であって、
統計量算出手段が、予め保持された前記物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出工程と、
信頼度算出手段が、前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出工程と、
画像特徴抽出手段が、撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出工程と、
対応付け手段が、前記幾何特徴と前記画像特徴とを対応付けする対応付け工程と、
ずれ量算出手段が、前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出工程と、
位置姿勢推定手段が、前記信頼度及び前記ずれ量に基づいて、前記物体の位置姿勢を推定する位置姿勢推定工程と
を有することを特徴とする位置姿勢推定方法。
【発明の効果】
【0014】
本発明によれば、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供することができる。
【図面の簡単な説明】
【0015】
【図1】第1の実施形態における構成例である。
【図2】第1の実施形態における処理のフローチャートである。
【図3】公差データから形状のずれを算出する処理の例を説明するフローチャート(1)である。
【図4】標準モデル及び公差データの例である。
【図5】ずれ候補量の模式図である。
【図6】抽出された線分及び信頼度データベースの例である。
【図7】物体の位置姿勢算出処理の例を説明するフローチャートである。
【図8】対応付けの例を説明する模式図(1)である。
【図9】計測データからずれを算出する処理の例を説明するフローチャートである。
【図10】対象物体と計測位置の関係の例を説明する図である。
【図11】公差データから形状のずれを算出する処理の例を説明するフローチャート(2)である。
【図12】対応付けを説明する模式図(2)である。
【図13】本願の実施形態を実現するためのコンピュータの構成例である。
【発明を実施するための形態】
【0016】
〔第1の実施形態〕
本実施形態では、物体の標準的な形状を表す三次元形状モデル(以下、標準モデルと呼ぶ)と実物体とのずれが一様ではないことを利用して高精度な物体の位置姿勢を推定する。すなわち、ずれが小さいと期待できる部分の情報を積極的に用いることで、高精度な位置姿勢の推定を行う。本実施形態では、物体は工業用部品であると想定し、物体の公差データをもとに、物体の標準モデルを構成する各特徴のずれの統計量を推定し、このずれの統計量をもとに、算出される特徴ごとの信頼度を用いて、物体の位置姿勢を推定する。
【0017】
図1を用いて、本実施形態における位置姿勢推定装置100の構成について説明する。図中矢印は、データの流れを示している。位置姿勢推定装置100は、計測データ取得部101、特徴抽出部102、蓄積部103、位置姿勢算出部104で構成される。位置姿勢推定装置100は、計測装置105に接続されている。
【0018】
計測装置105は、現実空間を計測する。例えば、カメラで二次元画像を撮像する。例えば、物体の表面の三次元位置を計測し、距離画像を撮像する。撮像された撮像画像は、計測データ取得部101へ送られる。
【0019】
計測データ取得部101は、計測装置105から計測データを取得する。例えば、計測データは、カメラによって撮像された二次元画像を取得する。取得した計測データは、特徴抽出部102へ送られる。
【0020】
特徴抽出部102は、計測データから特徴を抽出する。例えば、Cannyオペレータを用いて、画像からのエッジを特徴として抽出する。抽出した特徴は、位置姿勢算出部104へ送られる。
【0021】
蓄積部103には、物体の標準モデルを構成する各特徴のずれの統計量が保持されている。例えば、物体の標準モデルは、線分の集合によって記述される。線分は、物体表面を表す面と面の交線や、色や濃度が大きく変わる境界線である。しかしながら、物体の標準モデルはこれに限るものではなく、物体上の特徴的な点の集合や、物体を表す面の集合、またはそれらの組み合わせによって記述してもよい。保持されている物体の標準モデルは、位置姿勢算出部104へ送られる。
【0022】
位置姿勢算出部104は、特徴抽出部102で計測データから抽出された特徴と、蓄積部103に蓄積されている物体の標準モデル、物体の各特徴の標準モデルに対するずれの統計量を用いて物体の位置姿勢を推定する。
【0023】
図2を用いて、本実施形態における位置姿勢推定装置の全体処理フローの概要について説明する。
【0024】
ステップS201は、統計量算出ステップである。本ステップでは、物体の標準モデルを構成する幾何特徴のずれの統計量を算出する。前述したように、本実施形態では物体の標準モデルは線分の集合によって記述されている。ここでは、幾何特徴を線分として、各線分を三次元的に一定間隔で分割したときの分割点の公差にもとづくずれの統計量を算出する。分割点を幾何特徴として用いるのは、後述する位置姿勢の推定工程において、分割点単位処理を行うためである。
【0025】
ステップS202は、信頼度算出ステップである。本ステップでは、ステップS201で求めた各分割点のずれの統計量から、幾何特徴である分割点の信頼度を算出する。
【0026】
ステップS203は、位置姿勢算出ステップである。本ステップでは、ステップS202で算出した信頼度を用いて、物体の位置姿勢を算出する。
【0027】
なお、ステップS201及びステップS202は必ずしも位置姿勢の推定とともに行う必要はなく、ステップS203で実施される位置姿勢推定の前処理として、別途オフラインで実施してもよい。
【0028】
以下では、ステップS201で実施する分割点に対するずれの統計量の算出処理について詳しく説明する。ずれの統計量の算出結果は蓄積部103に保持される。ここでは、ずれの統計量を物体の公差データから算出する方法について、図3のフローチャートを用いて説明する。
【0029】
ステップS301は、モデル特徴抽出ステップである。本ステップでは、標準モデルから線分データを抽出する。
【0030】
ステップS302は、公差抽出ステップである。本ステップでは、注目する線分(以下、注目線分)のずれの要因となる公差(以下、ずれ要因公差)を抽出する。ずれ要因公差は、注目線分が含まれる稜線や面に対する公差とする。
【0031】
図4は、標準モデル及び公差データの一例である。標準モデル400から抽出される線分401を注目線分とした場合について説明する。
【0032】
線分401が含まれる稜線や面に対する公差である寸法公差402〜404及び平面公差405〜407がずれ要因公差の候補となる。これらの候補のうち、位置姿勢推定に寄与しない寸法公差402、平面公差405を除いた寸法公差403、404及び平面公差406、407がずれ要因公差となる。
【0033】
ステップS303は、公差値算出ステップである。本ステップでは、注目線分を三次元的に所定間隔に分割し、各分割点について、ステップS302で抽出した各ずれ要因公差によって生じるずれ量の最大値(以下、ずれ候補量)をそれぞれ求める。ずれ候補量は、標準モデル上に設定されたモデル座標系503の成分ごとの大きさで表される。
【0034】
図5は、注目線分を線分401としたときに、線分401のある分割点501に対し寸法公差404が与えるずれ候補量502を模式的に表した図である。この例では、分割点501のずれ候補量502はx成分のみとなる。この処理を、分割点すべてについて実施する。
【0035】
ステップS304は、統計量決定ステップである。本ステップでは、ステップS303で算出された各分割点のずれ候補量群の成分ごとの最大値を持つベクトルを生成し、このベクトルの大きさdを分割点におけるずれの統計量とする。これは、すべての許容公差を満たし、かつその中で最大となるずれ量を求めたこととなる。
【0036】
例えば、分割点501のずれ候補量が3つ存在した場合について説明する。各ずれ候補量を、(x成分,y成分,z成分)=(dx1,dy1,dz1)、(dx2,dy2,dz2)、(dx3,dy3,dz3)とし、それぞれdx1>dx2>dx3、dy3>dy2>dy1、dz2>dz3>dz1といった大小関係があるとする。この場合、成分ごとの最大値は、(dx1,dy3,dz2)となり、この大きさdは、
【0037】
【数1】
【0038】
となり、このdを分割点501におけるずれの統計量とする。
【0039】
ステップS305では、ステップS301で抽出した線分全てに対してステップS302〜304の処理が実施済みの場合には本処理を終了し、実施済みでない場合には、ステップS306に進み注目線分を変更し、ステップS302へ戻る。
【0040】
以下では、ステップS202で実施する分割点ごとの信頼度の算出方法について詳細に説明する。
【0041】
信頼度は、ずれの統計量が小さいものほど高く設定する。すなわち、工作精度が高い部分の信頼度は大きくし、低い部分の信頼度は小さくする。例えば、式(1)のようにずれの統計量が小さいものほど信頼度が高くなる関数を定義する。
【0042】
【数2】
【0043】
dは前述した分割点のずれの統計量であり、c1は定数である。信頼度を与える関数は上記関数に限らず、ずれ量が小さいものほど信頼度が高くなる他の関数であればいかなる関数であってもよい。算出された信頼度は、線分ごとに標準モデルから抽出された分割点の三次元位置と共に蓄積部103にデータベース(図6)として保持される。
【0044】
以下では、ステップS203で行う物体の位置姿勢算出処理について、図7のフローチャートを用いて説明する。
【0045】
ステップS701は、画像特徴抽出ステップである。本ステップでは、特徴抽出部102において撮像画像から画像特徴であるエッジを抽出する。
【0046】
ステップS702は、概略値入力ステップである。本ステップでは、標準モデルの位置姿勢の概略値を入力する。
【0047】
ステップS703は、対応付けステップである。本ステップでは、蓄積部103に保持されている分割点とステップS701で抽出したエッジとの対応付けを行う。図8は、対応付けの様子を模式的に表した図である。ここでは、入力された位置姿勢の概略値を用いて標準モデル中の線分801とその分割点802を画像上に投影し、分割点ごとに投影された線分の法線方向に一定範囲内でエッジ探索を行い、最も近接するエッジを該分割点との対応点803とする。
【0048】
ステップS704は、位置姿勢補正値算出ステップである。本ステップでは、各分割点802における信頼度を用いて、位置姿勢算出部104において位置姿勢を算出するための係数行列及び誤差ベクトルを算出する。ある分割点の画像での投影位置を(ui,0,vi,0)とし、この分割点802の対応点の画像上での検出位置を(ui’,vi’)とする。分割点(ui,0,vi,0)を含む標準モデル上の線分801(以下、モデル線分)の投影像の画像上での傾き804をθとする。該対応点803を通り、傾き804がθである直線805の方程式は式(2)となる。
【0049】
【数3】
【0050】
が表せる(θは定数とする)。ここで、
【0051】
【数4】
【0052】
(定数)である。
【0053】
分割点802の画像上での投影位置と物体の位置姿勢の関係を
【0054】
【数5】
【0055】
のように表す。物体の位置姿勢の微小変化をΔsとすると、式(3)は式(4)のように一次近似できる。
【0056】
【数6】
【0057】
物体の位置姿勢をΔsだけ変化させたときに、式(4)で表される分割点の投影位置が式(2)で表される直線上にのると仮定すると、式(5)で表されるΔsに関する方程式が得られる。
【0058】
【数7】
【0059】
ただし、
【0060】
【数8】
【0061】
(定数)である。標準モデル中の線分801の各分割点802について成り立つ式(5)を連立方程式として解くことにより、物体の位置姿勢の補正値Δsが得られる。位置姿勢に基づく剛体変換と画像上への透視投影変換は非線形の変換であるため、補正値Δsの算出とsの補正を繰り返すことにより、最終的な物体の位置姿勢が得られる。
【0062】
本実施形態では、さらに式(5)の両辺に分割点の信頼度hiを重みとして乗じることにより、工作精度の高い分割点の寄与度を高く、工作精度の低い分割点の寄与度を低くして位置姿勢の補正量Δsを算出する。すなわち、式(6)で表される連立方程式を解くことにより、位置姿勢の補正量Δsを算出する。
【0063】
【数9】
【0064】
ここで式(6)を式(7)のように表す。
【0065】
【数10】
【0066】
式(7)を解いて得られるΔsを用いて概略の位置姿勢を補正することで、物体の位置姿勢が算出する。
【0067】
ステップS705において、行列Jの一般化逆行列(JT・J)−1・JTを式(7)の両辺に乗じることで位置姿勢補正量Δsを求める。
【0068】
【数11】
【0069】
ステップS706は、位置姿勢補正ステップである。本ステップでは、算出した位置姿勢の補正量Δsにより、位置姿勢の概略値を補正する。
【0070】
【数12】
【0071】
ステップS707において、収束判定を行う。収束していれば、処理を終了し、そうでなければステップS701に戻る。収束判定では、補正値Δsがほぼ0である場合や、誤差ベクトルRの2乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。
【0072】
以上述べたように、第1の実施形態では、標準モデルを構成する幾何特徴のずれの統計量を公差データから求め、ずれの統計量が小さい特徴ほど高い重みを付与して物体の位置姿勢を求める方法について説明した。信頼度の高い特徴を積極的に用いることにより、高精度な位置姿勢推定が可能となる。
【0073】
〔第2の実施形態〕
第1の実施形態では、物体を構成する幾何特徴の標準モデルに対するずれの統計量を、公差から算出される最大のずれに基づいて特定の値として算出した。しかしながら、物体のずれは統計的に発生するのが一般的であり、最大のずれに基づいて信頼度を決定することによって予想以上に信頼度が低くなる特徴が発生する。一方、実際の公差は、一般的に標準値を中心とした正規分布となる。このため、公差は標準に対する差の値ではなく、正規分布のように確率分布によって表される場合もある。本実施形態では、標準モデルを構成する特徴のずれが確率分布によって表される場合の位置姿勢の推定方法について説明する。
【0074】
全体の処理フローは、第1の実施形態にて図2を用いて説明したものと同様である。以下では、ステップS201、S202及びS203で実施される各処理について説明する。
【0075】
まず、ステップS201で実施される標準モデルに対するずれの算出処理について、図3を用いて説明する。なお、ステップS304以外の処理は、第1の実施形態で説明した内容と同様であるため、ここではステップS304の処理についてのみ説明する。
【0076】
ステップS304において、ステップS303で算出された各分割点のずれ候補量群から成分ごとのずれの分布を求める。各分割点のずれの分布は成分間で独立であり、成分ごとに平均0、標準偏差がずれ候補量群の最大値の3分の1である1次元の正規分布に従うとする。例として、分割点501のずれ候補量が3つ存在した場合について説明する。各ずれ候補量を、(x成分,y成分,z成分)=(dx1,dy1,dz1)、(dx2,dy2,dz2)、(dx3,dy3,dz3)とし、それぞれdx1>dx2>dx3、dy3>dy2>dy1、dz2>dz3>dz1といった大小関係があるとする。この場合、各成分の最大値は(dx1,dy3,dz2)となり、ずれの標準偏差は(σx,σy,σz)=(dx1/3,dy3/3dz2/3)となる。これより、成分ごとのずれの確率分布は、
【0077】
【数13】
【0078】
となる。
【0079】
次に、ステップS202及びS203で実施される信頼度の算出処理及び位置姿勢推定処理について説明する。まず、標準モデル及び計測データの双方に統計的な誤差が含まれる場合の最尤推定法について説明し、次に第1の実施形態と同様に幾何特徴が線分の分割点とした場合について具体的に説明する。
【0080】
以下では、標準モデル及び計測データに統計的な誤差が含まれる場合の最尤推定法ついて説明する。
【0081】
まず、計測データの適合具合を示す尤度関数を算出する。計測データの特徴位置を表すベクトルをpi、piに対応する標準モデルの特徴位置を表すベクトルをqi、物体の位置姿勢を表す6次元ベクトルをsとする。計測データの特徴位置をpi、標準モデルの特徴位置qi及び物体の位置姿勢sには、式(9)の関係があるものとする。関数gは、三次元空間から二次元画像面への変換である透視投影変換や、モデル座標系からカメラ座標系への変換であるモデルビュー変換などを含む。
【0082】
【数14】
【0083】
標準モデルのずれをΔqi、計測誤差をΔpiとすると、計測データ空間での標準モデルの特徴と計測データの特徴との誤差eiは、式(10)のように線形近似される。
【0084】
【数15】
【0085】
Δqi、Δpiが統計的な誤差であるとすると、誤差eiには、平均が0で分散が、
【0086】
【数16】
【0087】
である統計的誤差が含まれる。Δqi、Δpiが正規分布に従うとするとeiに含まれる統計的誤差も正規分布に従うため、誤差eiの尤度関数は式(12)となる。
【0088】
【数17】
【0089】
最尤推定では、式(13)で表される尤度の積が最大となるように物体の位置姿勢を推定する。
【0090】
【数18】
【0091】
実際には、尤度の積の対数を取り、その符号を反転した
【0092】
【数19】
【0093】
を最小化する問題に置き換えられる。このうち第一項は位置姿勢に依存しない定数であるため、最終的には、
【0094】
【数20】
【0095】
を最小化する問題となる。式(15)は、分散Σiの逆行列を信頼度として重みつけした誤差ベクトルのノルムの2乗和である。
【0096】
以下では、幾何特徴を線分の分割点とした場合について説明する。
【0097】
式(9)の関数gは、分割点を画像面に投影する関数となり、式(16)で表される。
【0098】
【数21】
【0099】
ただし、
【0100】
【数22】
【0101】
及び、
【0102】
【数23】
【0103】
とする。fはカメラの焦点距離である。xc,yc,zcは、カメラ座標系における分割点の三次元位置、xw,yw,zwは世界座標系(物体座標系)における分割点の位置、行列R及びtは世界座標系からカメラ座標系への座標変換行列の回転成分及び平行移動成分である。
【0104】
誤差eiは、分割点の投影位置と対応点を通過する直線との距離(スカラ値)で表される。対応点を通過する直線の傾きθiには誤差はないものと仮定すると、誤差eiは式(19)となる。
【0105】
【数24】
【0106】
誤差eiがスカラ値であるため,eiの共分散行列Σi(式(11))もスカラとなる。ステップS202で信頼度をΣiの逆数として計算し、ステップS203で第1の実施形態で図7を用いて説明したフローを実施することで物体の位置姿勢を求める。なお、ステップS704で算出される係数拡大行列及び誤差ベクトルは、式(6)の重みhを式(18)で計算されるΣiの逆数に置き換えたものとなる。
【0107】
以下では、幾何特徴を線分にしたときの共分散行列Σiの算出方法について具体的に説明する。
【0108】
共分散行列Σi(式(11))の右辺第1項Σpiは、画像から検出される線分の検出誤差の共分散行列である。線分の検出誤差については、探索ラインの方向にのみあいまい性を持つと仮定し、線分の検出誤差が平均0、標準偏差σの1次元ガウス分布に従うと仮定する。実画像におけるσを推定するのは困難である。そのため、線分の検出誤差は主に画像の量子化誤差に起因するとして、例えばσを1画素に設定する。すなわち、Σpi は1となる。また、共分散行列Σi(式(11))の右辺第2項はステップS201で求めた標準モデルのずれの共分散行列Σqiの両辺にヤコビアン∂g/∂qiを乗じたものである。標準モデルのずれの共分散行列Σqiは、成分間に相関がないとして、ステップS201で求めたずれの標準偏差を(σx,σy,σz)とすると、
【0109】
【数25】
【0110】
となる。また、ヤコビアン∂g/∂qiは、式(16)、(17)及び(18)より、
【0111】
【数26】
【0112】
となる。式(20)及び式(21)より共分散行列Σi(式(11))の右辺第2項が求まる。
【0113】
以上述べたように、第2の実施形態では、標準モデルから抽出される特徴のずれが確率分布によって表される場合に、確率分布から算出される信頼度をもとに位置姿勢を求める方法について説明した。
【0114】
〔第3の実施形態〕
第1及び第2の実施形態では、ステップS201で実施される標準モデルに対する特徴のずれの算出を公差データから求める方法について説明したが、その他の方法であっても良い。例えば、実際に同種の多数の物体を計測して形状のばらつきを求めてもよい。本実施形態では、距離撮像装置による三次元形状計測を複数の物体に対して行い、各物体に対する三次元計測結果のばらつきをもとに標準モデルのずれを求める。物体全体の形状を計測するために、各サンプルに対して複数の視点から三次元計測を行い、その結果を統合して、線分によって記述される一つの標準モデルを生成する。このようにして生成された複数の標準モデルから、標準モデルに対するずれを算出する。
【0115】
また、ステップS201の前に、標準モデルを得るために物体の計測を行う物体計測ステップを実行してもよい。そして、物体の標準モデルはあらかじめ用意されているとするが、三次元計測結果から得られる複数の標準モデルをもとに生成してもよい。
【0116】
図9のフローチャートを用いて、実際に物体を計測することによる標準モデルのずれを求める方法について説明する。
【0117】
ステップS901において、ある計測位置から距離撮像装置によって物体の計測データを取得する。ここで物体の計測データとは、物体表面上の点群データであり、各点は距離撮像装置の座標系における三次元位置によって表される。
【0118】
ステップS902において、予め決めておいた計測位置すべてからの計測が終了していない場合には、ステップS903に進む。
【0119】
ステップS903において、計測位置を移動する。図10は、計測位置を模式的に表した図である。物体1001を原点とした球面上を適当な刻み幅で計測装置1002を移動させ、計測位置を移動させる。計測位置を移動した後、ステップS901へ戻り処理を続ける。予め決めておいた計測位置すべてからの計測が終了した場合には、ステップS904へ進む。これにより、物体を多方向から複数回計測し、計測精度を向上させる。
【0120】
ステップS904において、複数の視点位置において取得された点群データを一つの点群データに統合する(非特許文献2参照)。さらに、統合された点群データをメッシュモデルに変換する(非特許文献3参照)。生成されたメッシュモデルから、三次元線分を抽出する。ここでは、一定値以上の角度をなすメッシュの交線を三次元線分として抽出する。
【0121】
ステップS905において、予め定められていた物体すべての計測が終了していなかった場合には、ステップS906に進む。
【0122】
ステップS906において、計測対象物体を変更する。ここでいう物体は、同種の物体であり、個体ごとのばらつき情報を得ることを目的とし、物体の変更を行う。物体の変更後、ステップS901へ戻り処理を続ける。ステップS905において、予め決められた物体すべての計測が終了した場合には、ステップS907へ進む。
【0123】
ステップS907において、物体ごとに計測された結果から得られる三次元線分の標準モデルに対するばらつきを算出する。ここでは、個別のメッシュモデルからは同一の三次元線分が抽出されるものとする。標準モデルに対するばらつきは、第1実施形態、第2実施形態と同様に、三次元線分を一定間隔で分割したときの分割点ごとに算出する。
【0124】
以上述べたように、第3の実施形態では、標準モデルに対する実物体の形状のばらつきを、実物体を計測することで求める方法について説明した。これにより、あらかじめ公差データとしてばらつきの情報が得られない場合であっても、実計測によってばらつきを推定し、その結果を用いて高精度な位置姿勢の推定を行うことができる。
【0125】
〔変形例1〕
第1、第2及び第3の実施形態では、物体の形状が線分の集合として記述される場合の方法について説明したが、これに限らず別の特徴によって記述されてもよい。例えば、点の集合として記述してもよい。物体を表現する各点について、第1の実施形態や第2の実施形態で述べた方法と同様にして標準モデルに対するばらつきを算出し、物体の位置姿勢を推定する際の重みとして利用する。ここでは第1の実施形態と同様に、各点について、スカラ値の重みhiが算出されているものとする。物体の概略の位置姿勢が与えられているとすると、計測データに対し次のような観測方程式が立式される。
【0126】
【数27】
【0127】
ここでu’i、v’iは画像上での点の検出位置、ui、viは概略の位置姿勢によって算出される画像上での点の位置である。すべての点について立式される式(22)の両辺に重みhiを乗じた式を位置姿勢の補正値Δsiについての連立方程式とすることにより式(23)が得られる。
【0128】
【数28】
【0129】
式(23)を解いて位置姿勢の補正値Δsiを算出し、概略の位置姿勢を補正する処理を繰り返すことにより、最終的な物体の位置姿勢が算出される。
【0130】
〔変形例2〕
第1、第2及び第3の実施形態では、計測データをカメラから得られる二次元画像として説明したが、計測データを距離画像撮像装置から得られる三次元点群データとしてもよい。以下では、計測データを三次元点群とした場合の処理について説明する。
【0131】
まず、ステップS201で実施するずれの算出において、公差データを用いて求める方法について、図10及び11を用いて説明する。ここでは、物体の標準モデルは面の集合として記述されているものとして、ずれの統計量を面ごとに定める。
【0132】
ステップS1101において、注目する面(以下、注目面)のずれ要因公差を抽出する。ずれ要因公差は、注目面が含まれる稜線や面に対する公差とする。標準モデル400から抽出される面1201(図12)を注目面とした場合、面1201が含まれる稜線や面に対する公差となる寸法公差402〜404及び平面公差405〜407から、位置姿勢推定に寄与しない面の法線に対して直交する方向に誤差を与える寸法公差402、403及び平面公差405、407を除いたもの、つまり寸法公差404及び平面公差406がずれ要因公差となる。
【0133】
ステップS1102において、ステップS1102で抽出した各ずれ要因公差が注目面に与えるずれ量の最大値(ずれ候補量)をそれぞれ求める。ずれ量は、標準モデルに対する面の重心のずれとする。
【0134】
ステップS1103において、現在注目している面の重心の標準モデルに対するずれ候補量のうち、最大となるものを、この面のずれの統計量として抽出する。
【0135】
ステップS1104において、すべての面に対する処理が終了していると判断される場合には処理を終了し、そうでなければステップS1105において、注目面を変更してステップS1102に戻る。
【0136】
次に、ステップS202で実施する信頼度の算出方法について説明する。ここでは、前述した各面のずれの統計量について、式(1)に基づいて信頼度を求める。
【0137】
最後に、ステップS203で実施する位置姿勢の算出方法について説明する。算出処理は、第1の実施形態において説明した方法とほぼ同様であるため、以下では異なる部分についてのみ補足的に説明する。
【0138】
ステップS701では、距離画像撮像装置が撮像する距離画像から、三次元点群データを抽出する。
【0139】
ステップS703で実施される対応付けについて説明する。図12で示すように、計測点群の各点から法線方向に対応する面を探索する。標準モデルは、概略の位置姿勢に基づいて距離画像撮像装置の座標系に変換されているものとして、探索ラインと交わる面を対応面とする。
【0140】
ステップS704で実施される係数行列及び誤差ベクトルの算出方法について説明する。物体座標系における標準モデルから抽出される面の方程式をax+by+cz=kとする。物体の位置姿勢補正量をΔsとし、この補正によって点群データの各点が対応する標準モデルの面上にのると仮定すると、式(24)が得られる。
【0141】
【数29】
【0142】
ただし、
l=ax0+by0+cz0(定数)
式(24)の両辺に制御点が含まれる面の信頼度hiを乗じ、すべての分割点に対して一連の処理を行うことで、連立方程式(式(25))が得られる。
【0143】
【数30】
【0144】
ステップS705において、式(25)を解いて位置姿勢の補正値Δsiを算出し、概略の位置姿勢を補正する。
【0145】
本変形例では物体の標準モデルが面の集合として記述されている場合の方法について説明したが、他の特徴によって記述されていてもよい。例えば、標準モデルが点群データとして記述されていてもよい。
【0146】
〔変形例3〕
これまで説明した実施形態、変形例では、物体の概略の位置姿勢が与えられるとして、概略の位置姿勢を繰り返し補正することにより最終的な位置姿勢の算出を行っていた。しかしながら、位置姿勢の算出方法はこれに限るものではなく、概略の位置姿勢が未知の場合に、直接的に位置姿勢を算出してもよい。例えば、計測データが二次元画像上の特徴点である場合には、DLT(Direct Linear Transform)法によって直接的に位置姿勢を算出することが一般的に行われる(非特許文献4参照)。DLT法は、物体の位置を表す3変数と姿勢を表す回転行列の各要素である9変数を線形連立方程式を解くことによって算出する方法であり、未知変数は12個である。各点について2本の方程式が成り立つため、少なくとも6個の計測データが必要となる。この連立方程式を解く際に、第1の実施形態で説明したように各方程式に標準モデルからのずれに基づく重みを乗じることにより、工作精度の高い部分の情報を優先的に使うことができる。
【0147】
位置姿勢を直接的に算出する方法はこれに限るものではなく、三次元の点の対応をもとに上述の12変数を算出してもよい。また、特徴は点に限るものではなく、線分の対応をもとに算出してもよい(非特許文献5参照)。
【0148】
〔変形例4〕
これまで説明した実施形態、変形例では、標準モデルのずれ量を、線分等間隔に分割した点の距離または面の重心間の距離としたが、これに限らず他のものであってもよい。線分を特徴として扱う場合には、線分間の距離をずれ量としてもよい。線分間の距離は、線分の端点間の距離及び、1方の線分の端点とこの端点から下ろした法線ともう1方の線分との交点の距離のうち最小となる距離とする。また、面を特徴として扱う場合には、平面度をずれ量としてもよい。平面度は、面の歪の最大値(山)と最小値(谷)の高低差とする。ずれ量算出のフローは、図3または図9を用いて説明したものと同様であり、特徴及びずれ量を、線分及び線分間の距離または面及び平面度に置き換えることで実現される。
【0149】
〔その他の実施形態〕
図13は、本願の実施形態を実現するためのコンピュータの構成例である。
【0150】
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア(プログラム)を、ネットワーク1307又は各種記憶媒体1302,1303を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ1301(またはCPUやMPU等)がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
【技術分野】
【0001】
本発明は、三次元形状が既知である物体の位置姿勢を推定する技術に関する。
【背景技術】
【0002】
近年のロボット技術の発展とともに、工業製品の組立のようなこれまで人間が行っていた複雑なタスクをロボットが代わりに行うようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組立を行う。ロボットが部品を把持するためには、把持の対象となる部品とロボット(ハンド)との間の相対的な位置姿勢を高精度に推定する必要がある。このような位置姿勢の推定は、ロボットが部品を把持する場合だけでなく、ロボットが自律移動するための自己位置推定や、拡張現実感における現実空間と仮想物体の位置合わせなど様々な目的に応用される。
【0003】
位置姿勢を推定する方法として、カメラが撮像する二次元画像や距離センサから得られる距離画像を用いて行う方法がある。その中でも、撮像画像から抽出される画像特徴や距離画像から得られる点群データに対して物体の三次元形状モデルをあてはめるモデルフィッティングによる推定が一般的である。例えば、濃淡画像上で検出されるエッジに物体のワイヤフレームモデルの投影像があてはまるように物体の位置姿勢を推定する手法がある。また、距離画像から得られる点群データに対してメッシュモデルなどの三次元形状モデルをあてはめることにより物体の位置姿勢を推定する手法がある。
【0004】
工作精度やコストの問題から、大量生産される工業部品の形状やサイズには一般的にばらつきがある。このような部品について個体ごとに三次元形状モデルを作ることは現実的ではないため、通常は部品の標準的な形状を表す1つの三次元形状モデルを用いて位置姿勢の推定する。すなわち、三次元形状モデルと実部品の形状は必ずしも一致しない。この部品に対してモデルフィッティングによる位置姿勢を行う場合、モデルと実部品の形状の差が大きい場合、高精度な推定ができないという課題がある。
【0005】
特許文献1には、モデルを利用して物体の位置認識を行う際に物体の形状のばらつきを吸収する方法が開示されている。この方法では、物体を表す標準モデルと画像(計測データ)に基づいて物体の位置認識を行い、計測データを統計学的に処理し、標準モデルを逐次更新する。
【0006】
一方、非特許文献1には、顔の位置姿勢を推定する際に、顔の三次元モデルに対する個人差を吸収する方法が開示されている。この方法は、個体ごとの特徴点のずれを、予め求めておいたずれの分布と実際の計測データから求め、このずれを付与した特徴点に対してモデルフィッティングを行う。個体ごとにモデル生成を行うことで、形状のばらつきに左右されることなく位置姿勢を推定できる。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開平9−81744
【非特許文献】
【0008】
【非特許文献1】X.Chai,Shan,L.Qing,and W.Gao,”Pose estimation based on Gaussian error models,”International Conference on Biometrics(ICB 2006),LNCS 3832,pp.136−143,2005.
【非特許文献2】Y.Chen and G.Medioni,”Object modeling by registration of multiple range images,”Proc.1991 International Conference on Robotics and Automation(ICRA’91),pp.2724−2729,1991.
【非特許文献3】M.D.Wheeler,Y.Sato,and K.Ikeuchi,”Consensus surfaces for modeling 3D objects from multiple range images,”Proc.6th International Conference on Computer Vision(ICCV’98),pp.917−924,1998.
【非特許文献4】R.Hartley and A.Zisserman,”Multiple View Geometry in Computer Vision”,Cambridge University Press,2000.
【非特許文献5】Y.Liu,T.S.Huang,and O.D.Faugeras,”Determination of Camera Location from 2−D to 3−D Line and Point Correspondences,”IEEE Transactions on Pattern Analysis and Machine Intelligence,vol.12,no.1,pp.28−37,1990.
【発明の概要】
【発明が解決しようとする課題】
【0009】
特許文献1には、物体の形状のばらつきを吸収し、物体の認識率を向上させる。しかし、この方法は、実物体のばらつきを吸収するように標準モデルを更新するため、物体の認識や概略の位置姿勢推定には適しているが、高精度な位置姿勢推定には適さないという課題があった。
【0010】
非特許文献1には、この方法はずれを明示的に算出する方法であり、計測データからの特徴の誤検出などによって求めたずれ自体に誤差を含む可能性があるため、高精度な位置姿勢推定には適さないという課題があった。
【0011】
上記事情に鑑み、本発明では、標準的な形状の三次元形状モデルに比して、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供することを目的とする。
【課題を解決するための手段】
【0012】
上記の目的は以下の方法によって達成される。
【0013】
物体の位置姿勢を求める位置姿勢推定方法であって、
統計量算出手段が、予め保持された前記物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出工程と、
信頼度算出手段が、前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出工程と、
画像特徴抽出手段が、撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出工程と、
対応付け手段が、前記幾何特徴と前記画像特徴とを対応付けする対応付け工程と、
ずれ量算出手段が、前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出工程と、
位置姿勢推定手段が、前記信頼度及び前記ずれ量に基づいて、前記物体の位置姿勢を推定する位置姿勢推定工程と
を有することを特徴とする位置姿勢推定方法。
【発明の効果】
【0014】
本発明によれば、形状のばらつきを持つ物体であっても、当該物体の位置姿勢を高精度に推定する方法を提供することができる。
【図面の簡単な説明】
【0015】
【図1】第1の実施形態における構成例である。
【図2】第1の実施形態における処理のフローチャートである。
【図3】公差データから形状のずれを算出する処理の例を説明するフローチャート(1)である。
【図4】標準モデル及び公差データの例である。
【図5】ずれ候補量の模式図である。
【図6】抽出された線分及び信頼度データベースの例である。
【図7】物体の位置姿勢算出処理の例を説明するフローチャートである。
【図8】対応付けの例を説明する模式図(1)である。
【図9】計測データからずれを算出する処理の例を説明するフローチャートである。
【図10】対象物体と計測位置の関係の例を説明する図である。
【図11】公差データから形状のずれを算出する処理の例を説明するフローチャート(2)である。
【図12】対応付けを説明する模式図(2)である。
【図13】本願の実施形態を実現するためのコンピュータの構成例である。
【発明を実施するための形態】
【0016】
〔第1の実施形態〕
本実施形態では、物体の標準的な形状を表す三次元形状モデル(以下、標準モデルと呼ぶ)と実物体とのずれが一様ではないことを利用して高精度な物体の位置姿勢を推定する。すなわち、ずれが小さいと期待できる部分の情報を積極的に用いることで、高精度な位置姿勢の推定を行う。本実施形態では、物体は工業用部品であると想定し、物体の公差データをもとに、物体の標準モデルを構成する各特徴のずれの統計量を推定し、このずれの統計量をもとに、算出される特徴ごとの信頼度を用いて、物体の位置姿勢を推定する。
【0017】
図1を用いて、本実施形態における位置姿勢推定装置100の構成について説明する。図中矢印は、データの流れを示している。位置姿勢推定装置100は、計測データ取得部101、特徴抽出部102、蓄積部103、位置姿勢算出部104で構成される。位置姿勢推定装置100は、計測装置105に接続されている。
【0018】
計測装置105は、現実空間を計測する。例えば、カメラで二次元画像を撮像する。例えば、物体の表面の三次元位置を計測し、距離画像を撮像する。撮像された撮像画像は、計測データ取得部101へ送られる。
【0019】
計測データ取得部101は、計測装置105から計測データを取得する。例えば、計測データは、カメラによって撮像された二次元画像を取得する。取得した計測データは、特徴抽出部102へ送られる。
【0020】
特徴抽出部102は、計測データから特徴を抽出する。例えば、Cannyオペレータを用いて、画像からのエッジを特徴として抽出する。抽出した特徴は、位置姿勢算出部104へ送られる。
【0021】
蓄積部103には、物体の標準モデルを構成する各特徴のずれの統計量が保持されている。例えば、物体の標準モデルは、線分の集合によって記述される。線分は、物体表面を表す面と面の交線や、色や濃度が大きく変わる境界線である。しかしながら、物体の標準モデルはこれに限るものではなく、物体上の特徴的な点の集合や、物体を表す面の集合、またはそれらの組み合わせによって記述してもよい。保持されている物体の標準モデルは、位置姿勢算出部104へ送られる。
【0022】
位置姿勢算出部104は、特徴抽出部102で計測データから抽出された特徴と、蓄積部103に蓄積されている物体の標準モデル、物体の各特徴の標準モデルに対するずれの統計量を用いて物体の位置姿勢を推定する。
【0023】
図2を用いて、本実施形態における位置姿勢推定装置の全体処理フローの概要について説明する。
【0024】
ステップS201は、統計量算出ステップである。本ステップでは、物体の標準モデルを構成する幾何特徴のずれの統計量を算出する。前述したように、本実施形態では物体の標準モデルは線分の集合によって記述されている。ここでは、幾何特徴を線分として、各線分を三次元的に一定間隔で分割したときの分割点の公差にもとづくずれの統計量を算出する。分割点を幾何特徴として用いるのは、後述する位置姿勢の推定工程において、分割点単位処理を行うためである。
【0025】
ステップS202は、信頼度算出ステップである。本ステップでは、ステップS201で求めた各分割点のずれの統計量から、幾何特徴である分割点の信頼度を算出する。
【0026】
ステップS203は、位置姿勢算出ステップである。本ステップでは、ステップS202で算出した信頼度を用いて、物体の位置姿勢を算出する。
【0027】
なお、ステップS201及びステップS202は必ずしも位置姿勢の推定とともに行う必要はなく、ステップS203で実施される位置姿勢推定の前処理として、別途オフラインで実施してもよい。
【0028】
以下では、ステップS201で実施する分割点に対するずれの統計量の算出処理について詳しく説明する。ずれの統計量の算出結果は蓄積部103に保持される。ここでは、ずれの統計量を物体の公差データから算出する方法について、図3のフローチャートを用いて説明する。
【0029】
ステップS301は、モデル特徴抽出ステップである。本ステップでは、標準モデルから線分データを抽出する。
【0030】
ステップS302は、公差抽出ステップである。本ステップでは、注目する線分(以下、注目線分)のずれの要因となる公差(以下、ずれ要因公差)を抽出する。ずれ要因公差は、注目線分が含まれる稜線や面に対する公差とする。
【0031】
図4は、標準モデル及び公差データの一例である。標準モデル400から抽出される線分401を注目線分とした場合について説明する。
【0032】
線分401が含まれる稜線や面に対する公差である寸法公差402〜404及び平面公差405〜407がずれ要因公差の候補となる。これらの候補のうち、位置姿勢推定に寄与しない寸法公差402、平面公差405を除いた寸法公差403、404及び平面公差406、407がずれ要因公差となる。
【0033】
ステップS303は、公差値算出ステップである。本ステップでは、注目線分を三次元的に所定間隔に分割し、各分割点について、ステップS302で抽出した各ずれ要因公差によって生じるずれ量の最大値(以下、ずれ候補量)をそれぞれ求める。ずれ候補量は、標準モデル上に設定されたモデル座標系503の成分ごとの大きさで表される。
【0034】
図5は、注目線分を線分401としたときに、線分401のある分割点501に対し寸法公差404が与えるずれ候補量502を模式的に表した図である。この例では、分割点501のずれ候補量502はx成分のみとなる。この処理を、分割点すべてについて実施する。
【0035】
ステップS304は、統計量決定ステップである。本ステップでは、ステップS303で算出された各分割点のずれ候補量群の成分ごとの最大値を持つベクトルを生成し、このベクトルの大きさdを分割点におけるずれの統計量とする。これは、すべての許容公差を満たし、かつその中で最大となるずれ量を求めたこととなる。
【0036】
例えば、分割点501のずれ候補量が3つ存在した場合について説明する。各ずれ候補量を、(x成分,y成分,z成分)=(dx1,dy1,dz1)、(dx2,dy2,dz2)、(dx3,dy3,dz3)とし、それぞれdx1>dx2>dx3、dy3>dy2>dy1、dz2>dz3>dz1といった大小関係があるとする。この場合、成分ごとの最大値は、(dx1,dy3,dz2)となり、この大きさdは、
【0037】
【数1】
【0038】
となり、このdを分割点501におけるずれの統計量とする。
【0039】
ステップS305では、ステップS301で抽出した線分全てに対してステップS302〜304の処理が実施済みの場合には本処理を終了し、実施済みでない場合には、ステップS306に進み注目線分を変更し、ステップS302へ戻る。
【0040】
以下では、ステップS202で実施する分割点ごとの信頼度の算出方法について詳細に説明する。
【0041】
信頼度は、ずれの統計量が小さいものほど高く設定する。すなわち、工作精度が高い部分の信頼度は大きくし、低い部分の信頼度は小さくする。例えば、式(1)のようにずれの統計量が小さいものほど信頼度が高くなる関数を定義する。
【0042】
【数2】
【0043】
dは前述した分割点のずれの統計量であり、c1は定数である。信頼度を与える関数は上記関数に限らず、ずれ量が小さいものほど信頼度が高くなる他の関数であればいかなる関数であってもよい。算出された信頼度は、線分ごとに標準モデルから抽出された分割点の三次元位置と共に蓄積部103にデータベース(図6)として保持される。
【0044】
以下では、ステップS203で行う物体の位置姿勢算出処理について、図7のフローチャートを用いて説明する。
【0045】
ステップS701は、画像特徴抽出ステップである。本ステップでは、特徴抽出部102において撮像画像から画像特徴であるエッジを抽出する。
【0046】
ステップS702は、概略値入力ステップである。本ステップでは、標準モデルの位置姿勢の概略値を入力する。
【0047】
ステップS703は、対応付けステップである。本ステップでは、蓄積部103に保持されている分割点とステップS701で抽出したエッジとの対応付けを行う。図8は、対応付けの様子を模式的に表した図である。ここでは、入力された位置姿勢の概略値を用いて標準モデル中の線分801とその分割点802を画像上に投影し、分割点ごとに投影された線分の法線方向に一定範囲内でエッジ探索を行い、最も近接するエッジを該分割点との対応点803とする。
【0048】
ステップS704は、位置姿勢補正値算出ステップである。本ステップでは、各分割点802における信頼度を用いて、位置姿勢算出部104において位置姿勢を算出するための係数行列及び誤差ベクトルを算出する。ある分割点の画像での投影位置を(ui,0,vi,0)とし、この分割点802の対応点の画像上での検出位置を(ui’,vi’)とする。分割点(ui,0,vi,0)を含む標準モデル上の線分801(以下、モデル線分)の投影像の画像上での傾き804をθとする。該対応点803を通り、傾き804がθである直線805の方程式は式(2)となる。
【0049】
【数3】
【0050】
が表せる(θは定数とする)。ここで、
【0051】
【数4】
【0052】
(定数)である。
【0053】
分割点802の画像上での投影位置と物体の位置姿勢の関係を
【0054】
【数5】
【0055】
のように表す。物体の位置姿勢の微小変化をΔsとすると、式(3)は式(4)のように一次近似できる。
【0056】
【数6】
【0057】
物体の位置姿勢をΔsだけ変化させたときに、式(4)で表される分割点の投影位置が式(2)で表される直線上にのると仮定すると、式(5)で表されるΔsに関する方程式が得られる。
【0058】
【数7】
【0059】
ただし、
【0060】
【数8】
【0061】
(定数)である。標準モデル中の線分801の各分割点802について成り立つ式(5)を連立方程式として解くことにより、物体の位置姿勢の補正値Δsが得られる。位置姿勢に基づく剛体変換と画像上への透視投影変換は非線形の変換であるため、補正値Δsの算出とsの補正を繰り返すことにより、最終的な物体の位置姿勢が得られる。
【0062】
本実施形態では、さらに式(5)の両辺に分割点の信頼度hiを重みとして乗じることにより、工作精度の高い分割点の寄与度を高く、工作精度の低い分割点の寄与度を低くして位置姿勢の補正量Δsを算出する。すなわち、式(6)で表される連立方程式を解くことにより、位置姿勢の補正量Δsを算出する。
【0063】
【数9】
【0064】
ここで式(6)を式(7)のように表す。
【0065】
【数10】
【0066】
式(7)を解いて得られるΔsを用いて概略の位置姿勢を補正することで、物体の位置姿勢が算出する。
【0067】
ステップS705において、行列Jの一般化逆行列(JT・J)−1・JTを式(7)の両辺に乗じることで位置姿勢補正量Δsを求める。
【0068】
【数11】
【0069】
ステップS706は、位置姿勢補正ステップである。本ステップでは、算出した位置姿勢の補正量Δsにより、位置姿勢の概略値を補正する。
【0070】
【数12】
【0071】
ステップS707において、収束判定を行う。収束していれば、処理を終了し、そうでなければステップS701に戻る。収束判定では、補正値Δsがほぼ0である場合や、誤差ベクトルRの2乗和が補正前と補正後でほとんど変わらない場合に収束したと判定する。
【0072】
以上述べたように、第1の実施形態では、標準モデルを構成する幾何特徴のずれの統計量を公差データから求め、ずれの統計量が小さい特徴ほど高い重みを付与して物体の位置姿勢を求める方法について説明した。信頼度の高い特徴を積極的に用いることにより、高精度な位置姿勢推定が可能となる。
【0073】
〔第2の実施形態〕
第1の実施形態では、物体を構成する幾何特徴の標準モデルに対するずれの統計量を、公差から算出される最大のずれに基づいて特定の値として算出した。しかしながら、物体のずれは統計的に発生するのが一般的であり、最大のずれに基づいて信頼度を決定することによって予想以上に信頼度が低くなる特徴が発生する。一方、実際の公差は、一般的に標準値を中心とした正規分布となる。このため、公差は標準に対する差の値ではなく、正規分布のように確率分布によって表される場合もある。本実施形態では、標準モデルを構成する特徴のずれが確率分布によって表される場合の位置姿勢の推定方法について説明する。
【0074】
全体の処理フローは、第1の実施形態にて図2を用いて説明したものと同様である。以下では、ステップS201、S202及びS203で実施される各処理について説明する。
【0075】
まず、ステップS201で実施される標準モデルに対するずれの算出処理について、図3を用いて説明する。なお、ステップS304以外の処理は、第1の実施形態で説明した内容と同様であるため、ここではステップS304の処理についてのみ説明する。
【0076】
ステップS304において、ステップS303で算出された各分割点のずれ候補量群から成分ごとのずれの分布を求める。各分割点のずれの分布は成分間で独立であり、成分ごとに平均0、標準偏差がずれ候補量群の最大値の3分の1である1次元の正規分布に従うとする。例として、分割点501のずれ候補量が3つ存在した場合について説明する。各ずれ候補量を、(x成分,y成分,z成分)=(dx1,dy1,dz1)、(dx2,dy2,dz2)、(dx3,dy3,dz3)とし、それぞれdx1>dx2>dx3、dy3>dy2>dy1、dz2>dz3>dz1といった大小関係があるとする。この場合、各成分の最大値は(dx1,dy3,dz2)となり、ずれの標準偏差は(σx,σy,σz)=(dx1/3,dy3/3dz2/3)となる。これより、成分ごとのずれの確率分布は、
【0077】
【数13】
【0078】
となる。
【0079】
次に、ステップS202及びS203で実施される信頼度の算出処理及び位置姿勢推定処理について説明する。まず、標準モデル及び計測データの双方に統計的な誤差が含まれる場合の最尤推定法について説明し、次に第1の実施形態と同様に幾何特徴が線分の分割点とした場合について具体的に説明する。
【0080】
以下では、標準モデル及び計測データに統計的な誤差が含まれる場合の最尤推定法ついて説明する。
【0081】
まず、計測データの適合具合を示す尤度関数を算出する。計測データの特徴位置を表すベクトルをpi、piに対応する標準モデルの特徴位置を表すベクトルをqi、物体の位置姿勢を表す6次元ベクトルをsとする。計測データの特徴位置をpi、標準モデルの特徴位置qi及び物体の位置姿勢sには、式(9)の関係があるものとする。関数gは、三次元空間から二次元画像面への変換である透視投影変換や、モデル座標系からカメラ座標系への変換であるモデルビュー変換などを含む。
【0082】
【数14】
【0083】
標準モデルのずれをΔqi、計測誤差をΔpiとすると、計測データ空間での標準モデルの特徴と計測データの特徴との誤差eiは、式(10)のように線形近似される。
【0084】
【数15】
【0085】
Δqi、Δpiが統計的な誤差であるとすると、誤差eiには、平均が0で分散が、
【0086】
【数16】
【0087】
である統計的誤差が含まれる。Δqi、Δpiが正規分布に従うとするとeiに含まれる統計的誤差も正規分布に従うため、誤差eiの尤度関数は式(12)となる。
【0088】
【数17】
【0089】
最尤推定では、式(13)で表される尤度の積が最大となるように物体の位置姿勢を推定する。
【0090】
【数18】
【0091】
実際には、尤度の積の対数を取り、その符号を反転した
【0092】
【数19】
【0093】
を最小化する問題に置き換えられる。このうち第一項は位置姿勢に依存しない定数であるため、最終的には、
【0094】
【数20】
【0095】
を最小化する問題となる。式(15)は、分散Σiの逆行列を信頼度として重みつけした誤差ベクトルのノルムの2乗和である。
【0096】
以下では、幾何特徴を線分の分割点とした場合について説明する。
【0097】
式(9)の関数gは、分割点を画像面に投影する関数となり、式(16)で表される。
【0098】
【数21】
【0099】
ただし、
【0100】
【数22】
【0101】
及び、
【0102】
【数23】
【0103】
とする。fはカメラの焦点距離である。xc,yc,zcは、カメラ座標系における分割点の三次元位置、xw,yw,zwは世界座標系(物体座標系)における分割点の位置、行列R及びtは世界座標系からカメラ座標系への座標変換行列の回転成分及び平行移動成分である。
【0104】
誤差eiは、分割点の投影位置と対応点を通過する直線との距離(スカラ値)で表される。対応点を通過する直線の傾きθiには誤差はないものと仮定すると、誤差eiは式(19)となる。
【0105】
【数24】
【0106】
誤差eiがスカラ値であるため,eiの共分散行列Σi(式(11))もスカラとなる。ステップS202で信頼度をΣiの逆数として計算し、ステップS203で第1の実施形態で図7を用いて説明したフローを実施することで物体の位置姿勢を求める。なお、ステップS704で算出される係数拡大行列及び誤差ベクトルは、式(6)の重みhを式(18)で計算されるΣiの逆数に置き換えたものとなる。
【0107】
以下では、幾何特徴を線分にしたときの共分散行列Σiの算出方法について具体的に説明する。
【0108】
共分散行列Σi(式(11))の右辺第1項Σpiは、画像から検出される線分の検出誤差の共分散行列である。線分の検出誤差については、探索ラインの方向にのみあいまい性を持つと仮定し、線分の検出誤差が平均0、標準偏差σの1次元ガウス分布に従うと仮定する。実画像におけるσを推定するのは困難である。そのため、線分の検出誤差は主に画像の量子化誤差に起因するとして、例えばσを1画素に設定する。すなわち、Σpi は1となる。また、共分散行列Σi(式(11))の右辺第2項はステップS201で求めた標準モデルのずれの共分散行列Σqiの両辺にヤコビアン∂g/∂qiを乗じたものである。標準モデルのずれの共分散行列Σqiは、成分間に相関がないとして、ステップS201で求めたずれの標準偏差を(σx,σy,σz)とすると、
【0109】
【数25】
【0110】
となる。また、ヤコビアン∂g/∂qiは、式(16)、(17)及び(18)より、
【0111】
【数26】
【0112】
となる。式(20)及び式(21)より共分散行列Σi(式(11))の右辺第2項が求まる。
【0113】
以上述べたように、第2の実施形態では、標準モデルから抽出される特徴のずれが確率分布によって表される場合に、確率分布から算出される信頼度をもとに位置姿勢を求める方法について説明した。
【0114】
〔第3の実施形態〕
第1及び第2の実施形態では、ステップS201で実施される標準モデルに対する特徴のずれの算出を公差データから求める方法について説明したが、その他の方法であっても良い。例えば、実際に同種の多数の物体を計測して形状のばらつきを求めてもよい。本実施形態では、距離撮像装置による三次元形状計測を複数の物体に対して行い、各物体に対する三次元計測結果のばらつきをもとに標準モデルのずれを求める。物体全体の形状を計測するために、各サンプルに対して複数の視点から三次元計測を行い、その結果を統合して、線分によって記述される一つの標準モデルを生成する。このようにして生成された複数の標準モデルから、標準モデルに対するずれを算出する。
【0115】
また、ステップS201の前に、標準モデルを得るために物体の計測を行う物体計測ステップを実行してもよい。そして、物体の標準モデルはあらかじめ用意されているとするが、三次元計測結果から得られる複数の標準モデルをもとに生成してもよい。
【0116】
図9のフローチャートを用いて、実際に物体を計測することによる標準モデルのずれを求める方法について説明する。
【0117】
ステップS901において、ある計測位置から距離撮像装置によって物体の計測データを取得する。ここで物体の計測データとは、物体表面上の点群データであり、各点は距離撮像装置の座標系における三次元位置によって表される。
【0118】
ステップS902において、予め決めておいた計測位置すべてからの計測が終了していない場合には、ステップS903に進む。
【0119】
ステップS903において、計測位置を移動する。図10は、計測位置を模式的に表した図である。物体1001を原点とした球面上を適当な刻み幅で計測装置1002を移動させ、計測位置を移動させる。計測位置を移動した後、ステップS901へ戻り処理を続ける。予め決めておいた計測位置すべてからの計測が終了した場合には、ステップS904へ進む。これにより、物体を多方向から複数回計測し、計測精度を向上させる。
【0120】
ステップS904において、複数の視点位置において取得された点群データを一つの点群データに統合する(非特許文献2参照)。さらに、統合された点群データをメッシュモデルに変換する(非特許文献3参照)。生成されたメッシュモデルから、三次元線分を抽出する。ここでは、一定値以上の角度をなすメッシュの交線を三次元線分として抽出する。
【0121】
ステップS905において、予め定められていた物体すべての計測が終了していなかった場合には、ステップS906に進む。
【0122】
ステップS906において、計測対象物体を変更する。ここでいう物体は、同種の物体であり、個体ごとのばらつき情報を得ることを目的とし、物体の変更を行う。物体の変更後、ステップS901へ戻り処理を続ける。ステップS905において、予め決められた物体すべての計測が終了した場合には、ステップS907へ進む。
【0123】
ステップS907において、物体ごとに計測された結果から得られる三次元線分の標準モデルに対するばらつきを算出する。ここでは、個別のメッシュモデルからは同一の三次元線分が抽出されるものとする。標準モデルに対するばらつきは、第1実施形態、第2実施形態と同様に、三次元線分を一定間隔で分割したときの分割点ごとに算出する。
【0124】
以上述べたように、第3の実施形態では、標準モデルに対する実物体の形状のばらつきを、実物体を計測することで求める方法について説明した。これにより、あらかじめ公差データとしてばらつきの情報が得られない場合であっても、実計測によってばらつきを推定し、その結果を用いて高精度な位置姿勢の推定を行うことができる。
【0125】
〔変形例1〕
第1、第2及び第3の実施形態では、物体の形状が線分の集合として記述される場合の方法について説明したが、これに限らず別の特徴によって記述されてもよい。例えば、点の集合として記述してもよい。物体を表現する各点について、第1の実施形態や第2の実施形態で述べた方法と同様にして標準モデルに対するばらつきを算出し、物体の位置姿勢を推定する際の重みとして利用する。ここでは第1の実施形態と同様に、各点について、スカラ値の重みhiが算出されているものとする。物体の概略の位置姿勢が与えられているとすると、計測データに対し次のような観測方程式が立式される。
【0126】
【数27】
【0127】
ここでu’i、v’iは画像上での点の検出位置、ui、viは概略の位置姿勢によって算出される画像上での点の位置である。すべての点について立式される式(22)の両辺に重みhiを乗じた式を位置姿勢の補正値Δsiについての連立方程式とすることにより式(23)が得られる。
【0128】
【数28】
【0129】
式(23)を解いて位置姿勢の補正値Δsiを算出し、概略の位置姿勢を補正する処理を繰り返すことにより、最終的な物体の位置姿勢が算出される。
【0130】
〔変形例2〕
第1、第2及び第3の実施形態では、計測データをカメラから得られる二次元画像として説明したが、計測データを距離画像撮像装置から得られる三次元点群データとしてもよい。以下では、計測データを三次元点群とした場合の処理について説明する。
【0131】
まず、ステップS201で実施するずれの算出において、公差データを用いて求める方法について、図10及び11を用いて説明する。ここでは、物体の標準モデルは面の集合として記述されているものとして、ずれの統計量を面ごとに定める。
【0132】
ステップS1101において、注目する面(以下、注目面)のずれ要因公差を抽出する。ずれ要因公差は、注目面が含まれる稜線や面に対する公差とする。標準モデル400から抽出される面1201(図12)を注目面とした場合、面1201が含まれる稜線や面に対する公差となる寸法公差402〜404及び平面公差405〜407から、位置姿勢推定に寄与しない面の法線に対して直交する方向に誤差を与える寸法公差402、403及び平面公差405、407を除いたもの、つまり寸法公差404及び平面公差406がずれ要因公差となる。
【0133】
ステップS1102において、ステップS1102で抽出した各ずれ要因公差が注目面に与えるずれ量の最大値(ずれ候補量)をそれぞれ求める。ずれ量は、標準モデルに対する面の重心のずれとする。
【0134】
ステップS1103において、現在注目している面の重心の標準モデルに対するずれ候補量のうち、最大となるものを、この面のずれの統計量として抽出する。
【0135】
ステップS1104において、すべての面に対する処理が終了していると判断される場合には処理を終了し、そうでなければステップS1105において、注目面を変更してステップS1102に戻る。
【0136】
次に、ステップS202で実施する信頼度の算出方法について説明する。ここでは、前述した各面のずれの統計量について、式(1)に基づいて信頼度を求める。
【0137】
最後に、ステップS203で実施する位置姿勢の算出方法について説明する。算出処理は、第1の実施形態において説明した方法とほぼ同様であるため、以下では異なる部分についてのみ補足的に説明する。
【0138】
ステップS701では、距離画像撮像装置が撮像する距離画像から、三次元点群データを抽出する。
【0139】
ステップS703で実施される対応付けについて説明する。図12で示すように、計測点群の各点から法線方向に対応する面を探索する。標準モデルは、概略の位置姿勢に基づいて距離画像撮像装置の座標系に変換されているものとして、探索ラインと交わる面を対応面とする。
【0140】
ステップS704で実施される係数行列及び誤差ベクトルの算出方法について説明する。物体座標系における標準モデルから抽出される面の方程式をax+by+cz=kとする。物体の位置姿勢補正量をΔsとし、この補正によって点群データの各点が対応する標準モデルの面上にのると仮定すると、式(24)が得られる。
【0141】
【数29】
【0142】
ただし、
l=ax0+by0+cz0(定数)
式(24)の両辺に制御点が含まれる面の信頼度hiを乗じ、すべての分割点に対して一連の処理を行うことで、連立方程式(式(25))が得られる。
【0143】
【数30】
【0144】
ステップS705において、式(25)を解いて位置姿勢の補正値Δsiを算出し、概略の位置姿勢を補正する。
【0145】
本変形例では物体の標準モデルが面の集合として記述されている場合の方法について説明したが、他の特徴によって記述されていてもよい。例えば、標準モデルが点群データとして記述されていてもよい。
【0146】
〔変形例3〕
これまで説明した実施形態、変形例では、物体の概略の位置姿勢が与えられるとして、概略の位置姿勢を繰り返し補正することにより最終的な位置姿勢の算出を行っていた。しかしながら、位置姿勢の算出方法はこれに限るものではなく、概略の位置姿勢が未知の場合に、直接的に位置姿勢を算出してもよい。例えば、計測データが二次元画像上の特徴点である場合には、DLT(Direct Linear Transform)法によって直接的に位置姿勢を算出することが一般的に行われる(非特許文献4参照)。DLT法は、物体の位置を表す3変数と姿勢を表す回転行列の各要素である9変数を線形連立方程式を解くことによって算出する方法であり、未知変数は12個である。各点について2本の方程式が成り立つため、少なくとも6個の計測データが必要となる。この連立方程式を解く際に、第1の実施形態で説明したように各方程式に標準モデルからのずれに基づく重みを乗じることにより、工作精度の高い部分の情報を優先的に使うことができる。
【0147】
位置姿勢を直接的に算出する方法はこれに限るものではなく、三次元の点の対応をもとに上述の12変数を算出してもよい。また、特徴は点に限るものではなく、線分の対応をもとに算出してもよい(非特許文献5参照)。
【0148】
〔変形例4〕
これまで説明した実施形態、変形例では、標準モデルのずれ量を、線分等間隔に分割した点の距離または面の重心間の距離としたが、これに限らず他のものであってもよい。線分を特徴として扱う場合には、線分間の距離をずれ量としてもよい。線分間の距離は、線分の端点間の距離及び、1方の線分の端点とこの端点から下ろした法線ともう1方の線分との交点の距離のうち最小となる距離とする。また、面を特徴として扱う場合には、平面度をずれ量としてもよい。平面度は、面の歪の最大値(山)と最小値(谷)の高低差とする。ずれ量算出のフローは、図3または図9を用いて説明したものと同様であり、特徴及びずれ量を、線分及び線分間の距離または面及び平面度に置き換えることで実現される。
【0149】
〔その他の実施形態〕
図13は、本願の実施形態を実現するためのコンピュータの構成例である。
【0150】
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア(プログラム)を、ネットワーク1307又は各種記憶媒体1302,1303を介してシステム或いは装置に供給し、そのシステム或いは装置のコンピュータ1301(またはCPUやMPU等)がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータ読み取り可能な記録媒体に記録して提供してもよい。
【特許請求の範囲】
【請求項1】
物体の位置姿勢を求める位置姿勢推定方法であって、
統計量算出手段が、予め保持された前記物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出工程と、
信頼度算出手段が、前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出工程と、
画像特徴抽出手段が、撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出工程と、
対応付け手段が、前記幾何特徴と前記画像特徴とを対応付けする対応付け工程と、
ずれ量算出手段が、前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出工程と、
位置姿勢推定手段が、前記信頼度及び前記ずれ量に基づいて、前記物体の位置姿勢を推定する位置姿勢推定工程と
を有することを特徴とする位置姿勢推定方法。
【請求項2】
前記統計量算出工程では、
前記三次元形状モデルの公差データに基づいて、前記統計量を算出することを特徴とする請求項1に記載の位置姿勢推定方法。
【請求項3】
前記統計量算出工程では、
モデル特徴抽出手段が、前記三次元形状モデルを構成する線分を抽出するモデル特徴抽出工程と、
公差抽出手段が、前記線分のずれの要因となる公差を抽出する公差抽出工程と、
公差値算出手段が、前記線分の所定間隔ごとの前記公差の値を算出する公差値算出工程と、
統計量決定手段が、前記公差の値の最大値における公差のベクトルを生成し、前記ベクトルを統計量とする統計量決定工程と
を有することを特徴とする請求項2に記載の位置姿勢推定方法。
【請求項4】
前記統計量は、確率分布で表現されることを特徴とする請求項2に記載の位置姿勢推定方法。
【請求項5】
前記統計量算出工程では、
モデル特徴抽出手段が、前記三次元形状モデルを構成する線分を抽出するモデル特徴抽出工程と、
公差抽出手段が、前記線分のずれの要因となる公差を抽出する公差抽出工程と、
公差値算出手段が、前記線分の所定間隔ごとの前記公差の値を算出する公差値算出工程と、
統計量決定手段が、前記公差の値に基づく正規分布のベクトルを生成し、前記ベクトルを統計量とする統計量決定工程と
を有することを特徴とする請求項4に記載の位置姿勢推定方法。
【請求項6】
前記統計量算出工程では、
モデル特徴抽出手段が、前記三次元形状モデルを構成する面を抽出するモデル特徴抽出工程と、
公差値算出手段が、前記面の重心における前記公差の値を算出する公差値算出工程と、
統計量決定手段が、前記公差の値の最大値における公差のベクトルを生成し、前記ベクトルを統計量とする統計量決定工程と
を有することを特徴とする請求項2に記載の位置姿勢推定方法。
【請求項7】
物体計測手段が、前記物体の形状を構成する幾何特徴を計測する物体計測工程と
を更に有し、
前記統計量算出工程では、前記計測した結果と前記三次元形状モデルとのずれに基づいて、前記統計量を算出することを特徴とする請求項1に記載の位置姿勢推定方法。
【請求項8】
前記物体計測工程では、複数の物体ごとに複数回計測することを特徴とする請求項7に記載の位置姿勢推定方法。
【請求項9】
前記物体計測工程では、複数の計測位置ごとに複数回計測することを特徴とする請求項7又は8に記載の位置姿勢推定方法。
【請求項10】
前記幾何特徴が、点、若しくは、線分、若しくは、面であることを特徴とする請求項1乃至9の何れか1項に記載の位置姿勢推定方法。
【請求項11】
前記信頼度算出工程では、前記統計量が小さいほど信頼度を高く算出することを特徴とする請求項1乃至10の何れか1項に記載の位置姿勢推定方法。
【請求項12】
前記信頼度算出工程では、尤度関数を用いることにより、信頼度を算出することを特徴とする請求項1乃至11の何れか1項に記載の位置姿勢推定方法。
【請求項13】
概略値入力手段が、前記物体の位置姿勢の概略値を入力する概略値入力工程と、
を有し、
前記位置姿勢推定工程では、
位置姿勢補正値算出手段が、前記信頼度及び前記ずれ量及び前記概略値に基づいて、前記物体の位置姿勢の補正値を算出する位置姿勢補正値算出工程と、
位置姿勢補正手段が、前記補正値に基づいて、前記物体の位置姿勢の概略値を補正する位置姿勢補正工程と
を有することを特徴とする請求項1乃至12の何れか1項に記載の位置姿勢推定方法。
【請求項14】
請求項1乃至13の何れか1項に記載の位置姿勢推定方法をコンピュータに実行させることを特徴とするプログラム。
【請求項15】
物体の位置姿勢を求める位置姿勢推定装置であって、
前記物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出手段と、
前記ずれ統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出手段と、
撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出手段と、
前記幾何特徴と前記画像特徴とを対応付けする対応付け手段と、
前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出手段と、
前記信頼度及び前記ずれ量に基づいて、前記物体の位置姿勢を推定する位置姿勢推定手段と
を備えることを特徴とする位置姿勢推定装置。
【請求項16】
前記物体を撮像する撮像手段を更に備えることを特徴とする請求項15に記載の位置姿勢推定装置。
【請求項17】
前記撮像手段が、現実空間の距離画像を撮像画像として撮像することを特徴とする請求項16に記載の位置姿勢推定装置。
【請求項1】
物体の位置姿勢を求める位置姿勢推定方法であって、
統計量算出手段が、予め保持された前記物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出工程と、
信頼度算出手段が、前記ずれの統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出工程と、
画像特徴抽出手段が、撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出工程と、
対応付け手段が、前記幾何特徴と前記画像特徴とを対応付けする対応付け工程と、
ずれ量算出手段が、前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出工程と、
位置姿勢推定手段が、前記信頼度及び前記ずれ量に基づいて、前記物体の位置姿勢を推定する位置姿勢推定工程と
を有することを特徴とする位置姿勢推定方法。
【請求項2】
前記統計量算出工程では、
前記三次元形状モデルの公差データに基づいて、前記統計量を算出することを特徴とする請求項1に記載の位置姿勢推定方法。
【請求項3】
前記統計量算出工程では、
モデル特徴抽出手段が、前記三次元形状モデルを構成する線分を抽出するモデル特徴抽出工程と、
公差抽出手段が、前記線分のずれの要因となる公差を抽出する公差抽出工程と、
公差値算出手段が、前記線分の所定間隔ごとの前記公差の値を算出する公差値算出工程と、
統計量決定手段が、前記公差の値の最大値における公差のベクトルを生成し、前記ベクトルを統計量とする統計量決定工程と
を有することを特徴とする請求項2に記載の位置姿勢推定方法。
【請求項4】
前記統計量は、確率分布で表現されることを特徴とする請求項2に記載の位置姿勢推定方法。
【請求項5】
前記統計量算出工程では、
モデル特徴抽出手段が、前記三次元形状モデルを構成する線分を抽出するモデル特徴抽出工程と、
公差抽出手段が、前記線分のずれの要因となる公差を抽出する公差抽出工程と、
公差値算出手段が、前記線分の所定間隔ごとの前記公差の値を算出する公差値算出工程と、
統計量決定手段が、前記公差の値に基づく正規分布のベクトルを生成し、前記ベクトルを統計量とする統計量決定工程と
を有することを特徴とする請求項4に記載の位置姿勢推定方法。
【請求項6】
前記統計量算出工程では、
モデル特徴抽出手段が、前記三次元形状モデルを構成する面を抽出するモデル特徴抽出工程と、
公差値算出手段が、前記面の重心における前記公差の値を算出する公差値算出工程と、
統計量決定手段が、前記公差の値の最大値における公差のベクトルを生成し、前記ベクトルを統計量とする統計量決定工程と
を有することを特徴とする請求項2に記載の位置姿勢推定方法。
【請求項7】
物体計測手段が、前記物体の形状を構成する幾何特徴を計測する物体計測工程と
を更に有し、
前記統計量算出工程では、前記計測した結果と前記三次元形状モデルとのずれに基づいて、前記統計量を算出することを特徴とする請求項1に記載の位置姿勢推定方法。
【請求項8】
前記物体計測工程では、複数の物体ごとに複数回計測することを特徴とする請求項7に記載の位置姿勢推定方法。
【請求項9】
前記物体計測工程では、複数の計測位置ごとに複数回計測することを特徴とする請求項7又は8に記載の位置姿勢推定方法。
【請求項10】
前記幾何特徴が、点、若しくは、線分、若しくは、面であることを特徴とする請求項1乃至9の何れか1項に記載の位置姿勢推定方法。
【請求項11】
前記信頼度算出工程では、前記統計量が小さいほど信頼度を高く算出することを特徴とする請求項1乃至10の何れか1項に記載の位置姿勢推定方法。
【請求項12】
前記信頼度算出工程では、尤度関数を用いることにより、信頼度を算出することを特徴とする請求項1乃至11の何れか1項に記載の位置姿勢推定方法。
【請求項13】
概略値入力手段が、前記物体の位置姿勢の概略値を入力する概略値入力工程と、
を有し、
前記位置姿勢推定工程では、
位置姿勢補正値算出手段が、前記信頼度及び前記ずれ量及び前記概略値に基づいて、前記物体の位置姿勢の補正値を算出する位置姿勢補正値算出工程と、
位置姿勢補正手段が、前記補正値に基づいて、前記物体の位置姿勢の概略値を補正する位置姿勢補正工程と
を有することを特徴とする請求項1乃至12の何れか1項に記載の位置姿勢推定方法。
【請求項14】
請求項1乃至13の何れか1項に記載の位置姿勢推定方法をコンピュータに実行させることを特徴とするプログラム。
【請求項15】
物体の位置姿勢を求める位置姿勢推定装置であって、
前記物体の三次元形状モデルを構成する幾何特徴のずれの統計量を算出する統計量算出手段と、
前記ずれ統計量に基づいて、前記幾何特徴ごとの信頼度を算出する信頼度算出手段と、
撮像手段によって撮像された撮像画像の画像特徴を抽出する画像特徴抽出手段と、
前記幾何特徴と前記画像特徴とを対応付けする対応付け手段と、
前記幾何特徴と前記画像特徴とのずれ量を算出するずれ量算出手段と、
前記信頼度及び前記ずれ量に基づいて、前記物体の位置姿勢を推定する位置姿勢推定手段と
を備えることを特徴とする位置姿勢推定装置。
【請求項16】
前記物体を撮像する撮像手段を更に備えることを特徴とする請求項15に記載の位置姿勢推定装置。
【請求項17】
前記撮像手段が、現実空間の距離画像を撮像画像として撮像することを特徴とする請求項16に記載の位置姿勢推定装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【公開番号】特開2011−174880(P2011−174880A)
【公開日】平成23年9月8日(2011.9.8)
【国際特許分類】
【出願番号】特願2010−40595(P2010−40595)
【出願日】平成22年2月25日(2010.2.25)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
【公開日】平成23年9月8日(2011.9.8)
【国際特許分類】
【出願日】平成22年2月25日(2010.2.25)
【出願人】(000001007)キヤノン株式会社 (59,756)
【Fターム(参考)】
[ Back to top ]