(57)【特許請求の範囲】
【請求項１】
環境内を撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶する記憶手段と、
前記環境内を移動する移動体に設けられ、該移動体周囲の画像を取得する画像取得手段と、
前記画像取得手段により取得された画像から実特徴点を抽出する特徴点抽出手段と、
前記特徴点抽出手段により抽出された実特徴点の中から、動的物体の動的特徴点を除去する特徴点除去手段と、
前記記憶手段に記憶された各画像の特徴点と、前記特徴点除去手段により除去された画像の実特徴点と、の一致を判定するマッチングを行い、前記記憶手段の特徴点と前記特徴点除去手段により除去された実特徴点とを対応付けるマッチング手段と、
前記実特徴点の位置と、前記記憶手段に記憶された該実特徴点に対応する前記特徴点の位置と、の幾何学的位置関係に基づいて、前記移動体の自己位置を算出する自己位置算出手段と、
を備え、
前記マッチング手段は、前記画像取得手段により取得された画像と類似度の高い画像に含まれる特徴点を、他の画像に含まれる特徴点よりも優先して、前記画像取得手段により取得された画像から抽出された実特徴点と、マッチングし、
前記記憶手段には、前記環境内を複数の視点から撮影した画像が記憶されており、
前記マッチング手段は、前記類似度の高い順で、前記記憶手段に記憶された複数視点の画像群の中から画像を連続的に選択し、該画像の特徴点と、前記実特徴点と、のマッチングを行い、前記記憶手段の特徴点と実特徴点とを複数組対応付け、
前記自己位置算出手段は、前記複数組の、実特徴点の位置と、前記記憶手段に記憶された実特徴点に対応する特徴点の位置と、に基づいて、前記移動体の自己位置を算出する、
ことを特徴とする、自己位置推定装置。
【請求項２】
請求項１記載の自己位置推定装置であって、
前記マッチング手段は、
前記画像取得手段により取得された画像から抽出された各実特徴点と、前記記憶手段に記憶された画像群の各特徴点と、のマッチングを行って、前記記憶手段の画像毎に、一致した特徴点の数を算出し、
該一致した特徴点数が多い順に前記記憶手段の各画像をソートし、
該ソートした順で、前記記憶手段に記憶された画像群の中から画像を選択し、該選択した画像に含まれる特徴点と、前記画像取得手段により取得された画像から抽出された実特徴点と、のマッチングを行う、
ことを特徴とする自己位置推定装置。
【請求項３】
環境内を複数の視点から撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶するステップと、
前記環境内を移動する移動体に設けられ、該移動体周囲の画像を取得するステップと、
前記取得された画像から実特徴点を抽出するステップと、
前記抽出された実特徴点の中から、動的物体の動的特徴点を除去するステップと、
前記記憶された各画像の特徴点と、前記除去された画像の実特徴点と、の一致を判定するマッチングを行い、前記記憶された特徴点と前記除去された実特徴点とを対応付けるステップと、
前記実特徴点の位置と、該実特徴点に対応する前記特徴点の位置と、の幾何学的位置関係に基づいて、前記移動体の自己位置を算出するステップと、
を含み、
前記取得された画像と類似度の高い画像に含まれる特徴点を、他の画像に含まれる特徴点よりも優先して、前記取得された画像から抽出された実特徴点と、マッチングし、
前記類似度の高い順で、前記記憶された複数視点の画像群の中から画像を連続的に選択し、該画像の特徴点と、前記実特徴点と、のマッチングを行い、前記特徴点と実特徴点とを複数組対応付け、
前記複数組の、実特徴点の位置と、前記記憶された実特徴点に対応する特徴点の位置と、に基づいて、前記移動体の自己位置を算出する、
ことを特徴とする自己位置推定方法。
【請求項４】
環境内を複数の視点から撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶する処理と、
前記環境内を移動する移動体に設けられ、該移動体周囲の画像を取得する処理と、
前記取得された画像から実特徴点を抽出する処理と、
前記抽出された実特徴点の中から、動的物体の動的特徴点を除去する処理と、
前記取得された画像と類似度の高い前記記憶された画像に含まれる特徴点を、前記記憶された他の画像に含まれる特徴点よりも優先して、前記除去された実特徴点と、の一致を判定するマッチングを行い、前記記憶された特徴点と前記除去された実特徴点とを対応付ける処理と、
前記実特徴点の位置と、該実特徴点に対応する前記特徴点の位置と、の幾何学的位置関係に基づいて、前記移動体の自己位置を算出する処理と、
をコンピュータに実行させ、
前記類似度の高い順で、前記記憶された複数視点の画像群の中から画像を連続的に選択し、該画像の特徴点と、前記実特徴点と、のマッチングを行い、前記特徴点と実特徴点とを複数組対応付け、
前記複数組の、実特徴点の位置と、前記記憶された実特徴点に対応する特徴点の位置と、に基づいて、前記移動体の自己位置を算出する、
ことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体などの自己位置を推定する自己位置推定装置、自己位置推定方法及びプログラムに関するものである。
【背景技術】
【０００２】
環境内の特徴点と該特徴点の位置情報とを予め記憶し、記憶した特徴点と、移動体の撮影手段により撮影された画像から抽出した実特徴点と、を比較することで移動体の自己位置を推定する自己位置推定装置が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１１−２４８４４１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記自己位置推定装置においては、例えば、実特徴点を抽出するための撮影手段の視点と、予め特徴点を記憶させたときの視点と、が異なる場合に、自己位置推定精度が低下する虞がある。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、自己位置推定精度を向上させることができる自己位置推定装置、自己位置推定方法及びプログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、環境内を撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶する記憶手段と、前記環境内を移動する移動体に設けられ、該移動体周囲の画像を取得する画像取得手段と、前記画像取得手段により取得された画像から実特徴点を抽出する特徴点抽出手段と、前記記憶手段に記憶された各画像の特徴点と、前記特徴点抽出手段により抽出された画像の実特徴点と、の一致を判定するマッチングを行い、前記記憶手段の特徴点と前記特徴点抽出手段に抽出された実特徴点とを対応付けるマッチング手段と、前記実特徴点の位置と、前記記憶手段に記憶された該実特徴点に対応する前記特徴点の位置と、の幾何学的位置関係に基づいて、前記移動体の自己位置を算出する自己位置算出手段と、を備え、前記マッチング手段は、前記画像取得手段により取得された画像と類似度の高い画像に含まれる特徴点を、他の画像に含まれる特徴点よりも優先して、前記画像取得手段により取得された画像から抽出された実特徴点と、マッチングする、ことを特徴とする、自己位置推定装置である。
この一態様において、前記記憶手段には、前記環境内を複数の視点から撮影した画像が記憶されていてもよい。
この一態様において、前記特徴点抽出手段により抽出された実特徴点の中から、動的物体の動的特徴点を除去する特徴点除去手段を更に備えていてもよい。
この一態様において、前記マッチング手段は、前記画像取得手段により取得された画像から抽出された各実特徴点と、前記記憶手段に記憶された画像群の各特徴点と、のマッチングを行って、前記記憶手段の画像毎に、一致した特徴点の数を算出し、該一致した特徴点数が多い順に前記記憶手段の各画像をソートし、該ソートした順で、前記記憶手段に記憶された画像群の中から画像を選択し、該選択した画像に含まれる特徴点と、前記画像取得手段により取得された画像から抽出された実特徴点と、のマッチングを行ってもよい。
上記目的を達成するための本発明の一態様は、環境内を撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶するステップと、前記環境内を移動する移動体に設けられ、該移動体周囲の画像を取得するステップと、前記取得された画像から実特徴点を抽出するステップと、前記記憶された各画像の特徴点と、前記抽出された画像の実特徴点と、の一致を判定するマッチングを行い、前記記憶された特徴点と前記抽出された実特徴点とを対応付けるステップと、前記実特徴点の位置と、該実特徴点に対応する前記特徴点の位置と、の幾何学的位置関係に基づいて、前記移動体の自己位置を算出するステップと、を含み、前記取得された画像と類似度の高い画像に含まれる特徴点を、他の画像に含まれる特徴点よりも優先して、前記取得された画像から抽出された実特徴点と、マッチングする、ことを特徴とする自己位置推定方法であってもよい。
上記目的を達成するための本発明の一態様は、環境内を撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶する処理と、前記環境内を移動する移動体に設けられ、該移動体周囲の画像を取得するステップと、前記取得された画像から実特徴点を抽出する処理と、前記取得された画像と類似度の高い前記記憶された画像に含まれる特徴点を、前記記憶された他の画像に含まれる特徴点よりも優先して、前記取得された画像から抽出された実特徴点と、の一致を判定するマッチングを行い、前記記憶された特徴点と前記抽出された実特徴点とを対応付ける処理と、前記実特徴点の位置と、該実特徴点に対応する前記特徴点の位置と、の幾何学的位置関係に基づいて、前記移動体の自己位置を算出する処理と、をコンピュータに実行させることを特徴とするプログラムであってもよい。
【発明の効果】
【０００７】
本発明によれば、自己位置推定精度を向上させることができる自己位置推定装置、自己位置推定方法及びプログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係る自己位置推定装置の概略的なシステム構成を示すブロック図である。
【図２】輝度画像の特徴点とセンサ座標系の三次元位置と三次元グローバル位置との対応を示す図である。
【図３】特徴点インデックスと画像インデックスと三次元グローバル位置を対応付けた特徴点の三次元位置データの一例を示す図である。
【図４】メモリに記憶されるマッピングデータの一例を示す図である。
【図５】現在画像の各実特徴点とマッピングデータの各画像の各特徴点とのマッチングを示す図である。
【図６】マッピングデータの各画像インデックスおよびマッチング特徴点数の一例を示す図である。
【図７】マッチング特徴点数が多い順のマッピングデータの一例を示す図である。
【図８】現在画像の各実特徴点とマッピングデータの各画像インデックスの各特徴点とマッチングを説明するための図である。
【図９】センサ座標系の各実特徴点と、対応するグローバル座標系のマッピングデータの各画像の各特徴点と、を示す図である。
【図１０】本発明の一実施形態に係る自己位置推定装置のマッピング処理フローを示すフローチャートである。
【図１１】本発明の一実施形態に係る自己位置推定装置の自己位置推定処理フローを示すフローチャートである。
【発明を実施するための形態】
【０００９】
以下、図面を参照して本発明の実施形態について説明する。本実施形態に係る自己位置推定装置は、例えば、自律移動型ロボットなどの移動体に搭載され、移動体の自己位置を推定する。移動体は、自己位置推定装置により推定された自己位置に基づいて移動を行う。
【００１０】
自己位置推定装置は、まず、環境内の全体景色を学習したマッピングデータを記憶するマッピング処理を行う。その後、自己位置推定装置は、そのマッピング処理後に、取得した現在画像とマッピングデータとを対応させて、グローバルの自己位置推定処理を行い、移動体の自己位置を推定する。
【００１１】
図１は、本実施形態に係る自己位置推定装置の概略的なシステム構成を示すブロック図である。本実施形態に係る自己位置推定装置１は、画像取得部２と、特徴点抽出部３と、対応付け部４と、特徴点除去部５と、マッチング部６と、自己位置算出部７と、メモリ８と、を備えている。
【００１２】
なお、自己位置推定装置１は、例えば、演算処理等を行うＣＰＵ（Central Processing Unit）、ＣＰＵによって実行される演算プログラム等が記憶されたＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）からなるメモリ、外部と信号の入出力を行うインターフェイス部（Ｉ／Ｆ）などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。ＣＰＵ、メモリ及びインターフェイス部は、データバスなどを介して相互に接続されている。
【００１３】
画像取得部２は、画像取得手段の一具体例である。画像取得部２は、環境内における移動体周囲の画像を取得する。画像取得部２は、例えば、移動体に設けらたＲＧＢ−Ｄセンサなどの距離画像センサ２１を用いて、移動体周囲の輝度画像及び距離画像を取得する。
【００１４】
特徴点抽出部３は、特徴点抽出手段の一具体例である。特徴点抽出部３は、画像取得部２により取得された輝度画像から特徴点を抽出する。特徴点は、例えば、エッジ、コーナー、色分布などを含む。
【００１５】
対応付け部４は、特徴点抽出部３により抽出された特徴点とグローバル座標系の三次元位置（以下、三次元グローバル位置）とを対応付ける。なお、対応付け部４は、環境内の複数の視点で取得された画像の特徴点と、三次元グローバル位置と、を対応付ける。
【００１６】
例えば、対応付け部４は、画像取得部２により取得された距離画像に基づいて、特徴点抽出部３により抽出された輝度画像の特徴点ｆのセンサ座標系の三次元位置ＳＰを算出する（図２）。なお、このセンサ座標系の三次元位置ＳＰは、輝度画像の特徴点ｆに対して、一意に対応付けられるものとする。対応付け部４は、算出したセンサ座標系の三次元位置ＳＰに基づいて、下記式により、特徴点抽出部３により抽出された特徴点ｆに対応する三次元グローバル位置ＧＰを算出する。
ＧＰ＝ＧＴＳＳＰ
【００１７】
なお、上記式において、ＧＴＳは、グローバル座標系における距離画像センサ２１の位置姿勢の同次変換行列である。また、グローバル座標系における距離画像センサ２１の位置姿勢Ｔは、任意方法で計測あるいは算出できる。対応付け部４は、例えば、移動体の目的や制約条件（移動体に搭載されるセンサなど）に応じて、下記のような方法で、グローバル座標系における距離画像センサ２１の位置姿勢Ｔを算出してもよい。
【００１８】
例えば、対応付け部４は、移動体の立上げ位置をグローバル座標系の原点としたデットレコニング(Dead-Reckoning:DR)結果に基づいて、グローバル座標系における距離画像センサ２１の位置姿勢Ｔを算出する。対応付け部４は、ＳＬＡＭ（Simultaneous Localization and Mapping）による距離画像センサ２１の位置推定結果に基づいて、グローバル座標系における距離画像センサ２１の位置姿勢Ｔを算出する。対応付け部４は、外部計測器（モーションキャプチャなど）により計測された三次元位置に基づいて、グローバル座標系における距離画像センサ２１の位置姿勢Ｔを算出する。
【００１９】
対応付け部４は、算出した特徴点の三次元グローバル位置ＧＰを、その特徴点と対応付ける。
対応付け部４は、例えば、各特徴点にインデックスｉ（ｉ＝１、２、・・・）、及び各画像にインデックスｊ（ｊ＝１、２、・・・）を付ける。そして、対応付け部４は、特徴点のインデックス（以下、特徴点インデックス）ｉと、その特徴点が抽出された画像のインデックス（画像インデックス）ｊと、その特徴点の三次元グローバル位置Ｐｉｊ＝（ｘｉｊ、ｙｉｊ、ｚｉｊ）と、を対応付けた特徴点の三次元位置データ（図３）をメモリ８に記憶させる。
【００２０】
対応付け部４は、上記特徴点の三次元位置データと共に、特徴点を抽出した画像群をマッピングデータとしてメモリ８に記憶させる（図４）。メモリ８は、記憶手段の一具体例である。なお、自己位置推定装置１は、対応付け部４を有しない構成であってもよい。この場合、上記マッピングデータが予めメモリ８に記憶されている、あるいは、ユーザにより、上記マッピングデータがメモリ８に入力される。
【００２１】
特徴点除去部５は、特徴点除去手段の一具体例である。特徴点除去部５は、特徴点抽出部３により抽出された実特徴点（自己位置推定処理時に抽出された特徴点）群の中から、動的物体の実特徴点（動的特徴点）を除去する。この処理は、例えば、自己位置推定処理時に、上記取得した時間的連続する２枚の画像中に、移動している動的物体があれば、実特徴点群の中から、その動的物体に対応する実特徴点（動的特徴点）が除去させる。これにより、位置不変の特徴点のみを用いて自己位置推定を行うことができるため、後述の自己位置推定精度を向上させることができる。なお、本動的特徴点の除去処理方法は、既に本願発明者が提案した特願２０１２−２４４５４０に詳細に開示されており、これを援用できるものとする。自己位置推定装置１は、上記特徴点除去部５を有しない構成であってもよい。この場合、自己位置推定装置１の構成はより簡略化され、その処理速度が向上する。
【００２２】
ところで、例えば、移動体の自己位置推定処理時における実特徴点を抽出するための距離画像センサの視点と、マッピング処理時における予め特徴点を記憶させるための距離画像センサの視点と、が異なる場合に、自己位置推定精度が低下する虞がある。
【００２３】
例えば、図３に示す如く、視点が異なる画像１及び３に同一の特徴点１が存在するように、同一の特徴点が複数の異なる視点の画像に存在することがある。このため、同一の特徴点１であっても、画像１の特徴点１と画像３の特徴点１とでは、対応する三次元グローバル位置が異なる。したがって、後述の如く、この三次元グローバル位置を用いて自己位置推定を行った場合に、選択する画像によっては、その自己位置推定精度が変動する。特に、視点が大きく異なる画像を選択した場合に、対応する三次元グローバル位置が大きくずれるため、自己位置推定精度が大きく低下し得る。
【００２４】
これに対し、本実施形態においては、  マッチング部８は、画像取得部６により取得された画像と類似度の高いメモリ８の画像に含まれる特徴点を、メモリ８の他の画像に含まれる特徴点よりも優先して、画像取得部６により取得された画像から抽出された実特徴点と、マッチングし、メモリ８の特徴点と実特徴点とを対応付ける。すなわち、マッチング部６は、画像取得部２により取得された現在画像と類似度の高い順で、予めメモリ８に記憶された画像群の中から画像を選択し、該選択した画像に含まれる特徴点と、画像取得部２により取得された現在画像から抽出された実特徴点と、の一致を判定するマッチングを行い、メモリ８の特徴点と実特徴点とを対応付ける。そして、自己位置算出部７は、実特徴点の位置と、メモリ８に記憶された実特徴点に対応する特徴点の位置と、の幾何学的位置関係に基づいて、移動体の自己位置を算出する。
【００２５】
これにより、マッチング部６は、画像取得部２により取得された現在画像と類似度の高い順で、すなわち、自己位置推定処理時における距離画像センサ２１の視点に近い順で、メモリ８に記憶された画像群の中から画像を選択し、その画像の特徴点と、現在画像から抽出された実特徴点と、のマッチングを行うことができる。すなわち、自己位置推定処理時における距離画像センサ２１の視点に近い画像を優先的に選択し、その画像の特徴点の三次元グローバル位置を用いて、自己位置を推定できる。したがって、上述したマッピング処理時と自己位置推定処理時とにおける距離画像センサ２１の視点の相違によって生じる自己位置推定精度の誤差を抑制し、自己位置推定精度を向上させることができる。
【００２６】
また、メモリ８には、環境内の複数の視点で取得された画像の特徴点と、三次元グローバル位置と、を対応付けたマッピングデータが記憶され、マッチング部６は、このマッピングデータを用いてマッチングを行っている。したがって、たとえ、マッピング処理時と自己位置推定処理時とにおける距離画像センサ２１の視点が大きく相違する場合でも、複数の視点で取得された画像の中から、自己位置推定処理時における距離画像センサ２１の視点により近い画像を複数選択できる。このため、上記のような視点が異なる場合でも、自己位置推定を高精度かつ確実に行うことができる。
【００２７】
さらに、現在画像と類似度の高い順で、メモリ８に記憶された複数視点の画像群の中から画像を連続的に選択し、その画像の特徴点と、現在画像から抽出された実特徴点と、のマッチングを行い、メモリ８の特徴点と実特徴点とを複数組対応付ける。そして、自己位置算出部７は、複数組の、実特徴点の位置と、メモリ８に記憶された実特徴点に対応する特徴点の位置と、に基づいて、移動体の自己位置を算出する。このように、より多くの特徴点を用いることで、より高精度に自己位置推定できる。
【００２８】
マッチング部６は、特徴点除去部５により動的特徴点を除去した画像（以下、現在画像）の実特徴点群と、メモリ８に記憶されたマッピングデータの画像群の特徴点群と、が一致するか否を判定するマッチングを行う（図５）。マッチング部６は、マッピングデータの画像インデックス毎に、一致した特徴点の数（以下、マッチング特徴点数）を算出する。図６は、マッピングデータの各画像インデックス（画像１、画像２、・・・）およびマッチング特徴点数の一例を示す図である。
【００２９】
マッチング部６は、各画像インデックスを、例えば、図７に示す如く、マッチング特徴点数が多い順にソートする。これは、マッピングデータの各画像インデックスを、現在画像との類似度が高い順に並べることを意味する。すなわち、マッチング部６は、現在画像と類似度の高い順で、メモリ８に記憶されたマッピングデータの画像群の中から画像を選択し、該選択した画像に含まれる各特徴点と、現在画像から抽出された各実特徴点と、のマッチングを行うこととなる。
【００３０】
なお、マッチング部６は、マッチング特徴点数に基づいて、マッピングデータの各画像インデックスを現在画像との類似度が高い順に並べているが、これに限定されない。マッチング部６は、例えば、各特徴点の配置に基づいて、マッピングデータの各画像インデックスを現在画像との類似度が高い順に並べてもよい。より具体的には、マッチング部６は、現在画像の実特徴点群と、メモリ８に記憶されたマッピングデータの画像群の特徴点群と、を比較し、その特徴点の配置が近い順に、各画像インデックスをソートしてもよい。
【００３１】
マッチング部６は、上記ソートした順で、画像インデックス毎に、各実特徴点がマッピングデータの各画像インデックスの各特徴点と一致するか否かを判定する。
例えば、図８に示す如く、マッチング部６は、現在画像の実特徴点１がマッピングデータの画像２（マッチング特徴点数が１番多い画像インデックス）の各特徴点と一致するか否かを判定する（ａ）。マッチング部６は、現在画像の実特徴点１がマッピングデータの画像２の各特徴点と一致しないと判定し、続いて、現在画像の実特徴点１がマッピングデータの画像３（マッチング特徴点数が２番目に多い画像インデックス）の各特徴点と一致するか否かを判定する（ｂ）。ここで、マッチング部６は、現在画像の実特徴点１がマッピングデータの画像３の特徴点と一致したと判定し、この画像３の特徴点を抽出し、現在画像の実特徴点１に対応付ける。この時点で、マッチング部６は、現在画像の実特徴点１の判定処理を終了する。同様にして、マッチング部６は、現在画像の実特徴点２、３、４・・・の判定処理を繰り返し、各実特徴点をマッピングデータの各画像の各特徴点と対応付ける。
【００３２】
マッチング部６は、上記算出した現在画像の各実特徴点に対応したマッピングデータの各画像の各特徴点と、メモリ８に記憶された特徴点の三次元グローバル位置を含むマッピングデータと、に基づいて、現在画像の各実特徴点に対応する各特徴点の三次元グローバル位置を算出する。
【００３３】
自己位置算出部７は、自己位置算出手段の一具体例である。自己位置算出部７は、上記求めた現在画像の各実特徴点の位置と、対応するマッピングデータの各画像の各特徴点の位置と、の幾何学位置関係から、グローバル座標系基準における距離画像センサ２１の位置、すなわち、移動体の自己位置を算出する。
【００３４】
図９に示す如く、距離画像センサ２１により得られた各実特徴点１、２、・・・は、センサ座標系基準で位置が計測されており、対応するマッピングデータの各画像の各特徴点１、２、・・・は、グローバル座標系基準で表現されている。
【００３５】
自己位置算出部７は、上記求めた現在画像の各実特徴点と、対応付けたマッピングデータの各画像の各特徴点と、の対応をとり位置合わせを行うことで、距離画像センサ２１の位置を算出し、自己位置を推定する。自己位置算出部７は、例えば、現在画像の実特徴点のセンサ座標系位置を、対応付けたマッピングデータの画像の特徴点のグローバル座標系位置に、合わせるための変換パラメータを算出する。そして、自己位置算出部７は、算出した変換パラメータを用いて、距離画像センサ２１のグローバル座標系基準における位置を算出する。
【００３６】
自己位置算出部７は、例えば、ＲＡＮＳＡＣ（Random sample consensus）を用いて、あるいは、ＩＣＰ（Iterative Closest Point）アルゴリズムなどを用いて、上記変換パラメータを算出する。自己位置算出部７は、例えば、移動体の目的や制約条件（移動体に搭載されるセンサなど）に応じて、下記のような方法で、上記変換パラメータを算出してもよい。上記自己位置の算出方法は一例であり、これに限定されず、任意の幾何学的位置関係による算出方法を用いることができる。
【００３７】
次に、上述した自己位置推定装置のマッピング処理フロー及び自己位置推定処理フローについて、説明する。図１０は、本実施形態に係る自己位置推定装置のマッピング処理フローを示すフローチャートである。
【００３８】
画像取得部２は、距離画像センサ２１を用いて、複数の視点で環境内における輝度画像及び距離画像を取得する（ステップＳ１０１）。
特徴点抽出部３は、画像取得部２により取得された輝度画像から特徴点を抽出する（ステップＳ１０２）。なお、ここで、特徴点除去部５は、特徴点抽出部３により抽出された特徴点群の中から、動的特徴点を除去してもよい。
【００３９】
対応付け部４は、特徴点抽出部３により抽出された各特徴点の特徴点インデックスｉと、その各特徴点が抽出された各画像の画像インデックスｊと、その各特徴点の三次元グローバル位置Ｐｉｊ＝（ｘｉｊ、ｙｉｊ、ｚｉｊ）と、を対応付けた特徴点の三次元位置データをメモリ８に記憶させる（ステップＳ１０３）。
最後に、対応付け部４は、上記特徴点の三次元位置データと共に、特徴点を抽出した画像群をマッピングデータとしてメモリ８に記憶させる（ステップＳ１０４）。
【００４０】
図１１は、本実施形態に係る自己位置推定装置の自己位置推定処理フローを示すフローチャートである。
マッチング部６は、メモリ８からマッピングデータを読み込む（ステップＳ２０１）。
画像取得部２は、距離画像センサ２１を用いて、環境内における輝度画像及び距離画像を取得する（ステップＳ２０２）。
【００４１】
特徴点抽出部３は、画像取得部２により取得された輝度画像から実特徴点を抽出する（ステップＳ２０３）。
特徴点除去部５は、特徴点抽出部３により抽出された実特徴点群の中から、動的特徴点を除去する（ステップＳ２０４）。
【００４２】
マッチング部６は、特徴点除去部５により動的特徴点を除去した現在画像の実特徴点と、メモリ８に記憶されたマッピングデータの画像群の特徴点と、のマッチングを行う（ステップＳ２０５）
マッチング部６は、マッピングデータの画像毎に、マッチング特徴点数を算出する（ステップＳ２０６）。
【００４３】
マッチング部６は、各画像の画像インデックスを、マッチング特徴点数が多い順にソートする（ステップＳ２０７）。
マッチング部６は、上記ソートした順で、画像インデックス毎に、現在画像の各実特徴点とマッピングデータの各画像の各特徴点との一致を判定し、一致したものを対応付ける（ステップＳ２０８）。
【００４４】
自己位置算出部７は、上記求めた現在画像の各実特徴点の位置と、対応するマッピングデータの各画像の各特徴点の三次元グローバル位置と、の幾何学位置関係から、移動体の自己位置を算出する（ステップＳ２０９）。
【００４５】
以上、本実施形態に係る自己位置推定装置において、メモリ８には、環境内を撮影した複数の画像と、該各画像の各特徴点と、該各特徴点の位置情報と、を対応付けて記憶されている。そして、マッチング部８は、画像取得部６により取得された画像と類似度の高いメモリ８の画像に含まれる特徴点を、メモリ８の他の画像に含まれる特徴点よりも優先して、画像取得部６により取得された画像から抽出された実特徴点と、マッチングし、メモリ８の特徴点と実特徴点とを対応付ける。そして、自己位置算出部７は、実特徴点の位置と、メモリ８に記憶された実特徴点に対応する特徴点の位置と、の幾何学的位置関係に基づいて、移動体の自己位置を算出する。
これにより、マッチング部６は、自己位置推定処理時における距離画像センサ２１の視点に近い順で、メモリ８に記憶された画像群の中から画像を選択し、その画像の特徴点と、現在画像から抽出された実特徴点と、のマッチングを行うことができる。したがって、上述したマッピング処理時と自己位置推定処理時とにおける距離画像センサ２１の視点の相違によって生じる自己位置推定精度の誤差を抑制し、自己位置推定精度を向上させることができる。
【００４６】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上記実施形態において、自己位置推定装置１は、移動体に搭載される構成であるが、これに限定されない。例えば、画像取得部２のみが移動体に設けられ、特徴点抽出部３、対応付け部４、特徴点除去部５、マッチング部６、および自己位置算出部７は、移動体外に設けれる構成であってもよい。
【００４７】
また、本発明は、例えば、図１０又は図１１に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００４８】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００４９】
１  自己位置推定装置、２  画像取得部、３  特徴点抽出部、４  対応付け部、５  特徴点除去部、６  マッチング部、７  自己位置算出部、２１  距離画像センサ
【図１】
0006645051.tif 000002
【図２】
0006645051.tif 000003
【図３】
0006645051.tif 000004
【図４】
0006645051.tif 000005
【図５】
0006645051.tif 000006
【図６】
0006645051.tif 000007
【図７】
0006645051.tif 000008
【図８】
0006645051.tif 000009
【図９】
0006645051.tif 000010
【図１０】
0006645051.tif 000011
【図１１】
0006645051.tif 000012