(57)【要約】【課題】画角が狭い単一カメラを利用した場合でも、高い精度で移動体の位置及び姿勢を推定する。【解決手段】位置姿勢推定装置１０Ａは、画像情報取得部１２により取得された各時刻の画像情報の各々から特徴点を抽出する特徴点抽出部１４と、各時刻の画像情報の特徴点と、地図ＤＢ１６から取得された複数の照合用ランドマークの各々の特徴点とを照合して、各時刻の画像の各々に含まれるランドマークの特徴点の実空間における３次元位置を特定する３次元位置特定部２０と、各時刻の移動体の基準点に対する相対的な位置及び姿勢を算出する相対位置姿勢算出部２４と、各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置と、各時刻の移動体の相対的な位置及び姿勢と、を表す時系列情報に基づいて、移動体の実空間における３次元位置及び姿勢を推定する状態推定部２８と、を備える。【選択図】図１
2018185239.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載された撮影装置により前記移動体の周辺を時系列で撮影した各時刻の画像情報を取得する画像情報取得部と、
前記画像情報取得部により取得された各時刻の画像情報の各々から特徴点を抽出する抽出部と、
予め得られた複数の照合用ランドマークの各々の実空間における３次元位置及び前記複数の照合用ランドマークの各々の特徴点を対応付けて記憶した地図データベースと、
前記抽出部により抽出された各時刻の画像情報の特徴点と、前記地図データベースから取得された前記複数の照合用ランドマークの各々の特徴点とを照合することにより、各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置を特定する特定部と、
前記移動体の基準点に対する相対的な位置及び姿勢を各時刻について算出する算出部と、
前記特定部により特定された各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置と、前記算出部により算出された各時刻の移動体の基準点に対する相対的な位置及び姿勢と、を表す時系列情報に基づいて、前記移動体の実空間における３次元位置及び姿勢を推定する推定部と、
を備えた位置姿勢推定装置。
【請求項２】
前記移動体に搭載されたセンサにより検出された各時刻の速度及び角速度を含むセンサ情報を取得するセンサ情報取得部を更に備え、
前記算出部は、前記センサ情報取得部により取得されたセンサ情報に基づいて、前記移動体の基準点に対する相対的な位置及び姿勢を各時刻について算出する請求項１に記載の位置姿勢推定装置。
【請求項３】
前記時系列情報から、前記画像情報取得部により取得された各時刻の画像情報に含まれるランドマークの数に基づいて、前記推定部に提供する時系列情報の範囲を選択する選択部を更に備えた請求項１又は２に位置姿勢推定装置。
【請求項４】
前記ランドマークの数は、前記ランドマークの延べ数又は前記ランドマークの種類数である請求項３に記載の位置姿勢推定装置。
【請求項５】
コンピュータを、
移動体に搭載された撮影装置により前記移動体の周辺を時系列で撮影した各時刻の画像情報を取得する画像情報取得部、
前記画像情報取得部により取得された各時刻の画像情報の各々から特徴点を抽出する抽出部、
前記抽出部により抽出された各時刻の画像情報の特徴点と、予め得られた複数の照合用ランドマークの各々の実空間における３次元位置及び前記複数の照合用ランドマークの各々の特徴点を対応付けて記憶した地図データベースから取得された前記複数の照合用ランドマークの各々の特徴点とを照合することにより、各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置を特定する特定部、
前記移動体の基準点に対する相対的な位置及び姿勢を各時刻について算出する算出部、及び、
前記特定部により特定された各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置と、前記算出部により算出された各時刻の移動体の基準点に対する相対的な位置及び姿勢と、を表す時系列情報に基づいて、前記移動体の実空間における３次元位置及び姿勢を推定する推定部、
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置姿勢推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、地図データベースに予め格納された複数の照合用ランドマークの画像の各々と、移動体に搭載されたカメラで撮影されたランドマークの画像とを照合することで、移動体の位置を推定する技術がある。この種の技術では、照合対象となるランドマークの数が多く、かつ、３次元空間内の広い範囲にランドマークが分布しているほど、移動体の位置を推定する精度が向上する傾向がある。このため、一般には、画角が狭い単一カメラよりも画角が広い全方位カメラを利用したほうが、高い精度で移動体の位置を推定できると考えられる。
【０００３】
しかし、全方位カメラは、単一カメラよりも高価であるため、単一カメラを利用した場合であっても、高い精度で移動体の位置を推定できることが望ましい。
【０００４】
例えば、特許文献１には、複数のセンサを用いて、移動体の周辺に位置するランドマークに関する情報を取得し、取得した情報を選択的に組み合わせて、移動体の存在確率が最も高い位置を、移動体の位置として推定する技術が記載されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１６−０１４６４７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかし、上記特許文献１に記載の技術では、センサの計測誤差や、移動体の周辺に観測可能なランドマークが少ない場合には、移動体の存在確率が広範囲に渡ってなだらかに分布してしまう。このため、存在確率が最も高い位置を一意に特定することが難しく、移動体の位置を高い精度で推定することが困難となる場合がある。
【０００７】
本発明は、上記の問題点を解決するためになされたもので、画角が狭い単一カメラを利用した場合でも、高い精度で移動体の位置及び姿勢を推定することができる位置姿勢推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するために、請求項１に記載の位置姿勢推定装置は、移動体に搭載された撮影装置により前記移動体の周辺を時系列で撮影した各時刻の画像情報を取得する画像情報取得部と、前記画像情報取得部により取得された各時刻の画像情報の各々から特徴点を抽出する抽出部と、予め得られた複数の照合用ランドマークの各々の実空間における３次元位置及び前記複数の照合用ランドマークの各々の特徴点を対応付けて記憶した地図データベースと、前記抽出部により抽出された各時刻の画像情報の特徴点と、前記地図データベースから取得された前記複数の照合用ランドマークの各々の特徴点とを照合することにより、各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置を特定する特定部と、前記移動体の基準点に対する相対的な位置及び姿勢を各時刻について算出する算出部と、前記特定部により特定された各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置と、前記算出部により算出された各時刻の移動体の基準点に対する相対的な位置及び姿勢と、を表す時系列情報に基づいて、前記移動体の実空間における３次元位置及び姿勢を推定する推定部と、を備えたものである。
【０００９】
また、請求項２に記載の位置姿勢推定装置は、請求項１に記載の発明において、前記移動体に搭載されたセンサにより検出された各時刻の速度及び角速度を含むセンサ情報を取得するセンサ情報取得部を更に備え、前記算出部が、前記センサ情報取得部により取得されたセンサ情報に基づいて、前記移動体の基準点に対する相対的な位置及び姿勢を各時刻について算出するものである。
【００１０】
また、請求項３に記載の位置姿勢推定装置は、請求項１又は２に記載の発明において、前記時系列情報から、前記画像情報取得部により取得された各時刻の画像情報に含まれるランドマークの数に基づいて、前記推定部に提供する時系列情報の範囲を選択する選択部を更に備えたものである。
【００１１】
また、請求項４に記載の位置姿勢推定装置は、請求項３に記載の発明において、前記ランドマークの数が、前記ランドマークの延べ数又は前記ランドマークの種類数とされたものである。
【００１２】
更に、上記目的を達成するために、請求項５に記載のプログラムは、コンピュータを、移動体に搭載された撮影装置により前記移動体の周辺を時系列で撮影した各時刻の画像情報を取得する画像情報取得部、前記画像情報取得部により取得された各時刻の画像情報の各々から特徴点を抽出する抽出部、前記抽出部により抽出された各時刻の画像情報の特徴点と、予め得られた複数の照合用ランドマークの各々の実空間における３次元位置及び前記複数の照合用ランドマークの各々の特徴点を対応付けて記憶した地図データベースから取得された前記複数の照合用ランドマークの各々の特徴点とを照合することにより、各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置を特定する特定部、前記移動体の基準点に対する相対的な位置及び姿勢を各時刻について算出する算出部、及び、前記特定部により特定された各時刻の画像情報の各々に含まれるランドマークの特徴点の実空間における３次元位置と、前記算出部により算出された各時刻の移動体の基準点に対する相対的な位置及び姿勢と、を表す時系列情報に基づいて、前記移動体の実空間における３次元位置及び姿勢を推定する推定部、として機能させるものである。
【発明の効果】
【００１３】
以上説明したように、本発明によれば、画角が狭い単一カメラを利用した場合でも、高い精度で移動体の位置及び姿勢を推定することができる。
【図面の簡単な説明】
【００１４】
【図１】第１の実施形態に係る位置姿勢推定装置の構成の一例を示すブロック図である。
【図２】第１の実施形態に係る位置姿勢推定装置として機能するコンピュータの構成の一例を示すブロック図である。
【図３】実施形態に係る車両と複数のランドマークとの関係の一例を説明するための図である。
【図４】第１の実施形態に係る位置姿勢推定処理プログラムの処理の流れの一例を示すフローチャートである。
【図５】第２の実施形態に係る位置姿勢推定装置の構成の一例を示すブロック図である。
【発明を実施するための形態】
【００１５】
以下、図面を参照して、本発明を実施するための形態の一例について詳細に説明する。
【００１６】
[第１の実施形態]
図１は、第１の実施形態に係る位置姿勢推定装置１０Ａの構成の一例を示すブロック図である。
図１に示すように、本実施形態に係る位置姿勢推定装置１０Ａは、画像情報取得部１２、特徴点抽出部１４、地図データベース（以下、地図ＤＢという。）１６、地図情報取得部１８、３次元位置特定部２０、センサ情報取得部２２、相対位置姿勢算出部２４、時系列情報蓄積部２６、及び状態推定部２８を備えている。
【００１７】
図２は、第１の実施形態に係る位置姿勢推定装置１０Ａとして機能するコンピュータの構成の一例を示すブロック図である。
図２に示すように、本実施形態に係る位置姿勢推定装置１０Ａは、ＣＰＵ(Central Processing Unit)５０及び内部メモリ５２を含む汎用的なコンピュータとして構成される。
【００１８】
内部メモリ５２には、本実施形態に係る位置姿勢推定処理プログラムが格納されている。この位置姿勢推定処理プログラムは、例えば、位置姿勢推定装置１０Ａに予めインストールされていてもよい。また、位置姿勢推定処理プログラムは、不揮発性の記憶媒体に記憶して、又はネットワークを介して配布し、位置姿勢推定装置１０Ａに適宜インストールすることで実現してもよい。なお、不揮発性の記憶媒体の例としては、ＣＤ-ＲＯＭ(Compact Disc Read Only Memory)、光磁気ディスク、ＨＤＤ(Hard Disk Drive)、ＤＶＤ-ＲＯＭ(Digital Versatile Disc Read Only Memory)、フラッシュメモリ、メモリカード等が想定される。また、内部メモリ５２は、図１に示す地図ＤＢ１６及び時系列情報蓄積部２６としても機能する。
【００１９】
ＣＰＵ５０は、図１に示す画像情報取得部１２、特徴点抽出部１４、地図情報取得部１８、３次元位置特定部２０、センサ情報取得部２２、相対位置姿勢算出部２４、及び状態推定部２８として機能する。ＣＰＵ５０は、内部メモリ５２から位置姿勢推定処理プログラムを読み出して実行することで、これら各部として機能する。また、ＣＰＵ５０は、移動体の一例である車両６０に搭載されたカメラ４０及びセンサ４２の各々と接続されている。
【００２０】
以下、本実施形態では、位置姿勢推定装置１０Ａが車両６０に搭載されている場合について説明するが、位置姿勢推定装置１０Ａは車両６０の外部に設置されていてもよく、この場合、カメラ４０及びセンサ４２の各々と無線通信を介して接続される。
【００２１】
カメラ４０は、撮影装置の一例であり、各時刻の車両６０の周辺を撮影する。なお、カメラ４０は、１台でもよいし、複数台でもよい。また、カメラ４０の撮影範囲は、車両６０の進行方向（前方）を向いていてもよいし、後ろ向き又は全方位でもよい。
【００２２】
センサ４２は、例えば、各時刻の車両６０の速度を検出する速度センサと、各時刻の車両６０の角速度を検出する角速度センサと、を含む。速度センサは、一例として、車両６０の車輪のオドメーターで計測される車輪速を検出する。角速度センサには、例えば、ジャイロセンサが用いられ、一例として、ヨーレートを角速度として検出する。なお、３軸センサであれば、ヨーレートと併せて、ピッチ角速度、ロール角速度を検出してもよい。
【００２３】
画像情報取得部１２は、カメラ４０により各時刻に撮影された車両６０の周辺の画像情報（以下、単に画像という。）を取得する。
【００２４】
特徴点抽出部１４は、抽出部の一例である。特徴点抽出部１４は、画像情報取得部１２により取得された各時刻の画像の各々から特徴点を抽出する。
【００２５】
特徴点の選び方としては多様な手法が提案されている。ロバスト性の高いＳＩＦＴ法、実装が容易なＨａｒｒｉｓ法、計算負荷が低いＦＡＳＴ法等、所望の手法を用いればよい。また、点特徴以外にも、線や面等の特徴量を用いてもよい。また、特徴点の追跡方法としては、Ｌｕｃａｓ-Ｋａｎａｄｅ法等の一般的な手法でもよいし、その他の手法でもよい。
【００２６】
一方、地図ＤＢ１６には、予め広域で得られた複数の照合用ランドマークの各々の実空間における３次元位置、及び、予め撮影した画像から得られた複数の照合用ランドマークの各々を表す画像における照合用ランドマークの特徴点が対応付けられて記憶されている。なお、ここでいう照合用ランドマークとは、実空間において目印となり得る建物や、標識、看板等の物体であり、特徴点とは、照合用ランドマークを表す特徴的な画素パターンであり、特徴量と共に記憶される。各照合用ランドマークの実空間における３次元位置は、例えば、レーザー光を対象物に照射して測距を行うＬＩＤＡＲ(Light Detection and Ranging)と呼ばれる技術を用いて予め取得される。なお、照合用ランドマークの特徴点の特徴量としては、上記と同様に、点特徴をはじめ、線や面等の特徴量が用いられる。また、照合用ランドマークの特徴点としては、各照合用ランドマークについて、観測される所定の角度（例えば、１０°）毎に当該角度から撮影された画像から抽出された特徴点が記憶されている。
【００２７】
地図情報取得部１８は、地図ＤＢ１６から、車両６０の周辺に位置する照合用ランドマークの３次元位置及び特徴点を読み出す。例えば、車両６０の簡易的な位置を、図示しないＧＰＳ(Global Positioning System)機能を用いて取得し、取得した車両６０の位置から一定範囲内に位置する複数の照合用ランドマークの各々の３次元位置及び特徴点を地図ＤＢ１６から読み出し、内部メモリ５２に一時的に記憶させる。
【００２８】
３次元位置特定部２０は、特定部の一例である。３次元位置特定部２０は、特徴点抽出部１４により抽出された各時刻の画像の特徴点と、地図ＤＢ１６から取得された複数の照合用ランドマークの各々の特徴点とを照合することにより、各時刻の画像の各々に含まれるランドマークの特徴点の実空間における３次元位置を特定する。
【００２９】
より具体的には、３次元位置特定部２０は、車両６０と、内部メモリ５２に一時的に記憶させた各照合用ランドマークとの位置関係から、車両６０から各照合用ランドマークへの視線方向を算出し、照合用ランドマーク毎に、算出した視線方向に対応する角度を持つ特徴点を取得する。そして、特徴点抽出部１４により抽出された各時刻の画像の特徴点と一致又は類似する特徴点を有する照合用ランドマークを特定し、特定した照合用ランドマークの実空間における３次元位置を、上記各時刻の画像の各々に含まれるランドマークの特徴点の実空間における３次元位置として付与する。
【００３０】
なお、３次元位置特定部２０は、特徴点抽出部１４により抽出された各時刻の画像の特徴点の当該画像における２次元位置も特定する。３次元位置特定部２０は、各時刻の画像の各々に含まれるランドマークについて、画像中の２次元位置及び実空間中の３次元位置の組を出力する。
【００３１】
一方、センサ情報取得部２２は、車両６０に搭載されたセンサ４２により検出された各時刻の速度及び角速度を含むセンサ情報を取得する。このセンサ情報は、センサ４２の各センサから直接取得してもよいし、Ｂｌｕｅｔｏｏｔｈ(登録商標)等の無線通信を介して取得してもよい。
【００３２】
相対位置姿勢算出部２４は、算出部の一例である。相対位置姿勢算出部２４は、センサ情報取得部２２により取得されたセンサ情報（速度及び角速度）に基づいて、車両６０の基準点に対する相対的な位置及び姿勢を各時刻について算出する。なお、本実施形態では、基準点として、例えば、車両６０の相対的な位置及び姿勢の算出を開始した地点が適用されるが、その他の任意の地点を適用してもよい。
【００３３】
時系列情報蓄積部２６は、時系列情報を蓄積する。この時系列情報とは、３次元位置特定部２０により特定された各時刻の画像の各々に含まれるランドマークの特徴点の実空間における３次元位置と、相対位置姿勢算出部２４により算出された各時刻の車両６０の基準点に対する相対的な位置及び姿勢と、を表す情報である。具体的には、時刻ｔが更新されるたびに、ランドマークの特徴点の実空間における３次元位置と、車両６０の基準点に対する相対的な位置及び姿勢と、を時系列で蓄積する。なお、時系列情報蓄積部２６は、時刻ｔが更新されるたびに、各時刻の画像の各々に含まれるランドマークの当該画像における２次元位置についても蓄積する。また、時系列情報蓄積部２６は、３次元位置特定部２０で位置が特定された新たなランドマークの特徴点が得られた場合には、当該新たなランドマークの特徴点の実空間における３次元位置を蓄積する。
【００３４】
状態推定部２８は、推定部の一例である。状態推定部２８は、時系列情報蓄積部２６に蓄積された時系列情報に基づいて、車両６０の実空間における３次元位置及び姿勢を推定する。本実施形態では、以下に示す位置姿勢推定手法を適用する。
【００３５】
画像中におけるランドマークの特徴点の２次元位置、車両６０の位置、車両６０の姿勢、及び実空間中におけるランドマークの特徴点の３次元位置の関係式（観測方程式）は、下記式（１）のように表される。
【００３６】
【数１】
2018185239.tif 000003
（１）
【００３７】
ここで、Ｘ′ｔ，ｍは、画像中におけるランドマークの特徴点の２次元位置を示す。Ｋは、カメラ４０のキャリブレーション行列を示す。Ｒｔは、車両６０の姿勢（ピッチ、ヨー、ロール）を要素とした３次元回転行列を示す。ｘｔは、車両６０の実空間における３次元位置を示す。Ｘｍは、実空間中におけるランドマークの特徴点の３次元位置を示す。Ｉは、単位行列を示す。添え字のｔは時刻インデックスを示し、添え字のｍはランドマークのインデックスを示す。また、ｘｔ及びＲｔは未知数であり、それ以外は既知である。さらに、未知数ｘｔ及びＲｔは、以下の式（２）、（３）のように表すことができる。
【００３８】
【数２】
2018185239.tif 000004
（２）
【数３】
2018185239.tif 000005
（３）
【００３９】
ここで、Δｘｔは、相対座標系における基準点からの車両６０の相対位置を示す。ｘ０は、絶対座標系（実空間の座標系）における相対座標系基準点での車両６０の位置を示す。ΔＲｔは、相対座標系における基準点からの車両６０の相対姿勢を示す。Ｒ０は、絶対座標系における相対座標系基準点での車両６０の姿勢を示す。
【００４０】
上記式（２）、式（３）を、上記式（１）に代入した結果を式（４）に示す。
【００４１】
【数４】
2018185239.tif 000006
（４）
【００４２】
次に、式（４）における未知数ｘ０及びＲ０を、最小二乗法を用いて解く。式（５）にコスト関数を示す。上記式（２）は、非線形であるため、例えば、Ｌｅｖｅｎｂｅｒｇ-Ｍａｒｑｕａｒｄｔ法等の収束演算を用いて解くことができる。
【００４３】
【数５】
2018185239.tif 000007
（５）
【００４４】
但し、Ｍは推定に用いるランドマークの数を示し、Ｎは時系列情報の数を示す。
【００４５】
上記式（５）を最小にするｘ０及びＲ０を推定した後に、上記式（２）に、ｘ０を代入し、上記式（３）に、Ｒ０を代入することで、最新時刻における車両６０の実空間における３次元位置ｘｔ及び姿勢Ｒｔを得ることができる。
【００４６】
図３は、本実施形態に係る車両６０と複数のランドマークＬＭ１〜ＬＭ１０との関係の一例を説明するための図である。
【００４７】
図３に示すように、車両６０が矢印で示す進行方向に向けて移動しながら、車両６０の前方に搭載されたカメラ４０により複数のランドマークが撮影される。ここでは、１０個のランドマークＬＭ１〜ＬＭ１０を例示しているが、ランドマークの数はこれに限定されない。
【００４８】
時刻Ｔ１において、ランドマークＬＭ１〜ＬＭ４の４個のランドマークを含む画像が取得される。時刻Ｔ１では、上述したように、画像から抽出された特徴点が、地図ＤＢ１６に格納されている複数の照合用ランドマークの各々の特徴点と照合され、画像に含まれる４個のランドマークＬＭ１〜ＬＭ４の各々の特徴点の実空間における３次元位置（Ｘｍ）が特定される。
【００４９】
時刻Ｔ２において、ランドマークＬＭ５〜ＬＭ８の４個のランドマークを含む画像が取得される。時刻Ｔ２では、上記と同様に、画像から抽出された特徴点が、地図ＤＢ１６に格納されている複数の照合用ランドマークの各々の特徴点と照合され、画像に含まれる４個のランドマークＬＭ５〜ＬＭ８の各々の特徴点の実空間における３次元位置（Ｘｍ）が特定される。
【００５０】
時刻Ｔ３において、ランドマークＬＭ７〜ＬＭ１０の４個のランドマークを含む画像が取得される。時刻Ｔ３では、上記と同様に、画像から抽出された特徴点が、地図ＤＢ１６に格納されている複数の照合用ランドマークの各々の特徴点と照合され、画像に含まれる４個のランドマークＬＭ７〜ＬＭ１０の各々の特徴点の実空間における３次元位置（Ｘｍ）が特定される。本実施形態では、時刻Ｔ１〜Ｔ３の時間帯に含まれるランドマークの全てが照合対象とされる。
【００５１】
本実施形態によれば、各時刻における車両６０の基準点に対する相対的な位置及び姿勢を算出し、各時刻にカメラ４０により撮影された画像に含まれるランドマークの全てを照合対象として用いて、車両６０の位置及び姿勢の推定を行う。これにより、画角の狭い単一カメラを利用した場合でも、全方位カメラを利用した場合と同様に、推定に用いるランドマークの数が増加し、分布範囲も広くなるため、車両６０の位置及び姿勢を高い精度で推定することができる。
【００５２】
次に、図４を参照して、第１の実施形態に係る位置姿勢推定装置１０Ａの作用を説明する。なお、図４は、第１の実施形態に係る位置姿勢推定処理プログラムの処理の流れの一例を示すフローチャートである。
【００５３】
まず、図４のステップ１００では、画像情報取得部１２が、車両６０に搭載されたカメラ４０により各時刻に車両６０の周辺を撮影した画像を取得する。
【００５４】
ステップ１０２では、特徴点抽出部１４が、画像情報取得部１２により取得された各時刻の画像から特徴点を抽出する。
【００５５】
ステップ１０４では、３次元位置特定部２０が、特徴点抽出部１４により抽出された各時刻の画像の特徴点の当該画像における２次元位置を特定する。そして、３次元位置特定部２０は、特徴点抽出部１４により抽出された各時刻の画像の特徴点と、地図ＤＢ１６から取得された複数の照合用ランドマークの各々の特徴点とを照合することにより、各時刻の画像に含まれるランドマークの特徴点の実空間における３次元位置を特定する。
【００５６】
ステップ１０６では、相対位置姿勢算出部２４が、センサ情報取得部２２により取得されたセンサ情報に基づいて、車両６０の基準点に対する相対的な位置及び姿勢を各時刻について算出する。
【００５７】
ステップ１０８では、時系列情報蓄積部２６が、３次元位置特定部２０により特定された、各時刻の画像の特徴点の当該画像における２次元位置と、各時刻の画像に含まれるランドマークの特徴点の実空間における３次元位置と、相対位置姿勢算出部２４により算出された各時刻の車両６０の基準点に対する相対的な位置及び姿勢と、を表す時系列情報を蓄積する。
【００５８】
ステップ１１０では、状態推定部２８が、時系列情報蓄積部２６により蓄積された時系列情報に基づいて、上述の式（１）〜式（５）を用いて、車両６０の実空間における３次元位置及び姿勢を推定する。
【００５９】
[第２の実施形態]
図５は、第２の実施形態に係る位置姿勢推定装置１０Ｂの構成の一例を示すブロック図である。
図５に示すように、本実施形態に係る位置姿勢推定装置１０Ｂは、第１の実施形態における時系列情報蓄積部２６と状態推定部２８との間に時系列情報選択部３０を備えている。これ以外の構成については、第１の実施形態の構成と同様であるので、同様な部分には同一の符号を付して繰り返しの説明を省略する。
【００６０】
時系列情報選択部３０は、選択部の一例である。時系列情報選択部３０は、時系列情報蓄積部２６に蓄積された時系列情報から、画像情報取得部１２により取得された各時刻の画像に含まれるランドマークの数に基づいて、状態推定部２８に提供する時系列情報の範囲を選択する。ここでいうランドマークの数とは、一例として、ランドマークの延べ数である。なお、ランドマークの数として、ランドマークの種類数を用いてもよい。図３に示す例の場合、時刻Ｔ１〜Ｔ３の間において、ランドマークの延べ数では、１２個（ランドマーク４個／画像×３画像分）となり、ランドマークの種類数では、１０個となる。また、時系列情報の範囲とは、現時点から、過去のある時刻までの範囲である。
【００６１】
一般的に、センサを用いて算出した移動体の相対的な位置及び姿勢は、移動体の現在地点までに移動した距離が長く（つまり、時間が長く）なるほど、センサの計測誤差が蓄積していく傾向にある。このため、時系列情報の範囲を短くした方が、センサの計測誤差の影響を受け難くなる。一方、移動体の現在地点までに移動した距離が長いほど、推定に用いるランドマークの数が増加する。このため、時系列情報の範囲を長くした方が、推定の精度が向上する。このため、本実施形態では、車両６０の現在地点を基準とした距離範囲と、推定に用いるランドマークの数とを考慮の上、適切な時系列情報の範囲を選択する。例えば、ランドマークの数が一定数以上となる距離範囲の中で最小の距離範囲に対応する時系列情報の範囲を選択する方法や、センサ４２の計測誤差が一定値以内に収まると推定される距離範囲の中でランドマークの数が最大となる距離範囲に対応する時系列情報の範囲を選択する方法等が考えられる。
【００６２】
本実施形態によれば、センサ４２の計測誤差の影響を受け難くくし、一定数以上のランドマークの数が得られるように、最適な時系列情報の範囲を選択することができるため、車両６０の位置及び姿勢をより高い精度で推定することができる。
【００６３】
以上、実施形態として位置姿勢推定装置を例示して説明した。実施形態は、コンピュータを、位置姿勢推定装置が備える各部として機能させるためのプログラムの形態としてもよい。実施形態は、このプログラムを記憶したコンピュータが読み取り可能な記憶媒体の形態としてもよい。
【００６４】
その他、上記実施形態で説明した位置姿勢推定装置の構成は、一例であり、主旨を逸脱しない範囲内において状況に応じて変更してもよい。
【００６５】
また、上記実施形態で説明したプログラムの処理の流れも、一例であり、主旨を逸脱しない範囲内において不要なステップを削除したり、新たなステップを追加したり、処理順序を入れ替えたりしてもよい。
【００６６】
また、上記実施形態では、プログラムを実行することにより、実施形態に係る処理がコンピュータを利用してソフトウェア構成により実現される場合について説明したが、これに限らない。実施形態は、例えば、ハードウェア構成や、ハードウェア構成とソフトウェア構成との組み合わせによって実現してもよい。
【符号の説明】
【００６７】
１０Ａ、１０Ｂ位置姿勢推定装置
１２   画像情報取得部
１４   特徴点抽出部
１６   地図ＤＢ
１８   地図情報取得部
２０   ３次元位置特定部
２２   センサ情報取得部
２４   相対位置姿勢算出部
２６   時系列情報蓄積部
２８   状態推定部
３０   時系列情報選択部
４０   カメラ
４２   センサ
５０   ＣＰＵ
５２   内部メモリ
６０   車両
【図１】
2018185239.tif 000008
【図２】
2018185239.tif 000009
【図３】
2018185239.tif 000010
【図４】
2018185239.tif 000011
【図５】
2018185239.tif 000012