位置検知システム、それを用いた移動体、位置検知方法およびそのためのプログラム
【課題】センサの計測範囲に特徴的な被写体が存在していない場合や、そのセンサが他のセンサの計測範囲外にあるときにも、その位置姿勢の算出を可能にする。
【解決手段】第１のセンサは、環境内の存在物の位置情報および色情報を第１の検知情報として得るものであり、第２のセンサは、存在物の色情報を第２の検知情報として得るものであり、情報処理装置は、第２のセンサの位置姿勢を仮に設定する位置姿勢仮設定部と、位置姿勢仮設定部により仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、第１の検知情報を用いて仮想的に作成する仮想検知情報作成部と、仮想検知情報作成部で作成された仮想検知情報と第２のセンサで得られた第２の検知情報を比較する比較部と、比較部の比較結果と仮に設定された第２のセンサの位置姿勢から、第２のセンサの位置姿勢推定値を出力する位置姿勢推定部を備える。
【解決手段】第１のセンサは、環境内の存在物の位置情報および色情報を第１の検知情報として得るものであり、第２のセンサは、存在物の色情報を第２の検知情報として得るものであり、情報処理装置は、第２のセンサの位置姿勢を仮に設定する位置姿勢仮設定部と、位置姿勢仮設定部により仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、第１の検知情報を用いて仮想的に作成する仮想検知情報作成部と、仮想検知情報作成部で作成された仮想検知情報と第２のセンサで得られた第２の検知情報を比較する比較部と、比較部の比較結果と仮に設定された第２のセンサの位置姿勢から、第２のセンサの位置姿勢推定値を出力する位置姿勢推定部を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、カメラ等のセンサが、その検知情報から自己の位置姿勢を同定する技術に関する。
【背景技術】
【０００２】
ロボットや飛行体の自律移動制御、あるいは移動する物体や人の状況監視等では、それらの移動体の位置姿勢を逐次把握する必要がある。そのための手段として、それらの移動体にカメラ等のセンサを取り付け、そのセンサが周囲環境を検知して得られた検知情報から、センサの位置姿勢を推定する方法がある。
【０００３】
例えば、非特許文献１には、カメラによって３点以上の３次元位置既知の点が撮影されたときに、撮影画像からそのカメラの３次元位置をあらわす３パラメータと、カメラの３次元空間内の姿勢をあらわす３パラメータの合計６パラメータを求める方法が示されている。
【０００４】
また、複数のセンサを組み合わせることで位置推定を高性能に行うための関連技術として、特許文献１には、１段階目で第１の走査ヘッド（第１のセンサ）からの物体に照射されたビームの相対位置を求め、第２段階目で第２の走査ヘッド（第２のセンサ）から、同物体上のビームの照射位置と前記第１の走査ヘッドの位置を求めることで、同一の座標系で物体の形状を計測する方法が示されている。これにより第２のセンサの位置姿勢を推定することも可能となる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特表平０６−５０１７７４号公報
【非特許文献】
【０００６】
【非特許文献１】R．M．Haralick and C．Lee，”Analysis and solutions of the three point perspective pose estimation problem，”IEEE Proc．CVPR，Computer Vision and Pattern Recognition，pp．592−598，1991．
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記非特許文献１の技術では、特徴的な色や形状を持つことで画像上で識別でき、さらにその環境内の３次元位置が既知の「特徴点」と呼ばれる被写体が、画像上に複数撮影されること、さらにこれらの特徴点の画像上の２次元位置が明示的に求められることが前提となる。よってこのような特徴的な被写体が存在しない場合や、曲面形状をした物体や物体表面に描かれた複雑な模様のように、画像上の２次元座標値を明示的に求めることができない被写体しか存在しない場合に対処できない。
【０００８】
一方、上記特許文献１の技術では、あるセンサが他のセンサを計測できることを前提とする。よって、計測されるセンサが計測するセンサの計測範囲外に移動したときには対処できない。
【０００９】
本発明は、これらの問題点を解決し、特徴点のような特徴的な被写体や、画像上の２次元座標値を求めることができる被写体が計測されない場合でも位置姿勢の推定を可能とし、さらに、第２のセンサが、第１のセンサの計測範囲外に移動しても位置姿勢の推定を可能にすることを目的とする。
【課題を解決するための手段】
【００１０】
本願において開示される発明のうち代表的なものを挙げれば、次の通りである。
【００１１】
本発明の位置検知システムは、第１のセンサと、第２のセンサと、情報処理装置からなる位置検知システムにおいて、前記第１のセンサは、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るものであり、前記第２のセンサは、存在物の色情報を第２の検知情報として得るものであり、前記情報処理装置は、第２のセンサの位置姿勢を仮に設定する位置姿勢仮設定部と、前記位置姿勢仮設定部により仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成する仮想検知情報作成部と、前記仮想検知情報作成部で作成された前記仮想検知情報と前記第２のセンサで得られた前記第２の検知情報を比較する比較部と、前記比較部の比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力する位置姿勢推定部を備えることを特徴とするものである。
【００１２】
また、本発明の移動体は、前記の位置検知システムを組み込んだ移動体であって、前記第２のセンサは移動体に取り付けられ、前記移動体は、前記位置検知システムから出力された第２のセンサの位置姿勢推定値を用いて移動制御する手段を備えることを特徴とするものである。
【００１３】
また、本発明の位置検知方法は、第１のセンサを用いて、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るステップと、第２のセンサを用いて、存在物の色情報を第２の検知情報として得るステップと、第２のセンサの位置姿勢を仮に設定するステップと、前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、前記仮想検知情報と前記第２の検知情報を比較するステップと、前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップを備えることを特徴とするものである。
【００１４】
また、本発明のプログラムは、コンピュータに、第１のセンサを用いて得られた、環境内の存在物の位置情報および色情報から成る第１の検知情報、および、第２のセンサを用いて得られた、存在物の色情報から成る第２の検知情報から、第２のセンサの位置姿勢を求めさせるプログラムであって、第２のセンサの位置姿勢を仮に設定するステップと、前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、前記仮想検知情報と前記第２の検知情報を比較するステップと、前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップとを実行させることを特徴とするものである。
【発明の効果】
【００１５】
本発明は、第１のセンサおよび第２のセンサを組み合わせて、第２のセンサの位置姿勢値を求める手段を提供するが、その際に、特徴点のような特徴的な被写体や、画像上の２次元座標値を求めることができる被写体が計測されない場合でも位置姿勢の推定が可能となる。また、第２のセンサが、第１のセンサの計測範囲外に移動しても位置姿勢の推定が可能となる。さらに、３次元ＣＧ（コンピュータグラフィックス）モデルを使う方法に比べて、そのモデルをリアルかつ詳細に作る準備が省略可能となる。これらにより、ロボット等の自律移動や、物体や人の動き計測などの応用システムにおいて位置姿勢の推定を行う際に、簡単な準備作業によって、環境や移動範囲に関する制約を大きく削減することができるので、その実用性を向上させることができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の一実施形態に係る位置検知システムの、全体構成を示すブロック構成図である。
【図２】本発明の応用システムの一例となる、自律飛行船システムを説明する図である。
【図３】画像・距離センサの機能を説明する図である。
【図４】画像・距離センサの拡張機能を説明する図である。
【図５】画像・距離センサの計測方法の１例を説明する図である。
【図６】第１の検知情報の一例を説明する図である。
【図７】第２のセンサとしてカメラを用いて撮影する一例を説明する図である。
【図８】図７のカメラによる撮影によって得られる撮影画像を説明する図である。
【図９】カメラを仮の位置姿勢に置いてサンプル点を撮影する一例を説明する図である。
【図１０】図９の撮影によって得られる仮想的な撮影画像を説明する図である。
【図１１】カメラを図９とは異なる仮の位置姿勢に置いてサンプル点を撮影する一例を説明する図である。
【図１２】図１１の撮影によって得られる仮想的な撮影画像を説明する図である。
【図１３】カメラによる実際の撮影画像と、仮想的な撮影画像との比較を説明する図である。
【図１４】本発明の一実施形態に係わる装置構成の一例を説明する図である。
【図１５】本発明の一実施形態に係わる処理の流れを説明する図である。
【発明を実施するための形態】
【００１７】
以下、本発明の実施の形態を、図面を参照しつつ説明する。
【００１８】
図１は本発明の一実施形態に係る、位置検知システムの全体構成を示すブロック構成図である。
【００１９】
本システムの全体構成の説明に先立って、その使われ方の１例として、「自律飛行船のための位置姿勢推定」をあげ、本システムの位置づけを述べる。
【００２０】
自律飛行船とは、図２に示すように、例えば屋内環境の中空を、飛行船２００が人による操縦なしに、環境内の自分の位置姿勢を逐次把握することで自律的に移動するものである。ここで位置姿勢を把握するために用いる手段が、本実施の形態による位置検知システムである。具体的には、例えばカメラを第２のセンサ１４０として前記の飛行船に取り付け、これが地上にある環境内の存在物１００を撮影することで自分の位置姿勢を推定し、これから飛行船２００の位置姿勢を求め自律移動の制御を行う。一方、本発明では、あらかじめ第１のセンサ１３０を用いて、上記環境内の存在物を計測しておき、このときに計測した第１のセンサによる計測情報を、第２のセンサ１４０で位置姿勢を推定するときに利用する。
【００２１】
以上述べた使い方においては、あらかじめ第１のセンサを用いて環境内を計測しておく「準備フェーズ」と、機体に取り付けられた第２のセンサと準備フェーズで準備された情報を用いて自己の位置姿勢の推定を行いながら自律移動制御を行う「実行フェーズ」の２つのフェーズが存在することになる。このように本実施の形態は、２つのフェーズを組み合わせて利用することを想定した構成になっている。
【００２２】
次に図１を用いて、本実施の形態に係る位置検知システムの全体構成を説明する。
【００２３】
まず、環境内には、静止する存在物１００が存在していることを仮定する。本実施の形態では、この存在物を計測することができる第１のセンサ１３０と第２のセンサ１４０を有し、第１のセンサ１３０では上に述べた「準備フェーズ」における計測を行い、第２のセンサ１４０では「実行フェーズ」における計測を行う。最終的に位置姿勢値を求めるのは、この中で第２のセンサ１４０である。また第２のセンサは移動して、様々な位置姿勢値をとることを想定する。またセンサで得られた情報の処理は、情報処理装置１７５の内部で実施される。
【００２４】
「準備フェーズ」では、第１のセンサ１３０が存在物の計測を行い、第１の検知情報１５５として記録しておく。
【００２５】
一方「実行フェーズ」では、任意の位置姿勢で計測した第２のセンサ１４０による第２の検知情報１６５から、その第２のセンサの位置姿勢推定値を求める。このためにまず、後述する方法等を用いて第２のセンサの位置姿勢仮設定部１５８が、第２のセンサの仮の位置姿勢値を求める。次に、この第２のセンサの仮の位置姿勢値と、前記の第１の検知情報１５５を用いて、第２のセンサの仮想検知情報作成部１６０が、第２のセンサの仮想検知情報１６８を出力する。ここで第２のセンサの仮想検知情報１６８とは、第２のセンサ１４０が前記の仮の位置姿勢にあるときに、前記の存在物を計測したときに得られると予想されるセンサ情報を予測して作成するもので、その作成方法は後述する。次に比較部１７０が、作成された第２のセンサの仮想検知情報１６８と、第２のセンサで実際に計測して得られた前記第２の検知情報１６５を比較し、その一致度を求める。以上述べた、（1）第２のセンサの仮の位置姿勢を設定し、（2）第２のセンサの仮想検知情報を作成し、（3）これと第２のセンサの実際の検知情報を比較する、といった（1）（2）（3）の一連の処理を複数回行い、これらの中で例えば最も一致度が高いものを選択するなどにより、位置姿勢推定部１６３が第２のセンサの位置姿勢推定値を決定し出力する。
【００２６】
以上が、本発明の一実施形態に係る位置検知システムの全体構成である。
【００２７】
次に、本実施の形態における各構成要素の処理について説明する。
【００２８】
本実施の形態では、第１のセンサに画像・距離センサ、第２のセンサにカメラを使用するものとする。
【００２９】
ここで画像・距離センサとは、図３に示すように、センサ１３０から存在物１００上の計測点２１０までの距離r1、方向v1、ならびにその計測点の色を例えば（R1，G1，B1）の３原色情報として計測できるものである。現在そのような計測ができるセンサは、レーザレンジファインダとカメラを組み合わせたものを一例としてすでに開発されており、一般用途にて利用可能となっている。上記情報が計測でき、さらにセンサ１３０の空間内の位置と姿勢がＣ１としてわかっていれば、上記r1，v1を用いた簡単な座標計算により、上記計測点の空間内の位置が（X1，Y1，Z1）の３次元座標値として世界座標系として求めることができる。ここで世界座標系をどのように定義するかが問題となるが、本実施の形態では、まずは第１のセンサ１３０を固定とし、世界座標系をそのセンサの座標系と等しいものとして定義する。すなわち最初に固定した第１のセンサ１３０の位置が原点となり、センサの前後・左右・上下方向が各座標軸の方向を示すことになる。なお図４に示すように、一般の画像・距離センサ１３０では、センサの姿勢を変えることなく、複数の方向に向けた計測点を計測することが可能である。よって、上記のように座標系を定義することで、図４における第１のセンサによる複数の計測点２１０ Pi （i＝1，2，・・・，N）に関し、その３次元座標値と色情報を（Xi，Yi，Zi，Ri，Gi，Bi）（i＝1，2，・・・，N）の形式で得ることができることになる。なおここで、Nは計測点２１０の個数である。以上が、本実施の形態で第１のセンサとして利用する画像・距離センサの説明である。
【００３０】
本実施の形態では、まず「準備フェーズ」において、第１のセンサとなる画像・距離センサ１３０を用いて存在物１００上の任意の計測点２１０を計測し、その位置と色情報を得る。ここで計測した計測点２１０を、以下の説明ではサンプル点と呼ぶことにする。上記説明では、第１のセンサ１３０は図４に示す位置姿勢Ｃ１に固定としたが、図５に示すように、第１のセンサ１３０をＣ２の位置姿勢に移動することで、図４におけるＣ１の１箇所だけからは計測不可能なサンプル点２１１の計測を行うこともできる。なおこの場合は、第１のセンサ１３０の移動後の位置姿勢値Ｃ２を、例えば背景技術の項で述べた方法などにより計測することで求めておく。ここで求められた移動後の位置姿勢パラメータＣ２を用いて座標変換計算を行うことで、移動後のセンサで得られたサンプル点２１１の３次元位置を、移動前の位置姿勢を基準とした座標系、すなわち世界座標系として共通にあらわすことが可能となる。以上をまとめると、第１のセンサ１３０を固定して用いても、あるいは移動して用いても、どちらも図６に示すようにすべてのサンプル点２１０，２１１に関する情報を、同一の世界座標系であらわした位置情報と色情報の組み合わせとなるデータ （Xi，Yi，Zi，Ri，Gi，Bi）（i＝1， 2，・・・，N）の集合として得ることになる。なおここで、iはサンプル番号であり、Nはサンプル数である。上述べたように、第１のセンサにより環境内の存在物の計測を行い、複数のサンプル点の位置情報と色情報の組み合わせたデータを得る処理が「準備フェーズ」での処理となる。
【００３１】
一方「実行フェーズ」では、図１あるいは図２に示された、第２のセンサとなるカメラ１４０によって、任意の位置姿勢から得られた撮影画像から、そのカメラの位置姿勢を求める。ここで用いるカメラは、焦点距離fの透視投影モデルに従い、存在物の色情報が画像として得られるものを想定する。一般のカメラは、焦点距離が未知で、さらにレンズ歪みなどの影響で必ずしも透視投影のモデルに従うとは限らないが、一般にカメラの撮影画像は事前にキャリブレーションなどを行うことで、焦点距離既知の理想的な透視投影画像に補正可能である。よってこのような想定は説明上実際性を失わない。
【００３２】
まず第２のセンサとなるカメラ１４０によって撮影画像が１枚得られたときに、このカメラの位置姿勢値を仮に設定する。この設定を行うのは図１における第２のセンサの位置姿勢仮設定部１５８であり、その設定方法は後述する。この位置姿勢値と、前記のサンプル点データから、第２のセンサとなるカメラ１４０で得られるであろうこれらサンプル点の撮影画像を仮想的に作成する。この処理を行うのは図１における第２のセンサの仮想検知情報作成部１６０であり、その作成方法は後述する。次にこの仮想的に作成した仮想画像１６８と、実際に第２のセンサで得られた撮影画像１６５を比較する。この処理を行うのは、図１における比較部１７０である。ここでもし仮に設定したセンサの位置姿勢と、実際のセンサの位置姿勢が等しければ、サンプル点に関する画像は、両者画像上で等しい位置に写り、等しい色で写るはずである。一方、ここでもし仮に設定したセンサの位置姿勢と、実際のセンサの位置姿勢が異なれば、サンプル点に関する画像はお互い違いが生じる。これを図で説明するのが、図７、図８、図９、図１０、図１１である。
【００３３】
いま図７に示すとおり、存在物１００をカメラ等の第２のセンサ１４０で、図に示す位置姿勢D1から撮影したことを考える。この撮影により、図８に示す撮影画像１６６が得られる。一方図９と図１０に示すとおり、準備フェーズにて第１のセンサを用いて存在物を計測して取得したサンプル点の位置・色情報２１０，２１１から、もし仮にカメラ１４０が図の位置姿勢D1にあったときに得られるサンプル点の仮想的な撮影画像１６８を、図１０に示すものとして計算によって求める。また他方、図１１と図１２に示すとおり、同様にもし仮にカメラ１４０が図の位置姿勢D2にあったときに得られるサンプル点の仮想的な撮影画像１６９を、図１２に示すものとして計算によって求める。次に図１３に示すように、図８で示した撮影画像１６６と図１０に示したD1での仮想画像１６８の比較と、図８で示した撮影画像１６６と図１２に示したD2での仮想画像１６９の比較の２つの比較を行う。図１３からわかるように、実際のカメラの位置姿勢D1と、仮想画像を作成する際に設定した位置姿勢D1が等しい図１３の左側の比較では、サンプル点２１００の画像の色が実際の画像の色と一致する。一方、実際のカメラの位置姿勢D1と、仮想画像を作成する際に設定した位置姿勢D2が異なる図１３の右側の比較では、サンプル点２１１０の画像の色が実際の画像の色と一致しない。以上述べたように、このような違いが発生するか、しないかを評価することで、仮に設定したセンサの位置姿勢値が、実際の位置姿勢に等しいかどうかを判断することができる。すなわち、考えうる様々な位置姿勢値を仮に設定し、そこからの仮想的な撮影画像を作成し、比較評価を行うことで、実際の位置姿勢値を見つけることができる。これが本発明における位置姿勢推定の原理である。
【００３４】
なおここで、用いるサンプル点数が少ないなどで、それぞれのサンプル点の色の違いがない場合は、仮に設定したセンサの位置姿勢値が実際の位置姿勢と異なっていても、作成した仮想撮影画像のサンプル点の色が、実際の撮影画像の同じ位置の色と一致することも偶然的に発生する可能性がある。この問題に関しては、用いるサンプル点を数百から数千程度に増やすことで、そのような偶然的な事象がすべてのサンプル点で発生する確率は限りなく０に近づき解決する。さらに後述するとおり、本発明が想定するその利用方法では、第２のセンサの位置姿勢がある程度狭い範囲のみに限定されるという条件を利用することができるので、間違った仮の位置姿勢値を用いたときに画像の比較結果が一致することは、実際上ほぼありえないとみなすことができる。
【００３５】
次に、上記実施の形態における各要素の処理の詳細を補足的に述べる。
【００３６】
まず、図４等で示したように第１のセンサ１３０によって環境内存在物１００を計測する際、存在物のどの部分をサンプル点２１０として計測するか、その選択方法を説明する。これにはいくつかの方法が考えられが、その代表的な方法は、あらかじめ設定された角度ごとにスキャンする方法である。センサの周囲全体を対象に任意に向かう方向は、例えば水平方向φ、垂直方法ψの２つの角度パラメータであらわすことができるが、φについては1．0度ごとに0から360．0度、ψについては1．0度ごとに−90．0から90．0度の範囲をスキャンすれば、センサ周囲全体を1．0度ごとの解像度でスキャンできることになる。この場合スキャン方向数の合計は360×360 ＝ 129，600 個の方向となる。すなわち、センサの全周囲方向に対し最大で129，000個のサンプル点が計測されることになる。ただし実際には、すべての方向に向かって、センサの測定可能距離以下の範囲に存在物が存在するとは限らないので、計測されるサンプル数はそれ以下となることが一般的と考えられるが、その場合も以下の処理や作用は変わらない。なお以上述べた方法以外にも、存在物がある領域を細かくスキャンする方法、第２のセンサが撮影する可能性の高い場所を重点的に細かくスキャンする方法などをはじめ、運用条件等を考慮し適切な方法をとることができる。
【００３７】
また、第１のセンサによって計測できる色情報の表現方法としては、RGBの３原色であらわす方法以外に、色相・明度・色彩の３パラメータであらわす方法、明るさのみのグレースケールであらわす方法などいくつかの方法が利用できる。ただし以下の説明ではRGBの３原色を用いる方法で説明する。これによるとサンプル点データは、Pi＝（Xi，Yi，Zi，Ri，Gi，Bi）（i＝1，2，・・・，N） の形式で準備されることになる。なおここで、i はサンプル点番号、（Xi，Yi，Zi） はその番号のサンプル点の３次元座標値を世界座標系であらわしたもの、（Ri，Gi，Bi） はその点の色情報である。
【００３８】
次に全体処理の中で、第２のセンサの位置姿勢仮設定部が、仮の位置姿勢値を設定する方法について詳しく説明する。なお以降の説明では、第２のセンサの仮の位置姿勢設定値をGk＝（Xk，Yk，Zk，Rk）（k＝1，2，・・・，K）とあらわすこととする。ここで、kは仮に設定した複数の位置姿勢値を識別する番号である。また、（Xk，Yk，Zk）はk番目の仮設定値における位置を３次元座標値として世界座標系であらわしたもの、Rkはk番目の仮設定値における姿勢を３×３の３次元回転行列として世界座標系であらわしたものである。この仮の位置姿勢値を設定する方法としていくつかの方法が考えられるが、その代表的なものは、可能性のあるすべての位置姿勢値を、適当な量子化幅をもって網羅的に列挙する方法である。ただしこの方法によると、列挙する位置姿勢値の数が極めて膨大になる問題が発生する。すなわち、位置姿勢パラメータは、位置（Xk，Yk，Zk）の３パラメータに３つの自由度をもつ姿勢 Rk パラメータが加わり、合計で６つの自由度がある。ここである程度の量子化幅をもって、第２のセンサが存在しうる空間全域を羅列しようとすると、例えば１つの自由度について100の候補を考えるとすべての組み合わせ数は100の６乗となり、現実的な計算処理量をはるかに超えたものとなってしまう。そこで実際の応用においては、応用システムのもつ様々な条件を利用することで、その候補の数を限定する。例えば自律飛行船の位置姿勢推定においては、例えば０．１秒といった単位時間ごとに連続して逐次位置姿勢推定を行なうことが一つの制御方法として想定されるが、その際に０．１秒の時間間隔で変化しうる自律飛行船の位置姿勢はほんのわずかと考えることができる。この条件より、ある時刻tの位置姿勢を推定するにあたって、その位置姿勢は、直前の時刻t−1における位置姿勢から、高々前後左右数cm程度、姿勢もすべての方向に対し高々プラスマイナス1度程度の範囲にあると限定することができる。これによって仮に設定する位置姿勢値を、位置に関し１cm程度、角度に関し０．５度程度の分解能（量子化幅）で設定しても、そのすべての組み合わせで得られる可能性のある位置姿勢値は高々数百から数千、多くても数万程度となり、現在の計算機の能力から十分に処理可能な数になる。
【００３９】
次に全体の処理の中で、仮想検知情報を作成する方法について詳しく述べる。第２のセンサの仮想検知情報作成部は、上で言及したサンプル点データ群 Pi（i＝1， 2，・・・，N）と、k番目の仮の位置姿勢設定値Dkから、カメラの透視投影モデルの式に従って、それぞれのサンプル点の画像上の位置 pi＝（xi，yi，f）を算出する。ここで（xi，yi）はサンプル点の画像上の位置を画像の中心を原点とした画像座標系であらわしたもの、fは焦点距離をカメラ座標系の単位長さであらわしたものである。
【００４０】
【数１】
【００４１】
この式にしたがって求められる各サンプル点の画像上の位置pi（i＝1， 2，・・・，N） に、それぞれのサンプル点の色（Ri，Gi，Bi）を割り当てる。ここでその位置 pi が画像のサイズ、すなわち縦横長さの範囲外を示しているときは、そのサンプル点を無視し、画像への色の割り当ては行わないものとする。なお色の割り当てを行うとは、たとえばデジタル画像上において、その位置 pi が指し示すピクセルのメモリに、その色の値（Ri，Gi，Bi）を記録することを意味する。このようにして作成された画像が、仮の位置姿勢設定値Dkから作成される、第２のセンサの仮想検知情報Mkである。本実施の形態では、このような画像が、設定されたすべての仮の位置姿勢設定値Dk（k＝ 1，2，・・・，K）それぞれについて作成される。
【００４２】
次に全体の処理の中で、比較部が行う処理について詳しく述べる。比較部は、このように作成された仮想画像におけるすべてのサンプル点の色情報と、第２のセンサで実際に得られた撮影画像における同じ場所の色情報を比較し、その類似度を評価値 V として算出することで類似度を判断できるようにする。評価値Vの算出方法としては、例えばそれぞれの色成分の差の二乗平均を、以下の式により求める方法が考えられる。
【００４３】
【数２】
【００４４】
ここで、（R’i，G’i，B’i）は、仮想画像におけるi番目のサンプル点と同じ位置にある、撮影画像上の点の色である。評価値Vは、もし仮想画像と撮影画像がすべてのサンプル点で同じ色をもてば0の値をとり、その違いが大きくなるにしたがってVの値は大きくなる。すなわち評価値Vが小さいほど類似度は大きいと判断でき、逆に評価値Vが大きいほど類似度は小さいと判断できる。またVの値は必ず0以上となる。第２のセンサの位置姿勢仮設定部は、上に述べたとおり可能性のあるすべての位置姿勢値を、仮の位置姿勢値Dk（k＝1，2，・・・，K）として出力する。第２のセンサの仮想検知情報作成部はこれらの位置姿勢において得られる仮想検知情報を作成し、比較部は、これらの仮想検知情報それぞれについて評価値VをVk（k＝1，2，・・・，K）として求める。もし計測誤差や、仮想検知情報の作成誤差、あるいは量子化誤差等、すべての誤差が0であれば、第２のセンサの実際の位置姿勢値と、仮に設定した位置姿勢値が一致したときに、上で定義した評価値Vは0の値となる。しかし実際には様々な誤差要因により両者は完全には一致せず、評価値も完全に0にはならない。そこで例えば最も0に近い値を持つものが、実際の位置姿勢値である可能性が最も高い、あるいは実際の位置姿勢値に最も近いであろうと判断し、すべての評価値Vk（k＝1，2，・・・，K）からその最小値となるものをVk’として選び、このときの仮の位置姿勢値Dk’を第２のセンサの位置姿勢の推定値として出力する。
【００４５】
次に図１４を用いて、図１を用いて説明した一実施形態における装置構成の一例を説明する。本装置は、演算等を行う中央演算装置１４１０、プログラムやデータを一時的に保存する主記憶装置１４２０、プログラムやデータを長期的に保存する外部記憶装置１４３０、外部ネットワークや外部機器からデータの送受信を行うネットワークインタフェース１４４０および、ネットワークインタフェースを介して接続される第１のセンサ１３０および第２のセンサ１４０から大きく構成される。
【００４６】
主記憶装置１４２０には、図１における第２のセンサの位置姿勢仮設定部１５８の処理を行う第２のセンサの位置姿勢仮設定プログラム、図１における第２のセンサの仮想検知情報作成部１６０の処理を行う第２のセンサの仮想検知情報作成プログラム、図１における比較部１７０の処理を行う比較プログラム、図１における位置姿勢推定部１６３の処理を行う位置姿勢推定プログラムなどのプログラムと、図１における第１の検知情報１５５、第２の検知情報１６５、第２のセンサの仮想検知情報１６８などのデータを記憶する。なお上記プログラムやデータを外部記憶装置１４３０で長期的に保存することもできる。本実施の形態における準備フェーズでは、第１のセンサ１３０で得られるセンシングデータを、ネットワークインタフェース１４４０を介して入力し、変換処理等を中央演算装置１４１０がプログラムに従って行い、第１のセンシング情報データとして記憶する。一方、本実施の形態における実行フェーズでは、第２のセンサ１４０で得られる検知情報を、ネットワークインタフェース１４４０を介して入力し、各種処理を中央演算装置１４１０がプログラムに従って行い、第２のセンサの位置姿勢推定値を求め、主記憶装置１４２０に記憶、あるいはネットワークインタフェース１４４０を介して外部に出力する。
【００４７】
なお、本実施の形態では、準備フェーズを行う装置と、実行フェーズを行う装置を同じ装置で行うものとしたが、これらの処理を別の装置で行う構成にすることもできる。この場合は、準備フェーズで準備した第１の検知情報を、外部記憶装置やネットワークインタフェースを用いて、実行フェーズを行う装置に転送する。
【００４８】
次に、図１５を用いて本発明の処理の流れを説明する。
【００４９】
まず準備フェーズでは、第１のセンサによる存在物の計測を行うステップＳ１５１０において、例えば存在物表面上の複数のサンプル点の位置情報と色情報を計測する。この計測された情報は、第１の検知情報の作成と記録のステップＳ１５２０において、メモリ等の媒体に第１の検知情報として記録される。これで準備フェーズは終了する。
【００５０】
一方実行フェーズでは、第２のセンサによる存在物の計測を行うステップＳ１５５０において、存在物が例えばカメラ画像として計測され、これが第２の検知情報となる。一方、第２のセンサの仮の位置姿勢の設定ステップＳ１５３０において、第２のセンサの仮の位置姿勢値が設定される。次に、第２のセンサの仮想検知情報の作成ステップＳ１５４０では、ステップＳ１５３０において設定された仮の位置姿勢値と、準備フェーズで記録された第１の検知情報から、第２のセンサの仮想検知情報を作成する。次に、第２のセンサの仮想検知情報と、第２の検知情報を比較するステップＳ１５７０では、ステップＳ１５３０の出力とステップＳ１５４０の出力を比較し、その一致度を求める。次にステップＳ１５７０においてすべての仮の位置姿勢値で求めたかどうかの判断を行い、求めていなければステップＳ１５３０から繰り返す。もしすべてを求めていれば、推定値を出力するステップＳ１５８０において、ステップＳ１５６０で求めた一致度の最も高い仮の位置姿勢値を探し、これを推定値として出力する。以上で実行フェーズを終了する。
【００５１】
以上、本発明の一実施形態について詳しく説明した。
【００５２】
本実施の形態によると、位置姿勢の推定に用いるサンプル点は、準備フェーズにおいて任意に選ぶことができるので、背景技術で必要だった、画像上で一意に判別可能でその画像上の位置が認識可能な特徴的な点を計測しなければならないという、背景技術に記載した第１の制約がなくなる。また、存在物を第１のセンサおよび第２のセンサで共通に計測することで、第１のセンサの計測範囲に第２のセンサが存在していなければならないとする、背景技術に記載した第２の制約が解決される。また本実施の形態によれば、準備フェーズでの処理は、第１のセンサによる計測と自動的な情報処理のみで構成されるので、存在物の詳細なＣＧモデルを作成するといった、手作業やスキルを伴う処理は不要であり、その実施は容易となる。
【００５３】
なお、上に述べた実施の形態において、第２のセンサは透視投影モデルに従うカメラを使うことを想定したが、それ以外に例えば魚眼カメラなど異なる投影モデルに従うカメラを使う、ステレオカメラ等の複数カメラの組み合わせたセンシングシステムを使う、あるいはレーザ距離センサのように色情報以外の情報を取得できるセンサを使うこともできる。この場合は、それぞれのセンサの計測モデルに従うように、仮想検知情報生成部の処理内容を構成すればよい。
【００５４】
また、第２のセンサの位置姿勢仮設定部における設定方法としては、応用システムにおいて何らかの手段を用いて得た、位置姿勢の予測値を用いる方法がある。ここでその予測をする手段としては、ジャイロやＧＰＳといった精度の低い他の位置姿勢予測手段を用いたり、ロボット等に用いる場合は、どのような方向や距離の移動を行ったかという、移動制御情報を用いる方法などが考えられる。これらは応用システムの設計に依存するものであり、その原理をここでは詳述せず、そのような予測値が得られたものとしてその後の処理について説明する。まずこの予測値の形式であるが、ここではそれが確率的にあらわされることを前提とする。ここで確率的にあらわされるとは、予測値となる位置姿勢値を例えばX1と決定的に１つの値として限定せず、例えばX1である確率がp（X1）であり、X2である確率がp（X2）であり、X3である確率がp（X3）であるなどと、可能性をすべて網羅することを言う。このようにして列挙された可能性のある位置姿勢値を、その確率値、あるいは相対的に確率の大きさをあらわす値w1，w2，・・・，wM とともに、（X1，w1），（X2，w2），・・・，（XM，wM）の形式で準備する。本発明による本実施の形態では、これらの位置姿勢値X1，X2，・・・，XKを仮の位置姿勢値とし、第２のセンサの仮想的な画像を作成し、実際の画像との比較を行って得られたお互いの類似度を示す評価値V1，V2，・・・，VKに基づき、位置姿勢推定値を選ぶ。その選び方は、評価値V1，V2，・・・，VKの最も小さいものを選ぶ前述の方法に加え、予測で得られた確率的な値 w1，w2，・・・，wkをあわせて考慮することで判断する方法もありうる。このときは、２つの値の積 w1×V1，w2×V2，・・・，wk×VKを考えその大きなものを選ぶ、２つの値の和を考える、あるいは対数の和を考えるなど、いくつかの方法がある。このように２つの値を合わせて考えることで、予測による位置姿勢の推定と第２のセンサによる位置姿勢の推定を合わせて考えることができ、推定の精度をより高める効果がある。
【００５５】
また位置姿勢推定値の選択方法としては、上記算出した値が最も大きくなるものを選ぶだけでなく、いくつかの位置姿勢候補値の重心を求めるなど、複数の候補値での最終評価値の値の演算により求める方法をとることで、より推定の精度を高めたり、大きな誤差が発生する可能性を排除することもできる。
【符号の説明】
【００５６】
１００ 存在物
１３０ 第１のセンサ
１４０ 第２のセンサ
１５５ 第１の検知情報
１５８ 第２のセンサの位置姿勢仮設定部
１６０ 第２のセンサの仮想検知情報作成部
１６３ 位置姿勢推定部
１６５ 第２の検知情報
１７０ 比較部
１７５ 情報処理装置
２００ 飛行船
２１０，２１１ サンプリング点
１４１０ 中央演算装置
１４２０ 主記憶装置
１４３０ 外部記憶装置
１４４０ ネットワークインターフェース
【技術分野】
【０００１】
本発明は、カメラ等のセンサが、その検知情報から自己の位置姿勢を同定する技術に関する。
【背景技術】
【０００２】
ロボットや飛行体の自律移動制御、あるいは移動する物体や人の状況監視等では、それらの移動体の位置姿勢を逐次把握する必要がある。そのための手段として、それらの移動体にカメラ等のセンサを取り付け、そのセンサが周囲環境を検知して得られた検知情報から、センサの位置姿勢を推定する方法がある。
【０００３】
例えば、非特許文献１には、カメラによって３点以上の３次元位置既知の点が撮影されたときに、撮影画像からそのカメラの３次元位置をあらわす３パラメータと、カメラの３次元空間内の姿勢をあらわす３パラメータの合計６パラメータを求める方法が示されている。
【０００４】
また、複数のセンサを組み合わせることで位置推定を高性能に行うための関連技術として、特許文献１には、１段階目で第１の走査ヘッド（第１のセンサ）からの物体に照射されたビームの相対位置を求め、第２段階目で第２の走査ヘッド（第２のセンサ）から、同物体上のビームの照射位置と前記第１の走査ヘッドの位置を求めることで、同一の座標系で物体の形状を計測する方法が示されている。これにより第２のセンサの位置姿勢を推定することも可能となる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特表平０６−５０１７７４号公報
【非特許文献】
【０００６】
【非特許文献１】R．M．Haralick and C．Lee，”Analysis and solutions of the three point perspective pose estimation problem，”IEEE Proc．CVPR，Computer Vision and Pattern Recognition，pp．592−598，1991．
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上記非特許文献１の技術では、特徴的な色や形状を持つことで画像上で識別でき、さらにその環境内の３次元位置が既知の「特徴点」と呼ばれる被写体が、画像上に複数撮影されること、さらにこれらの特徴点の画像上の２次元位置が明示的に求められることが前提となる。よってこのような特徴的な被写体が存在しない場合や、曲面形状をした物体や物体表面に描かれた複雑な模様のように、画像上の２次元座標値を明示的に求めることができない被写体しか存在しない場合に対処できない。
【０００８】
一方、上記特許文献１の技術では、あるセンサが他のセンサを計測できることを前提とする。よって、計測されるセンサが計測するセンサの計測範囲外に移動したときには対処できない。
【０００９】
本発明は、これらの問題点を解決し、特徴点のような特徴的な被写体や、画像上の２次元座標値を求めることができる被写体が計測されない場合でも位置姿勢の推定を可能とし、さらに、第２のセンサが、第１のセンサの計測範囲外に移動しても位置姿勢の推定を可能にすることを目的とする。
【課題を解決するための手段】
【００１０】
本願において開示される発明のうち代表的なものを挙げれば、次の通りである。
【００１１】
本発明の位置検知システムは、第１のセンサと、第２のセンサと、情報処理装置からなる位置検知システムにおいて、前記第１のセンサは、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るものであり、前記第２のセンサは、存在物の色情報を第２の検知情報として得るものであり、前記情報処理装置は、第２のセンサの位置姿勢を仮に設定する位置姿勢仮設定部と、前記位置姿勢仮設定部により仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成する仮想検知情報作成部と、前記仮想検知情報作成部で作成された前記仮想検知情報と前記第２のセンサで得られた前記第２の検知情報を比較する比較部と、前記比較部の比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力する位置姿勢推定部を備えることを特徴とするものである。
【００１２】
また、本発明の移動体は、前記の位置検知システムを組み込んだ移動体であって、前記第２のセンサは移動体に取り付けられ、前記移動体は、前記位置検知システムから出力された第２のセンサの位置姿勢推定値を用いて移動制御する手段を備えることを特徴とするものである。
【００１３】
また、本発明の位置検知方法は、第１のセンサを用いて、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るステップと、第２のセンサを用いて、存在物の色情報を第２の検知情報として得るステップと、第２のセンサの位置姿勢を仮に設定するステップと、前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、前記仮想検知情報と前記第２の検知情報を比較するステップと、前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップを備えることを特徴とするものである。
【００１４】
また、本発明のプログラムは、コンピュータに、第１のセンサを用いて得られた、環境内の存在物の位置情報および色情報から成る第１の検知情報、および、第２のセンサを用いて得られた、存在物の色情報から成る第２の検知情報から、第２のセンサの位置姿勢を求めさせるプログラムであって、第２のセンサの位置姿勢を仮に設定するステップと、前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、前記仮想検知情報と前記第２の検知情報を比較するステップと、前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップとを実行させることを特徴とするものである。
【発明の効果】
【００１５】
本発明は、第１のセンサおよび第２のセンサを組み合わせて、第２のセンサの位置姿勢値を求める手段を提供するが、その際に、特徴点のような特徴的な被写体や、画像上の２次元座標値を求めることができる被写体が計測されない場合でも位置姿勢の推定が可能となる。また、第２のセンサが、第１のセンサの計測範囲外に移動しても位置姿勢の推定が可能となる。さらに、３次元ＣＧ（コンピュータグラフィックス）モデルを使う方法に比べて、そのモデルをリアルかつ詳細に作る準備が省略可能となる。これらにより、ロボット等の自律移動や、物体や人の動き計測などの応用システムにおいて位置姿勢の推定を行う際に、簡単な準備作業によって、環境や移動範囲に関する制約を大きく削減することができるので、その実用性を向上させることができる。
【図面の簡単な説明】
【００１６】
【図１】本発明の一実施形態に係る位置検知システムの、全体構成を示すブロック構成図である。
【図２】本発明の応用システムの一例となる、自律飛行船システムを説明する図である。
【図３】画像・距離センサの機能を説明する図である。
【図４】画像・距離センサの拡張機能を説明する図である。
【図５】画像・距離センサの計測方法の１例を説明する図である。
【図６】第１の検知情報の一例を説明する図である。
【図７】第２のセンサとしてカメラを用いて撮影する一例を説明する図である。
【図８】図７のカメラによる撮影によって得られる撮影画像を説明する図である。
【図９】カメラを仮の位置姿勢に置いてサンプル点を撮影する一例を説明する図である。
【図１０】図９の撮影によって得られる仮想的な撮影画像を説明する図である。
【図１１】カメラを図９とは異なる仮の位置姿勢に置いてサンプル点を撮影する一例を説明する図である。
【図１２】図１１の撮影によって得られる仮想的な撮影画像を説明する図である。
【図１３】カメラによる実際の撮影画像と、仮想的な撮影画像との比較を説明する図である。
【図１４】本発明の一実施形態に係わる装置構成の一例を説明する図である。
【図１５】本発明の一実施形態に係わる処理の流れを説明する図である。
【発明を実施するための形態】
【００１７】
以下、本発明の実施の形態を、図面を参照しつつ説明する。
【００１８】
図１は本発明の一実施形態に係る、位置検知システムの全体構成を示すブロック構成図である。
【００１９】
本システムの全体構成の説明に先立って、その使われ方の１例として、「自律飛行船のための位置姿勢推定」をあげ、本システムの位置づけを述べる。
【００２０】
自律飛行船とは、図２に示すように、例えば屋内環境の中空を、飛行船２００が人による操縦なしに、環境内の自分の位置姿勢を逐次把握することで自律的に移動するものである。ここで位置姿勢を把握するために用いる手段が、本実施の形態による位置検知システムである。具体的には、例えばカメラを第２のセンサ１４０として前記の飛行船に取り付け、これが地上にある環境内の存在物１００を撮影することで自分の位置姿勢を推定し、これから飛行船２００の位置姿勢を求め自律移動の制御を行う。一方、本発明では、あらかじめ第１のセンサ１３０を用いて、上記環境内の存在物を計測しておき、このときに計測した第１のセンサによる計測情報を、第２のセンサ１４０で位置姿勢を推定するときに利用する。
【００２１】
以上述べた使い方においては、あらかじめ第１のセンサを用いて環境内を計測しておく「準備フェーズ」と、機体に取り付けられた第２のセンサと準備フェーズで準備された情報を用いて自己の位置姿勢の推定を行いながら自律移動制御を行う「実行フェーズ」の２つのフェーズが存在することになる。このように本実施の形態は、２つのフェーズを組み合わせて利用することを想定した構成になっている。
【００２２】
次に図１を用いて、本実施の形態に係る位置検知システムの全体構成を説明する。
【００２３】
まず、環境内には、静止する存在物１００が存在していることを仮定する。本実施の形態では、この存在物を計測することができる第１のセンサ１３０と第２のセンサ１４０を有し、第１のセンサ１３０では上に述べた「準備フェーズ」における計測を行い、第２のセンサ１４０では「実行フェーズ」における計測を行う。最終的に位置姿勢値を求めるのは、この中で第２のセンサ１４０である。また第２のセンサは移動して、様々な位置姿勢値をとることを想定する。またセンサで得られた情報の処理は、情報処理装置１７５の内部で実施される。
【００２４】
「準備フェーズ」では、第１のセンサ１３０が存在物の計測を行い、第１の検知情報１５５として記録しておく。
【００２５】
一方「実行フェーズ」では、任意の位置姿勢で計測した第２のセンサ１４０による第２の検知情報１６５から、その第２のセンサの位置姿勢推定値を求める。このためにまず、後述する方法等を用いて第２のセンサの位置姿勢仮設定部１５８が、第２のセンサの仮の位置姿勢値を求める。次に、この第２のセンサの仮の位置姿勢値と、前記の第１の検知情報１５５を用いて、第２のセンサの仮想検知情報作成部１６０が、第２のセンサの仮想検知情報１６８を出力する。ここで第２のセンサの仮想検知情報１６８とは、第２のセンサ１４０が前記の仮の位置姿勢にあるときに、前記の存在物を計測したときに得られると予想されるセンサ情報を予測して作成するもので、その作成方法は後述する。次に比較部１７０が、作成された第２のセンサの仮想検知情報１６８と、第２のセンサで実際に計測して得られた前記第２の検知情報１６５を比較し、その一致度を求める。以上述べた、（1）第２のセンサの仮の位置姿勢を設定し、（2）第２のセンサの仮想検知情報を作成し、（3）これと第２のセンサの実際の検知情報を比較する、といった（1）（2）（3）の一連の処理を複数回行い、これらの中で例えば最も一致度が高いものを選択するなどにより、位置姿勢推定部１６３が第２のセンサの位置姿勢推定値を決定し出力する。
【００２６】
以上が、本発明の一実施形態に係る位置検知システムの全体構成である。
【００２７】
次に、本実施の形態における各構成要素の処理について説明する。
【００２８】
本実施の形態では、第１のセンサに画像・距離センサ、第２のセンサにカメラを使用するものとする。
【００２９】
ここで画像・距離センサとは、図３に示すように、センサ１３０から存在物１００上の計測点２１０までの距離r1、方向v1、ならびにその計測点の色を例えば（R1，G1，B1）の３原色情報として計測できるものである。現在そのような計測ができるセンサは、レーザレンジファインダとカメラを組み合わせたものを一例としてすでに開発されており、一般用途にて利用可能となっている。上記情報が計測でき、さらにセンサ１３０の空間内の位置と姿勢がＣ１としてわかっていれば、上記r1，v1を用いた簡単な座標計算により、上記計測点の空間内の位置が（X1，Y1，Z1）の３次元座標値として世界座標系として求めることができる。ここで世界座標系をどのように定義するかが問題となるが、本実施の形態では、まずは第１のセンサ１３０を固定とし、世界座標系をそのセンサの座標系と等しいものとして定義する。すなわち最初に固定した第１のセンサ１３０の位置が原点となり、センサの前後・左右・上下方向が各座標軸の方向を示すことになる。なお図４に示すように、一般の画像・距離センサ１３０では、センサの姿勢を変えることなく、複数の方向に向けた計測点を計測することが可能である。よって、上記のように座標系を定義することで、図４における第１のセンサによる複数の計測点２１０ Pi （i＝1，2，・・・，N）に関し、その３次元座標値と色情報を（Xi，Yi，Zi，Ri，Gi，Bi）（i＝1，2，・・・，N）の形式で得ることができることになる。なおここで、Nは計測点２１０の個数である。以上が、本実施の形態で第１のセンサとして利用する画像・距離センサの説明である。
【００３０】
本実施の形態では、まず「準備フェーズ」において、第１のセンサとなる画像・距離センサ１３０を用いて存在物１００上の任意の計測点２１０を計測し、その位置と色情報を得る。ここで計測した計測点２１０を、以下の説明ではサンプル点と呼ぶことにする。上記説明では、第１のセンサ１３０は図４に示す位置姿勢Ｃ１に固定としたが、図５に示すように、第１のセンサ１３０をＣ２の位置姿勢に移動することで、図４におけるＣ１の１箇所だけからは計測不可能なサンプル点２１１の計測を行うこともできる。なおこの場合は、第１のセンサ１３０の移動後の位置姿勢値Ｃ２を、例えば背景技術の項で述べた方法などにより計測することで求めておく。ここで求められた移動後の位置姿勢パラメータＣ２を用いて座標変換計算を行うことで、移動後のセンサで得られたサンプル点２１１の３次元位置を、移動前の位置姿勢を基準とした座標系、すなわち世界座標系として共通にあらわすことが可能となる。以上をまとめると、第１のセンサ１３０を固定して用いても、あるいは移動して用いても、どちらも図６に示すようにすべてのサンプル点２１０，２１１に関する情報を、同一の世界座標系であらわした位置情報と色情報の組み合わせとなるデータ （Xi，Yi，Zi，Ri，Gi，Bi）（i＝1， 2，・・・，N）の集合として得ることになる。なおここで、iはサンプル番号であり、Nはサンプル数である。上述べたように、第１のセンサにより環境内の存在物の計測を行い、複数のサンプル点の位置情報と色情報の組み合わせたデータを得る処理が「準備フェーズ」での処理となる。
【００３１】
一方「実行フェーズ」では、図１あるいは図２に示された、第２のセンサとなるカメラ１４０によって、任意の位置姿勢から得られた撮影画像から、そのカメラの位置姿勢を求める。ここで用いるカメラは、焦点距離fの透視投影モデルに従い、存在物の色情報が画像として得られるものを想定する。一般のカメラは、焦点距離が未知で、さらにレンズ歪みなどの影響で必ずしも透視投影のモデルに従うとは限らないが、一般にカメラの撮影画像は事前にキャリブレーションなどを行うことで、焦点距離既知の理想的な透視投影画像に補正可能である。よってこのような想定は説明上実際性を失わない。
【００３２】
まず第２のセンサとなるカメラ１４０によって撮影画像が１枚得られたときに、このカメラの位置姿勢値を仮に設定する。この設定を行うのは図１における第２のセンサの位置姿勢仮設定部１５８であり、その設定方法は後述する。この位置姿勢値と、前記のサンプル点データから、第２のセンサとなるカメラ１４０で得られるであろうこれらサンプル点の撮影画像を仮想的に作成する。この処理を行うのは図１における第２のセンサの仮想検知情報作成部１６０であり、その作成方法は後述する。次にこの仮想的に作成した仮想画像１６８と、実際に第２のセンサで得られた撮影画像１６５を比較する。この処理を行うのは、図１における比較部１７０である。ここでもし仮に設定したセンサの位置姿勢と、実際のセンサの位置姿勢が等しければ、サンプル点に関する画像は、両者画像上で等しい位置に写り、等しい色で写るはずである。一方、ここでもし仮に設定したセンサの位置姿勢と、実際のセンサの位置姿勢が異なれば、サンプル点に関する画像はお互い違いが生じる。これを図で説明するのが、図７、図８、図９、図１０、図１１である。
【００３３】
いま図７に示すとおり、存在物１００をカメラ等の第２のセンサ１４０で、図に示す位置姿勢D1から撮影したことを考える。この撮影により、図８に示す撮影画像１６６が得られる。一方図９と図１０に示すとおり、準備フェーズにて第１のセンサを用いて存在物を計測して取得したサンプル点の位置・色情報２１０，２１１から、もし仮にカメラ１４０が図の位置姿勢D1にあったときに得られるサンプル点の仮想的な撮影画像１６８を、図１０に示すものとして計算によって求める。また他方、図１１と図１２に示すとおり、同様にもし仮にカメラ１４０が図の位置姿勢D2にあったときに得られるサンプル点の仮想的な撮影画像１６９を、図１２に示すものとして計算によって求める。次に図１３に示すように、図８で示した撮影画像１６６と図１０に示したD1での仮想画像１６８の比較と、図８で示した撮影画像１６６と図１２に示したD2での仮想画像１６９の比較の２つの比較を行う。図１３からわかるように、実際のカメラの位置姿勢D1と、仮想画像を作成する際に設定した位置姿勢D1が等しい図１３の左側の比較では、サンプル点２１００の画像の色が実際の画像の色と一致する。一方、実際のカメラの位置姿勢D1と、仮想画像を作成する際に設定した位置姿勢D2が異なる図１３の右側の比較では、サンプル点２１１０の画像の色が実際の画像の色と一致しない。以上述べたように、このような違いが発生するか、しないかを評価することで、仮に設定したセンサの位置姿勢値が、実際の位置姿勢に等しいかどうかを判断することができる。すなわち、考えうる様々な位置姿勢値を仮に設定し、そこからの仮想的な撮影画像を作成し、比較評価を行うことで、実際の位置姿勢値を見つけることができる。これが本発明における位置姿勢推定の原理である。
【００３４】
なおここで、用いるサンプル点数が少ないなどで、それぞれのサンプル点の色の違いがない場合は、仮に設定したセンサの位置姿勢値が実際の位置姿勢と異なっていても、作成した仮想撮影画像のサンプル点の色が、実際の撮影画像の同じ位置の色と一致することも偶然的に発生する可能性がある。この問題に関しては、用いるサンプル点を数百から数千程度に増やすことで、そのような偶然的な事象がすべてのサンプル点で発生する確率は限りなく０に近づき解決する。さらに後述するとおり、本発明が想定するその利用方法では、第２のセンサの位置姿勢がある程度狭い範囲のみに限定されるという条件を利用することができるので、間違った仮の位置姿勢値を用いたときに画像の比較結果が一致することは、実際上ほぼありえないとみなすことができる。
【００３５】
次に、上記実施の形態における各要素の処理の詳細を補足的に述べる。
【００３６】
まず、図４等で示したように第１のセンサ１３０によって環境内存在物１００を計測する際、存在物のどの部分をサンプル点２１０として計測するか、その選択方法を説明する。これにはいくつかの方法が考えられが、その代表的な方法は、あらかじめ設定された角度ごとにスキャンする方法である。センサの周囲全体を対象に任意に向かう方向は、例えば水平方向φ、垂直方法ψの２つの角度パラメータであらわすことができるが、φについては1．0度ごとに0から360．0度、ψについては1．0度ごとに−90．0から90．0度の範囲をスキャンすれば、センサ周囲全体を1．0度ごとの解像度でスキャンできることになる。この場合スキャン方向数の合計は360×360 ＝ 129，600 個の方向となる。すなわち、センサの全周囲方向に対し最大で129，000個のサンプル点が計測されることになる。ただし実際には、すべての方向に向かって、センサの測定可能距離以下の範囲に存在物が存在するとは限らないので、計測されるサンプル数はそれ以下となることが一般的と考えられるが、その場合も以下の処理や作用は変わらない。なお以上述べた方法以外にも、存在物がある領域を細かくスキャンする方法、第２のセンサが撮影する可能性の高い場所を重点的に細かくスキャンする方法などをはじめ、運用条件等を考慮し適切な方法をとることができる。
【００３７】
また、第１のセンサによって計測できる色情報の表現方法としては、RGBの３原色であらわす方法以外に、色相・明度・色彩の３パラメータであらわす方法、明るさのみのグレースケールであらわす方法などいくつかの方法が利用できる。ただし以下の説明ではRGBの３原色を用いる方法で説明する。これによるとサンプル点データは、Pi＝（Xi，Yi，Zi，Ri，Gi，Bi）（i＝1，2，・・・，N） の形式で準備されることになる。なおここで、i はサンプル点番号、（Xi，Yi，Zi） はその番号のサンプル点の３次元座標値を世界座標系であらわしたもの、（Ri，Gi，Bi） はその点の色情報である。
【００３８】
次に全体処理の中で、第２のセンサの位置姿勢仮設定部が、仮の位置姿勢値を設定する方法について詳しく説明する。なお以降の説明では、第２のセンサの仮の位置姿勢設定値をGk＝（Xk，Yk，Zk，Rk）（k＝1，2，・・・，K）とあらわすこととする。ここで、kは仮に設定した複数の位置姿勢値を識別する番号である。また、（Xk，Yk，Zk）はk番目の仮設定値における位置を３次元座標値として世界座標系であらわしたもの、Rkはk番目の仮設定値における姿勢を３×３の３次元回転行列として世界座標系であらわしたものである。この仮の位置姿勢値を設定する方法としていくつかの方法が考えられるが、その代表的なものは、可能性のあるすべての位置姿勢値を、適当な量子化幅をもって網羅的に列挙する方法である。ただしこの方法によると、列挙する位置姿勢値の数が極めて膨大になる問題が発生する。すなわち、位置姿勢パラメータは、位置（Xk，Yk，Zk）の３パラメータに３つの自由度をもつ姿勢 Rk パラメータが加わり、合計で６つの自由度がある。ここである程度の量子化幅をもって、第２のセンサが存在しうる空間全域を羅列しようとすると、例えば１つの自由度について100の候補を考えるとすべての組み合わせ数は100の６乗となり、現実的な計算処理量をはるかに超えたものとなってしまう。そこで実際の応用においては、応用システムのもつ様々な条件を利用することで、その候補の数を限定する。例えば自律飛行船の位置姿勢推定においては、例えば０．１秒といった単位時間ごとに連続して逐次位置姿勢推定を行なうことが一つの制御方法として想定されるが、その際に０．１秒の時間間隔で変化しうる自律飛行船の位置姿勢はほんのわずかと考えることができる。この条件より、ある時刻tの位置姿勢を推定するにあたって、その位置姿勢は、直前の時刻t−1における位置姿勢から、高々前後左右数cm程度、姿勢もすべての方向に対し高々プラスマイナス1度程度の範囲にあると限定することができる。これによって仮に設定する位置姿勢値を、位置に関し１cm程度、角度に関し０．５度程度の分解能（量子化幅）で設定しても、そのすべての組み合わせで得られる可能性のある位置姿勢値は高々数百から数千、多くても数万程度となり、現在の計算機の能力から十分に処理可能な数になる。
【００３９】
次に全体の処理の中で、仮想検知情報を作成する方法について詳しく述べる。第２のセンサの仮想検知情報作成部は、上で言及したサンプル点データ群 Pi（i＝1， 2，・・・，N）と、k番目の仮の位置姿勢設定値Dkから、カメラの透視投影モデルの式に従って、それぞれのサンプル点の画像上の位置 pi＝（xi，yi，f）を算出する。ここで（xi，yi）はサンプル点の画像上の位置を画像の中心を原点とした画像座標系であらわしたもの、fは焦点距離をカメラ座標系の単位長さであらわしたものである。
【００４０】
【数１】
【００４１】
この式にしたがって求められる各サンプル点の画像上の位置pi（i＝1， 2，・・・，N） に、それぞれのサンプル点の色（Ri，Gi，Bi）を割り当てる。ここでその位置 pi が画像のサイズ、すなわち縦横長さの範囲外を示しているときは、そのサンプル点を無視し、画像への色の割り当ては行わないものとする。なお色の割り当てを行うとは、たとえばデジタル画像上において、その位置 pi が指し示すピクセルのメモリに、その色の値（Ri，Gi，Bi）を記録することを意味する。このようにして作成された画像が、仮の位置姿勢設定値Dkから作成される、第２のセンサの仮想検知情報Mkである。本実施の形態では、このような画像が、設定されたすべての仮の位置姿勢設定値Dk（k＝ 1，2，・・・，K）それぞれについて作成される。
【００４２】
次に全体の処理の中で、比較部が行う処理について詳しく述べる。比較部は、このように作成された仮想画像におけるすべてのサンプル点の色情報と、第２のセンサで実際に得られた撮影画像における同じ場所の色情報を比較し、その類似度を評価値 V として算出することで類似度を判断できるようにする。評価値Vの算出方法としては、例えばそれぞれの色成分の差の二乗平均を、以下の式により求める方法が考えられる。
【００４３】
【数２】
【００４４】
ここで、（R’i，G’i，B’i）は、仮想画像におけるi番目のサンプル点と同じ位置にある、撮影画像上の点の色である。評価値Vは、もし仮想画像と撮影画像がすべてのサンプル点で同じ色をもてば0の値をとり、その違いが大きくなるにしたがってVの値は大きくなる。すなわち評価値Vが小さいほど類似度は大きいと判断でき、逆に評価値Vが大きいほど類似度は小さいと判断できる。またVの値は必ず0以上となる。第２のセンサの位置姿勢仮設定部は、上に述べたとおり可能性のあるすべての位置姿勢値を、仮の位置姿勢値Dk（k＝1，2，・・・，K）として出力する。第２のセンサの仮想検知情報作成部はこれらの位置姿勢において得られる仮想検知情報を作成し、比較部は、これらの仮想検知情報それぞれについて評価値VをVk（k＝1，2，・・・，K）として求める。もし計測誤差や、仮想検知情報の作成誤差、あるいは量子化誤差等、すべての誤差が0であれば、第２のセンサの実際の位置姿勢値と、仮に設定した位置姿勢値が一致したときに、上で定義した評価値Vは0の値となる。しかし実際には様々な誤差要因により両者は完全には一致せず、評価値も完全に0にはならない。そこで例えば最も0に近い値を持つものが、実際の位置姿勢値である可能性が最も高い、あるいは実際の位置姿勢値に最も近いであろうと判断し、すべての評価値Vk（k＝1，2，・・・，K）からその最小値となるものをVk’として選び、このときの仮の位置姿勢値Dk’を第２のセンサの位置姿勢の推定値として出力する。
【００４５】
次に図１４を用いて、図１を用いて説明した一実施形態における装置構成の一例を説明する。本装置は、演算等を行う中央演算装置１４１０、プログラムやデータを一時的に保存する主記憶装置１４２０、プログラムやデータを長期的に保存する外部記憶装置１４３０、外部ネットワークや外部機器からデータの送受信を行うネットワークインタフェース１４４０および、ネットワークインタフェースを介して接続される第１のセンサ１３０および第２のセンサ１４０から大きく構成される。
【００４６】
主記憶装置１４２０には、図１における第２のセンサの位置姿勢仮設定部１５８の処理を行う第２のセンサの位置姿勢仮設定プログラム、図１における第２のセンサの仮想検知情報作成部１６０の処理を行う第２のセンサの仮想検知情報作成プログラム、図１における比較部１７０の処理を行う比較プログラム、図１における位置姿勢推定部１６３の処理を行う位置姿勢推定プログラムなどのプログラムと、図１における第１の検知情報１５５、第２の検知情報１６５、第２のセンサの仮想検知情報１６８などのデータを記憶する。なお上記プログラムやデータを外部記憶装置１４３０で長期的に保存することもできる。本実施の形態における準備フェーズでは、第１のセンサ１３０で得られるセンシングデータを、ネットワークインタフェース１４４０を介して入力し、変換処理等を中央演算装置１４１０がプログラムに従って行い、第１のセンシング情報データとして記憶する。一方、本実施の形態における実行フェーズでは、第２のセンサ１４０で得られる検知情報を、ネットワークインタフェース１４４０を介して入力し、各種処理を中央演算装置１４１０がプログラムに従って行い、第２のセンサの位置姿勢推定値を求め、主記憶装置１４２０に記憶、あるいはネットワークインタフェース１４４０を介して外部に出力する。
【００４７】
なお、本実施の形態では、準備フェーズを行う装置と、実行フェーズを行う装置を同じ装置で行うものとしたが、これらの処理を別の装置で行う構成にすることもできる。この場合は、準備フェーズで準備した第１の検知情報を、外部記憶装置やネットワークインタフェースを用いて、実行フェーズを行う装置に転送する。
【００４８】
次に、図１５を用いて本発明の処理の流れを説明する。
【００４９】
まず準備フェーズでは、第１のセンサによる存在物の計測を行うステップＳ１５１０において、例えば存在物表面上の複数のサンプル点の位置情報と色情報を計測する。この計測された情報は、第１の検知情報の作成と記録のステップＳ１５２０において、メモリ等の媒体に第１の検知情報として記録される。これで準備フェーズは終了する。
【００５０】
一方実行フェーズでは、第２のセンサによる存在物の計測を行うステップＳ１５５０において、存在物が例えばカメラ画像として計測され、これが第２の検知情報となる。一方、第２のセンサの仮の位置姿勢の設定ステップＳ１５３０において、第２のセンサの仮の位置姿勢値が設定される。次に、第２のセンサの仮想検知情報の作成ステップＳ１５４０では、ステップＳ１５３０において設定された仮の位置姿勢値と、準備フェーズで記録された第１の検知情報から、第２のセンサの仮想検知情報を作成する。次に、第２のセンサの仮想検知情報と、第２の検知情報を比較するステップＳ１５７０では、ステップＳ１５３０の出力とステップＳ１５４０の出力を比較し、その一致度を求める。次にステップＳ１５７０においてすべての仮の位置姿勢値で求めたかどうかの判断を行い、求めていなければステップＳ１５３０から繰り返す。もしすべてを求めていれば、推定値を出力するステップＳ１５８０において、ステップＳ１５６０で求めた一致度の最も高い仮の位置姿勢値を探し、これを推定値として出力する。以上で実行フェーズを終了する。
【００５１】
以上、本発明の一実施形態について詳しく説明した。
【００５２】
本実施の形態によると、位置姿勢の推定に用いるサンプル点は、準備フェーズにおいて任意に選ぶことができるので、背景技術で必要だった、画像上で一意に判別可能でその画像上の位置が認識可能な特徴的な点を計測しなければならないという、背景技術に記載した第１の制約がなくなる。また、存在物を第１のセンサおよび第２のセンサで共通に計測することで、第１のセンサの計測範囲に第２のセンサが存在していなければならないとする、背景技術に記載した第２の制約が解決される。また本実施の形態によれば、準備フェーズでの処理は、第１のセンサによる計測と自動的な情報処理のみで構成されるので、存在物の詳細なＣＧモデルを作成するといった、手作業やスキルを伴う処理は不要であり、その実施は容易となる。
【００５３】
なお、上に述べた実施の形態において、第２のセンサは透視投影モデルに従うカメラを使うことを想定したが、それ以外に例えば魚眼カメラなど異なる投影モデルに従うカメラを使う、ステレオカメラ等の複数カメラの組み合わせたセンシングシステムを使う、あるいはレーザ距離センサのように色情報以外の情報を取得できるセンサを使うこともできる。この場合は、それぞれのセンサの計測モデルに従うように、仮想検知情報生成部の処理内容を構成すればよい。
【００５４】
また、第２のセンサの位置姿勢仮設定部における設定方法としては、応用システムにおいて何らかの手段を用いて得た、位置姿勢の予測値を用いる方法がある。ここでその予測をする手段としては、ジャイロやＧＰＳといった精度の低い他の位置姿勢予測手段を用いたり、ロボット等に用いる場合は、どのような方向や距離の移動を行ったかという、移動制御情報を用いる方法などが考えられる。これらは応用システムの設計に依存するものであり、その原理をここでは詳述せず、そのような予測値が得られたものとしてその後の処理について説明する。まずこの予測値の形式であるが、ここではそれが確率的にあらわされることを前提とする。ここで確率的にあらわされるとは、予測値となる位置姿勢値を例えばX1と決定的に１つの値として限定せず、例えばX1である確率がp（X1）であり、X2である確率がp（X2）であり、X3である確率がp（X3）であるなどと、可能性をすべて網羅することを言う。このようにして列挙された可能性のある位置姿勢値を、その確率値、あるいは相対的に確率の大きさをあらわす値w1，w2，・・・，wM とともに、（X1，w1），（X2，w2），・・・，（XM，wM）の形式で準備する。本発明による本実施の形態では、これらの位置姿勢値X1，X2，・・・，XKを仮の位置姿勢値とし、第２のセンサの仮想的な画像を作成し、実際の画像との比較を行って得られたお互いの類似度を示す評価値V1，V2，・・・，VKに基づき、位置姿勢推定値を選ぶ。その選び方は、評価値V1，V2，・・・，VKの最も小さいものを選ぶ前述の方法に加え、予測で得られた確率的な値 w1，w2，・・・，wkをあわせて考慮することで判断する方法もありうる。このときは、２つの値の積 w1×V1，w2×V2，・・・，wk×VKを考えその大きなものを選ぶ、２つの値の和を考える、あるいは対数の和を考えるなど、いくつかの方法がある。このように２つの値を合わせて考えることで、予測による位置姿勢の推定と第２のセンサによる位置姿勢の推定を合わせて考えることができ、推定の精度をより高める効果がある。
【００５５】
また位置姿勢推定値の選択方法としては、上記算出した値が最も大きくなるものを選ぶだけでなく、いくつかの位置姿勢候補値の重心を求めるなど、複数の候補値での最終評価値の値の演算により求める方法をとることで、より推定の精度を高めたり、大きな誤差が発生する可能性を排除することもできる。
【符号の説明】
【００５６】
１００ 存在物
１３０ 第１のセンサ
１４０ 第２のセンサ
１５５ 第１の検知情報
１５８ 第２のセンサの位置姿勢仮設定部
１６０ 第２のセンサの仮想検知情報作成部
１６３ 位置姿勢推定部
１６５ 第２の検知情報
１７０ 比較部
１７５ 情報処理装置
２００ 飛行船
２１０，２１１ サンプリング点
１４１０ 中央演算装置
１４２０ 主記憶装置
１４３０ 外部記憶装置
１４４０ ネットワークインターフェース
【特許請求の範囲】
【請求項１】
第１のセンサと、第２のセンサと、情報処理装置からなる位置検知システムにおいて、
前記第１のセンサは、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るものであり、
前記第２のセンサは、存在物の色情報を第２の検知情報として得るものであり、
前記情報処理装置は、
第２のセンサの位置姿勢を仮に設定する位置姿勢仮設定部と、
前記位置姿勢仮設定部により仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成する仮想検知情報作成部と、
前記仮想検知情報作成部で作成された前記仮想検知情報と前記第２のセンサで得られた前記第２の検知情報を比較する比較部と、
前記比較部の比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力する位置姿勢推定部を備えることを特徴とする位置検知システム。
【請求項２】
請求項１記載の位置検知システムにおいて、
前記第１のセンサは、存在物の表面の複数の点までの距離と、それらの点の色を計測する画像・距離センサであることを特徴とする位置検知システム。
【請求項３】
請求項１または請求項２記載の位置検知システムにおいて、
前記第２のセンサは、存在物の表面の色をカメラの投影モデルに従う画像情報として計測するカメラであることを特徴とする位置検知システム。
【請求項４】
請求項１記載の位置検知システムにおいて、
前記第１のセンサによる前記第１の検知情報は、前記存在物の表面を３次元座標値と色情報で表現される複数の点の集合として表されるものであり、
前記第２のセンサの前記仮想検知情報作成部は、前記仮に設定した第２のセンサの位置姿勢と、前記第１の検知情報における複数の点の３次元座標値と色情報から、前記第２のセンサの計測モデルに従う画像情報を作成するものであることを特徴とする位置検知システム。
【請求項５】
請求項１〜４の何れか一つに記載の位置検知システムにおいて、
第２のセンサが、移動体に取り付けられていることを特徴とする位置検知システム。
【請求項６】
請求項１記載の位置検知システムを組み込んだ移動体であって、
前記第２のセンサは移動体に取り付けられ、
前記移動体は、前記位置検知システムから出力された第２のセンサの位置姿勢推定値を用いて移動制御する手段を備えることを特徴とする移動体。
【請求項７】
請求項６記載の移動体において、
前記移動体には移動体の位置姿勢予測部を備え、
前記第２のセンサの位置姿勢仮設定部は、前記位置姿勢予測部で予測された位置姿勢予測値を用いて、前記第２のセンサの仮の位置姿勢値を求めることを特徴とする移動体。
【請求項８】
請求項６記載の移動体において、
前記第２のセンサの位置姿勢仮設定部は、前記移動体の移動制御情報を用いて、前記第２のセンサの仮の位置姿勢値を求めることを特徴とする移動体。
【請求項９】
請求項６〜８の何れか一つに記載の移動体において、
前記移動体は、ロボットまたは飛行体であることを特徴とする移動体。
【請求項１０】
第１のセンサを用いて、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るステップと、
第２のセンサを用いて、存在物の色情報を第２の検知情報として得るステップと、
第２のセンサの位置姿勢を仮に設定するステップと、
前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、
前記仮想検知情報と前記第２の検知情報を比較するステップと、
前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップを備えることを特徴とする位置検知方法。
【請求項１１】
請求項１０記載の位置検知方法において、
前記第１のセンサは、存在物の表面の複数の点までの距離と、それらの点の色を計測する画像・距離センサであることを特徴とする位置検知方法。
【請求項１２】
請求項１１記載の位置検知方法システムにおいて、
前記第２のセンサは、存在物の表面の色をカメラの投影モデルに従う画像情報として計測するカメラであることを特徴とする位置検知方法。
【請求項１３】
請求項１０記載の位置検知方法において、
前記第１のセンサによる前記第１の検知情報は、前記存在物の表面を３次元座標値と色情報で表現される複数の点の集合として表されるものであり、
前記第２のセンサの前記仮想検知情報を作成するステップは、前記仮に設定した第２のセンサの位置姿勢と、前記第１の検知情報における複数の点の３次元座標値と色情報から、前記第２のセンサの計測モデルに従う画像情報を作成するものであることを特徴とする位置検知方法。
【請求項１４】
コンピュータに、第１のセンサを用いて得られた、環境内の存在物の位置情報および色情報から成る第１の検知情報、および、第２のセンサを用いて得られた、存在物の色情報から成る第２の検知情報から、第２のセンサの位置姿勢を求めさせるプログラムであって、
第２のセンサの位置姿勢を仮に設定するステップと、
前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、
前記仮想検知情報と前記第２の検知情報を比較するステップと、
前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップとを実行させることを特徴とするプログラム。
【請求項１５】
請求項１４記載のプログラムにおいて、
前記第１の検知情報は、前記存在物の表面を３次元座標値と色情報で表現される複数の点の集合として表されるものであり、
前記第２のセンサの前記仮想検知情報を作成するステップは、前記仮に設定した第２のセンサの位置姿勢と、前記第１の検知情報における複数の点の３次元座標値と色情報から、前記第２のセンサの計測モデルに従う画像情報を作成するものであることを特徴とするプログラム。
【請求項１】
第１のセンサと、第２のセンサと、情報処理装置からなる位置検知システムにおいて、
前記第１のセンサは、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るものであり、
前記第２のセンサは、存在物の色情報を第２の検知情報として得るものであり、
前記情報処理装置は、
第２のセンサの位置姿勢を仮に設定する位置姿勢仮設定部と、
前記位置姿勢仮設定部により仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成する仮想検知情報作成部と、
前記仮想検知情報作成部で作成された前記仮想検知情報と前記第２のセンサで得られた前記第２の検知情報を比較する比較部と、
前記比較部の比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力する位置姿勢推定部を備えることを特徴とする位置検知システム。
【請求項２】
請求項１記載の位置検知システムにおいて、
前記第１のセンサは、存在物の表面の複数の点までの距離と、それらの点の色を計測する画像・距離センサであることを特徴とする位置検知システム。
【請求項３】
請求項１または請求項２記載の位置検知システムにおいて、
前記第２のセンサは、存在物の表面の色をカメラの投影モデルに従う画像情報として計測するカメラであることを特徴とする位置検知システム。
【請求項４】
請求項１記載の位置検知システムにおいて、
前記第１のセンサによる前記第１の検知情報は、前記存在物の表面を３次元座標値と色情報で表現される複数の点の集合として表されるものであり、
前記第２のセンサの前記仮想検知情報作成部は、前記仮に設定した第２のセンサの位置姿勢と、前記第１の検知情報における複数の点の３次元座標値と色情報から、前記第２のセンサの計測モデルに従う画像情報を作成するものであることを特徴とする位置検知システム。
【請求項５】
請求項１〜４の何れか一つに記載の位置検知システムにおいて、
第２のセンサが、移動体に取り付けられていることを特徴とする位置検知システム。
【請求項６】
請求項１記載の位置検知システムを組み込んだ移動体であって、
前記第２のセンサは移動体に取り付けられ、
前記移動体は、前記位置検知システムから出力された第２のセンサの位置姿勢推定値を用いて移動制御する手段を備えることを特徴とする移動体。
【請求項７】
請求項６記載の移動体において、
前記移動体には移動体の位置姿勢予測部を備え、
前記第２のセンサの位置姿勢仮設定部は、前記位置姿勢予測部で予測された位置姿勢予測値を用いて、前記第２のセンサの仮の位置姿勢値を求めることを特徴とする移動体。
【請求項８】
請求項６記載の移動体において、
前記第２のセンサの位置姿勢仮設定部は、前記移動体の移動制御情報を用いて、前記第２のセンサの仮の位置姿勢値を求めることを特徴とする移動体。
【請求項９】
請求項６〜８の何れか一つに記載の移動体において、
前記移動体は、ロボットまたは飛行体であることを特徴とする移動体。
【請求項１０】
第１のセンサを用いて、あらかじめ環境内の存在物の位置情報および色情報を第１の検知情報として得るステップと、
第２のセンサを用いて、存在物の色情報を第２の検知情報として得るステップと、
第２のセンサの位置姿勢を仮に設定するステップと、
前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、
前記仮想検知情報と前記第２の検知情報を比較するステップと、
前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップを備えることを特徴とする位置検知方法。
【請求項１１】
請求項１０記載の位置検知方法において、
前記第１のセンサは、存在物の表面の複数の点までの距離と、それらの点の色を計測する画像・距離センサであることを特徴とする位置検知方法。
【請求項１２】
請求項１１記載の位置検知方法システムにおいて、
前記第２のセンサは、存在物の表面の色をカメラの投影モデルに従う画像情報として計測するカメラであることを特徴とする位置検知方法。
【請求項１３】
請求項１０記載の位置検知方法において、
前記第１のセンサによる前記第１の検知情報は、前記存在物の表面を３次元座標値と色情報で表現される複数の点の集合として表されるものであり、
前記第２のセンサの前記仮想検知情報を作成するステップは、前記仮に設定した第２のセンサの位置姿勢と、前記第１の検知情報における複数の点の３次元座標値と色情報から、前記第２のセンサの計測モデルに従う画像情報を作成するものであることを特徴とする位置検知方法。
【請求項１４】
コンピュータに、第１のセンサを用いて得られた、環境内の存在物の位置情報および色情報から成る第１の検知情報、および、第２のセンサを用いて得られた、存在物の色情報から成る第２の検知情報から、第２のセンサの位置姿勢を求めさせるプログラムであって、
第２のセンサの位置姿勢を仮に設定するステップと、
前記仮に設定された第２のセンサの位置姿勢から存在物を検知したときに得られる検知情報を、仮想検知情報として、前記第１の検知情報を用いて仮想的に作成するステップと、
前記仮想検知情報と前記第２の検知情報を比較するステップと、
前記比較結果と前記仮に設定された第２のセンサの位置姿勢から、前記第２のセンサの位置姿勢推定値を出力するステップとを実行させることを特徴とするプログラム。
【請求項１５】
請求項１４記載のプログラムにおいて、
前記第１の検知情報は、前記存在物の表面を３次元座標値と色情報で表現される複数の点の集合として表されるものであり、
前記第２のセンサの前記仮想検知情報を作成するステップは、前記仮に設定した第２のセンサの位置姿勢と、前記第１の検知情報における複数の点の３次元座標値と色情報から、前記第２のセンサの計測モデルに従う画像情報を作成するものであることを特徴とするプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−２４２３１５（Ｐ２０１２−２４２３１５Ａ）
【公開日】平成２４年１２月１０日（２０１２．１２．１０）
【国際特許分類】
【出願番号】特願２０１１−１１４４８２（Ｐ２０１１−１１４４８２）
【出願日】平成２３年５月２３日（２０１１．５．２３）
【出願人】（０００００５１０８）株式会社日立製作所 (27,607)
【Ｆターム（参考）】
【公開日】平成２４年１２月１０日（２０１２．１２．１０）
【国際特許分類】
【出願日】平成２３年５月２３日（２０１１．５．２３）
【出願人】（０００００５１０８）株式会社日立製作所 (27,607)
【Ｆターム（参考）】
[ Back to top ]