位置姿勢計測装置、その計測処理方法及びプログラム
【課題】
計測対象物体の位置及び姿勢に関する複数の計測手法の中からいずれかを選択して実施する。
【解決手段】
位置姿勢計測装置は、撮像装置により撮像された計測対象物体の二次元画像を入力し、距離センサにより計測された計測対象物体の距離データを取得し、二次元画像から計測対象物体の画像特徴を検出し、計測対象物体の状態を判定し、当該判定した状態に基づいて、画像特徴と距離データとを用いて計測を行なう第１のモードと、画像特徴と距離データとのうち一方のみを用いて計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定し、当該設定したモードに従って計測対象物体の位置及び姿勢の計測を行なう。
計測対象物体の位置及び姿勢に関する複数の計測手法の中からいずれかを選択して実施する。
【解決手段】
位置姿勢計測装置は、撮像装置により撮像された計測対象物体の二次元画像を入力し、距離センサにより計測された計測対象物体の距離データを取得し、二次元画像から計測対象物体の画像特徴を検出し、計測対象物体の状態を判定し、当該判定した状態に基づいて、画像特徴と距離データとを用いて計測を行なう第１のモードと、画像特徴と距離データとのうち一方のみを用いて計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定し、当該設定したモードに従って計測対象物体の位置及び姿勢の計測を行なう。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢計測装置、その計測処理方法及びプログラムに関する。
【背景技術】
【０００２】
近年、ロボット技術の発展とともに、これまで人間が行なっていた複雑なタスク（例えば、工業製品の組み立て）をロボットが代わりに行なうようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組み立てを行なう。この組み立てに際しては、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。
【０００３】
位置及び姿勢を計測する方法としては、撮像装置で撮像した二次元画像から検出される特徴に対してモデルフィッティングする方法が知られている。また、距離センサにより得られた距離画像の各点を三次元点群データに変換して当該三次元点群に対してモデルフィッティングする方法等も知られている。
【０００４】
二次元画像に対するモデルフィッティングでは、物体の位置及び姿勢に基づいて三次元形状モデルを二次元画像上に投影した投影像を、二次元画像から検出された特徴にフィティングさせることにより物体の位置及び姿勢を計測する。
【０００５】
距離画像に対するモデルフィッティングでは、距離データを表現する距離画像の各点を三次元座標を持つ三次元点群データに変換し、三次元形状モデルに当該三次元点群データをフィッティングさせることにより物体の位置及び姿勢を計測する。この他、二次元画像から得られる計測情報と距離画像から得られる計測情報とを併用して物体の位置及び姿勢を計測する方法も知られている。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】R.Y.Tsai,“A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses,"IEEE Journal of Robotics and Automation,vol.RA-3,no.4,1987.
【非特許文献２】L.Vacchetti,V.Lepetit,and P.Fua,“Combining edge and texture information for real-time accurate 3D camera tracking,"Proc.3rd IEEE/ACM International Symposium on Mixed and Augmented Reality(ISMAR'04),pp.48-57,2004.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
二次元画像から得られる計測情報と距離画像から得られる計測情報とを併用して物体の位置及び姿勢を計測するように構成した場合、一般に、その計測精度の向上が期待できる。
【０００８】
しかし、計測を行なうときの状態（例えば、物体の形状や物体の置かれ方）によっては、二次元画像から得られる計測情報、又は距離画像から得られる計測情報のいずれかを利用して計測を行なった場合の方が、精度良く計測を行なえる場合がある。
【０００９】
また、二次元画像から得られる計測情報、又は距離画像から得られる計測情報のいずれかを利用して計測を行なうだけで計測精度を十分に満たせる場合もある。このような場合、両手法を併用した計測を行なう必要はない。計算時間や計算リソース等を考慮すると、却って処理が無駄となる。
【００１０】
本発明は、上記課題に鑑みてなされたものであり、計測対象物体の位置及び姿勢に関する複数の計測手法の中からいずれかを選択して実施するようにした技術を提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記課題を解決するため、本発明の一態様は、計測対象物体の位置及び姿勢の計測を行なう位置姿勢計測装置であって、撮像装置により撮像された前記計測対象物体の二次元画像を入力する入力手段と、距離センサにより計測された前記計測対象物体の距離データを取得する取得手段と、前記二次元画像から前記計測対象物体の画像特徴を検出する検出手段と、前記計測対象物体の状態を判定する判定手段と、前記判定手段により判定された状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定するモード設定手段と、前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう計測手段とを具備することを特徴とする。
【発明の効果】
【００１２】
本発明によれば、計測対象物体の位置及び姿勢の計測に関する複数の計測手法の中からいずれかを選択して実施することができる。
【図面の簡単な説明】
【００１３】
【図１】本実施形態に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図２】三次元形状モデルの一例を示す図。
【図３】図１に示す位置姿勢計測部４６の機能的な構成の一例を示す図。
【図４】図１に示す位置姿勢計測装置４０の動作の一例を示すフローチャート。
【図５】エッジ検出の概要の一例を示す図。
【図６】第１の計測モード設定時における位置姿勢算出処理の流れの一例を示すフローチャート。
【図７】投影像（線分）と検出されたエッジとの関係の一例を示す図。
【図８】二次元画像上における誤差を次元変換させる処理の概要の一例を示す図。
【図９】本実施形態に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図１０】実施形態２に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図１１】実施形態３に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図１２】ロボットシステムを制御する制御システムの構成の一例を示す図。
【発明を実施するための形態】
【００１４】
以下、本発明の一実施の形態について添付図面を参照して詳細に説明する。
【００１５】
（実施形態１）
図１は、本実施形態に係わる位置姿勢計測装置４０における機能的な構成の一例を示す図である。
【００１６】
位置姿勢計測装置４０は、計測対象物体を模したモデルデータ（三次元形状モデル）の形状に基づいて、計測対象物体（以下、単に物体と呼ぶ場合もある）の位置及び姿勢を計測（算出）する処理を切り替えて行なう。詳細については後述するが、第１のモード（本実施形態においては、第１の計測モード）又は第２のモード（本実施形態においては、第２の計測モード）のいずれかを設定し、そのモードに従って計測対象物体の位置及び姿勢を計測する。第１の計測モードでは、二次元画像から得られる計測情報（画像特徴）と距離画像から得られる計測情報（三次元点群データ）とを同時に利用したモデルフィッティングを行なう。第２の計測モードでは、二次元画像から得られる計測情報（画像特徴）のみを利用したモデルフィッティングを行なう。
【００１７】
ここで、位置姿勢計測装置４０は、二次元画像撮像部２０と、距離画像計測部３０とに接続されている。
【００１８】
二次元画像撮像部２０は、二次元画像を撮像する役割を果たし、例えば、撮像装置（例えば、カメラ）で実現される。撮像される二次元撮像画像は、濃淡画像であっても良いし、また、カラー画像であっても良い。
【００１９】
距離画像計測部３０は、計測対象である物体表面までの距離を示す距離画像を計測する役割を果たし、例えば、距離センサで実現される。距離センサは、距離画像を出力する。距離画像は、各画素が視点位置（距離センサの位置）からの奥行きの情報を持つ画像であり、距離センサから物体までの距離データを表現する。距離センサには、対象に照射したレーザ光やスリット光の反射光をカメラで撮像し、三角測量により距離を計測するアクティブ式を採用すれば良い。なお、距離センサは、距離画像を撮像できればよく、その方式は特に問わない。例えば、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式を採用しても良いし、ステレオカメラが撮像した画像から三角測量によって各画素の奥行きを計算するパッシブ式を採用しても良い。
【００２０】
ここで、位置姿勢計測装置４０は、三次元形状モデル保持部４５と、概略値入力部４４と、二次元画像入力部４１と、画像特徴検出部４２と、三次元座標情報取得部４３と、モード設定部４７と、位置姿勢計測部４６とを具備して構成される。
【００２１】
二次元画像入力部４１は、二次元画像撮像部２０により撮像された画像を装置内（位置姿勢計測装置４０）に入力する。撮像装置の焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、事前にキャリブレーションしておいてもよい（非特許文献１参照）。
【００２２】
三次元座標情報取得部４３は、距離画像計測部３０により計測された距離画像に基づいて三次元点群データ（計測対象物体の表面形状を示す情報）を生成し、それを装置内に入力する。三次元座標情報取得部４３は、距離画像の各画素をカメラ座標系（二次元画像撮像部２０の座標系）における三次元点群データに変換し、それを三次元座標情報として取得する。三次元点群データへの変換は、距離センサ（距離画像計測部３０）とカメラ（二次元画像撮像部２０）との相対的な位置姿勢（既知の情報）に基づいて行なわれる。より具体的には、距離画像の各画素について、画素位置に対応する視線ベクトルに奥行き値を乗算することにより上記変換を行なう。なお、距離センサ（距離画像計測部３０）及びカメラ（二次元画像撮像部２０）はそれぞれ固定されており、両者の相対的な位置姿勢は変化しないと仮定し、事前にキャリブレーションしておいてもよい。例えば、三次元形状が既知であるキャリブレーション物体を様々な方向から観察し、二次元画像に基づいたキャリブレーション物体の位置姿勢と、距離画像に基づいたキャリブレーション物体の位置姿勢との差に基づいて相対的な位置姿勢を求める。なお、二次元画像撮像部２０による二次元画像の撮像と距離画像計測部３０による距離画像の計測とは、同時に行なわれるのが望ましい。但し、計測対象物体が静止している場合など、位置姿勢計測装置４０と計測対象物体との間の位置及び姿勢の関係が変化しない場合には、必ずしも同時に行なう必要はない。
【００２３】
三次元形状モデル保持部４５は、位置及び姿勢の計測対象である物体（計測対象物体）を模した三次元形状のモデルデータ（三次元形状モデル）を表す情報を保持する。ここで、図２を用いて、本実施形態に係わる三次元形状モデルの一例について説明する。三次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。三次元形状モデル１０は、図２（ａ）に示すように、点Ｐ１〜点Ｐ１４の１４点から構成される。また、三次元形状モデル１０は、図２（ｂ）に示すように、線分Ｌ１〜Ｌ１６により構成されている。点Ｐ１〜点Ｐ１４は、図２（ｃ）に示すように、三次元座標値により表される。また、線分Ｌ１〜Ｌ１６は、図２（ｄ）に示すように、線分を構成する点のＩＤ（識別子）により表される。
【００２４】
なお、ここでは図示を省略するが、三次元形状モデルは、面の情報も保持する。各面は、各面を構成する点のＩＤにより表される。図２に示す三次元形状モデルでは、直方体を構成する６つの面の情報が定義されている。三次元形状モデルは、位置姿勢計測部４６により物体の位置及び姿勢が計測される時や、モード設定部４７によるモード設定時に用いられる。
【００２５】
概略値入力部４４は、位置姿勢計測装置４０に対する物体の位置及び姿勢の概略値を入力する。本実施形態において、位置姿勢計測装置４０に対する物体の位置及び姿勢とは、二次元画像撮像部２０を基準とした物体の位置及び姿勢を示すが、必ずしも二次元画像撮像部２０を基準にする必要はない。例えば、二次元画像撮像部２０の座標系に対する物体の相対的な位置及び姿勢が既知であり、且つその位置及び姿勢が変化しないのであれば、位置姿勢計測装置４０におけるその他の部分を基準としてもよい。また、本実施形態においては、物体の位置及び姿勢の概略値として、位置姿勢計測装置４０が当該物体から過去（例えば、直前）に計測した計測値を用いる。但し、物体の位置及び姿勢の概略値は、必ずしもこのような値である必要はない。例えば、過去に計測した物体の位置及び姿勢の計測値に対して時系列フィルタリング処理（例えば、線形フィルタ、カルマンフィルタ）を実施し、物体の運動速度や角速度を推定する。そして、その推定結果に基づいて物体の位置及び姿勢を予測した値を概略値としてもよい。また、センサから得られる物体の位置及び姿勢を概略値としてもよい。ここで、センサは、物体の位置及び姿勢を６自由度で計測できればよく、その方式（例えば、磁気式、光学式、超音波式）は、特に問わない。なお、物体の置かれているおおよその位置や姿勢をパターンマッチングなどにより求めてもよいし、また、物体の置かれているおおよその位置や姿勢が予め分かっているのであれば、その値を概略値として用いてもよい。
【００２６】
画像特徴検出部４２は、二次元画像入力部４１により入力された二次元画像から計測対象物体を示す画像特徴を検出する。本実施形態においては、画像特徴としてエッジ（濃度勾配の極値）を検出する場合を例に挙げる。
【００２７】
モード設定部４７は、計測対象物体の形状（三次元形状モデル保持部４５により保持される三次元形状モデルの形状）に基づいて、計測対象物体の位置及び姿勢を計測するモードを切り替える。モード設定部４７には、三次元形状モデルの形状を類推する形状類推部４８が設けられる。モード設定部４７おいては、第１のモード（本実施形態においては、第１の計測モード）及び第２のモード（本実施形態においては、第２の計測モード）を切り替えて設定する。第１の計測モード設定時には、二次元画像入力部４１により入力された二次元画像から得られる計測情報と、三次元座標情報取得部４３により取得された距離画像から得られる計測情報とを（同時に）利用したモデルフィッティングが行なわれる。第２の計測モード設定時には、二次元画像入力部４１により入力された二次元画像（のみ）を利用したモデルフィッティングが行なわれる。
【００２８】
ここで、二次元画像から得られる計測情報と距離画像から得られる計測情報とを利用したモデルフィッティング（第１の計測モード）は、物体の位置及び姿勢を高精度に計測できる。その反面、距離センサの特性上、物体端部の計測データが部分的に欠損してしまう場合がある。そのため、例えば、物体が小さい、物体が細い、物体に凹凸が多い等の場合には、物体の端部の距離計測結果の信用性は低い。また、距離センサの奥行き方向の分解能には限界がある。例えば、物体が紙のように薄い場合、その下にある台や他の物体とほとんど同じ距離データが取得されることがあり、これは誤計測の要因となる。
【００２９】
これらのケースから、計測対象物体の形状によっては、二次元画像と距離画像とを利用したモデルフィッティング（第１の計測モード）よりも、距離画像は使用せずに、二次元画像を利用したモデルフィッティング（第２の計測モード）により計測を行なう方が精度良く計測を行なえる場合がある。また、物体の位置及び姿勢の計測時における距離画像の寄与度が小さい場合もある。更に、二次元画像を利用した計測を行なうだけで計測精度を十分に満たせる場合もある。このように距離画像が有意でない場合、二次元画像のみを用いて物体の位置及び姿勢の計測を行なった方が、計算時間や計算リソースの抑制を図れる他、計測精度が向上する場合もある。
【００３０】
そこで、本実施形態においては、計測対象物体の三次元形状モデルの形状を類推し、その類推された形状に基づいて計測モード（第１の計測モード、第２の計測モード）を切り替えて設定する。そして、その設定された計測モードにより物体の位置及び姿勢の計測を行なう。
【００３１】
位置姿勢計測部４６は、モード設定部４７により設定されたモードに従って計測対象物体の位置及び姿勢を計測（算出）する。第１の計測モード設定時には、画像特徴検出部４２により検出された画像特徴に対して、三次元形状モデル保持部４５により保持された三次元形状モデルをフィッティングさせる。また、第１の計測モード設定時には、三次元座標情報取得部４３により取得された三次元点群に対しても、三次元形状モデルをフィッティングさせる。これにより、位置姿勢算出処理を実施し、計測対象物体の位置及び姿勢を計測（算出）する。一方、第２の計測モード設定時には、画像特徴検出部４２により検出された画像特徴に対して、三次元形状モデル保持部４５により保持された三次元形状モデルをフィティングさせる。これにより、位置姿勢算出処理を実施し、計測対象物体の位置及び姿勢を計測（算出）する。
【００３２】
次に、図３を用いて、図１に示す位置姿勢計測部４６における機能的な構成の一例について説明する。
【００３３】
位置姿勢計測部４６は、二次元画像ずれ算出部４６１と、三次元空間ずれ算出部４６２と、尺度変換部４６３と、位置姿勢補正部４６４とを具備して構成される。
【００３４】
二次元画像ずれ算出部４６１は、二次元画像上で検出された画像特徴（エッジ）と、二次元画像上における三次元形状モデルの投影像（線分）との二次元画像上における距離を算出する。なお、三次元形状モデルの投影像は、計測対象物体の位置及び姿勢の概略値と、二次元画像撮像部２０の内部パラメータ（例えば、キャリブレーション後の値）とに基づいて投影される。
【００３５】
三次元空間ずれ算出部４６２は、距離画像に基づいて生成された三次元点群データを構成する各点と、三次元形状モデルの面との三次元空間中における距離を算出する。
【００３６】
尺度変換部４６３は、二次元画像ずれ算出部４６１及び三次元空間ずれ算出部４６２により算出された双方の距離の尺度を一致させる。より具体的には、二次元画像上での対応点（点）と投影像（線分）の符号付距離を算出し、三次元空間中での三次元点群データにおける点と三次元形状モデルの平面との符号付距離を算出し、双方の符号付距離を物体の位置及び姿勢の関数として線形近似する。そして、符号付距離を０としたときに各計測情報について成り立つ線形方程式を用意する。
【００３７】
位置姿勢補正部４６４は、この線形方程式を連立方程式として解くことにより、物体の位置及び姿勢の微小変化を求める。そして、その求めた値に基づいて物体の位置及び姿勢を示す計測値を補正する。この処理を繰り返すことにより最終的な計測値を算出する。
【００３８】
以上が、位置姿勢計測装置４０の構成の一例についての説明である。なお、上記説明した、位置姿勢計測装置４０には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。
【００３９】
次に、図４を用いて、図１に示す位置姿勢計測装置４０において計測対象物体の位置及び姿勢を計測する際の処理の流れの一例について説明する。
【００４０】
（Ｓ１０１）
位置姿勢計測装置４０は、概略値入力部４４において、位置姿勢計測装置４０に対する物体の位置及び姿勢の概略値を入力する。本実施形態においては、上述した通り、物体の位置及び姿勢の概略値として、位置姿勢計測装置４０が当該物体から過去（例えば、直前）に計測した計測値を入力する。
【００４１】
（Ｓ１０２）
位置姿勢計測装置４０は、二次元画像入力部４１において、二次元画像撮像部２０により撮像された二次元画像を入力する。
【００４２】
（Ｓ１０３）
位置姿勢計測装置４０は、三次元座標情報取得部４３において、距離画像計測部３０により計測された三次元座標情報を取得する。上述した通り、三次元座標情報取得部４３は、距離画像の各画素をカメラ座標系（二次元画像撮像部２０の座標系）における三次元点群データに変換し、それを三次元座標情報として取得する。
【００４３】
（Ｓ１０４）
ここで、位置姿勢計測装置４０は、画像特徴検出部４２において、Ｓ１０２の処理で入力された二次元画像から計測対象物体を示す画像特徴を検出する。本実施形態においては、上述した通り、画像特徴としてエッジを検出する。
【００４４】
ここで、図５を用いて、本実施形態に係わるエッジ検出の概要について説明する。エッジ検出処理では、まず、計測対象物体が撮像された二次元画像に対して三次元形状モデルを投影する。この投影像（線分）は、Ｓ１０１の処理で入力された計測対象物体の位置及び姿勢の概略値と、二次元画像撮像部２０の内部パラメータ（例えば、キャリブレーション後の値）とに基づいて求められる。次に、投影された各線分５１上において、等間隔に制御点５２を設定する。そして、投影線分５１の法線方向に平行で且つ制御点５２を通過する線分（以下、探索ライン）５３上からエッジ５６を１次元探索する（図５（ａ））。エッジは、画素値の濃度勾配の極値となる（図５（ｂ））。場合によっては、探索ライン５３上から複数のエッジが検出されることがある。この場合、本実施形態においては、複数検出されたエッジを仮説として保持する（非特許文献２参照）。このようにして各制御点５２に対応するエッジ５６を探索する。
【００４５】
（Ｓ１０５）
位置姿勢計測装置４０は、形状類推部４８において、三次元形状モデル保持部４５により保持される三次元形状モデルの形状を類推する。三次元形状モデルの形状は、例えば、三次元形状モデルを包含するバウンディングボックス（直方体）で代替してもよい。バウンディングボックスは、三次元形状モデルが属する三次元座標系のｘｙｚ軸それぞれに垂直に交わる面をバウンディングボックスの各面として有する。なお、このバウンディングボックスの各面は、ｘｙｚ軸上で三次元形状モデルが含まれる最大値や最小値に基づいて決められる（Ａｘｉｓ−Ａｌｉｇｎｅｄ−Ｂｏｕｎｄｉｎｇ−Ｂｏｘ）。また、三次元形状モデルに適合した形でバウンディングボックスを形成しても良い。形状類推部４８においては、三次元形状モデルを代替するバウンディングボックスを求めた後、その各辺の長さ（ｘ１、ｙ１、ｚ１）や、辺の比（ｘ１／ｙ１、ｘ１／ｚ１、ｙ１／ｘ１、ｙ１／ｚ１、ｚ１／ｘ１、ｚ１／ｙ１）から三次元形状モデルの形状を類推する。なお、三次元形状モデルの形状の類推は、バウンディングボックスを用いずに行なっても良い。例えば、予め用意された要素モデル（プリミティブモデル）の組み合わせ方から求めてもよい。この方法を採用した場合、三次元形状モデルに凹凸が多いか少ないかを類推できる。
【００４６】
ここで、三次元形状モデルの形状の類推は、例えば、以下のルールに従って行なう。
【００４７】
１）．各辺の長さが全て、予め定められた第１の閾値（例えば、１ｃｍ）よりも小さい場合は、三次元形状モデルは小さい形状であると類推する。
【００４８】
２）．辺の比のうちの２つが、予め定められた第２の閾値（例えば、１０）よりも大きく、且つ辺の比のうちの上記以外の２つが、予め定められた第３の閾値（例えば、０．１）よりも小さい。更に、各辺の長さのうちの１つが、予め定められた第４の閾値（例えば、３ｃｍ）よりも大きい場合は、三次元形状モデルは、細い形状であると類推する。
【００４９】
３）．辺の比のうちの２つが、予め定められた第２の閾値（例えば、１０）よりも大きく、且つ辺の比のうちの上記以外の２つが、予め定められた第３の閾値（例えば、０．１）より小さい。更に、各辺の長さのうちの２つが、予め定められた第４の閾値（例えば、３ｃｍ）よりも大きい場合は、三次元形状モデルは、薄い形状であると類推する。
【００５０】
（Ｓ１０６）
次に、位置姿勢計測装置４０は、モード設定部４７において、Ｓ１０５の処理で類推された三次元形状モデルの形状に基づいて、計測モードの設定を行なう。すなわち、計測対象物体（三次元形状モデル）の形状に基づいて、距離画像により得られた計測情報を用いた計測を行なうか否かを判定し、その判定結果に基づいて第１の計測モードか第２の計測モードかのいずれかを設定する。初期モードは、第１の計測モードとする。
【００５１】
例えば、三次元形状モデルの形状が、小さい、細い、凹凸が多い、薄い等の少なくともいずれかの場合は、第１の計測モードから第２の計測モードに切り替える。これは、上述した通り、距離センサ（距離画像計測部３０）による計測に誤りが含まれている可能性が高いためである。第２の計測モードでは、距離画像から得られる計測情報を用いずに物体の位置及び姿勢を計測するため、距離センサによる計測に誤りが含まれている場合であっても、精度良く計測が行なえる。なお、三次元形状モデルの形状が、小さい、細い、凹凸が多い、薄い等以外の場合は、第１の計測モードを設定（維持）する。
【００５２】
（Ｓ１０７）
位置姿勢計測装置４０は、位置姿勢計測部４６において、Ｓ１０６の処理で設定されたモードに従って計測対象物体の位置及び姿勢を計測（算出）する。詳細については後述するが、Ｓ１０６の処理で設定されたモードに基づき、Ｓ１０４の処理で検出された画像特徴（エッジ）、Ｓ１０３の処理で入力された三次元座標情報（三次元点群）の少なくともいずれかに対して、三次元形状モデルをフィッティングさせる。これにより、計測対象物体の位置及び姿勢を求める。
【００５３】
ここで、図４のＳ１０７に示す位置姿勢計測部４６における動作の一例について説明する。まず、第１の計測モード設定時における処理の流れについて説明する。第１の計測モードでは、二次元画像上の距離（エッジと投影像との距離）を三次元空間中の距離に変換し、尺度を統一する。これにより、両手法の寄与率が偏ることをなくす。
【００５４】
図６には、第１の計測モード設定時における位置姿勢算出処理の流れの一例が示される。この処理では、ガウス−ニュートン（Gauss-Newton）法により反復演算し、計測対象物体の位置及び姿勢（以下、６次元ベクトルｓで表す）の概略値を補正する。これにより、計測対象物体の位置及び姿勢を求める。なお、位置姿勢算出処理は、ガウス−ニュートン法に限られない。例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いてもよいし、よりシンプルな方法である最急降下法を用いてもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。
【００５５】
（Ｓ２０１）
位置姿勢計測装置４０は、位置姿勢計測部４６において、まず、初期化処理を行なう。この初期化処理では、例えば、図４のＳ１０１の処理で得られた計測対象物体の位置及び姿勢の概略値を取得する。
【００５６】
（Ｓ２０２）
位置姿勢計測装置４０は、位置姿勢計測部４６において、画像特徴（エッジ）及び三次元点群データ（距離画像に基づく三次元点群データ）と、三次元形状モデルとをそれぞれ対応付ける。この処理では、まず、Ｓ２０１の処理で取得した概略値に基づいて三次元形状モデルの各線分を二次元画像上へ投影し、当該投影された線分に対して、二次元画像上で最も近い画像特徴（エッジ）を制御点と対応付ける。これにより、画像特徴（エッジ）と三次元計上モデルとを対応付ける。
【００５７】
また、Ｓ２０１の処理で取得した概略値に基づいて三次元形状モデル又は三次元点群データを座標変換し、三次元点群データの各点に対して、三次元空間中で最も近傍する三次元形状モデルの面を探索する。これにより、三次元点群データを三次元形状モデルと対応付ける。
【００５８】
（Ｓ２０３）
位置姿勢計測装置４０は、位置姿勢計測部４６において、線形連立方程式を解くための係数行列と、誤差ベクトル（エッジ、三次元点群データ）との算出を行なう。ここで、係数行列の各要素は、Ｓ２０１の処理で取得した概略値の微小変化に対する一次の偏微分係数である。エッジについての要素は、対応点（点）と投影像（線分）との二次元画像上での距離を位置姿勢の関数とした一次の偏微分係数である。三次元点群データについての要素は、三次元点群データにおける点と三次元形状モデルの平面との三次元空間中における距離を位置姿勢の関数とした一次の偏微分係数である。
【００５９】
ここで、係数行列の算出方法について説明する。図７は、投影像（線分）と、検出されたエッジとの関係の一例を示す図である。この場合、画像の水平方向をｕ軸６１とし、垂直方向をｖ軸６２としている。ある制御点（投影された各線分を画像上で等間隔に分割した点）６３の画像上における座標６４を（ｕ０，ｖ０）と表す。当該制御点が属する線分Ｌの画像上における傾きを（ｕ軸に対する傾き）θと表す。傾き６５は、線分（投影線分）６６の両端の三次元座標を、位置及び姿勢ｓに基づいて画像上（二次元上）に投影し、その両端の座標を結んだ直線の傾きを示している。線分６６の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、制御点と対応する点（対応点６７）の画像上における座標６８を（ｕ’，ｖ’）とする。ここで、対応点６７の座標（ｕ’，ｖ’）を通り、傾き６５がθである直線（破線）上の点（ｕ，ｖ）は、
【００６０】
制御点６３の画像上における位置は、計測対象物体の位置及び姿勢により変化する。計測対象物体の位置及び姿勢ｓは、６自由度である。すなわち、ｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素とからなる。姿勢を表す３つの要素は、例えば、オイラー角による表現や、方向が原点を通る回転軸を表し大きさが回転角を表す三次元ベクトルなどによって表現される。計測対象物体の位置及び姿勢により変化する点の画像上での座標は、制御点６３（すなわち、座標（ｕ０，ｖ０）６４）の近傍で１次のテイラー展開によって式（２）のように近似できる。但し、Δｓｉ（ｉ＝１，２，・・・，６）は、ｓの各成分の微小変化を表す。
【００６１】
物体の位置及び姿勢の概略値と、実際の物体の位置及び姿勢との差がそれほどないと仮定すると、正しいｓによって得られる制御点６３の画像上での位置は、式（１）が表す直線上にあると仮定できる。式（２）によって近似されるｕ、ｖを式（１）に代入することにより、式（３）が得られる。式（３）は、Ｓ２０２の処理で対応付けされた全てのエッジについて立式できる。
【００６２】
二次元画像撮像部２０の座標系で表される三次元点群データの三次元座標は、計測対象物体の位置及び姿勢ｓを用いて、計測対象物体の座標系における三次元座標（ｘ，ｙ，ｚ）に変換することができる。Ｓ２０１の処理で取得した概略値により、三次元点群データ中のある点が計測対象物体の座標系の座標（ｘ０，ｙ０，ｚ０）に変換されるとする。（ｘ，ｙ，ｚ）は、計測対象物体の位置及び姿勢により変化するため、（ｘ０，ｙ０，ｚ０）の近傍で１次のテイラー展開によって式（４）のように近似できる。
【００６３】
Ｓ２０２の処理により、三次元点群データ中のある点に対応付けられた三次元形状モデルの面に対する計測対象物体の座標系における方程式を「ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１、ａ，ｂ，ｃ，ｅは定数）」とする。正しいｓによって変換される（ｘ，ｙ，ｚ）は、平面の方程式「ａｘ＋ｂｙ＋ｃｚ＝ｅ」を満たすと仮定する。式（４）を平面の方程式に代入すると式（５）が得られる。式（５）は、Ｓ２０２の処理で対応付けされた全ての三次元点群データについて立式することができる。
【００６４】
式（３）、式（５）は、ｓの各成分の微小変化△ｓｉ（ｉ＝１，２，・・・，６）についての方程式である。そのため、式（６）のような△ｓｉに関する線形連立方程式が成り立つ。
【００６５】
式（６）の右辺の誤差ベクトルは、上述した通り、エッジについては二次元画像上での符号付距離であり、三次元点群データについては三次元空間中での符号付距離である。そのため、両者の距離の尺度は一致していない。そこで、二次元画像上での誤差にエッジの奥行きを乗じることにより、二次元画像上での誤差を三次元空間中での誤差に近似させる。具体的には、エッジについての方程式の両辺に、エッジの奥行きを乗じる。これにより、誤差の尺度を三次元空間中の距離に統一する。なお、二次元画像からは奥行き情報を得ることはできないため、エッジの奥行きは何らかの近似的な方法により求める必要がある。
【００６６】
図８を用いて、二次元画像上の誤差７１を三次元空間中の誤差７２に近似させる方法の一例について説明する。本実施形態においては、物体の位置及び姿勢の概略値に基づいて算出された各制御点７３の視点７４からの奥行き７５を、二次元画像上の誤差７１に乗じる。これにより、二次元画像上の誤差７１を三次元空間中の誤差７２に変換する。また、奥行き７５の代わりに、三次元空間中の制御点７３から画像面７６におけるエッジ７７を通過する視線ベクトルに対して下ろした垂線の長さとなるようなスケーリング係数を乗じるようにしてもよい。
【００６７】
ここで、解くべき連立方程式は、式（７）になる。
ここで、式（７）を式（８）のように表す。
線形連立方程式の係数行列Ｊを算出するため、偏微分係数を算出する。
【００６８】
（Ｓ２０４）
続いて、位置姿勢計測装置４０は、位置姿勢計測部４６において、式（８）をもとに、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いて補正値Δｓを求める。ここで、エッジや三次元点群データには、誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジや点群データでは、式（７）の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータに対しては小さな重みを与え、誤差の絶対値が小さいデータに対しては大きな重みを与える。重みは、例えば、式（９）に示すようなＴｕｋｅｙの関数により与える。
【００６９】
なお、重みを与える関数は、Ｔｕｋｅｙの関数である必要はなく、誤差の絶対値が大きいデータに対して小さな重みを与え、誤差の絶対値が小さいデータに対して大きな重みを与える関数であれば良い。そのため、例えば、Ｈｕｂｅｒの関数などを用いてもよい。
【００７０】
各計測情報（エッジ又は点群データ）に対応する重みをｗｉとする。ここで、式（１０）のように重み行列Ｗを定義する。
重み行列Ｗは、対角成分以外は全て「０」の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、式（８）を式（１１）のように変形する。
式（１２）のように式（１１）を解くことにより補正値Δｓを求める。
（Ｓ２０５）
位置姿勢計測装置４０は、位置姿勢計測部４６において、Ｓ２０４の処理で算出された補正値Δｓにより、Ｓ２０１の処理で得られた概略値を補正する。これにより、計測対象物体の位置及び姿勢を求める。
ｓ←ｓ＋△ｓ
（Ｓ２０６）
位置姿勢計測装置４０は、位置姿勢計測部４６において、計測対象物体の位置及び姿勢の計測値が収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、Ｓ２０２の処理に戻る。なお、収束しているか否かは、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。
【００７１】
次に、第２の計測モード設定時における処理の流れについて説明する。第２の計測モードでは、上述した式（３）を解くことで物体の位置及び姿勢を算出する。第２の計測モード設定時には、距離画像に基づき生成した三次元点群データを物体の位置及び姿勢の計測に用いないため、図３に示す三次元空間ずれ算出部４６２では処理を行なわない。また、尺度変換部４６３においても、距離のずれの尺度の変換を行なわない。
【００７２】
また、第２の計測モード設定時には、図６で説明した第１の計測モード設定時と同様の流れで処理が行なわれるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ２０２の処理で三次元点群データの対応付けを行なわない点や、Ｓ２０３の処理で尺度変換を行わない点等である。
【００７３】
以上説明したように実施形態１によれば、三次元形状モデルの形状に基づいてモードを設定し、当該モードに従って計測対象物体の位置及び姿勢を計測（算出）する。これにより、計測対象物体の形状に応じた手法で計測を行なえるため、計測の精度向上や高速化、ロバスト性の向上を図れる。
【００７４】
なお、上述した実施形態１においては、三次元形状モデルの形状に基づいてモードを切り替えて設定していたが、これに限られない。例えば、距離画像計測部３０により計測された距離画像に基づいてモードを設定するように構成してもよい。この場合、位置姿勢計測装置４０は、図９に示すように、モード設定部４７には、三次元座標情報取得部４３により取得された三次元点群データが入力される。これにより、形状類推部４８においては、距離画像に基づき生成された三次元点群データに基づいて計測対象物体の形状を類推し、モード設定部４７においては、当該類推された形状に基づいてモードを設定することができる。
【００７５】
（実施形態２）
次に、実施形態２について説明する。実施形態２においては、三次元形状モデルの形状と、距離画像から得られる計測情報とに基づいて計測モードを設定する場合について説明する。
【００７６】
図１０を用いて、実施形態２に係わる位置姿勢計測装置４０における機能的な構成の一例について説明する。なお、実施形態１を説明した図１と同一の符号が付された構成は、基本的には、実施形態１同様の機能を果たす。ここでは、実施形態１との相違点について重点的に説明する。
【００７７】
実施形態１との相違点としては、モード設定部４７におけるモード設定処理にある。具体的には、モード設定部４７では、三次元形状モデル保持部４５により保持される三次元形状モデルの形状と、三次元座標情報取得部４３により取得される三次元点群データとに基づいてモードを設定する。
【００７８】
モード設定部４７においては、これら両データの比較を行うために、三次元点群データ中の各々の点から最も近い三次元形状モデル上の表面に当該各々の点を対応付ける。対応付けの結果、三次元形状モデルから三次元空間的に離れた点が所定数より多ければ、三次元形状モデルと距離画像における距離データとが大きく異なると判定し、第２の計測モードを設定する。すなわち、二次元画像から得られる計測情報を利用した物体の位置及び姿勢の計測が行なわれることになる。
【００７９】
三次元形状モデルから三次元空間的に離れた点が所定数以内であれば、三次元形状モデルと距離画像における距離データとが比較的似ていると判定し、第１の計測モードを設定する。すなわち、二次元画像から得られる計測情報と距離画像から得られる計測情報とを利用した物体の位置及び姿勢の計測が行なわれることになる。なお、これら両データの比較を行なう前に、三次元点群データの中から計測対象物体の三次元点群データのみをセグメンテーションで選択するように構成してもよい。
【００８０】
なお、実施形態２に係わる位置姿勢計測装置４０における処理は、実施形態１で説明した図４と同様の流れとなるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ１０５の処理を省く点とＳ１０６の処理が異なる点とが挙げられる。具体的には、Ｓ１０６の処理において、モード設定部４７は、三次元形状モデル保持部４５により保持される三次元形状モデルの形状と、三次元座標情報取得部４３により取得される三次元点群データとに基づいてモード設定を行なう。
【００８１】
以上説明したように実施形態２によれば、三次元形状モデルの形状と、距離画像から得られる計測情報（距離画像に基づき生成された三次元点群データ）とを比較し、その比較結果に基づいて計測モードを設定する。具体的には、三次元形状モデルの形状と、距離画像から得られる計測情報とを比較する。そして、その比較結果に基づいて三次元形状モデルと大きく異なる距離データが計測されたと判定した場合、距離画像を信用できないため、第２の計測モードを設定する。そうでなければ、第１の計測モードを設定する。
【００８２】
これにより、実施形態２の構成によれば、実施形態１の構成よりも更に適切な計測モードで物体の位置及び姿勢の計測が行なえる。
【００８３】
（実施形態３）
次に、実施形態３について説明する。実施形態３においては、二次元画像上の画像特徴に基づいて計測モードを設定する場合について説明する。また、実施形態３においては、上述した第１の計測モード（第１のモード）と、新たな第３の計測モード（第２のモード）との間でモードを切り替えて設定する。第３の計測モードは、二次元画像から得られる計測情報は利用せず、距離画像から得られる計測情報を利用したモデルフィッティングを行なう。
【００８４】
ここで、二次元画像から得られる計測情報（画像特徴）と距離画像から得られる計測情報（三次元点群データ）とを利用したモデルフィッティング（第１の計測モード）は、物体の位置及び姿勢を高精度に計測できる。しかし、二次元画像から得られる画像特徴を利用したモデルフィッティングを行なう場合には、二次元画像上にある程度の画像特徴が存在していないといけない。そのため、例えば、計測対象物体の表面に模様（テクスチャ）がほとんどない場合やエッジが少ない場合などは、二次元画像を利用して計測を実施するのが難しい。また、計測精度も低くなる。
【００８５】
従って、計測対象物体の状態（表面状態）によっては、二次元画像と距離画像とを利用したモデルフィッティング（第１の計測モード）よりも、距離画像を利用したモデルフィッティング（第３の計測モード）により計測を行なう方が精度良く計測を行なえる場合がある。また、物体の位置及び姿勢の計測時における二次元画像の寄与度が小さい場合もある。更に、距離画像を利用して計測を行なうだけで計測精度を十分に満たせる場合もある。このように画像特徴が有意でない場合、距離画像のみを用いて物体の位置及び姿勢の計測を行なった方が、計算時間や計算リソースの抑制を図れる他、計測精度が向上する場合もある。
【００８６】
そこで、実施形態３においては、二次元画像上から画像特徴を検出し、その計測された画像特徴に基づいて計測モード（第１の計測モード、第３の計測モード）を切り替えて設定する。そして、その計測モードに応じて物体の位置及び姿勢の計測を行なう。
【００８７】
図１１を用いて、実施形態３に係わる位置姿勢計測装置４０における機能的な構成の一例について説明する。なお、実施形態１を説明した図１と同一の符号が付された構成は、基本的には、実施形態１同様の機能を果たす。ここでは、実施形態１との相違点について重点的に説明する。
【００８８】
実施形態１との相違点としては、モード設定部４７におけるモード設定処理にある。具体的には、モード設定部４７では、画像特徴検出部４２により二次元画像から検出された計測対象物体を示す画像特徴に基づいてモードを設定する。
【００８９】
モード設定部４７には、二次元画像上から周波数成分を抽出する周波数成分抽出部４９が設けられる。周波数成分抽出部４９により二次元画像から抽出された周波数成分が、所定の周波数より低ければ、モード設定部４７は、二次元画像上の画像特徴が少ないと判定し、第３の計測モードを設定する。そうでなければ、第１の計測モードが設定される。
【００９０】
位置姿勢計測部４６は、第３の計測モードが設定されている場合、三次元座標情報取得部４３により取得された三次元点群に対して、三次元形状モデルをフィッティングさせる。これにより、位置姿勢算出処理を実施し、計測対象物体の位置及び姿勢を計測（算出）する。
【００９１】
ここで、第３の計測モード設定時における処理の流れについて説明する。第３の計測モードでは、実施形態１で説明した式（７）を解くことで物体の位置及び姿勢を算出する。第３の計測モード設定時には、二次元画像の画像特徴を物体の位置及び姿勢の計測に用いないため、図３に示す二次元空間ずれ算出部４６１では処理を行なわない。また、尺度変換部４６３においても、距離のずれの尺度の変換を行なわない。
【００９２】
なお、実施形態３に係わる位置姿勢計測装置４０における処理は、実施形態１で説明した図４と同様の流れとなるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ１０４の処理が異なる点と、Ｓ１０５の処理を省く点と、Ｓ１０６の処理が異なる点とが挙げられる。具体的には、Ｓ１０４の処理では、画像特徴検出部４２において、二次元画像から計測対象物体を示す画像特徴を検出するとともに、周波数成分抽出部４９において、二次元画像の周波数成分を抽出する。また、Ｓ１０６の処理では、モード設定部４７は、周波数成分抽出部４９により抽出された周波数成分に基づいてモード設定を行なう。
【００９３】
また、第３の計測モード設定時は、図６を用いて説明した第１の計測モード設定時と同様の流れで物体の位置及び姿勢の計測処理が行なわれるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ２０２の処理でエッジの対応付けを行なわない点や、Ｓ２０３の処理で尺度変換を行わない点等である。
【００９４】
以上説明したように実施形態３によれば、二次元画像上の画像特徴の検出状態（多いか少ないか）に基づいて計測モードを設定する。例えば、計測対象物体の表面に模様（テクスチャ）がほとんどない場合やエッジが少ない場合などは、二次元画像を利用して計測を実施するのが難しいと判定し、第３の計測モードを設定する。そうでなければ、第１の計測モードを設定する。
【００９５】
なお、上述した実施形態３においては、二次元画像上の周波数成分に基づいてモード設定を判定しているが、これに限られず、二次元画像上に画像特徴が多いか少ないかを判定できれば良い。そのため、周波数成分でなく、エッジや特徴点の数に基づいてモード設定を行なうように構成してもよい。
【００９６】
また、上述した実施形態３においては、二次元画像上の画像特徴に基づいてモード設定を判定しているが、これに限られない。例えば、距離画像計測部３０により計測され距離画像に基づいてモードを設定するように構成してもよい。この場合、モード設定部４７においては、計測対象物体の距離画像に基づいて、計測対象物体に三次元的なカーブが多いか否かを判定する。判定の結果、三次元的なカーブが多ければ、エッジが少なく二次元画像を利用した計測が難しいため、第３の計測モードを設定する。そうではない場合は、第１の計測モードを設定する。
【００９７】
以上が本発明の代表的な実施形態の例であるが、本発明は、上記及び図面に示す実施形態に限定することなく、その要旨を変更しない範囲内で適宜変形して実施できるものである。例えば、上述した実施形態１〜３を組み合わせて実施しても良い。すなわち、実施形態１〜３で説明した手法に計測対象物体の状態等を計測し、その結果に基づいて第１の計測モード〜第３の計測モードの内のいずれかを設定し、計測を実施しても良い。
【００９８】
ここで、上述した実施形態の変形例をいくつか例示する。
【００９９】
（変形例１）
過去（例えば、直前）に計測した計測対象物体の位置及び姿勢、又は計測対象物体の概略値等に基づいて三次元形状モデルを三次元空間中に配置し、当該三次元形状モデルの観察結果に基づいてモードを変更しても良い。その観察結果により、例えば、カメラから観察される見た目の面積が少ない、カメラから見て凹凸が多い、薄い対象物体が山積みで積まれている等する場合には、距離画像を信用できない。このような場合には、第２の計測モードを設定する。
【０１００】
（変形例２）
計測対象物体の模様（テクスチャ）の少ない領域が、カメラから観察される画面中の多くを占める初期位置姿勢の場合や、三次元的なカーブがエッジ部分に多く見られる初期位置姿勢の場合は、二次元画像を利用した計測が難しい。このような場合には、第３の計測モードを設定するように構成しても良い。
【０１０１】
（変形例３）
上述したモードは、それぞれのモードを排他的に設定する必要はなく、物体の計測を行う状態に応じて連続的に切り替えてもよい。その場合、実施形態１で説明した式（７）のスケーリング係数を操作することで、モードを連続的に切り替えればよい。
【０１０２】
（変形例４）
ユーザインタフェース（入出力手段）を介したユーザの指示に基づいてモードを切り替えて設定しても良い。ユーザインタフェースは、例えば、ＧＵＩメニューや、ハードウェアスイッチ等により実現される。
【０１０３】
（変形例５）
システム側（位置姿勢計測装置４０）で自動的にモード設定を行なわず、最終的なモード設定は、ユーザに決めさせるように構成しても良い。すなわち、位置姿勢計測装置４０においては、上述した処理を実施することにより、ディスプレイ画面等に推奨モード（設定対象となるモードの候補）を提示（表示）する。この場合、ユーザは、ユーザインタフェース（入出力手段）を介してモードを選択し、設定する。
【０１０４】
（変形例６）
ユーザに対して現在どのモードでシステム（位置姿勢計測装置４０）が動作しているのかを提示するように構成してもよい。提示は、ディスプレイ画面に表示することにより行なっても良いし、ランプやＬＥＤなどでハードウェア的に行なうようにしても良い。
【０１０５】
（変形例７）
上述した位置姿勢計測装置４０は、例えば、計測対象物体を把持又は吸着するロボットシステムに適用することもできる。図１２は、計測対象物体の位置及び姿勢に基づいてロボットシステムを制御する制御システムの構成を示している。
【０１０６】
位置姿勢計測装置４０は、上述した実施形態同様に、二次元画像撮像部２０により撮像された二次元画像と、距離画像計測部３０により計測された距離画像と装置内に入力する。位置姿勢計測装置４０は、計測対象物体の位置及び姿勢を計測し、計測対象物体の位置及び姿勢の情報（位置姿勢情報）をロボット制御装置８２に向けて出力する。ロボット制御装置８２は、当該位置姿勢情報に基づいて、計測対象物体の把持、又は計測対象物体を吸着を指示する制御情報をロボット装置８１に向けて出力する。これにより、ロボット装置８１は、当該制御情報に基づいて、計測対象物体を把持、又は吸着する。
【０１０７】
なお、本発明は、例えば、システム、装置、方法、プログラム若しくは記憶媒体等としての実施態様を採ることもできる。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給する。そして、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ、ＧＰＵ等）がプログラムを読み出して実行する処理である。
【技術分野】
【０００１】
本発明は、位置姿勢計測装置、その計測処理方法及びプログラムに関する。
【背景技術】
【０００２】
近年、ロボット技術の発展とともに、これまで人間が行なっていた複雑なタスク（例えば、工業製品の組み立て）をロボットが代わりに行なうようになりつつある。このようなロボットは、ハンドなどのエンドエフェクタによって部品を把持して組み立てを行なう。この組み立てに際しては、把持の対象となる部品とロボット（ハンド）との間の相対的な位置及び姿勢を計測する必要がある。
【０００３】
位置及び姿勢を計測する方法としては、撮像装置で撮像した二次元画像から検出される特徴に対してモデルフィッティングする方法が知られている。また、距離センサにより得られた距離画像の各点を三次元点群データに変換して当該三次元点群に対してモデルフィッティングする方法等も知られている。
【０００４】
二次元画像に対するモデルフィッティングでは、物体の位置及び姿勢に基づいて三次元形状モデルを二次元画像上に投影した投影像を、二次元画像から検出された特徴にフィティングさせることにより物体の位置及び姿勢を計測する。
【０００５】
距離画像に対するモデルフィッティングでは、距離データを表現する距離画像の各点を三次元座標を持つ三次元点群データに変換し、三次元形状モデルに当該三次元点群データをフィッティングさせることにより物体の位置及び姿勢を計測する。この他、二次元画像から得られる計測情報と距離画像から得られる計測情報とを併用して物体の位置及び姿勢を計測する方法も知られている。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】R.Y.Tsai,“A versatile camera calibration technique for high-accuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses,"IEEE Journal of Robotics and Automation,vol.RA-3,no.4,1987.
【非特許文献２】L.Vacchetti,V.Lepetit,and P.Fua,“Combining edge and texture information for real-time accurate 3D camera tracking,"Proc.3rd IEEE/ACM International Symposium on Mixed and Augmented Reality(ISMAR'04),pp.48-57,2004.
【発明の概要】
【発明が解決しようとする課題】
【０００７】
二次元画像から得られる計測情報と距離画像から得られる計測情報とを併用して物体の位置及び姿勢を計測するように構成した場合、一般に、その計測精度の向上が期待できる。
【０００８】
しかし、計測を行なうときの状態（例えば、物体の形状や物体の置かれ方）によっては、二次元画像から得られる計測情報、又は距離画像から得られる計測情報のいずれかを利用して計測を行なった場合の方が、精度良く計測を行なえる場合がある。
【０００９】
また、二次元画像から得られる計測情報、又は距離画像から得られる計測情報のいずれかを利用して計測を行なうだけで計測精度を十分に満たせる場合もある。このような場合、両手法を併用した計測を行なう必要はない。計算時間や計算リソース等を考慮すると、却って処理が無駄となる。
【００１０】
本発明は、上記課題に鑑みてなされたものであり、計測対象物体の位置及び姿勢に関する複数の計測手法の中からいずれかを選択して実施するようにした技術を提供することを目的とする。
【課題を解決するための手段】
【００１１】
上記課題を解決するため、本発明の一態様は、計測対象物体の位置及び姿勢の計測を行なう位置姿勢計測装置であって、撮像装置により撮像された前記計測対象物体の二次元画像を入力する入力手段と、距離センサにより計測された前記計測対象物体の距離データを取得する取得手段と、前記二次元画像から前記計測対象物体の画像特徴を検出する検出手段と、前記計測対象物体の状態を判定する判定手段と、前記判定手段により判定された状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定するモード設定手段と、前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう計測手段とを具備することを特徴とする。
【発明の効果】
【００１２】
本発明によれば、計測対象物体の位置及び姿勢の計測に関する複数の計測手法の中からいずれかを選択して実施することができる。
【図面の簡単な説明】
【００１３】
【図１】本実施形態に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図２】三次元形状モデルの一例を示す図。
【図３】図１に示す位置姿勢計測部４６の機能的な構成の一例を示す図。
【図４】図１に示す位置姿勢計測装置４０の動作の一例を示すフローチャート。
【図５】エッジ検出の概要の一例を示す図。
【図６】第１の計測モード設定時における位置姿勢算出処理の流れの一例を示すフローチャート。
【図７】投影像（線分）と検出されたエッジとの関係の一例を示す図。
【図８】二次元画像上における誤差を次元変換させる処理の概要の一例を示す図。
【図９】本実施形態に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図１０】実施形態２に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図１１】実施形態３に係わる位置姿勢計測装置４０の機能的な構成の一例を示す図。
【図１２】ロボットシステムを制御する制御システムの構成の一例を示す図。
【発明を実施するための形態】
【００１４】
以下、本発明の一実施の形態について添付図面を参照して詳細に説明する。
【００１５】
（実施形態１）
図１は、本実施形態に係わる位置姿勢計測装置４０における機能的な構成の一例を示す図である。
【００１６】
位置姿勢計測装置４０は、計測対象物体を模したモデルデータ（三次元形状モデル）の形状に基づいて、計測対象物体（以下、単に物体と呼ぶ場合もある）の位置及び姿勢を計測（算出）する処理を切り替えて行なう。詳細については後述するが、第１のモード（本実施形態においては、第１の計測モード）又は第２のモード（本実施形態においては、第２の計測モード）のいずれかを設定し、そのモードに従って計測対象物体の位置及び姿勢を計測する。第１の計測モードでは、二次元画像から得られる計測情報（画像特徴）と距離画像から得られる計測情報（三次元点群データ）とを同時に利用したモデルフィッティングを行なう。第２の計測モードでは、二次元画像から得られる計測情報（画像特徴）のみを利用したモデルフィッティングを行なう。
【００１７】
ここで、位置姿勢計測装置４０は、二次元画像撮像部２０と、距離画像計測部３０とに接続されている。
【００１８】
二次元画像撮像部２０は、二次元画像を撮像する役割を果たし、例えば、撮像装置（例えば、カメラ）で実現される。撮像される二次元撮像画像は、濃淡画像であっても良いし、また、カラー画像であっても良い。
【００１９】
距離画像計測部３０は、計測対象である物体表面までの距離を示す距離画像を計測する役割を果たし、例えば、距離センサで実現される。距離センサは、距離画像を出力する。距離画像は、各画素が視点位置（距離センサの位置）からの奥行きの情報を持つ画像であり、距離センサから物体までの距離データを表現する。距離センサには、対象に照射したレーザ光やスリット光の反射光をカメラで撮像し、三角測量により距離を計測するアクティブ式を採用すれば良い。なお、距離センサは、距離画像を撮像できればよく、その方式は特に問わない。例えば、光の飛行時間を利用するＴｉｍｅ−ｏｆ−ｆｌｉｇｈｔ方式を採用しても良いし、ステレオカメラが撮像した画像から三角測量によって各画素の奥行きを計算するパッシブ式を採用しても良い。
【００２０】
ここで、位置姿勢計測装置４０は、三次元形状モデル保持部４５と、概略値入力部４４と、二次元画像入力部４１と、画像特徴検出部４２と、三次元座標情報取得部４３と、モード設定部４７と、位置姿勢計測部４６とを具備して構成される。
【００２１】
二次元画像入力部４１は、二次元画像撮像部２０により撮像された画像を装置内（位置姿勢計測装置４０）に入力する。撮像装置の焦点距離や主点位置、レンズ歪みパラメータなどの内部パラメータは、事前にキャリブレーションしておいてもよい（非特許文献１参照）。
【００２２】
三次元座標情報取得部４３は、距離画像計測部３０により計測された距離画像に基づいて三次元点群データ（計測対象物体の表面形状を示す情報）を生成し、それを装置内に入力する。三次元座標情報取得部４３は、距離画像の各画素をカメラ座標系（二次元画像撮像部２０の座標系）における三次元点群データに変換し、それを三次元座標情報として取得する。三次元点群データへの変換は、距離センサ（距離画像計測部３０）とカメラ（二次元画像撮像部２０）との相対的な位置姿勢（既知の情報）に基づいて行なわれる。より具体的には、距離画像の各画素について、画素位置に対応する視線ベクトルに奥行き値を乗算することにより上記変換を行なう。なお、距離センサ（距離画像計測部３０）及びカメラ（二次元画像撮像部２０）はそれぞれ固定されており、両者の相対的な位置姿勢は変化しないと仮定し、事前にキャリブレーションしておいてもよい。例えば、三次元形状が既知であるキャリブレーション物体を様々な方向から観察し、二次元画像に基づいたキャリブレーション物体の位置姿勢と、距離画像に基づいたキャリブレーション物体の位置姿勢との差に基づいて相対的な位置姿勢を求める。なお、二次元画像撮像部２０による二次元画像の撮像と距離画像計測部３０による距離画像の計測とは、同時に行なわれるのが望ましい。但し、計測対象物体が静止している場合など、位置姿勢計測装置４０と計測対象物体との間の位置及び姿勢の関係が変化しない場合には、必ずしも同時に行なう必要はない。
【００２３】
三次元形状モデル保持部４５は、位置及び姿勢の計測対象である物体（計測対象物体）を模した三次元形状のモデルデータ（三次元形状モデル）を表す情報を保持する。ここで、図２を用いて、本実施形態に係わる三次元形状モデルの一例について説明する。三次元形状モデルは、点の集合及び各点を結んで構成される線分の集合によって定義される。三次元形状モデル１０は、図２（ａ）に示すように、点Ｐ１〜点Ｐ１４の１４点から構成される。また、三次元形状モデル１０は、図２（ｂ）に示すように、線分Ｌ１〜Ｌ１６により構成されている。点Ｐ１〜点Ｐ１４は、図２（ｃ）に示すように、三次元座標値により表される。また、線分Ｌ１〜Ｌ１６は、図２（ｄ）に示すように、線分を構成する点のＩＤ（識別子）により表される。
【００２４】
なお、ここでは図示を省略するが、三次元形状モデルは、面の情報も保持する。各面は、各面を構成する点のＩＤにより表される。図２に示す三次元形状モデルでは、直方体を構成する６つの面の情報が定義されている。三次元形状モデルは、位置姿勢計測部４６により物体の位置及び姿勢が計測される時や、モード設定部４７によるモード設定時に用いられる。
【００２５】
概略値入力部４４は、位置姿勢計測装置４０に対する物体の位置及び姿勢の概略値を入力する。本実施形態において、位置姿勢計測装置４０に対する物体の位置及び姿勢とは、二次元画像撮像部２０を基準とした物体の位置及び姿勢を示すが、必ずしも二次元画像撮像部２０を基準にする必要はない。例えば、二次元画像撮像部２０の座標系に対する物体の相対的な位置及び姿勢が既知であり、且つその位置及び姿勢が変化しないのであれば、位置姿勢計測装置４０におけるその他の部分を基準としてもよい。また、本実施形態においては、物体の位置及び姿勢の概略値として、位置姿勢計測装置４０が当該物体から過去（例えば、直前）に計測した計測値を用いる。但し、物体の位置及び姿勢の概略値は、必ずしもこのような値である必要はない。例えば、過去に計測した物体の位置及び姿勢の計測値に対して時系列フィルタリング処理（例えば、線形フィルタ、カルマンフィルタ）を実施し、物体の運動速度や角速度を推定する。そして、その推定結果に基づいて物体の位置及び姿勢を予測した値を概略値としてもよい。また、センサから得られる物体の位置及び姿勢を概略値としてもよい。ここで、センサは、物体の位置及び姿勢を６自由度で計測できればよく、その方式（例えば、磁気式、光学式、超音波式）は、特に問わない。なお、物体の置かれているおおよその位置や姿勢をパターンマッチングなどにより求めてもよいし、また、物体の置かれているおおよその位置や姿勢が予め分かっているのであれば、その値を概略値として用いてもよい。
【００２６】
画像特徴検出部４２は、二次元画像入力部４１により入力された二次元画像から計測対象物体を示す画像特徴を検出する。本実施形態においては、画像特徴としてエッジ（濃度勾配の極値）を検出する場合を例に挙げる。
【００２７】
モード設定部４７は、計測対象物体の形状（三次元形状モデル保持部４５により保持される三次元形状モデルの形状）に基づいて、計測対象物体の位置及び姿勢を計測するモードを切り替える。モード設定部４７には、三次元形状モデルの形状を類推する形状類推部４８が設けられる。モード設定部４７おいては、第１のモード（本実施形態においては、第１の計測モード）及び第２のモード（本実施形態においては、第２の計測モード）を切り替えて設定する。第１の計測モード設定時には、二次元画像入力部４１により入力された二次元画像から得られる計測情報と、三次元座標情報取得部４３により取得された距離画像から得られる計測情報とを（同時に）利用したモデルフィッティングが行なわれる。第２の計測モード設定時には、二次元画像入力部４１により入力された二次元画像（のみ）を利用したモデルフィッティングが行なわれる。
【００２８】
ここで、二次元画像から得られる計測情報と距離画像から得られる計測情報とを利用したモデルフィッティング（第１の計測モード）は、物体の位置及び姿勢を高精度に計測できる。その反面、距離センサの特性上、物体端部の計測データが部分的に欠損してしまう場合がある。そのため、例えば、物体が小さい、物体が細い、物体に凹凸が多い等の場合には、物体の端部の距離計測結果の信用性は低い。また、距離センサの奥行き方向の分解能には限界がある。例えば、物体が紙のように薄い場合、その下にある台や他の物体とほとんど同じ距離データが取得されることがあり、これは誤計測の要因となる。
【００２９】
これらのケースから、計測対象物体の形状によっては、二次元画像と距離画像とを利用したモデルフィッティング（第１の計測モード）よりも、距離画像は使用せずに、二次元画像を利用したモデルフィッティング（第２の計測モード）により計測を行なう方が精度良く計測を行なえる場合がある。また、物体の位置及び姿勢の計測時における距離画像の寄与度が小さい場合もある。更に、二次元画像を利用した計測を行なうだけで計測精度を十分に満たせる場合もある。このように距離画像が有意でない場合、二次元画像のみを用いて物体の位置及び姿勢の計測を行なった方が、計算時間や計算リソースの抑制を図れる他、計測精度が向上する場合もある。
【００３０】
そこで、本実施形態においては、計測対象物体の三次元形状モデルの形状を類推し、その類推された形状に基づいて計測モード（第１の計測モード、第２の計測モード）を切り替えて設定する。そして、その設定された計測モードにより物体の位置及び姿勢の計測を行なう。
【００３１】
位置姿勢計測部４６は、モード設定部４７により設定されたモードに従って計測対象物体の位置及び姿勢を計測（算出）する。第１の計測モード設定時には、画像特徴検出部４２により検出された画像特徴に対して、三次元形状モデル保持部４５により保持された三次元形状モデルをフィッティングさせる。また、第１の計測モード設定時には、三次元座標情報取得部４３により取得された三次元点群に対しても、三次元形状モデルをフィッティングさせる。これにより、位置姿勢算出処理を実施し、計測対象物体の位置及び姿勢を計測（算出）する。一方、第２の計測モード設定時には、画像特徴検出部４２により検出された画像特徴に対して、三次元形状モデル保持部４５により保持された三次元形状モデルをフィティングさせる。これにより、位置姿勢算出処理を実施し、計測対象物体の位置及び姿勢を計測（算出）する。
【００３２】
次に、図３を用いて、図１に示す位置姿勢計測部４６における機能的な構成の一例について説明する。
【００３３】
位置姿勢計測部４６は、二次元画像ずれ算出部４６１と、三次元空間ずれ算出部４６２と、尺度変換部４６３と、位置姿勢補正部４６４とを具備して構成される。
【００３４】
二次元画像ずれ算出部４６１は、二次元画像上で検出された画像特徴（エッジ）と、二次元画像上における三次元形状モデルの投影像（線分）との二次元画像上における距離を算出する。なお、三次元形状モデルの投影像は、計測対象物体の位置及び姿勢の概略値と、二次元画像撮像部２０の内部パラメータ（例えば、キャリブレーション後の値）とに基づいて投影される。
【００３５】
三次元空間ずれ算出部４６２は、距離画像に基づいて生成された三次元点群データを構成する各点と、三次元形状モデルの面との三次元空間中における距離を算出する。
【００３６】
尺度変換部４６３は、二次元画像ずれ算出部４６１及び三次元空間ずれ算出部４６２により算出された双方の距離の尺度を一致させる。より具体的には、二次元画像上での対応点（点）と投影像（線分）の符号付距離を算出し、三次元空間中での三次元点群データにおける点と三次元形状モデルの平面との符号付距離を算出し、双方の符号付距離を物体の位置及び姿勢の関数として線形近似する。そして、符号付距離を０としたときに各計測情報について成り立つ線形方程式を用意する。
【００３７】
位置姿勢補正部４６４は、この線形方程式を連立方程式として解くことにより、物体の位置及び姿勢の微小変化を求める。そして、その求めた値に基づいて物体の位置及び姿勢を示す計測値を補正する。この処理を繰り返すことにより最終的な計測値を算出する。
【００３８】
以上が、位置姿勢計測装置４０の構成の一例についての説明である。なお、上記説明した、位置姿勢計測装置４０には、コンピュータが内蔵されている。コンピュータには、ＣＰＵ等の主制御手段、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disk Drive）等の記憶手段が具備される。また、コンピュータにはその他、ボタンやディスプレイ又はタッチパネル等の入出力手段、ネットワークカード等の通信手段等も具備されていてもよい。なお、これら各構成部は、バス等により接続され、主制御手段が記憶手段に記憶されたプログラムを実行することで制御される。
【００３９】
次に、図４を用いて、図１に示す位置姿勢計測装置４０において計測対象物体の位置及び姿勢を計測する際の処理の流れの一例について説明する。
【００４０】
（Ｓ１０１）
位置姿勢計測装置４０は、概略値入力部４４において、位置姿勢計測装置４０に対する物体の位置及び姿勢の概略値を入力する。本実施形態においては、上述した通り、物体の位置及び姿勢の概略値として、位置姿勢計測装置４０が当該物体から過去（例えば、直前）に計測した計測値を入力する。
【００４１】
（Ｓ１０２）
位置姿勢計測装置４０は、二次元画像入力部４１において、二次元画像撮像部２０により撮像された二次元画像を入力する。
【００４２】
（Ｓ１０３）
位置姿勢計測装置４０は、三次元座標情報取得部４３において、距離画像計測部３０により計測された三次元座標情報を取得する。上述した通り、三次元座標情報取得部４３は、距離画像の各画素をカメラ座標系（二次元画像撮像部２０の座標系）における三次元点群データに変換し、それを三次元座標情報として取得する。
【００４３】
（Ｓ１０４）
ここで、位置姿勢計測装置４０は、画像特徴検出部４２において、Ｓ１０２の処理で入力された二次元画像から計測対象物体を示す画像特徴を検出する。本実施形態においては、上述した通り、画像特徴としてエッジを検出する。
【００４４】
ここで、図５を用いて、本実施形態に係わるエッジ検出の概要について説明する。エッジ検出処理では、まず、計測対象物体が撮像された二次元画像に対して三次元形状モデルを投影する。この投影像（線分）は、Ｓ１０１の処理で入力された計測対象物体の位置及び姿勢の概略値と、二次元画像撮像部２０の内部パラメータ（例えば、キャリブレーション後の値）とに基づいて求められる。次に、投影された各線分５１上において、等間隔に制御点５２を設定する。そして、投影線分５１の法線方向に平行で且つ制御点５２を通過する線分（以下、探索ライン）５３上からエッジ５６を１次元探索する（図５（ａ））。エッジは、画素値の濃度勾配の極値となる（図５（ｂ））。場合によっては、探索ライン５３上から複数のエッジが検出されることがある。この場合、本実施形態においては、複数検出されたエッジを仮説として保持する（非特許文献２参照）。このようにして各制御点５２に対応するエッジ５６を探索する。
【００４５】
（Ｓ１０５）
位置姿勢計測装置４０は、形状類推部４８において、三次元形状モデル保持部４５により保持される三次元形状モデルの形状を類推する。三次元形状モデルの形状は、例えば、三次元形状モデルを包含するバウンディングボックス（直方体）で代替してもよい。バウンディングボックスは、三次元形状モデルが属する三次元座標系のｘｙｚ軸それぞれに垂直に交わる面をバウンディングボックスの各面として有する。なお、このバウンディングボックスの各面は、ｘｙｚ軸上で三次元形状モデルが含まれる最大値や最小値に基づいて決められる（Ａｘｉｓ−Ａｌｉｇｎｅｄ−Ｂｏｕｎｄｉｎｇ−Ｂｏｘ）。また、三次元形状モデルに適合した形でバウンディングボックスを形成しても良い。形状類推部４８においては、三次元形状モデルを代替するバウンディングボックスを求めた後、その各辺の長さ（ｘ１、ｙ１、ｚ１）や、辺の比（ｘ１／ｙ１、ｘ１／ｚ１、ｙ１／ｘ１、ｙ１／ｚ１、ｚ１／ｘ１、ｚ１／ｙ１）から三次元形状モデルの形状を類推する。なお、三次元形状モデルの形状の類推は、バウンディングボックスを用いずに行なっても良い。例えば、予め用意された要素モデル（プリミティブモデル）の組み合わせ方から求めてもよい。この方法を採用した場合、三次元形状モデルに凹凸が多いか少ないかを類推できる。
【００４６】
ここで、三次元形状モデルの形状の類推は、例えば、以下のルールに従って行なう。
【００４７】
１）．各辺の長さが全て、予め定められた第１の閾値（例えば、１ｃｍ）よりも小さい場合は、三次元形状モデルは小さい形状であると類推する。
【００４８】
２）．辺の比のうちの２つが、予め定められた第２の閾値（例えば、１０）よりも大きく、且つ辺の比のうちの上記以外の２つが、予め定められた第３の閾値（例えば、０．１）よりも小さい。更に、各辺の長さのうちの１つが、予め定められた第４の閾値（例えば、３ｃｍ）よりも大きい場合は、三次元形状モデルは、細い形状であると類推する。
【００４９】
３）．辺の比のうちの２つが、予め定められた第２の閾値（例えば、１０）よりも大きく、且つ辺の比のうちの上記以外の２つが、予め定められた第３の閾値（例えば、０．１）より小さい。更に、各辺の長さのうちの２つが、予め定められた第４の閾値（例えば、３ｃｍ）よりも大きい場合は、三次元形状モデルは、薄い形状であると類推する。
【００５０】
（Ｓ１０６）
次に、位置姿勢計測装置４０は、モード設定部４７において、Ｓ１０５の処理で類推された三次元形状モデルの形状に基づいて、計測モードの設定を行なう。すなわち、計測対象物体（三次元形状モデル）の形状に基づいて、距離画像により得られた計測情報を用いた計測を行なうか否かを判定し、その判定結果に基づいて第１の計測モードか第２の計測モードかのいずれかを設定する。初期モードは、第１の計測モードとする。
【００５１】
例えば、三次元形状モデルの形状が、小さい、細い、凹凸が多い、薄い等の少なくともいずれかの場合は、第１の計測モードから第２の計測モードに切り替える。これは、上述した通り、距離センサ（距離画像計測部３０）による計測に誤りが含まれている可能性が高いためである。第２の計測モードでは、距離画像から得られる計測情報を用いずに物体の位置及び姿勢を計測するため、距離センサによる計測に誤りが含まれている場合であっても、精度良く計測が行なえる。なお、三次元形状モデルの形状が、小さい、細い、凹凸が多い、薄い等以外の場合は、第１の計測モードを設定（維持）する。
【００５２】
（Ｓ１０７）
位置姿勢計測装置４０は、位置姿勢計測部４６において、Ｓ１０６の処理で設定されたモードに従って計測対象物体の位置及び姿勢を計測（算出）する。詳細については後述するが、Ｓ１０６の処理で設定されたモードに基づき、Ｓ１０４の処理で検出された画像特徴（エッジ）、Ｓ１０３の処理で入力された三次元座標情報（三次元点群）の少なくともいずれかに対して、三次元形状モデルをフィッティングさせる。これにより、計測対象物体の位置及び姿勢を求める。
【００５３】
ここで、図４のＳ１０７に示す位置姿勢計測部４６における動作の一例について説明する。まず、第１の計測モード設定時における処理の流れについて説明する。第１の計測モードでは、二次元画像上の距離（エッジと投影像との距離）を三次元空間中の距離に変換し、尺度を統一する。これにより、両手法の寄与率が偏ることをなくす。
【００５４】
図６には、第１の計測モード設定時における位置姿勢算出処理の流れの一例が示される。この処理では、ガウス−ニュートン（Gauss-Newton）法により反復演算し、計測対象物体の位置及び姿勢（以下、６次元ベクトルｓで表す）の概略値を補正する。これにより、計測対象物体の位置及び姿勢を求める。なお、位置姿勢算出処理は、ガウス−ニュートン法に限られない。例えば、より計算がロバストであるＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いてもよいし、よりシンプルな方法である最急降下法を用いてもよい。また、共役勾配法やＩＣＣＧ法など、他の非線形最適化計算手法を用いてもよい。
【００５５】
（Ｓ２０１）
位置姿勢計測装置４０は、位置姿勢計測部４６において、まず、初期化処理を行なう。この初期化処理では、例えば、図４のＳ１０１の処理で得られた計測対象物体の位置及び姿勢の概略値を取得する。
【００５６】
（Ｓ２０２）
位置姿勢計測装置４０は、位置姿勢計測部４６において、画像特徴（エッジ）及び三次元点群データ（距離画像に基づく三次元点群データ）と、三次元形状モデルとをそれぞれ対応付ける。この処理では、まず、Ｓ２０１の処理で取得した概略値に基づいて三次元形状モデルの各線分を二次元画像上へ投影し、当該投影された線分に対して、二次元画像上で最も近い画像特徴（エッジ）を制御点と対応付ける。これにより、画像特徴（エッジ）と三次元計上モデルとを対応付ける。
【００５７】
また、Ｓ２０１の処理で取得した概略値に基づいて三次元形状モデル又は三次元点群データを座標変換し、三次元点群データの各点に対して、三次元空間中で最も近傍する三次元形状モデルの面を探索する。これにより、三次元点群データを三次元形状モデルと対応付ける。
【００５８】
（Ｓ２０３）
位置姿勢計測装置４０は、位置姿勢計測部４６において、線形連立方程式を解くための係数行列と、誤差ベクトル（エッジ、三次元点群データ）との算出を行なう。ここで、係数行列の各要素は、Ｓ２０１の処理で取得した概略値の微小変化に対する一次の偏微分係数である。エッジについての要素は、対応点（点）と投影像（線分）との二次元画像上での距離を位置姿勢の関数とした一次の偏微分係数である。三次元点群データについての要素は、三次元点群データにおける点と三次元形状モデルの平面との三次元空間中における距離を位置姿勢の関数とした一次の偏微分係数である。
【００５９】
ここで、係数行列の算出方法について説明する。図７は、投影像（線分）と、検出されたエッジとの関係の一例を示す図である。この場合、画像の水平方向をｕ軸６１とし、垂直方向をｖ軸６２としている。ある制御点（投影された各線分を画像上で等間隔に分割した点）６３の画像上における座標６４を（ｕ０，ｖ０）と表す。当該制御点が属する線分Ｌの画像上における傾きを（ｕ軸に対する傾き）θと表す。傾き６５は、線分（投影線分）６６の両端の三次元座標を、位置及び姿勢ｓに基づいて画像上（二次元上）に投影し、その両端の座標を結んだ直線の傾きを示している。線分６６の画像上での法線ベクトルは（ｓｉｎθ，−ｃｏｓθ）となる。また、制御点と対応する点（対応点６７）の画像上における座標６８を（ｕ’，ｖ’）とする。ここで、対応点６７の座標（ｕ’，ｖ’）を通り、傾き６５がθである直線（破線）上の点（ｕ，ｖ）は、
【００６０】
制御点６３の画像上における位置は、計測対象物体の位置及び姿勢により変化する。計測対象物体の位置及び姿勢ｓは、６自由度である。すなわち、ｓは６次元ベクトルであり、計測対象物体の位置を表す３つの要素と、姿勢を表す３つの要素とからなる。姿勢を表す３つの要素は、例えば、オイラー角による表現や、方向が原点を通る回転軸を表し大きさが回転角を表す三次元ベクトルなどによって表現される。計測対象物体の位置及び姿勢により変化する点の画像上での座標は、制御点６３（すなわち、座標（ｕ０，ｖ０）６４）の近傍で１次のテイラー展開によって式（２）のように近似できる。但し、Δｓｉ（ｉ＝１，２，・・・，６）は、ｓの各成分の微小変化を表す。
【００６１】
物体の位置及び姿勢の概略値と、実際の物体の位置及び姿勢との差がそれほどないと仮定すると、正しいｓによって得られる制御点６３の画像上での位置は、式（１）が表す直線上にあると仮定できる。式（２）によって近似されるｕ、ｖを式（１）に代入することにより、式（３）が得られる。式（３）は、Ｓ２０２の処理で対応付けされた全てのエッジについて立式できる。
【００６２】
二次元画像撮像部２０の座標系で表される三次元点群データの三次元座標は、計測対象物体の位置及び姿勢ｓを用いて、計測対象物体の座標系における三次元座標（ｘ，ｙ，ｚ）に変換することができる。Ｓ２０１の処理で取得した概略値により、三次元点群データ中のある点が計測対象物体の座標系の座標（ｘ０，ｙ０，ｚ０）に変換されるとする。（ｘ，ｙ，ｚ）は、計測対象物体の位置及び姿勢により変化するため、（ｘ０，ｙ０，ｚ０）の近傍で１次のテイラー展開によって式（４）のように近似できる。
【００６３】
Ｓ２０２の処理により、三次元点群データ中のある点に対応付けられた三次元形状モデルの面に対する計測対象物体の座標系における方程式を「ａｘ＋ｂｙ＋ｃｚ＝ｅ（ａ２＋ｂ２＋ｃ２＝１、ａ，ｂ，ｃ，ｅは定数）」とする。正しいｓによって変換される（ｘ，ｙ，ｚ）は、平面の方程式「ａｘ＋ｂｙ＋ｃｚ＝ｅ」を満たすと仮定する。式（４）を平面の方程式に代入すると式（５）が得られる。式（５）は、Ｓ２０２の処理で対応付けされた全ての三次元点群データについて立式することができる。
【００６４】
式（３）、式（５）は、ｓの各成分の微小変化△ｓｉ（ｉ＝１，２，・・・，６）についての方程式である。そのため、式（６）のような△ｓｉに関する線形連立方程式が成り立つ。
【００６５】
式（６）の右辺の誤差ベクトルは、上述した通り、エッジについては二次元画像上での符号付距離であり、三次元点群データについては三次元空間中での符号付距離である。そのため、両者の距離の尺度は一致していない。そこで、二次元画像上での誤差にエッジの奥行きを乗じることにより、二次元画像上での誤差を三次元空間中での誤差に近似させる。具体的には、エッジについての方程式の両辺に、エッジの奥行きを乗じる。これにより、誤差の尺度を三次元空間中の距離に統一する。なお、二次元画像からは奥行き情報を得ることはできないため、エッジの奥行きは何らかの近似的な方法により求める必要がある。
【００６６】
図８を用いて、二次元画像上の誤差７１を三次元空間中の誤差７２に近似させる方法の一例について説明する。本実施形態においては、物体の位置及び姿勢の概略値に基づいて算出された各制御点７３の視点７４からの奥行き７５を、二次元画像上の誤差７１に乗じる。これにより、二次元画像上の誤差７１を三次元空間中の誤差７２に変換する。また、奥行き７５の代わりに、三次元空間中の制御点７３から画像面７６におけるエッジ７７を通過する視線ベクトルに対して下ろした垂線の長さとなるようなスケーリング係数を乗じるようにしてもよい。
【００６７】
ここで、解くべき連立方程式は、式（７）になる。
ここで、式（７）を式（８）のように表す。
線形連立方程式の係数行列Ｊを算出するため、偏微分係数を算出する。
【００６８】
（Ｓ２０４）
続いて、位置姿勢計測装置４０は、位置姿勢計測部４６において、式（８）をもとに、行列Ｊの一般化逆行列（ＪＴ・Ｊ）−１・ＪＴを用いて補正値Δｓを求める。ここで、エッジや三次元点群データには、誤検出などによる外れ値が多いため、次に述べるようなロバスト推定手法を用いる。一般に、外れ値であるエッジや点群データでは、式（７）の右辺の誤差ベクトルの値が大きくなる。そこで、誤差の絶対値が大きいデータに対しては小さな重みを与え、誤差の絶対値が小さいデータに対しては大きな重みを与える。重みは、例えば、式（９）に示すようなＴｕｋｅｙの関数により与える。
【００６９】
なお、重みを与える関数は、Ｔｕｋｅｙの関数である必要はなく、誤差の絶対値が大きいデータに対して小さな重みを与え、誤差の絶対値が小さいデータに対して大きな重みを与える関数であれば良い。そのため、例えば、Ｈｕｂｅｒの関数などを用いてもよい。
【００７０】
各計測情報（エッジ又は点群データ）に対応する重みをｗｉとする。ここで、式（１０）のように重み行列Ｗを定義する。
重み行列Ｗは、対角成分以外は全て「０」の正方行列であり、対角成分には重みｗｉが入る。この重み行列Ｗを用いて、式（８）を式（１１）のように変形する。
式（１２）のように式（１１）を解くことにより補正値Δｓを求める。
（Ｓ２０５）
位置姿勢計測装置４０は、位置姿勢計測部４６において、Ｓ２０４の処理で算出された補正値Δｓにより、Ｓ２０１の処理で得られた概略値を補正する。これにより、計測対象物体の位置及び姿勢を求める。
ｓ←ｓ＋△ｓ
（Ｓ２０６）
位置姿勢計測装置４０は、位置姿勢計測部４６において、計測対象物体の位置及び姿勢の計測値が収束しているか否かの判定を行なう。収束していれば、この処理は終了し、そうでなければ、再度、Ｓ２０２の処理に戻る。なお、収束しているか否かは、補正値Δｓがほぼ０である場合や、誤差ベクトルの二乗和の補正前と補正後の差がほぼ０である場合に収束したと判定する。
【００７１】
次に、第２の計測モード設定時における処理の流れについて説明する。第２の計測モードでは、上述した式（３）を解くことで物体の位置及び姿勢を算出する。第２の計測モード設定時には、距離画像に基づき生成した三次元点群データを物体の位置及び姿勢の計測に用いないため、図３に示す三次元空間ずれ算出部４６２では処理を行なわない。また、尺度変換部４６３においても、距離のずれの尺度の変換を行なわない。
【００７２】
また、第２の計測モード設定時には、図６で説明した第１の計測モード設定時と同様の流れで処理が行なわれるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ２０２の処理で三次元点群データの対応付けを行なわない点や、Ｓ２０３の処理で尺度変換を行わない点等である。
【００７３】
以上説明したように実施形態１によれば、三次元形状モデルの形状に基づいてモードを設定し、当該モードに従って計測対象物体の位置及び姿勢を計測（算出）する。これにより、計測対象物体の形状に応じた手法で計測を行なえるため、計測の精度向上や高速化、ロバスト性の向上を図れる。
【００７４】
なお、上述した実施形態１においては、三次元形状モデルの形状に基づいてモードを切り替えて設定していたが、これに限られない。例えば、距離画像計測部３０により計測された距離画像に基づいてモードを設定するように構成してもよい。この場合、位置姿勢計測装置４０は、図９に示すように、モード設定部４７には、三次元座標情報取得部４３により取得された三次元点群データが入力される。これにより、形状類推部４８においては、距離画像に基づき生成された三次元点群データに基づいて計測対象物体の形状を類推し、モード設定部４７においては、当該類推された形状に基づいてモードを設定することができる。
【００７５】
（実施形態２）
次に、実施形態２について説明する。実施形態２においては、三次元形状モデルの形状と、距離画像から得られる計測情報とに基づいて計測モードを設定する場合について説明する。
【００７６】
図１０を用いて、実施形態２に係わる位置姿勢計測装置４０における機能的な構成の一例について説明する。なお、実施形態１を説明した図１と同一の符号が付された構成は、基本的には、実施形態１同様の機能を果たす。ここでは、実施形態１との相違点について重点的に説明する。
【００７７】
実施形態１との相違点としては、モード設定部４７におけるモード設定処理にある。具体的には、モード設定部４７では、三次元形状モデル保持部４５により保持される三次元形状モデルの形状と、三次元座標情報取得部４３により取得される三次元点群データとに基づいてモードを設定する。
【００７８】
モード設定部４７においては、これら両データの比較を行うために、三次元点群データ中の各々の点から最も近い三次元形状モデル上の表面に当該各々の点を対応付ける。対応付けの結果、三次元形状モデルから三次元空間的に離れた点が所定数より多ければ、三次元形状モデルと距離画像における距離データとが大きく異なると判定し、第２の計測モードを設定する。すなわち、二次元画像から得られる計測情報を利用した物体の位置及び姿勢の計測が行なわれることになる。
【００７９】
三次元形状モデルから三次元空間的に離れた点が所定数以内であれば、三次元形状モデルと距離画像における距離データとが比較的似ていると判定し、第１の計測モードを設定する。すなわち、二次元画像から得られる計測情報と距離画像から得られる計測情報とを利用した物体の位置及び姿勢の計測が行なわれることになる。なお、これら両データの比較を行なう前に、三次元点群データの中から計測対象物体の三次元点群データのみをセグメンテーションで選択するように構成してもよい。
【００８０】
なお、実施形態２に係わる位置姿勢計測装置４０における処理は、実施形態１で説明した図４と同様の流れとなるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ１０５の処理を省く点とＳ１０６の処理が異なる点とが挙げられる。具体的には、Ｓ１０６の処理において、モード設定部４７は、三次元形状モデル保持部４５により保持される三次元形状モデルの形状と、三次元座標情報取得部４３により取得される三次元点群データとに基づいてモード設定を行なう。
【００８１】
以上説明したように実施形態２によれば、三次元形状モデルの形状と、距離画像から得られる計測情報（距離画像に基づき生成された三次元点群データ）とを比較し、その比較結果に基づいて計測モードを設定する。具体的には、三次元形状モデルの形状と、距離画像から得られる計測情報とを比較する。そして、その比較結果に基づいて三次元形状モデルと大きく異なる距離データが計測されたと判定した場合、距離画像を信用できないため、第２の計測モードを設定する。そうでなければ、第１の計測モードを設定する。
【００８２】
これにより、実施形態２の構成によれば、実施形態１の構成よりも更に適切な計測モードで物体の位置及び姿勢の計測が行なえる。
【００８３】
（実施形態３）
次に、実施形態３について説明する。実施形態３においては、二次元画像上の画像特徴に基づいて計測モードを設定する場合について説明する。また、実施形態３においては、上述した第１の計測モード（第１のモード）と、新たな第３の計測モード（第２のモード）との間でモードを切り替えて設定する。第３の計測モードは、二次元画像から得られる計測情報は利用せず、距離画像から得られる計測情報を利用したモデルフィッティングを行なう。
【００８４】
ここで、二次元画像から得られる計測情報（画像特徴）と距離画像から得られる計測情報（三次元点群データ）とを利用したモデルフィッティング（第１の計測モード）は、物体の位置及び姿勢を高精度に計測できる。しかし、二次元画像から得られる画像特徴を利用したモデルフィッティングを行なう場合には、二次元画像上にある程度の画像特徴が存在していないといけない。そのため、例えば、計測対象物体の表面に模様（テクスチャ）がほとんどない場合やエッジが少ない場合などは、二次元画像を利用して計測を実施するのが難しい。また、計測精度も低くなる。
【００８５】
従って、計測対象物体の状態（表面状態）によっては、二次元画像と距離画像とを利用したモデルフィッティング（第１の計測モード）よりも、距離画像を利用したモデルフィッティング（第３の計測モード）により計測を行なう方が精度良く計測を行なえる場合がある。また、物体の位置及び姿勢の計測時における二次元画像の寄与度が小さい場合もある。更に、距離画像を利用して計測を行なうだけで計測精度を十分に満たせる場合もある。このように画像特徴が有意でない場合、距離画像のみを用いて物体の位置及び姿勢の計測を行なった方が、計算時間や計算リソースの抑制を図れる他、計測精度が向上する場合もある。
【００８６】
そこで、実施形態３においては、二次元画像上から画像特徴を検出し、その計測された画像特徴に基づいて計測モード（第１の計測モード、第３の計測モード）を切り替えて設定する。そして、その計測モードに応じて物体の位置及び姿勢の計測を行なう。
【００８７】
図１１を用いて、実施形態３に係わる位置姿勢計測装置４０における機能的な構成の一例について説明する。なお、実施形態１を説明した図１と同一の符号が付された構成は、基本的には、実施形態１同様の機能を果たす。ここでは、実施形態１との相違点について重点的に説明する。
【００８８】
実施形態１との相違点としては、モード設定部４７におけるモード設定処理にある。具体的には、モード設定部４７では、画像特徴検出部４２により二次元画像から検出された計測対象物体を示す画像特徴に基づいてモードを設定する。
【００８９】
モード設定部４７には、二次元画像上から周波数成分を抽出する周波数成分抽出部４９が設けられる。周波数成分抽出部４９により二次元画像から抽出された周波数成分が、所定の周波数より低ければ、モード設定部４７は、二次元画像上の画像特徴が少ないと判定し、第３の計測モードを設定する。そうでなければ、第１の計測モードが設定される。
【００９０】
位置姿勢計測部４６は、第３の計測モードが設定されている場合、三次元座標情報取得部４３により取得された三次元点群に対して、三次元形状モデルをフィッティングさせる。これにより、位置姿勢算出処理を実施し、計測対象物体の位置及び姿勢を計測（算出）する。
【００９１】
ここで、第３の計測モード設定時における処理の流れについて説明する。第３の計測モードでは、実施形態１で説明した式（７）を解くことで物体の位置及び姿勢を算出する。第３の計測モード設定時には、二次元画像の画像特徴を物体の位置及び姿勢の計測に用いないため、図３に示す二次元空間ずれ算出部４６１では処理を行なわない。また、尺度変換部４６３においても、距離のずれの尺度の変換を行なわない。
【００９２】
なお、実施形態３に係わる位置姿勢計測装置４０における処理は、実施形態１で説明した図４と同様の流れとなるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ１０４の処理が異なる点と、Ｓ１０５の処理を省く点と、Ｓ１０６の処理が異なる点とが挙げられる。具体的には、Ｓ１０４の処理では、画像特徴検出部４２において、二次元画像から計測対象物体を示す画像特徴を検出するとともに、周波数成分抽出部４９において、二次元画像の周波数成分を抽出する。また、Ｓ１０６の処理では、モード設定部４７は、周波数成分抽出部４９により抽出された周波数成分に基づいてモード設定を行なう。
【００９３】
また、第３の計測モード設定時は、図６を用いて説明した第１の計測モード設定時と同様の流れで物体の位置及び姿勢の計測処理が行なわれるため、ここでは、図を用いた説明については省略する。相違点としては、Ｓ２０２の処理でエッジの対応付けを行なわない点や、Ｓ２０３の処理で尺度変換を行わない点等である。
【００９４】
以上説明したように実施形態３によれば、二次元画像上の画像特徴の検出状態（多いか少ないか）に基づいて計測モードを設定する。例えば、計測対象物体の表面に模様（テクスチャ）がほとんどない場合やエッジが少ない場合などは、二次元画像を利用して計測を実施するのが難しいと判定し、第３の計測モードを設定する。そうでなければ、第１の計測モードを設定する。
【００９５】
なお、上述した実施形態３においては、二次元画像上の周波数成分に基づいてモード設定を判定しているが、これに限られず、二次元画像上に画像特徴が多いか少ないかを判定できれば良い。そのため、周波数成分でなく、エッジや特徴点の数に基づいてモード設定を行なうように構成してもよい。
【００９６】
また、上述した実施形態３においては、二次元画像上の画像特徴に基づいてモード設定を判定しているが、これに限られない。例えば、距離画像計測部３０により計測され距離画像に基づいてモードを設定するように構成してもよい。この場合、モード設定部４７においては、計測対象物体の距離画像に基づいて、計測対象物体に三次元的なカーブが多いか否かを判定する。判定の結果、三次元的なカーブが多ければ、エッジが少なく二次元画像を利用した計測が難しいため、第３の計測モードを設定する。そうではない場合は、第１の計測モードを設定する。
【００９７】
以上が本発明の代表的な実施形態の例であるが、本発明は、上記及び図面に示す実施形態に限定することなく、その要旨を変更しない範囲内で適宜変形して実施できるものである。例えば、上述した実施形態１〜３を組み合わせて実施しても良い。すなわち、実施形態１〜３で説明した手法に計測対象物体の状態等を計測し、その結果に基づいて第１の計測モード〜第３の計測モードの内のいずれかを設定し、計測を実施しても良い。
【００９８】
ここで、上述した実施形態の変形例をいくつか例示する。
【００９９】
（変形例１）
過去（例えば、直前）に計測した計測対象物体の位置及び姿勢、又は計測対象物体の概略値等に基づいて三次元形状モデルを三次元空間中に配置し、当該三次元形状モデルの観察結果に基づいてモードを変更しても良い。その観察結果により、例えば、カメラから観察される見た目の面積が少ない、カメラから見て凹凸が多い、薄い対象物体が山積みで積まれている等する場合には、距離画像を信用できない。このような場合には、第２の計測モードを設定する。
【０１００】
（変形例２）
計測対象物体の模様（テクスチャ）の少ない領域が、カメラから観察される画面中の多くを占める初期位置姿勢の場合や、三次元的なカーブがエッジ部分に多く見られる初期位置姿勢の場合は、二次元画像を利用した計測が難しい。このような場合には、第３の計測モードを設定するように構成しても良い。
【０１０１】
（変形例３）
上述したモードは、それぞれのモードを排他的に設定する必要はなく、物体の計測を行う状態に応じて連続的に切り替えてもよい。その場合、実施形態１で説明した式（７）のスケーリング係数を操作することで、モードを連続的に切り替えればよい。
【０１０２】
（変形例４）
ユーザインタフェース（入出力手段）を介したユーザの指示に基づいてモードを切り替えて設定しても良い。ユーザインタフェースは、例えば、ＧＵＩメニューや、ハードウェアスイッチ等により実現される。
【０１０３】
（変形例５）
システム側（位置姿勢計測装置４０）で自動的にモード設定を行なわず、最終的なモード設定は、ユーザに決めさせるように構成しても良い。すなわち、位置姿勢計測装置４０においては、上述した処理を実施することにより、ディスプレイ画面等に推奨モード（設定対象となるモードの候補）を提示（表示）する。この場合、ユーザは、ユーザインタフェース（入出力手段）を介してモードを選択し、設定する。
【０１０４】
（変形例６）
ユーザに対して現在どのモードでシステム（位置姿勢計測装置４０）が動作しているのかを提示するように構成してもよい。提示は、ディスプレイ画面に表示することにより行なっても良いし、ランプやＬＥＤなどでハードウェア的に行なうようにしても良い。
【０１０５】
（変形例７）
上述した位置姿勢計測装置４０は、例えば、計測対象物体を把持又は吸着するロボットシステムに適用することもできる。図１２は、計測対象物体の位置及び姿勢に基づいてロボットシステムを制御する制御システムの構成を示している。
【０１０６】
位置姿勢計測装置４０は、上述した実施形態同様に、二次元画像撮像部２０により撮像された二次元画像と、距離画像計測部３０により計測された距離画像と装置内に入力する。位置姿勢計測装置４０は、計測対象物体の位置及び姿勢を計測し、計測対象物体の位置及び姿勢の情報（位置姿勢情報）をロボット制御装置８２に向けて出力する。ロボット制御装置８２は、当該位置姿勢情報に基づいて、計測対象物体の把持、又は計測対象物体を吸着を指示する制御情報をロボット装置８１に向けて出力する。これにより、ロボット装置８１は、当該制御情報に基づいて、計測対象物体を把持、又は吸着する。
【０１０７】
なお、本発明は、例えば、システム、装置、方法、プログラム若しくは記憶媒体等としての実施態様を採ることもできる。具体的には、複数の機器から構成されるシステムに適用しても良いし、また、一つの機器からなる装置に適用しても良い。
（その他の実施形態）
また、本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、ネットワーク又は各種記憶媒体を介してシステム或いは装置に供給する。そして、そのシステム或いは装置のコンピュータ（又はＣＰＵやＭＰＵ、ＧＰＵ等）がプログラムを読み出して実行する処理である。
【特許請求の範囲】
【請求項１】
計測対象物体の位置及び姿勢の計測を行なう位置姿勢計測装置であって、
撮像装置により撮像された前記計測対象物体の二次元画像を入力する入力手段と、
距離センサにより計測された前記計測対象物体の距離データを取得する取得手段と、
前記二次元画像から前記計測対象物体の画像特徴を検出する検出手段と、
前記計測対象物体の状態を判定する判定手段と、
前記判定手段により判定された状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定するモード設定手段と、
前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう計測手段と
を具備することを特徴とする位置姿勢計測装置。
【請求項２】
前記判定手段は、
前記計測対象物体の状態として前記計測対象物体の形状を判定する
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項３】
前記計測対象物体の三次元形状モデルを表す情報を保持する保持手段
を更に具備し、
前記判定手段は、
前記計測対象物体の形状として前記三次元形状モデルの形状を判定する
ことを特徴とする請求項２記載の位置姿勢計測装置。
【請求項４】
前記取得手段は、
前記距離データから三次元座標情報を取得し、
前記判定手段は、
前記取得手段により取得された三次元座標情報により示される前記計測対象物体の形状に基づいて前記判定を行なう
ことを特徴とする請求項２記載の位置姿勢計測装置。
【請求項５】
前記計測対象物体の三次元形状モデルを表す情報を保持する保持手段
を更に具備し、
前記判定手段は、
前記取得手段により取得された三次元座標情報により示される前記計測対象物体の形状と前記計測対象物体の前記三次元形状モデルの形状との比較に基づいて前記判定を行なう
ことを特徴とする請求項４記載の位置姿勢計測装置。
【請求項６】
前記計測対象物体の三次元形状モデルを表す情報を保持する保持手段
を更に具備し、
前記判定手段は、
前記計測手段により過去に計測された前記計測対象物体の位置及び姿勢の計測値を用いて前記計測対象物体の前記三次元形状モデルを三次元空間中に投影した結果に基づいて前記判定を行なう
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項７】
前記判定手段は、
前記計測対象物体の形状が有意な前記距離データが得られる形状であるか否かを判定し、
前記モード設定手段は、
有意な前記距離データが得られる形状であると判定された場合には、前記第１のモードを設定し、有意な前記距離データが得られない形状であると判定された場合には、前記第２のモードとして画像特徴のみを用いて前記計測を行なうモードを設定する
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項８】
前記判定手段は、
前記計測対象物体の状態として前記計測対象物体の表面状態を判定する
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項９】
前記判定手段は、
前記計測対象物体の表面状態が有意な前記画像特徴が得られる表面状態であるか否かを判定し、
前記モード設定手段は、
有意な前記画像特徴が得られる表面状態であると判定された場合には、前記第１のモードを設定し、有意な前記画像特徴が得られない表面状態であると判定された場合には、前記第２のモードとして距離データのみを用いて前記計測を行なうモードを設定する
ことを特徴とする請求項８記載の位置姿勢計測装置。
【請求項１０】
計測対象物体の位置及び姿勢の計測を行なう位置姿勢計測装置の計測処理方法であって、
入力手段が、撮像装置により撮像された前記計測対象物体の二次元画像を入力する工程と、
取得手段が、距離センサにより計測された前記計測対象物体の距離データを取得する工程と、
検出手段が、前記二次元画像から前記計測対象物体の画像特徴を検出する工程と、
判定手段が、前記計測対象物体の状態を判定する工程と、
モード設定手段が、前記判定された前記計測対象物体の状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定する工程と、
計測手段が、前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう工程と
を含むことを特徴とする位置姿勢計測装置における計測処理方法。
【請求項１１】
計測対象物体の位置及び姿勢の計測を行なうコンピュータを、
撮像装置により撮像された前記計測対象物体の二次元画像を入力する入力手段、
距離センサにより計測された前記計測対象物体の距離データを取得する取得手段、
前記二次元画像から前記計測対象物体の画像特徴を検出する検出手段、
前記計測対象物体の状態を判定する判定手段、
前記判定手段により判定された状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定するモード設定手段、
前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう計測手段
として機能させるためのプログラム。
【請求項１】
計測対象物体の位置及び姿勢の計測を行なう位置姿勢計測装置であって、
撮像装置により撮像された前記計測対象物体の二次元画像を入力する入力手段と、
距離センサにより計測された前記計測対象物体の距離データを取得する取得手段と、
前記二次元画像から前記計測対象物体の画像特徴を検出する検出手段と、
前記計測対象物体の状態を判定する判定手段と、
前記判定手段により判定された状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定するモード設定手段と、
前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう計測手段と
を具備することを特徴とする位置姿勢計測装置。
【請求項２】
前記判定手段は、
前記計測対象物体の状態として前記計測対象物体の形状を判定する
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項３】
前記計測対象物体の三次元形状モデルを表す情報を保持する保持手段
を更に具備し、
前記判定手段は、
前記計測対象物体の形状として前記三次元形状モデルの形状を判定する
ことを特徴とする請求項２記載の位置姿勢計測装置。
【請求項４】
前記取得手段は、
前記距離データから三次元座標情報を取得し、
前記判定手段は、
前記取得手段により取得された三次元座標情報により示される前記計測対象物体の形状に基づいて前記判定を行なう
ことを特徴とする請求項２記載の位置姿勢計測装置。
【請求項５】
前記計測対象物体の三次元形状モデルを表す情報を保持する保持手段
を更に具備し、
前記判定手段は、
前記取得手段により取得された三次元座標情報により示される前記計測対象物体の形状と前記計測対象物体の前記三次元形状モデルの形状との比較に基づいて前記判定を行なう
ことを特徴とする請求項４記載の位置姿勢計測装置。
【請求項６】
前記計測対象物体の三次元形状モデルを表す情報を保持する保持手段
を更に具備し、
前記判定手段は、
前記計測手段により過去に計測された前記計測対象物体の位置及び姿勢の計測値を用いて前記計測対象物体の前記三次元形状モデルを三次元空間中に投影した結果に基づいて前記判定を行なう
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項７】
前記判定手段は、
前記計測対象物体の形状が有意な前記距離データが得られる形状であるか否かを判定し、
前記モード設定手段は、
有意な前記距離データが得られる形状であると判定された場合には、前記第１のモードを設定し、有意な前記距離データが得られない形状であると判定された場合には、前記第２のモードとして画像特徴のみを用いて前記計測を行なうモードを設定する
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項８】
前記判定手段は、
前記計測対象物体の状態として前記計測対象物体の表面状態を判定する
ことを特徴とする請求項１記載の位置姿勢計測装置。
【請求項９】
前記判定手段は、
前記計測対象物体の表面状態が有意な前記画像特徴が得られる表面状態であるか否かを判定し、
前記モード設定手段は、
有意な前記画像特徴が得られる表面状態であると判定された場合には、前記第１のモードを設定し、有意な前記画像特徴が得られない表面状態であると判定された場合には、前記第２のモードとして距離データのみを用いて前記計測を行なうモードを設定する
ことを特徴とする請求項８記載の位置姿勢計測装置。
【請求項１０】
計測対象物体の位置及び姿勢の計測を行なう位置姿勢計測装置の計測処理方法であって、
入力手段が、撮像装置により撮像された前記計測対象物体の二次元画像を入力する工程と、
取得手段が、距離センサにより計測された前記計測対象物体の距離データを取得する工程と、
検出手段が、前記二次元画像から前記計測対象物体の画像特徴を検出する工程と、
判定手段が、前記計測対象物体の状態を判定する工程と、
モード設定手段が、前記判定された前記計測対象物体の状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定する工程と、
計測手段が、前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう工程と
を含むことを特徴とする位置姿勢計測装置における計測処理方法。
【請求項１１】
計測対象物体の位置及び姿勢の計測を行なうコンピュータを、
撮像装置により撮像された前記計測対象物体の二次元画像を入力する入力手段、
距離センサにより計測された前記計測対象物体の距離データを取得する取得手段、
前記二次元画像から前記計測対象物体の画像特徴を検出する検出手段、
前記計測対象物体の状態を判定する判定手段、
前記判定手段により判定された状態に基づいて、前記画像特徴と前記距離データとを用いて前記計測を行なう第１のモードと、前記画像特徴と前記距離データとのうち一方のみを用いて前記計測を行なう第２のモードとを含むモードの中からいずれかのモードを設定するモード設定手段、
前記モード設定手段により設定されたモードに従って前記計測対象物体の位置及び姿勢の計測を行なう計測手段
として機能させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１２−２１９５８（Ｐ２０１２−２１９５８Ａ）
【公開日】平成２４年２月２日（２０１２．２．２）
【国際特許分類】
【出願番号】特願２０１０−１６２２０９（Ｐ２０１０−１６２２０９）
【出願日】平成２２年７月１６日（２０１０．７．１６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
【公開日】平成２４年２月２日（２０１２．２．２）
【国際特許分類】
【出願日】平成２２年７月１６日（２０１０．７．１６）
【出願人】（０００００１００７）キヤノン株式会社 (59,756)
【Ｆターム（参考）】
[ Back to top ]