推定装置、推定方法、及び推定プログラム
【課題】物体の３次元位置・姿勢を示す物体座標系を高精度で推定することができる推定装置、推定方法、及び推定プログラムを提供すること。
【解決手段】本発明の一態様にかかる推定装置は、空間中における対象物体の３次元位置・姿勢を示す物体座標系を推定する推定装置であって、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するマーカ検出部３と、マーカ検出部３で検出された複数のマーカに対応する複数のマーカ座標系を算出するため、それぞれのマーカ画像からマーカ座標系を推定するマーカ座標系推定部と４、複数のマーカ座標系に基づいて、対象物体の物体座標系を決定する物体座標系決定部６と、を備えるものである。
【解決手段】本発明の一態様にかかる推定装置は、空間中における対象物体の３次元位置・姿勢を示す物体座標系を推定する推定装置であって、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するマーカ検出部３と、マーカ検出部３で検出された複数のマーカに対応する複数のマーカ座標系を算出するため、それぞれのマーカ画像からマーカ座標系を推定するマーカ座標系推定部と４、複数のマーカ座標系に基づいて、対象物体の物体座標系を決定する物体座標系決定部６と、を備えるものである。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、マーカを用いて空間中における対象物体の３次元位置・姿勢を示す物体座標系を推定する推定装置、推定方法、及び推定プログラムに関する。
【背景技術】
【０００２】
位置・姿勢識別用のマーカが貼られた物体を撮像して、その物体の３次元・位置姿勢を計測する計測装置が開示されている（特許文献１、非特許文献１）。このような計測装置では、カメラで取得した２次元画像から３次元の情報を得るため、計測誤差が生じてしまうことがある。
特許文献１の方法では、マーカを３点以上貼って、それぞれのマーカの３次元点情報を用いて物体の位置・姿勢を推定している。一方、非特許文献１の方法では、１つの物体に対して、３点以上の特徴点を有するマーカを１つ貼っている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−２５４７１６号公報
【非特許文献】
【０００４】
【非特許文献１】画像理解―３次元認識の数理 金谷健一著 森北出版株式会社 １９９０年第１版発行
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１の方法では、カメラが物体の近くに寄ると、マーカが画角から外れてしまう。マーカが１点でも外れてしまうと、物体の位置姿勢を算出することができない。すなわち、３つのマーカが画角に収まらない場合、位置・姿勢を推定することができない。また、非特許文献１の方法では、１つのマーカとして物体に貼らなくてはならないため、遠方から高精度に位置・姿勢を推定するためには、マーカを大きくしなければならない。例えば、物体によっては、マーカを貼るスペースが限られてしまっている。よって、物体や撮像条件によっては、高精度の位置・姿勢の推定が困難となる。
【０００６】
本発明は、上記の問題点に鑑みてなされたものであり、物体の３次元位置・姿勢を示す物体座標系を高精度で安定して推定することができる推定装置、推定方法、及び推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の第１の態様にかかる推定装置は、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定装置であって、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するマーカ検出部と、前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するマーカ座標系推定部と、前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定する物体座標系決定部と、を備えるものである。このような構成では、複数のマーカ座標系を用いて、物体座標系を推定することができるため、高精度で推定することができる。
【０００８】
本発明の第２の態様にかかる推定装置は、上記の推定装置であって、前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、前記仮決めされた物体座標系を第２のマーカ座標系に変換し、前記マーカ画像から推定した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系との誤差を最小化することで、前記物体座標系を決定するものである。これにより、より高精度で推定することができる。
【０００９】
本発明の第３の態様にかかる推定装置は、上記の推定装置であって、前記第１のマーカ座標系と前記第２のマーカ座標系との前記誤差が最小となるように、複数の前記第１のマーカ座標系に対する重みを最適化することで前記物体座標系を決定するものである。これにより、簡便な方法で、高精度の推定が可能となる。
【００１０】
本発明の第４の態様にかかる推定装置は、上記の推定装置であって、前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられているものである。これにより、短い処理時間で高精度の推定が可能になる。
【００１１】
本発明の第５の態様にかかる推定装置は、上記の推定装置であって、前記マーカ検出部で検出したマーカの数が一つである場合に、前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定するものである。これにより、画角によらず、安定した推定が可能となる。
【００１２】
本発明の第６の態様にかかる推定方法は、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定方法であって、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するステップと、前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するステップと、前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定するステップと、を備えるものである。このような構成では、複数の第１のマーカ座標系を用いて、物体座標系を推定することができるため、高精度で推定することができる。
【００１３】
本発明の第７の態様にかかる推定方法は、上記の推定方法であって、前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、前記仮決めされた物体座標系を第２のマーカ座標系に変換し、前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定するものである。
【００１４】
本発明の第８の態様にかかる推定方法は、上記の推定方法であって、前記第１のマーカ座標系と前記第２のマーカ座標系との前記誤差が最小となるように、複数の第１のマーカ座標系に対する重み付け関数を最適化することで前記物体座標系を決定するものである。
【００１５】
本発明の第９の態様にかかる推定方法は、上記の推定方法であって、前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられているものである。
【００１６】
本発明の第１０の態様にかかる推定方法は、上記の推定方法であって、検出された前記マーカの数が一つである場合に、前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定するものである。
【００１７】
本発明の第１１の態様にかかる推定プログラムは、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定プログラムであって、コンピュータに対して、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出させるステップと、前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定させるステップと、前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定させるステップと、を備えるものである。
【００１８】
本発明の第１２の態様にかかる推定プログラムは、上記の推定プログラムであって、コンピュータに対して、前記物体画像に含まれているマーカ画像から算出された複数の第１のマーカ座標系に基づいて物体座標系を仮決めさせ、前記仮決めされた物体座標系を第２のマーカ座標系に変換させ、前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定させるものである。
【００１９】
本発明の第１３の態様にかかる推定プログラムは、上記の推定プログラムであって、コンピュータに対して、前記第１のマーカ座標系と前記第２のマーカ座標系との誤差が最小となるように、複数の第１のマーカ座標系に対する重みを最適化することで前記物体座標系を決定させるものである。
【００２０】
本発明の第１４の態様にかかる推定プログラムは、上記の推定プログラムであって、前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられているものである。
【００２１】
本発明の第１５の態様にかかる推定プログラムは、上記の推定プログラムであって、検出された前記マーカの数が一つである場合に、前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定するものである。これにより、画角によらず、安定した推定が可能となる。
【発明の効果】
【００２２】
本発明によれば、物体の３次元位置・姿勢を示す物体座標系を高精度で推定することができる推定装置、推定方法、及び推定プログラムを提供することができる。
【図面の簡単な説明】
【００２３】
【図１】３次元位置・姿勢を推定する推定システムの全体構成を示す図である。
【図２】本発明の実施形態にかかる推定装置に用いられる処理装置を示すブロック図である。
【図３】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図４】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図５】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図６】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図７】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図８】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図９】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図１０】本発明の実施形態にかかる推定方法を示す概念図である。
【発明を実施するための形態】
【００２４】
以下、本発明に係る移動体の実施形態を、図面に基づいて詳細に説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００２５】
３次元位置・座標を推定するシステムについて、図１を用いて説明する。本実施の形態にかかる推定システムは、カメラ１と処理装置２とを備えている。物体１０が３次元位置・姿勢（ｘ、ｙ、ｚ、ロール角度、ピッチ角度、ヨー角度）を推定する対象となる対象物体である。すなわち、物体１０をカメラ１で撮像して、その物体画像を基に物体１０の３次元位置・姿勢を推定する。対象物体１０は複数のマーカを有している。本実施形態では、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する。
【００２６】
カメラ１は、３次元位置・座標を求める対象となる物体を撮像する。カメラ１は、たとえば、ＣＣＤカメラなどであり、物体の全部又は一部を含む物体画像を撮像する。なお、カメラ１として、ステレオカメラを用いてもよい。カメラ１で取得された物体画像は、処理装置２に入力される。
【００２７】
物体１０には、マーカ１１、１２が設けられている。ここでは、２つのマーカ１１、１２が設けられている例を示しているが、物体１０に３以上のマーカを設けてもよい。それぞれのマーカ１１、１２は、単体で３次元の位置・姿勢を検出可能な識別用マーカである。例えば、各マーカ１１、１２が３以上の特徴点を有している。たとえば、マーカ１１、１２は正面視において正方形の枠と、枠内に設けられたパターンなどを有している。正方形の枠の頂点がマーカ特徴点となる。さらに、それぞれのマーカは、異なる形状、色、パターン、点滅等になっており、他のマーカと識別可能になっている。また、物体１０における各マーカ１１、１２の位置・姿勢は既知となっている。
【００２８】
処理装置２は、たとえば、パーソナルコンピュータであり、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、処理装置２は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。もちろん、処理装置２は、物理的に一つの構成に限られるものではない。処理装置２は、カメラ１で取得した物体画像のデータに対して、所定の画像処理を行う。これにより、物体１０の３次元空間における３次元位置・姿勢を示す物体座標系３０が推定される。なお、物体座標系とは、Ｘ方向（横方向）位置、Ｙ方向（縦方向）位置、Ｚ方向（奥行き方向）位置、ロール角度、ピッチ角度、及びヨー角度に関する情報を含むものである。例えば、物体座標系は、６次元のベクトルで示される。この物体座標系によって、空間中の物体１０の３次元位置・姿勢を特定することができる。
【００２９】
本発明の特徴の一つである処理装置２における処理について図２乃至図１０を用いて説明する。図２は、処理装置２の構成を示すブロック図である。図３は、処理方法を示すフローチャートである。図４乃至図９は、各処理工程を説明するための図である。図１０は本実施形態にかかる推定方法を示す概念図である。処理装置２は、マーカ検出部３と、マーカ座標系推定部４と、設定記憶部５と、物体座標系決定部６と、座標系変換部７と、を備えている。
【００３０】
まず、カメラ１によって物体が撮像されると、その物体画像が処理装置２に入力される（ステップＳ１０１）。処理装置２は、この物体画像をメモリなどの記憶装置に記憶する。ここでは、例えば、図４に示す物体画像が入力されたとする。カメラ１で取得された１フレームの画像には、物体１０の一部と、物体４０の一部と、物体６０の一部が現れている。物体１０に設けられた２つのマーカ１１、１２が画像の中に含まれている。また、物体４０に設けられたマーカ４１が画像の中に含まれている。同様に、物体６０に設けられたマーカ６１が画像の中に含まれている。以下の説明では、２つのマーカ１１、１２が撮像された物体１０の３次元位置・姿勢を推定する例を中心に説明する。
【００３１】
カメラ１で取得した物体画像の中から、マーカ検出部３がマーカを検出する（ステップＳ１０２）。これにより、物体画像の中から、複数のマーカ画像が抽出される。ここでは、図４に示す点線枠に囲まれている部分がマーカ画像として抽出される。それぞれのマーカには固有のパターン、色等が付されているため、それぞれのマーカを区別することができる。例えば、設定記憶部５には、固有のパターン、色等の設定情報が予め対応づけて記憶されている。マーカ検出部３が、設定情報を参照することによって、それぞれのマーカ画像が、どの物体のどのマーカであるかを特定することができる。
【００３２】
そして、各マーカの３次元位置・姿勢を推定する（ステップＳ１０３）。ここでは、マーカ１１、マーカ１２、マーカ４１、マーカ６１のマーカ３次元位置・姿勢が推定される。ここでは、３次元位置・姿勢を示す第１のマーカ座標系が算出される。マーカ座標系推定部４が、各マーカの第１のマーカ座標系を推定する。例えば、複数のマーカ特徴点の間のベクトルを用いて、第１のマーカ座標系を算出することができる。ここでは、図５に示すように、マーカ１１のマーカ画像によって第１のマーカ座標系２１が算出されている。同様に、マーカ１２のマーカ画像によって第１のマーカ座標系２２が算出され、マーカ４１のマーカ画像によって第１のマーカ座標系５１が算出され、マーカ６１のマーカ画像によって第１のマーカ座標系７１が算出されている。マーカ座標系とは、Ｘ軸、Ｙ軸、及びＺ軸における位置と、Ｘ軸、Ｙ軸、及びＺ軸の方向を示すものである。このマーカ座標系によって、空間中におけるマーカの位置・姿勢を特定することができる。なお、ステップＳ１０３、及びステップＳ１０２については、例えば、非特許文献１に示されている様々な計算手法を用いることができる。
【００３３】
そして、１つの物体に対して複数のマーカを検出したか否かを判定する（ステップＳ１０４）。図６に示すように、物体４０については、１つのマーカ４１しか検出されていないため、ステップＳ１０４のＮＯに進む。同様に、物体６０についても、１つのマーカ６１しか検出されていないため、ステップＳ１０４のＮＯに進む。物体４０、６０では、１つのマーカによって、物体座標系が決定される（ステップＳ１０８）。すなわち、第１のマーカ座標系を物体座標系に変換する。物体４０におけるマーカ４１の位置・姿勢は既知であるため、変換行列を用いることで、容易に物体座標系を決定することができる。これにより、推定処理が終了する。この変換行列は、予め設定記憶部５に記憶されている。すなわち、この処理は、公知の処理と同様であるため、説明を省略する。
【００３４】
１つの物体に対して検出されたマーカの数が複数である場合、物体座標系決定部６が、物体座標系を仮決定する（ステップＳ１０５）。例えば、物体１０については、２つのマーカ１１、１２が検出されているため、ステップＳ１０５に進む。図７に示すように、２つの第１のマーカ座標系２１、２２に基づいて、物体座標系３０ａを仮決定する。仮決定された物体座標系を物体座標系３０ａとしている。この仮決定の一例について、説明する。
【００３５】
まず、それぞれのマーカについて重みを式（１）で求め、式（２）によって、解（物体座標系）を仮決めする。
【００３６】
【数１】
【数２】
【００３７】
このように、重み付け平均によって、物体座標系を仮決めする。重みは、物体１０のカメラ１に対する奥行き距離、マーカの大きさ、ロール角度、ピッチ角度、ヨー角度の関数となっている。カメラに近いマーカほど重みが重くなり、大きいマーカほど重みが重くなり、回転角が小さいマーカほど重みが大きくなる。このように、各マーカの重みをマーカ画像から求めている。なお、重みの初期値の算出方法は特に限定されるものではない。なお、仮物体座標系はカメラ１による測定誤差を含んでいる。また、物体とマーカの相対位置が既知であるため、マーカ座標系から物体座標系への変換行列Ｋｉは、既知となっている。すなわち、マーカと物体の相対位置は、予め決まっているため、マーカ座標系と物体座標系の変換行列を予め設定しておくことができる。各マーカに対する変換行列は、それぞれ設定記憶部５に記憶されている。なお、式（２）では、マーカ数をｎ（２以上の自然数）として一般化している。物体１０の場合は、ｎ＝２となる。
【００３８】
次に、仮決めされた物体座標系３０ａから、第２のマーカ座標系を算出する（ステップＳ１０６）。ここでは、座標系変換部７が、仮決めされた物体座標系を第２のマーカ座標系に変換する。これにより、１つの物体座標系に基づいて、複数のマーカの第２のマーカ座標系が算出される。図８に示すように、マーカ１１、１２に対する第２のマーカ座標系２１ａ、２２ａが物体座標系３０ａから得られる。具体的には、式（３）によって、第２のマーカ座標系２１ａ、２２ａを算出することができる。
【００３９】
【数３】
【００４０】
なお、物体とマーカの相対位置が既知であるため、物体座標系を第２のマーカ座標系に変換する変換行列は既知となっている。もちろん、変換行列はマーカ毎に異なっている。ここで、重み付平均を用いて仮決められた物体座標系３０ａから、第２のマーカ座標系２１ａ、２２ａを算出している。よって、カメラのマーカ画像から算出された第１のマーカ座標系２１、２２は、物体座標系から推定された第２のマーカ座標系２１ａ、２２ａと異なっている。換言すると、マーカ座標系が誤差を含んでいるということができる。
【００４１】
検出された第１のマーカ座標系と、重みを考慮して決定された物体座標系から推定された第２のマーカ座標系との誤差は以下のように計算することができる。一般に、第１のマーカ座標系と第２のマーカ座標系との誤差は、以下の式（４）に示すようにマーカ座標系の差分で示すことができる。
【００４２】
【数４】
【００４３】
ここで、第１のマーカ座標系と第２のマーカ座標系の誤差は、３次元空間誤差又は画面投影誤差として表すことができる。３次元空間誤差は、３次元の位置・姿勢それぞれにおける誤差であり６次元のベクトルとして示すことができる。物体座標系から推定された第２のマーカ座標系の３次元位置・姿勢（距離・角度）と、検出された第１のマーカ座標系の位置・姿勢（距離・角度）を評価式に代入して、３次元空間誤差とする。なお、距離と角度という単位の異なる指標を１つの評価式に入れるので、適切に評価式を定義する必要がある。
【００４４】
また、画面投影誤差は、マーカ特徴点の投影点の誤差であり、カメラ画像中におけるピクセルとして示すことができる。すなわち、物体座標系から推定された第２のマーカ座標系の位置・姿勢から、マーカ特徴点を求める。マーカ特徴点をカメラ画像に投影した画像座標点を算出する。すなわち、射影行列を用いて、マーカ特徴点を２次元のカメラ画像に射影する。算出した画像座標点と、カメラ画像から得たマーカ特徴点の画像座標点の差（画像上の距離）を画面投影誤差とする。なお、射影行列は、カメラの焦点距離やレンズの歪みによって求めることができる。
【００４５】
次に、物体座標系の誤差が最小となっているか否かを判定する（ステップＳ１０７）。すなわち、誤差が最小となるまで、ステップＳ１０５〜ステップＳ１０７の処理を繰り返し実行する。そして、誤差が最小となったら、処理を終了する。これにより、図９に示すように、物体座標系３０ｂが確定する。具体的には、図１０に示すように、物体座標系の誤差が最小となるように最適化を行う。誤差が収束するまで、以下の計算を繰り返し行う。（１）第１のマーカ座標系から物体座標系を重み付け平均によって求める。（２）物体座標系を第２のマーカ座標系に変換する。（３）重み付け関数を更新して、誤差を小さくする。
この（１）〜（３）の処理を繰り返し行い、誤差を最小化する。このようにすることで、式（１）で示した重み付け関数を最適化することができ、誤差を最小化することができる。よって、精度の高い３次元位置・姿勢の推定が可能となる。
【００４６】
重み付き最小二乗法を繰り返し計算で解くことで、高精度で位置・姿勢を推定することができる。重み付き最小二乗法を解くことは、非線形最適化問題となるため、本実施形態では、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いて、変数を更新して、式（５）の解を求めている。
【００４７】
【数５】
【数６】
【００４８】
ここでは、誤差を、式（４）で示した画面投影誤差としている。画面投影誤差の２乗和を最小とするように、反復計算を行う。これにより、第１のマーカ座標系に対する重みを最適化することができる。よって、高精度で、位置・姿勢を算出することができる。
【００４９】
また、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いることで、精度の高い推定を短時間で行うことができる。これにより、処理時間を短縮することができる。なお、３次元空間誤差を最小化する場合、式（４）で示した、３次元空間誤差をマーカ毎に求め、それらの２乗和を最小化する。このようにしても、同様に、高精度の３次元位置・姿勢の推定が可能になる。
【００５０】
物体１０とカメラ１が遠い場合、カメラ１の画角には、複数のマーカが収まる。よって、複数のマーカから、位置・姿勢を推定することができる。マーカを複数用いることで、高精度の推定が可能となる。カメラ１と物体１０が近い場合、１つのマーカによる３次元位置・姿勢が検出可能である。すなわち、カメラ１の画角に１つのマーカしか収まらない場合でも、１つのマーカが３つ以上の特徴点を持っているので、位置・姿勢の推定が可能である。カメラ１が物体１０に近い場合、大きなマーカを用いることができるため、高精度の推定が可能となる。
【００５１】
このようにカメラ画像に含まれるマーカに基づいて位置推定を行うことで、精度の高い推定を安定して行うことができる。すなわち、物体とカメラの距離が遠い場合は、複数のマーカによる推定によって精度を上げることができ、近い場合には、１つのマーカが大きなマーカ画像となるため、少ないマーカ数でも精度を向上することができる。よって、遠い場合でも、近い場合でも、精度の高い推定が可能となる。なお、マーカを設ける配置や、マーカの大きさは、使用するカメラ、空間、物体の大きさ、物体の形状などに応じて決定すればよい。また、物体に設けるマーカの個数は特に限定されるものではないが、より多いほど好ましい。すなわち、より多くのマーカが１つの画角に収まることで、推定精度を向上すすることができる。
【００５２】
このような推定装置は、ロボットに搭載することが好適である。物体を把持するアームを持つロボットに、推定装置を搭載することで、確実に物体を把持することができる。特に、物体を把持する産業用ロボットや、家庭用ロボットに好適である。さらには、工場内の設備管理用途にも好適である。
【００５３】
なお、上記の説明では、重み付け最小二乗法によって誤差を最小化したが、他の方法を用いて、誤差を最小化しても良い。さらには、複数の第１のマーカ座標系から、物体座標系を求める手法は特に限定されるものではない。
【００５４】
上記の推定処理はコンピュータプログラムによって実行させても良い。３次元・位置姿勢の推定処理をコンピュータに行わせるための命令群を含むプログラムは、非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いてコンピュータに供給できる。非一時的なコンピュータ可読媒体は、様々な種類の実体のある記憶媒体（tangible storage medium）を含む。例えば、非一時的なコンピュータ可読媒体は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ，ＣＤ−Ｒ／Ｗ、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、ＵＶ−ＥＰＲＯＭ（Erasable ROM）、ＥＥＰＲＯＭ（Electrical EPROM）、フラッシュＲＯＭを含む。また、プログラムは、非一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的な媒体は、プログラムがエンコードされた電気信号、光信号、及び電磁波を含む。プログラムがエンコードされた電気信号、光信号、及び電磁波は、電線及び光ファイバ等の有線伝送路、又は無線伝送路（空間）を伝搬することでコンピュータに供給される。
【符号の説明】
【００５５】
１ カメラ
２ 処理装置
１０ 物体
１１ マーカ
１２ マーカ
２１ 第１のマーカ座標系
２１ａ 第２のマーカ座標系
２２ 第１のマーカ座標系
２２ａ 第２のマーカ座標系
３０ 物体座標系
４０ 物体
４１ マーカ
５１ 第１のマーカ座標系
６０ 物体
６０ 物体
６１ マーカ
７１ 第１のマーカ座標系
【技術分野】
【０００１】
本発明は、マーカを用いて空間中における対象物体の３次元位置・姿勢を示す物体座標系を推定する推定装置、推定方法、及び推定プログラムに関する。
【背景技術】
【０００２】
位置・姿勢識別用のマーカが貼られた物体を撮像して、その物体の３次元・位置姿勢を計測する計測装置が開示されている（特許文献１、非特許文献１）。このような計測装置では、カメラで取得した２次元画像から３次元の情報を得るため、計測誤差が生じてしまうことがある。
特許文献１の方法では、マーカを３点以上貼って、それぞれのマーカの３次元点情報を用いて物体の位置・姿勢を推定している。一方、非特許文献１の方法では、１つの物体に対して、３点以上の特徴点を有するマーカを１つ貼っている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−２５４７１６号公報
【非特許文献】
【０００４】
【非特許文献１】画像理解―３次元認識の数理 金谷健一著 森北出版株式会社 １９９０年第１版発行
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１の方法では、カメラが物体の近くに寄ると、マーカが画角から外れてしまう。マーカが１点でも外れてしまうと、物体の位置姿勢を算出することができない。すなわち、３つのマーカが画角に収まらない場合、位置・姿勢を推定することができない。また、非特許文献１の方法では、１つのマーカとして物体に貼らなくてはならないため、遠方から高精度に位置・姿勢を推定するためには、マーカを大きくしなければならない。例えば、物体によっては、マーカを貼るスペースが限られてしまっている。よって、物体や撮像条件によっては、高精度の位置・姿勢の推定が困難となる。
【０００６】
本発明は、上記の問題点に鑑みてなされたものであり、物体の３次元位置・姿勢を示す物体座標系を高精度で安定して推定することができる推定装置、推定方法、及び推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明の第１の態様にかかる推定装置は、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定装置であって、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するマーカ検出部と、前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するマーカ座標系推定部と、前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定する物体座標系決定部と、を備えるものである。このような構成では、複数のマーカ座標系を用いて、物体座標系を推定することができるため、高精度で推定することができる。
【０００８】
本発明の第２の態様にかかる推定装置は、上記の推定装置であって、前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、前記仮決めされた物体座標系を第２のマーカ座標系に変換し、前記マーカ画像から推定した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系との誤差を最小化することで、前記物体座標系を決定するものである。これにより、より高精度で推定することができる。
【０００９】
本発明の第３の態様にかかる推定装置は、上記の推定装置であって、前記第１のマーカ座標系と前記第２のマーカ座標系との前記誤差が最小となるように、複数の前記第１のマーカ座標系に対する重みを最適化することで前記物体座標系を決定するものである。これにより、簡便な方法で、高精度の推定が可能となる。
【００１０】
本発明の第４の態様にかかる推定装置は、上記の推定装置であって、前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられているものである。これにより、短い処理時間で高精度の推定が可能になる。
【００１１】
本発明の第５の態様にかかる推定装置は、上記の推定装置であって、前記マーカ検出部で検出したマーカの数が一つである場合に、前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定するものである。これにより、画角によらず、安定した推定が可能となる。
【００１２】
本発明の第６の態様にかかる推定方法は、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定方法であって、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するステップと、前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するステップと、前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定するステップと、を備えるものである。このような構成では、複数の第１のマーカ座標系を用いて、物体座標系を推定することができるため、高精度で推定することができる。
【００１３】
本発明の第７の態様にかかる推定方法は、上記の推定方法であって、前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、前記仮決めされた物体座標系を第２のマーカ座標系に変換し、前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定するものである。
【００１４】
本発明の第８の態様にかかる推定方法は、上記の推定方法であって、前記第１のマーカ座標系と前記第２のマーカ座標系との前記誤差が最小となるように、複数の第１のマーカ座標系に対する重み付け関数を最適化することで前記物体座標系を決定するものである。
【００１５】
本発明の第９の態様にかかる推定方法は、上記の推定方法であって、前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられているものである。
【００１６】
本発明の第１０の態様にかかる推定方法は、上記の推定方法であって、検出された前記マーカの数が一つである場合に、前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定するものである。
【００１７】
本発明の第１１の態様にかかる推定プログラムは、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定プログラムであって、コンピュータに対して、対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出させるステップと、前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定させるステップと、前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定させるステップと、を備えるものである。
【００１８】
本発明の第１２の態様にかかる推定プログラムは、上記の推定プログラムであって、コンピュータに対して、前記物体画像に含まれているマーカ画像から算出された複数の第１のマーカ座標系に基づいて物体座標系を仮決めさせ、前記仮決めされた物体座標系を第２のマーカ座標系に変換させ、前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定させるものである。
【００１９】
本発明の第１３の態様にかかる推定プログラムは、上記の推定プログラムであって、コンピュータに対して、前記第１のマーカ座標系と前記第２のマーカ座標系との誤差が最小となるように、複数の第１のマーカ座標系に対する重みを最適化することで前記物体座標系を決定させるものである。
【００２０】
本発明の第１４の態様にかかる推定プログラムは、上記の推定プログラムであって、前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられているものである。
【００２１】
本発明の第１５の態様にかかる推定プログラムは、上記の推定プログラムであって、検出された前記マーカの数が一つである場合に、前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定するものである。これにより、画角によらず、安定した推定が可能となる。
【発明の効果】
【００２２】
本発明によれば、物体の３次元位置・姿勢を示す物体座標系を高精度で推定することができる推定装置、推定方法、及び推定プログラムを提供することができる。
【図面の簡単な説明】
【００２３】
【図１】３次元位置・姿勢を推定する推定システムの全体構成を示す図である。
【図２】本発明の実施形態にかかる推定装置に用いられる処理装置を示すブロック図である。
【図３】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図４】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図５】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図６】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図７】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図８】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図９】本発明の実施形態にかかる推定方法における処理工程を説明するための図である。
【図１０】本発明の実施形態にかかる推定方法を示す概念図である。
【発明を実施するための形態】
【００２４】
以下、本発明に係る移動体の実施形態を、図面に基づいて詳細に説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００２５】
３次元位置・座標を推定するシステムについて、図１を用いて説明する。本実施の形態にかかる推定システムは、カメラ１と処理装置２とを備えている。物体１０が３次元位置・姿勢（ｘ、ｙ、ｚ、ロール角度、ピッチ角度、ヨー角度）を推定する対象となる対象物体である。すなわち、物体１０をカメラ１で撮像して、その物体画像を基に物体１０の３次元位置・姿勢を推定する。対象物体１０は複数のマーカを有している。本実施形態では、複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する。
【００２６】
カメラ１は、３次元位置・座標を求める対象となる物体を撮像する。カメラ１は、たとえば、ＣＣＤカメラなどであり、物体の全部又は一部を含む物体画像を撮像する。なお、カメラ１として、ステレオカメラを用いてもよい。カメラ１で取得された物体画像は、処理装置２に入力される。
【００２７】
物体１０には、マーカ１１、１２が設けられている。ここでは、２つのマーカ１１、１２が設けられている例を示しているが、物体１０に３以上のマーカを設けてもよい。それぞれのマーカ１１、１２は、単体で３次元の位置・姿勢を検出可能な識別用マーカである。例えば、各マーカ１１、１２が３以上の特徴点を有している。たとえば、マーカ１１、１２は正面視において正方形の枠と、枠内に設けられたパターンなどを有している。正方形の枠の頂点がマーカ特徴点となる。さらに、それぞれのマーカは、異なる形状、色、パターン、点滅等になっており、他のマーカと識別可能になっている。また、物体１０における各マーカ１１、１２の位置・姿勢は既知となっている。
【００２８】
処理装置２は、たとえば、パーソナルコンピュータであり、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、処理装置２は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。もちろん、処理装置２は、物理的に一つの構成に限られるものではない。処理装置２は、カメラ１で取得した物体画像のデータに対して、所定の画像処理を行う。これにより、物体１０の３次元空間における３次元位置・姿勢を示す物体座標系３０が推定される。なお、物体座標系とは、Ｘ方向（横方向）位置、Ｙ方向（縦方向）位置、Ｚ方向（奥行き方向）位置、ロール角度、ピッチ角度、及びヨー角度に関する情報を含むものである。例えば、物体座標系は、６次元のベクトルで示される。この物体座標系によって、空間中の物体１０の３次元位置・姿勢を特定することができる。
【００２９】
本発明の特徴の一つである処理装置２における処理について図２乃至図１０を用いて説明する。図２は、処理装置２の構成を示すブロック図である。図３は、処理方法を示すフローチャートである。図４乃至図９は、各処理工程を説明するための図である。図１０は本実施形態にかかる推定方法を示す概念図である。処理装置２は、マーカ検出部３と、マーカ座標系推定部４と、設定記憶部５と、物体座標系決定部６と、座標系変換部７と、を備えている。
【００３０】
まず、カメラ１によって物体が撮像されると、その物体画像が処理装置２に入力される（ステップＳ１０１）。処理装置２は、この物体画像をメモリなどの記憶装置に記憶する。ここでは、例えば、図４に示す物体画像が入力されたとする。カメラ１で取得された１フレームの画像には、物体１０の一部と、物体４０の一部と、物体６０の一部が現れている。物体１０に設けられた２つのマーカ１１、１２が画像の中に含まれている。また、物体４０に設けられたマーカ４１が画像の中に含まれている。同様に、物体６０に設けられたマーカ６１が画像の中に含まれている。以下の説明では、２つのマーカ１１、１２が撮像された物体１０の３次元位置・姿勢を推定する例を中心に説明する。
【００３１】
カメラ１で取得した物体画像の中から、マーカ検出部３がマーカを検出する（ステップＳ１０２）。これにより、物体画像の中から、複数のマーカ画像が抽出される。ここでは、図４に示す点線枠に囲まれている部分がマーカ画像として抽出される。それぞれのマーカには固有のパターン、色等が付されているため、それぞれのマーカを区別することができる。例えば、設定記憶部５には、固有のパターン、色等の設定情報が予め対応づけて記憶されている。マーカ検出部３が、設定情報を参照することによって、それぞれのマーカ画像が、どの物体のどのマーカであるかを特定することができる。
【００３２】
そして、各マーカの３次元位置・姿勢を推定する（ステップＳ１０３）。ここでは、マーカ１１、マーカ１２、マーカ４１、マーカ６１のマーカ３次元位置・姿勢が推定される。ここでは、３次元位置・姿勢を示す第１のマーカ座標系が算出される。マーカ座標系推定部４が、各マーカの第１のマーカ座標系を推定する。例えば、複数のマーカ特徴点の間のベクトルを用いて、第１のマーカ座標系を算出することができる。ここでは、図５に示すように、マーカ１１のマーカ画像によって第１のマーカ座標系２１が算出されている。同様に、マーカ１２のマーカ画像によって第１のマーカ座標系２２が算出され、マーカ４１のマーカ画像によって第１のマーカ座標系５１が算出され、マーカ６１のマーカ画像によって第１のマーカ座標系７１が算出されている。マーカ座標系とは、Ｘ軸、Ｙ軸、及びＺ軸における位置と、Ｘ軸、Ｙ軸、及びＺ軸の方向を示すものである。このマーカ座標系によって、空間中におけるマーカの位置・姿勢を特定することができる。なお、ステップＳ１０３、及びステップＳ１０２については、例えば、非特許文献１に示されている様々な計算手法を用いることができる。
【００３３】
そして、１つの物体に対して複数のマーカを検出したか否かを判定する（ステップＳ１０４）。図６に示すように、物体４０については、１つのマーカ４１しか検出されていないため、ステップＳ１０４のＮＯに進む。同様に、物体６０についても、１つのマーカ６１しか検出されていないため、ステップＳ１０４のＮＯに進む。物体４０、６０では、１つのマーカによって、物体座標系が決定される（ステップＳ１０８）。すなわち、第１のマーカ座標系を物体座標系に変換する。物体４０におけるマーカ４１の位置・姿勢は既知であるため、変換行列を用いることで、容易に物体座標系を決定することができる。これにより、推定処理が終了する。この変換行列は、予め設定記憶部５に記憶されている。すなわち、この処理は、公知の処理と同様であるため、説明を省略する。
【００３４】
１つの物体に対して検出されたマーカの数が複数である場合、物体座標系決定部６が、物体座標系を仮決定する（ステップＳ１０５）。例えば、物体１０については、２つのマーカ１１、１２が検出されているため、ステップＳ１０５に進む。図７に示すように、２つの第１のマーカ座標系２１、２２に基づいて、物体座標系３０ａを仮決定する。仮決定された物体座標系を物体座標系３０ａとしている。この仮決定の一例について、説明する。
【００３５】
まず、それぞれのマーカについて重みを式（１）で求め、式（２）によって、解（物体座標系）を仮決めする。
【００３６】
【数１】
【数２】
【００３７】
このように、重み付け平均によって、物体座標系を仮決めする。重みは、物体１０のカメラ１に対する奥行き距離、マーカの大きさ、ロール角度、ピッチ角度、ヨー角度の関数となっている。カメラに近いマーカほど重みが重くなり、大きいマーカほど重みが重くなり、回転角が小さいマーカほど重みが大きくなる。このように、各マーカの重みをマーカ画像から求めている。なお、重みの初期値の算出方法は特に限定されるものではない。なお、仮物体座標系はカメラ１による測定誤差を含んでいる。また、物体とマーカの相対位置が既知であるため、マーカ座標系から物体座標系への変換行列Ｋｉは、既知となっている。すなわち、マーカと物体の相対位置は、予め決まっているため、マーカ座標系と物体座標系の変換行列を予め設定しておくことができる。各マーカに対する変換行列は、それぞれ設定記憶部５に記憶されている。なお、式（２）では、マーカ数をｎ（２以上の自然数）として一般化している。物体１０の場合は、ｎ＝２となる。
【００３８】
次に、仮決めされた物体座標系３０ａから、第２のマーカ座標系を算出する（ステップＳ１０６）。ここでは、座標系変換部７が、仮決めされた物体座標系を第２のマーカ座標系に変換する。これにより、１つの物体座標系に基づいて、複数のマーカの第２のマーカ座標系が算出される。図８に示すように、マーカ１１、１２に対する第２のマーカ座標系２１ａ、２２ａが物体座標系３０ａから得られる。具体的には、式（３）によって、第２のマーカ座標系２１ａ、２２ａを算出することができる。
【００３９】
【数３】
【００４０】
なお、物体とマーカの相対位置が既知であるため、物体座標系を第２のマーカ座標系に変換する変換行列は既知となっている。もちろん、変換行列はマーカ毎に異なっている。ここで、重み付平均を用いて仮決められた物体座標系３０ａから、第２のマーカ座標系２１ａ、２２ａを算出している。よって、カメラのマーカ画像から算出された第１のマーカ座標系２１、２２は、物体座標系から推定された第２のマーカ座標系２１ａ、２２ａと異なっている。換言すると、マーカ座標系が誤差を含んでいるということができる。
【００４１】
検出された第１のマーカ座標系と、重みを考慮して決定された物体座標系から推定された第２のマーカ座標系との誤差は以下のように計算することができる。一般に、第１のマーカ座標系と第２のマーカ座標系との誤差は、以下の式（４）に示すようにマーカ座標系の差分で示すことができる。
【００４２】
【数４】
【００４３】
ここで、第１のマーカ座標系と第２のマーカ座標系の誤差は、３次元空間誤差又は画面投影誤差として表すことができる。３次元空間誤差は、３次元の位置・姿勢それぞれにおける誤差であり６次元のベクトルとして示すことができる。物体座標系から推定された第２のマーカ座標系の３次元位置・姿勢（距離・角度）と、検出された第１のマーカ座標系の位置・姿勢（距離・角度）を評価式に代入して、３次元空間誤差とする。なお、距離と角度という単位の異なる指標を１つの評価式に入れるので、適切に評価式を定義する必要がある。
【００４４】
また、画面投影誤差は、マーカ特徴点の投影点の誤差であり、カメラ画像中におけるピクセルとして示すことができる。すなわち、物体座標系から推定された第２のマーカ座標系の位置・姿勢から、マーカ特徴点を求める。マーカ特徴点をカメラ画像に投影した画像座標点を算出する。すなわち、射影行列を用いて、マーカ特徴点を２次元のカメラ画像に射影する。算出した画像座標点と、カメラ画像から得たマーカ特徴点の画像座標点の差（画像上の距離）を画面投影誤差とする。なお、射影行列は、カメラの焦点距離やレンズの歪みによって求めることができる。
【００４５】
次に、物体座標系の誤差が最小となっているか否かを判定する（ステップＳ１０７）。すなわち、誤差が最小となるまで、ステップＳ１０５〜ステップＳ１０７の処理を繰り返し実行する。そして、誤差が最小となったら、処理を終了する。これにより、図９に示すように、物体座標系３０ｂが確定する。具体的には、図１０に示すように、物体座標系の誤差が最小となるように最適化を行う。誤差が収束するまで、以下の計算を繰り返し行う。（１）第１のマーカ座標系から物体座標系を重み付け平均によって求める。（２）物体座標系を第２のマーカ座標系に変換する。（３）重み付け関数を更新して、誤差を小さくする。
この（１）〜（３）の処理を繰り返し行い、誤差を最小化する。このようにすることで、式（１）で示した重み付け関数を最適化することができ、誤差を最小化することができる。よって、精度の高い３次元位置・姿勢の推定が可能となる。
【００４６】
重み付き最小二乗法を繰り返し計算で解くことで、高精度で位置・姿勢を推定することができる。重み付き最小二乗法を解くことは、非線形最適化問題となるため、本実施形態では、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いて、変数を更新して、式（５）の解を求めている。
【００４７】
【数５】
【数６】
【００４８】
ここでは、誤差を、式（４）で示した画面投影誤差としている。画面投影誤差の２乗和を最小とするように、反復計算を行う。これにより、第１のマーカ座標系に対する重みを最適化することができる。よって、高精度で、位置・姿勢を算出することができる。
【００４９】
また、Ｌｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法を用いることで、精度の高い推定を短時間で行うことができる。これにより、処理時間を短縮することができる。なお、３次元空間誤差を最小化する場合、式（４）で示した、３次元空間誤差をマーカ毎に求め、それらの２乗和を最小化する。このようにしても、同様に、高精度の３次元位置・姿勢の推定が可能になる。
【００５０】
物体１０とカメラ１が遠い場合、カメラ１の画角には、複数のマーカが収まる。よって、複数のマーカから、位置・姿勢を推定することができる。マーカを複数用いることで、高精度の推定が可能となる。カメラ１と物体１０が近い場合、１つのマーカによる３次元位置・姿勢が検出可能である。すなわち、カメラ１の画角に１つのマーカしか収まらない場合でも、１つのマーカが３つ以上の特徴点を持っているので、位置・姿勢の推定が可能である。カメラ１が物体１０に近い場合、大きなマーカを用いることができるため、高精度の推定が可能となる。
【００５１】
このようにカメラ画像に含まれるマーカに基づいて位置推定を行うことで、精度の高い推定を安定して行うことができる。すなわち、物体とカメラの距離が遠い場合は、複数のマーカによる推定によって精度を上げることができ、近い場合には、１つのマーカが大きなマーカ画像となるため、少ないマーカ数でも精度を向上することができる。よって、遠い場合でも、近い場合でも、精度の高い推定が可能となる。なお、マーカを設ける配置や、マーカの大きさは、使用するカメラ、空間、物体の大きさ、物体の形状などに応じて決定すればよい。また、物体に設けるマーカの個数は特に限定されるものではないが、より多いほど好ましい。すなわち、より多くのマーカが１つの画角に収まることで、推定精度を向上すすることができる。
【００５２】
このような推定装置は、ロボットに搭載することが好適である。物体を把持するアームを持つロボットに、推定装置を搭載することで、確実に物体を把持することができる。特に、物体を把持する産業用ロボットや、家庭用ロボットに好適である。さらには、工場内の設備管理用途にも好適である。
【００５３】
なお、上記の説明では、重み付け最小二乗法によって誤差を最小化したが、他の方法を用いて、誤差を最小化しても良い。さらには、複数の第１のマーカ座標系から、物体座標系を求める手法は特に限定されるものではない。
【００５４】
上記の推定処理はコンピュータプログラムによって実行させても良い。３次元・位置姿勢の推定処理をコンピュータに行わせるための命令群を含むプログラムは、非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いてコンピュータに供給できる。非一時的なコンピュータ可読媒体は、様々な種類の実体のある記憶媒体（tangible storage medium）を含む。例えば、非一時的なコンピュータ可読媒体は、磁気記憶媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記憶媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ，ＣＤ−Ｒ／Ｗ、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、ＵＶ−ＥＰＲＯＭ（Erasable ROM）、ＥＥＰＲＯＭ（Electrical EPROM）、フラッシュＲＯＭを含む。また、プログラムは、非一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的な媒体は、プログラムがエンコードされた電気信号、光信号、及び電磁波を含む。プログラムがエンコードされた電気信号、光信号、及び電磁波は、電線及び光ファイバ等の有線伝送路、又は無線伝送路（空間）を伝搬することでコンピュータに供給される。
【符号の説明】
【００５５】
１ カメラ
２ 処理装置
１０ 物体
１１ マーカ
１２ マーカ
２１ 第１のマーカ座標系
２１ａ 第２のマーカ座標系
２２ 第１のマーカ座標系
２２ａ 第２のマーカ座標系
３０ 物体座標系
４０ 物体
４１ マーカ
５１ 第１のマーカ座標系
６０ 物体
６０ 物体
６１ マーカ
７１ 第１のマーカ座標系
【特許請求の範囲】
【請求項１】
複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定装置であって、
対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するマーカ検出部と、
前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するマーカ座標系推定部と、
前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定する物体座標系決定部と、を備える推定装置。
【請求項２】
前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、
前記仮決めされた物体座標系を第２のマーカ座標系に変換し、
前記マーカ画像から推定した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系との誤差を最小化することで、前記物体座標系を決定する請求項１に記載の推定装置。
【請求項３】
前記第１のマーカ座標系と前記第２のマーカ座標系との前記誤差が最小となるように、複数のマーカ座標系に対する重み付け関数を最適化することで前記物体座標系を決定する請求項２に記載の推定装置。
【請求項４】
前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられている請求項３に記載の推定装置。
【請求項５】
前記マーカ検出部で検出したマーカの数が一つである場合に、
前記一つのマーカの前記第１のマーカ座標系から、前記物体座標系を推定する請求項１乃至４のいずれか１項に記載の推定装置。
【請求項６】
複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定方法であって、
対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するステップと、
前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するステップと、
前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定するステップと、を備える推定方法。
【請求項７】
前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、
前記仮決めされた物体座標系を第２のマーカ座標系に変換し、
前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定する請求項６に記載の推定方法。
【請求項８】
前記第１のマーカ座標系と前記第１のマーカ座標系との前記誤差が最小となるように、複数のマーカ座標系に対する重み付け関数を最適化することで前記物体座標系を決定する請求項７に記載の推定方法。
【請求項９】
前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられている請求項８に記載の推定方法。
【請求項１０】
検出された前記マーカの数が一つである場合に、
前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定する請求項６乃至９のいずれか１項に記載の推定方法。
【請求項１１】
複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定プログラムであって、
コンピュータに対して、
対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出させるステップと、
前記マーカ検出部で検出された複数のマーカに対応する複数の前記第１のマーカ座標系マーカ座標系を算出するため、それぞれのマーカ画像から前記第１のマーカ座標系マーカ座標系を推定させるステップと、
前記複数の前記第１のマーカ座標系マーカ座標系に基づいて、前記対象物体の物体座標系を決定させるステップと、を備える推定プログラム。
【請求項１２】
コンピュータに対して、
前記物体画像に含まれているマーカ画像から算出された複数の前記第１のマーカ座標系マーカ座標系に基づいて物体座標系を仮決めさせ、
前記仮決めされた物体座標系を第２のマーカ座標系に変換させ、
前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定させる請求項１１に記載の推定プログラム。
【請求項１３】
コンピュータに対して、
前記第１のマーカ座標系と前記第２のマーカ座標系との誤差が最小となるように、複数のマーカ座標系に対する重みを最適化することで前記物体座標系を決定させる請求項１２に記載の推定プログラム。
【請求項１４】
前記重みの最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられている請求項１３に記載の推定プログラム。
【請求項１５】
検出された前記マーカの数が一つである場合に、
前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定する請求項１１乃至１４のいずれか１項に記載の推定プログラム。
【請求項１】
複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定装置であって、
対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するマーカ検出部と、
前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するマーカ座標系推定部と、
前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定する物体座標系決定部と、を備える推定装置。
【請求項２】
前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、
前記仮決めされた物体座標系を第２のマーカ座標系に変換し、
前記マーカ画像から推定した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系との誤差を最小化することで、前記物体座標系を決定する請求項１に記載の推定装置。
【請求項３】
前記第１のマーカ座標系と前記第２のマーカ座標系との前記誤差が最小となるように、複数のマーカ座標系に対する重み付け関数を最適化することで前記物体座標系を決定する請求項２に記載の推定装置。
【請求項４】
前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられている請求項３に記載の推定装置。
【請求項５】
前記マーカ検出部で検出したマーカの数が一つである場合に、
前記一つのマーカの前記第１のマーカ座標系から、前記物体座標系を推定する請求項１乃至４のいずれか１項に記載の推定装置。
【請求項６】
複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定方法であって、
対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出するステップと、
前記マーカ検出部で検出された複数のマーカに対応する複数の第１のマーカ座標系を算出するため、それぞれのマーカ画像から第１のマーカ座標系を推定するステップと、
前記複数の第１のマーカ座標系に基づいて、前記対象物体の物体座標系を決定するステップと、を備える推定方法。
【請求項７】
前記物体画像に含まれているマーカ画像から推定された複数の第１のマーカ座標系に基づいて物体座標系を仮決めし、
前記仮決めされた物体座標系を第２のマーカ座標系に変換し、
前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定する請求項６に記載の推定方法。
【請求項８】
前記第１のマーカ座標系と前記第１のマーカ座標系との前記誤差が最小となるように、複数のマーカ座標系に対する重み付け関数を最適化することで前記物体座標系を決定する請求項７に記載の推定方法。
【請求項９】
前記重み付け関数の最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられている請求項８に記載の推定方法。
【請求項１０】
検出された前記マーカの数が一つである場合に、
前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定する請求項６乃至９のいずれか１項に記載の推定方法。
【請求項１１】
複数のマーカを有する対象物体の空間中における３次元位置・姿勢を示す物体座標系を推定する推定プログラムであって、
コンピュータに対して、
対象物体を撮像した物体画像から、前記対象物体に設けられ、単体で３次元位置・姿勢を推定可能なマーカを検出させるステップと、
前記マーカ検出部で検出された複数のマーカに対応する複数の前記第１のマーカ座標系マーカ座標系を算出するため、それぞれのマーカ画像から前記第１のマーカ座標系マーカ座標系を推定させるステップと、
前記複数の前記第１のマーカ座標系マーカ座標系に基づいて、前記対象物体の物体座標系を決定させるステップと、を備える推定プログラム。
【請求項１２】
コンピュータに対して、
前記物体画像に含まれているマーカ画像から算出された複数の前記第１のマーカ座標系マーカ座標系に基づいて物体座標系を仮決めさせ、
前記仮決めされた物体座標系を第２のマーカ座標系に変換させ、
前記マーカ画像から算出した第１のマーカ座標系と、前記物体座標系から変換された第２のマーカ座標系の誤差を最小化することで、前記物体座標系を決定させる請求項１１に記載の推定プログラム。
【請求項１３】
コンピュータに対して、
前記第１のマーカ座標系と前記第２のマーカ座標系との誤差が最小となるように、複数のマーカ座標系に対する重みを最適化することで前記物体座標系を決定させる請求項１２に記載の推定プログラム。
【請求項１４】
前記重みの最適化にＬｅｖｅｎｂｅｒｇ−Ｍａｒｑｕａｒｄｔ法が用いられている請求項１３に記載の推定プログラム。
【請求項１５】
検出された前記マーカの数が一つである場合に、
前記一つのマーカの第１のマーカ座標系から、前記物体座標系を推定する請求項１１乃至１４のいずれか１項に記載の推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１１−２０３１４８（Ｐ２０１１−２０３１４８Ａ）
【公開日】平成２３年１０月１３日（２０１１．１０．１３）
【国際特許分類】
【出願番号】特願２０１０−７１３３２（Ｐ２０１０−７１３３２）
【出願日】平成２２年３月２６日（２０１０．３．２６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２３年１０月１３日（２０１１．１０．１３）
【国際特許分類】
【出願日】平成２２年３月２６日（２０１０．３．２６）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]