JP2019132664A - 自車位置推定装置、自車位置推定方法、及び自車位置推定プログラム - Google Patents

Abstract

【課題】周囲の明るさが変化した場合であっても、精度良く自車位置を推定する。【解決手段】自車位置推定装置１０は、走行経路に沿った複数の位置でカメラ１４により撮影された複数の基準画像の各々に関連付けられた、基準画像の各特徴点、特徴量、カメラ１４の位置及び姿勢を含む地図情報１６Ａと、走行経路に沿って自車を走行させた状態でカメラ１４により撮影された走行画像から特徴点を検出する検出部２０と、検出された特徴点の特徴を示す特徴量を算出する算出部２２と、算出された特徴量に基づいて、複数の基準画像から走行画像に類似する類似画像を選定し、画像間の特徴量の比較により、走行画像の特徴点と類似画像の特徴点との対応付けを行った結果に基づいて、自車の位置及び姿勢を推定する推定部２４と、対応付けられた走行画像の特徴点の特徴量を、類似画像の特徴点の特徴量として地図情報１６Ａに追加する追加部２６と、を備える。【選択図】図３

Description

本発明は、自車位置推定装置、自車位置推定方法、及び自車位置推定プログラムに関する。

移動体に搭載されたカメラ等の撮影部によって撮影された撮影画像に基づいて、３次元空間上における移動体の位置を推定する技術が提案されている。

従来、自車位置の推定に関連する技術として、例えば特許文献１に開示された自動運転制御装置が知られている。特許文献１に係る自動運転制御装置は、車両が運転者に運転される登録モードにおいて車両の周囲環境を撮像した画像である登録時画像に基づき、車両を自動運転するための自動運転情報を生成する自動運転情報登録部と、車両が自動運転される自動運転モードにおいて車両の周囲環境を撮像した画像である自動運転時画像と、自動運転情報とに基づき、車両を自動運転する自動運転制御部と、を有し、自動運転情報登録部は、登録時画像に基づき、車両の周囲環境に存在する候補特徴点を抽出する候補特徴点抽出部と、車両の移動中に撮像された複数の登録時画像に基づき、候補特徴点のうち、車両の目的箇所の周囲に固定して配置された構造物であると判断した候補特徴点を、特徴点として選定し、所定の原点座標に対する特徴点の位置の情報である自動運転情報を生成する自動運転情報生成部と、を有し、自動運転制御部は、自動運転時画像と自動運転情報とに基づき、原点座標に対する車両の位置の情報である車両位置情報を算出する車両位置算出部と、車両位置情報に基づき、車両を目的箇所まで自動運転する自動運転実行制御部と、を有している。すなわち、特許文献１に係る自動運転制御装置では、事前に走行した際の画像から構造物の特徴点を抽出し、その３次元位置を推定して地図に登録し、自動運転時に画像から構造物の特徴点を抽出し、地図に登録された構造物の特徴点と照合することにより自車位置を推定している。

特開２０１７−１３８６６４号公報

ところで、画像から抽出される特徴点として、周囲との輝度差が大きい点を抽出する場合がある。この場合、例えば、日中と夜間とでは、周囲の明るさが変化するため、特徴点の特徴量が変化することがある。この特徴量の変化によって、地図に登録された特徴点との対応付けがうまくいかず、自車位置を精度良く推定することができない場合がある。上記特許文献１では、周囲の明るさの変化について考慮されていないため、このような場合、精度良く自車位置を推定することは困難である。

本発明は、上述した課題を解決するためになされたものであり、周囲の明るさが変化した場合であっても、精度良く自車位置を推定することができる自車位置推定装置、自車位置推定方法、及び自車位置推定プログラムを提供することを目的とする。

上記目的を達成するために、請求項１に記載の自車位置推定装置は、所定の走行経路に沿った複数の位置で自車に搭載された撮影部により撮影された複数の基準画像の各々に関連付けられた、前記基準画像から検出された各特徴点と、各特徴点について登録された特徴点の特徴を示す少なくとも１つの特徴量と、前記基準画像の撮影時における前記撮影部の位置及び姿勢と、を含む地図情報を記憶した記憶部と、前記所定の走行経路に沿って自車を走行させた状態で前記撮影部により撮影された走行画像から特徴点を検出する検出部と、前記検出部により検出された特徴点の特徴を示す特徴量を算出する算出部と、前記算出部により算出された特徴量に基づいて、前記複数の基準画像から前記走行画像に類似する類似画像を選定し、前記走行画像の特徴点の特徴量と前記類似画像の特徴点の特徴量とを比較することにより、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行い、対応付けの結果に基づいて、前記所定の走行経路における自車の位置及び姿勢を推定する推定部と、前記推定部により前記類似画像の特徴点と対応付けられた前記走行画像の特徴点の特徴量を、前記類似画像である基準画像の特徴点の特徴量として前記地図情報に追加する追加部と、を備えている。

また、請求項２に記載の発明は、請求項１に記載の発明において、前記推定部が、複数の特徴量が登録されている前記類似画像の特徴点との対応付けを行う際には、前記複数の特徴量の各々について、前記走行画像の特徴点の特徴量との間で距離を計算し、計算した距離の最小値が予め定められた値以下となる場合に、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行う。

また、請求項３に記載の発明は、請求項１又は２に記載の発明において、前記追加部が、前記類似画像の特徴点に登録されている複数の特徴量の数が上限数に達した場合に、登録済みの複数の特徴量及び追加する特徴量のうちの特徴量のペア毎に相互の距離を計算し、計算された各特徴量との距離の中間値が最小となる特徴量を削除する。

また、請求項４に記載の発明は、請求項１〜３のいずれか１項に記載の発明において、前記地図情報が、前記複数の基準画像の各々から検出された特徴点の前記地図情報における位置を更に含み、前記推定部が、対応付けた前記走行画像の特徴点の位置と前記類似画像の特徴点の位置とに基づいて、前記撮影部の位置及び姿勢を推定し、推定した前記撮影部の位置及び姿勢を自車の代表点に換算することにより、前記所定の走行経路における自車の位置及び姿勢を推定する。

また、請求項５に記載の発明は、請求項４に記載の発明において、前記推定部が、前記走行画像の特徴点の位置と、前記類似画像の撮影時における前記撮影部の位置及び姿勢に基づいて前記類似画像の特徴点を前記走行画像に投影して得られる投影点の位置との差で表される投影誤差の総和が最小となる前記撮影部の位置及び姿勢を推定する。

また、請求項６に記載の発明は、請求項５に記載の発明において、前記追加部が、前記推定部により対応付けられた前記走行画像の特徴点のうち、前記投影誤差が予め定められた値以下となる特徴点の特徴量を選択的に前記地図情報に追加する。

一方、上記目的を達成するために、請求項７に記載の自車位置推定方法は、所定の走行経路に沿った複数の位置で自車に搭載された撮影部により撮影された複数の基準画像の各々に関連付けられた、前記基準画像から検出された各特徴点と、各特徴点について登録された特徴点の特徴を示す少なくとも１つの特徴量と、前記基準画像の撮影時における前記撮影部の位置及び姿勢と、を含む地図情報を記憶した記憶部を備えた自車位置推定装置による自車位置推定方法であって、検出部が、前記所定の走行経路に沿って自車を走行させた状態で前記撮影部により撮影された走行画像から特徴点を検出するステップと、算出部が、前記検出部により検出された特徴点の特徴を示す特徴量を算出するステップと、推定部が、前記算出部により算出された特徴量に基づいて、前記複数の基準画像から前記走行画像に類似する類似画像を選定し、前記走行画像の特徴点の特徴量と前記類似画像の特徴点の特徴量とを比較することにより、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行い、対応付けの結果に基づいて、前記所定の走行経路における自車の位置及び姿勢を推定するステップと、追加部が、前記推定部により前記類似画像の特徴点と対応付けられた前記走行画像の特徴点の特徴量を、前記類似画像である基準画像の特徴点の特徴量として前記地図情報に追加するステップと、を含んでいる。

更に、上記目的を達成するために、請求項８に記載の自車位置推定プログラムは、所定の走行経路に沿った複数の位置で自車に搭載された撮影部により撮影された複数の基準画像の各々に関連付けられた、前記基準画像から検出された各特徴点と、各特徴点について登録された特徴点の特徴を示す少なくとも１つの特徴量と、前記基準画像の撮影時における前記撮影部の位置及び姿勢と、を含む地図情報を記憶した記憶部を備えた自車位置推定装置で実行される自車位置推定プログラムであって、前記所定の走行経路に沿って自車を走行させた状態で前記撮影部により撮影された走行画像から特徴点を検出する検出部、前記検出部により検出された特徴点の特徴を示す特徴量を算出する算出部、前記算出部により算出された特徴量に基づいて、前記複数の基準画像から前記走行画像に類似する類似画像を選定し、前記走行画像の特徴点の特徴量と前記類似画像の特徴点の特徴量とを比較することにより、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行い、対応付けの結果に基づいて、前記所定の走行経路における自車の位置及び姿勢を推定する推定部、及び前記推定部により前記類似画像の特徴点と対応付けられた前記走行画像の特徴点の特徴量を、前記類似画像である基準画像の特徴点の特徴量として前記地図情報に追加する追加部、として機能させる。

本発明によれば、周囲の明るさが変化した場合であっても、精度良く自車位置を推定することができる自車位置推定装置、自車位置推定方法、及び自車位置推定プログラムを提供することができる。

（ａ）は実施形態に係るカメラを搭載した車両の一例を示す側面図である。（ｂ）は実施形態に係る自車位置推定装置の電気的な構成の一例を示すブロック図である。 （ａ）は実施形態に係る駐車経路における特徴点の一例を示す図である。（ｂ）は実施形態に係るキーフレームのカメラ位置と特徴点の位置の一例を示す図である。 実施形態に係る自車位置推定装置の機能的な構成の一例を示すブロック図である。 実施形態に係る走行画像の特徴点と類似キーフレームの特徴点との対応付けの説明に供する図である。 実施形態に係る走行画像の特徴量と類似キーフレームの特徴量との間の距離の説明に供する図である。 実施形態に係る投影誤差の説明に供する図である。 実施形態に係る類似キーフレームの特徴点に登録されている複数の特徴量の数が上限数に達した場合に削除する特徴量の説明に供する図である。 実施形態に係る自車位置推定プログラムによる処理の流れの一例を示すフローチャートである。

以下、図面を参照して、本発明の実施の形態について詳細に説明する。なお、以下の説明では、移動体として車両を例にとり、車両が自車の位置を推定する際の周囲環境を駐車場とし、走行経路として駐車場外部から駐車地点までの経路を例示して説明する。

図１から図８を参照して、本実施形態に係る自車位置推定装置、自車位置推定方法、及び自車位置推定プログラムについて説明する。

図１（ａ）は、本実施形態に係るカメラ１４を搭載した車両５０の一例を示す側面図である。
図１（ａ）に示すように、車両５０は、本実施形態に係る自車位置の推定処理（以下、「自車位置推定処理」という。）において使用する、撮影部の一例としてのカメラ１４を備えている。

本実施形態に係るカメラ１４は、車両５０の後部のトランク等に設けられ、車両後方を撮影する。カメラ１４は、例えば、車幅方向の略中央部付近に設けられ、かつカメラ１４の光軸が水平方向より若干下側を向くように配置されている。なお、本実施形態では、カメラ１４を車両５０の後部に設ける形態を例示して説明するが、これに限られず、環境等に応じて例えば前部に設けてもよい。また、本実施形態では、カメラ１４として単眼カメラを例示して説明するが、これに限られず他の形態のカメラ、例えばステレオカメラ等であってもよい。

図１（ｂ）は、本実施形態に係る自車位置推定装置１０の電気的な構成の一例を示すブロック図である。
図１（ｂ）に示すように、本実施形態に係る自車位置推定装置１０は、車両５０に搭載され、制御部１２、カメラ１４、記憶部１６、及び表示部１８を含んで構成されている。

制御部１２は、自車位置を推定するための演算等を行う。制御部１２は、一例としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１２Ａ、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２Ｂ、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１２Ｃ、入出力インタフェースであるＩ／Ｏ１２Ｄ等を含んで構成されている。ＣＰＵ１２Ａ、ＲＯＭ１２Ｂ、ＲＡＭ１２Ｃ、及びＩ／Ｏ１２Ｄの各々はバス１２Ｅにより相互に接続されている。

ＣＰＵ１２Ａは、自車位置推定装置１０の全体を統括、制御する。ＲＯＭ１２Ｂは、本実施形態で用いる地図を生成するための地図生成プログラムや、自車位置を推定する自車位置推定プログラムを含む各種プログラムやデータ等が記憶されている。ＲＡＭ１２Ｃは、各種プログラムの実行時のワークエリアとして用いられるメモリである。ＲＯＭ１２Ｂに記憶されたプログラムをＲＡＭ１２Ｃに展開してＣＰＵ１２Ａが実行することにより、地図の生成や、自車位置の推定が行われる。

制御部１２には、Ｉ／Ｏ１２Ｄを介して、カメラ１４、記憶部１６、及び表示部１８が接続されている。カメラ１４により撮影された画像は、Ｉ／Ｏ１２Ｄを介して制御部１２に取り込まれる。

また、記憶部１６には、本実施形態に係る自車位置推定処理に用いる地図情報等が格納されている。記憶部１６の形態に特に制限はないが、一例としてＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）、フラッシュメモリ等を用いることができる。なお、記憶部１６は制御部１２内に設けてもよいし、外部接続可能としてもよい。また、記憶部１６には制御部１２の制御によって生成した地図情報の他に、ＲＯＭ１２Ｂに代えて、地図生成プログラムや自車位置推定プログラムを記憶してもよい。

表示部１８は、カメラ１４が撮影した画像等を表示する。表示部１８の形態に制限はないが、例えば、液晶モニタ、ＣＲＴ（Ｃａｔｈｏｄｅ Ｒａｙ Ｔｕｂｅ）モニタ、ＦＰＤ（Ｆｌａｔ Ｐａｎｅｌ Ｄｉｓｐｌａｙ）モニタ等が用いられる。

図２（ａ）は、本実施形態に係る駐車経路における特徴点の一例を示す図である。
図２（ａ）では、本実施形態で想定する駐車場の様子（以下、「環境」という。）を示している。

本実施形態では、車両５０の走行経路として、図２（ａ）に示す「スタート」の位置（出発地点）から、「ゴール」の位置（駐車地点）までを想定している。図２（ａ）に示す符号「ＦＰ」は、以下で説明する特徴点の位置を示している。車両５０の自車位置を推定する場合には、所定の大きさを有する車両５０内の位置（以下、「車両代表点Ｘ」）を予め決めておく必要があるが、図２（ｂ）に示すように、本実施形態ではこの車両代表点Ｘを後輪の中央の点としている。しかしながら、代表点の位置はこれに限られず、例えば車両５０の重心の位置としてもよい。

図２（ｂ）は、本実施形態に係るキーフレームのカメラ位置と特徴点の位置の一例を示す図で、図２（ａ）に示す環境の平面図である。

図２（ｂ）に示すように、カメラ１４を搭載した車両５０は、スタート地点ＳＰからゴール地点ＧＰまで走行する。本実施形態に係る自車位置推定処理では、実際の走行の前に、カメラ１４で環境を撮影し、撮影された画像に関連付けられた所定の付随データを地図として予め生成しておくが、符号「Ｓ」は、地図の生成の際の車両５０の走行軌跡を示している。実際の駐車においては、基本的に走行軌跡Ｓに沿って走行する。以下では、走行軌跡Ｓと区別して、実際の駐車時（走行時）に走行する経路を「走行経路」という場合がある。

図２（ｂ）に示す小円は、特徴点ＦＰの位置を示している。本実施形態において、「特徴点」とは、建物の凹凸に基づく陰影、壁面の模様等、撮影画像において輝度の濃淡差が所定値よりも大きい点をいう。従って、特徴点は、図２（ｂ）に示すように、環境における建物３０の壁面ＷＳ、あるいは建物３０の角部に現れることが多く、また、１つの地図において複数の特徴点ＦＰが選定される。さらに、本実施形態では、複数の特徴点ＦＰの各々を区別するために、個々の特徴点ＦＰに「特徴量」を対応付けている。本実施形態に係る「特徴量」とは、一例として特徴点近傍における輝度の濃淡差のパターンをいう。

図２（ｂ）中の符号「ＣＰ」は、地図を生成する際、カメラ１４による撮影が行われた複数（図２（ｂ）の例では１２）の撮影点を示している。本実施形態では、この複数の撮影点ＣＰで撮影された画像を「キーフレーム」といい、特徴点ＦＰ及び特徴点ＦＰに関連付けられた情報とともに地図を生成する際のデータを構成する。ここでいう「キーフレーム」は、「基準画像」の一例である。すなわち、キーフレームは、予め複数の撮影点ＣＰで撮影された複数の画像であり、特徴点に関する情報はこのキーフレームに関連付けられる。なお、以下の説明では、地図を生成する際の撮影点を「地図撮影点ＣＰ」といい、実際の駐車時の撮影点を「走行撮影点」という。また、走行撮影点において撮影された画像を「走行画像」という。

上述したように、本実施形態に係るＣＰＵ１２Ａは、ＲＯＭ１２Ｂに記憶されている自車位置推定プログラムをＲＡＭ１２Ｃに書き出して実行することにより、図３に示す各部として機能する。

図３は、本実施形態に係る自車位置推定装置１０の機能的な構成の一例を示すブロック図である。
図３に示すように、本実施形態に係る自車位置推定装置１０のＣＰＵ１２Ａは、検出部２０、算出部２２、推定部２４、及び追加部２６として機能する。また、地図情報１６Ａは、記憶部１６に予め記憶されている。

本実施形態に係る地図情報１６Ａについて説明する。地図情報１６Ａには以下の情報が含まれている。
（１）各特徴点ＦＰの３次元で表示した座標（３次元位置座標（Ｘｐ、Ｙｐ、Ｚｐ））。
（２）各キーフレームにおけるカメラの３次元位置座標（Ｘｃ、Ｙｃ、Ｚｃ）と姿勢（ロール角、ピッチ角、ヨー角）。
（３）各キーフレームにおける各特徴量。

地図の生成は、予め人が運転して走行軌跡Ｓを走行し、走行の際のカメラ１４による撮影画像から特徴点の座標（３次元位置）とキーフレームにおけるカメラ１４の位置を推定する。これらの推定は、例えばＶｉｓｕａｌ ＳＬＡＭ（Simultaneous Localization and Mapping）を用いて行うことができる。すなわち、撮影画像から、Ｖｉｓｕａｌ ＳＬＡＭを用いて特徴点の３次元位置とキーフレームのカメラ位置を推定し、これらの推定値とキーフレームにおける特徴点の特徴量を地図情報１６Ａに登録する。

より詳細には、特徴点の検出方法としては、例えば、ＯＲＢ（Oriented FAST and Rotated BRIEF）−ＳＬＡＭを適用することができる。ＯＲＢ−ＳＬＡＭは、特徴点としてコーナーを検出し、検出手法にＦＡＳＴ（Features from Accelerated Segment Test）を用いるものである。また、ＯＲＢ−ＳＬＡＭは、特徴量の記述にＯＲＢを用いる。ＯＲＢは、ＢＲＩＥＦ（Binary Robust Independent Elementary Features）をベースとして、スケール不変性と回転不変性とを有するように発展させたものである。ＯＲＢ−ＳＬＡＭについては以下の参考文献１に開示されているため、詳細な説明を省略する。

［参考文献１］Raul Mur-Artal, J.M.M.Montiel and Juan D.Tardos. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics, vol.31, no.5, pp.1147-1163, 2015.

以上まとめると、地図生成時に地図撮影点ＣＰで撮影された複数の画像であるキーフレームが撮影された際の特徴点の座標、各特徴点について登録された少なくとも１つの特徴量、カメラ１４の位置及び姿勢が関連付けられ、地図情報１６Ａとして記憶部１６に格納される。また、記憶部１６に格納された地図情報１６Ａは、後述する自車位置推定処理において参照される。

本実施形態に係る検出部２０は、走行経路に沿って自車を走行させた状態でカメラ１４により撮影された走行画像から特徴点を検出する。この特徴点の検出には、上述したように、一例としてＦＡＳＴ等が用いられる。

本実施形態に係る算出部２２は、検出部２０により検出された特徴点の特徴を示す特徴量を算出する。この特徴量には、上述したように、一例としてＯＲＢ等が用いられる。

本実施形態に係る推定部２４は、算出部２２により算出された特徴量に基づいて、複数のキーフレームから走行画像に類似する類似キーフレームを選定する。ここでいう類似キーフレームは、「類似画像」の一例である。なお、走行画像が初回（つまり、最初のフレーム）の場合には、走行画像の特徴点の特徴量から、一例として、Ｂａｇ ｏｆ Ｖｉｓｕａｌ Ｗｏｒｄｓ等を用いて、走行画像に最も類似する類似キーフレームを選定する。このＢａｇ ｏｆ Ｖｉｓｕａｌ Ｗｏｒｄｓは、画像中の多数の局所特徴をベクトル量子化しヒストグラムにしたものであり、画像同士の類似の程度を判定するツールである。Ｂａｇ ｏｆ Ｖｉｓｕａｌ Ｗｏｒｄｓによるキーフレームの抽出には一定の時間を要するが、スタート地点ＳＰに停車している際の初回の走行画像では、処理時間が比較的長いＢａｇ ｏｆ Ｖｉｓｕａｌ Ｗｏｒｄｓを用いても差し支えない。一方、走行画像の２回目以降は、前回推定したカメラ位置に最も近いキーフレームを類似キーフレームとして選定する。

次に、推定部２４は、走行画像の特徴点の特徴量と類似キーフレームの特徴点の特徴量とを比較することにより、走行画像の特徴点と類似キーフレームの特徴点との対応付けを行う。

図４は、本実施形態に係る走行画像の特徴点と類似キーフレームの特徴点との対応付けの説明に供する図である。

図４に示すように、類似キーフレームの特徴点Ｒ１〜Ｒ６に対して、走行画像の特徴点Ｐ１〜Ｐ６が対応付けられる。具体的には、特徴量としてＯＲＢを用いる場合、走行画像のＯＲＢ特徴量と類似キーフレームのＯＲＢ特徴量とを比較して、ＯＲＢ特徴量間の距離を計算する。このＯＲＢ特徴量間の距離としては、一例としてハミング距離を用いて計算される。

図５は、本実施形態に係る走行画像の特徴量と類似キーフレームの特徴量との間の距離の説明に供する図である。

図５では、走行画像のある特徴点のＯＲＢ特徴量、及び類似キーフレームのある特徴点の複数のＯＲＢ特徴量の各々を、説明を簡単にするために、一例として８ビットで示している。なお、ＯＲＢ特徴量のビット数は特に限定されない。複数のＯＲＢ特徴量の各々には、識別のためのインデックス１、２、・・・が割り当てられている。ここでいうハミング距離は、走行画像のＯＲＢ特徴量と、類似キーフレームの各ＯＲＢ特徴量（インデックス１、２、・・・）との間で値が異なるビットの数で表される。図５の例では、各インデックス１、２、・・・において値が異なるビットに下線が付与されている。つまり、インデックス１のハミング距離は「４」であり、インデックス２のハミング距離は「２」である。この場合、ハミング距離の最小値を特徴量間の距離として採用する。

ここで、走行画像が初回の場合、類似キーフレームの各特徴点には１つの特徴量のみが登録されている。つまり、初回では、各特徴点の特徴量を示すインデックス数は「１」となる。一方、走行画像の２回目以降の場合、図５に示すように、後述する追加部２６により特徴量が追加登録されるため、登録される特徴量のインデックス数が「２」以上になる場合がある。このため、２回目以降では、特徴量間で計算されたハミング距離の最小値を、特徴量間の距離として採用する。

つまり、類似キーフレームの特徴点に複数の特徴量（一例としてＯＲＢ特徴量）が登録されている場合、推定部２４は、複数の特徴量が登録されている類似キーフレームの特徴点との対応付けを行う際には、複数の特徴量の各々について、走行画像の特徴点の特徴量との間で距離（一例としてハミング距離）を計算し、計算した距離の最小値が所定値以下となる場合に、走行画像の特徴点と類似キーフレームの特徴点との対応付けを行う。これにより、特徴量間の距離が所定値以下に収まり易くなり、周囲の明るさの変化によって走行画像の特徴点の特徴量が変化した場合であっても、対応付けが可能となる。なお、特徴量としては、ＯＲＢに代えて、特徴点間のベクトルを適用してもよい。この場合、ハミング距離に代えて、ユークリッド距離が適用される。

次に、推定部２４は、上記の対応付けの結果に基づいて、走行経路における自車の位置及び姿勢を推定する。具体的には、推定部２４は、対応付けた走行画像の特徴点の位置と類似キーフレームの特徴点の位置とに基づいて、カメラ１４の位置及び姿勢を推定する。本実施形態では、走行画像の特徴点の位置と、類似キーフレームの撮影時におけるカメラ１４の位置及び姿勢に基づいて類似キーフレームの特徴点を走行画像に投影して得られる投影点の位置との差で表される投影誤差の総和が最小となるカメラ１４の位置及び姿勢を推定する。

図６は、本実施形態に係る投影誤差の説明に供する図である。
図６では、走行画像の特徴点Ｒ１〜Ｒ６の位置と、地図に対応付けられた地図座標系ＣＭにおける特徴点Ｐ１〜Ｐ６の位置（３次元位置）と、地図に対応付けられた特徴点Ｐ１〜Ｐ６の位置を走行画像に投影した投影点Ｖ１〜Ｖ６の位置と、を示している。なお、地図座標系ＣＭにおいて、Ｘ軸は水平方向を示し、Ｙ軸は奥行方向を示し、Ｚ軸は垂直方向を示す。この際、特徴点Ｐ１の位置についての投影誤差ε１は、特徴点Ｒ１の位置と投影点Ｖ１の位置との差となる。特徴点Ｐ２〜Ｐ６の各々の位置についても同様である。本実施形態に係る特徴点の対応によれば、誤対応が少ないため、カメラ１４の位置及び姿勢がより正確に推定される。

次に、推定部２４は、上記で推定したカメラ１４の位置及び姿勢を自車の代表点に換算することにより、走行経路における自車の位置及び姿勢を推定する。ここでいう自車の位置とは、車両代表点Ｘの地図上における位置をいう。本実施形態では、車両代表点Ｘとカメラ１４の相対的位置が予め分かっているので、カメラ１４の位置及び姿勢を、相対的位置関係に基づいて車両代表点Ｘの位置に換算する。

本実施形態に係る追加部２６は、推定部２４により類似キーフレームの特徴点に対応付けられた走行画像の特徴点の特徴量を、類似キーフレームであるキーフレームの特徴点の特徴量として地図情報１６Ａに追加登録する。例えば、追加部２６は、推定部２４により対応付けられた走行画像の特徴点のうち、投影誤差が所定値以下となる特徴点（これらの特徴点を「インライア」ともいう。）の特徴量を選択的に地図情報１６Ａに追加登録するようにしてもよい。図６の例では、走行画像の特徴点Ｒ１、Ｒ２、Ｒ３、Ｒ６の特徴量が追加登録される。これにより、投影誤差の大きい特徴点の特徴量が登録されることが防止される。

なお、登録数を抑制するため、追加部２６は、類似キーフレームの特徴点に登録されている複数の特徴量の数が上限数に達した場合に、登録済みの複数の特徴量及び追加する特徴量のうちの特徴量のペア毎に相互の距離（一例としてハミング距離）を計算し、計算された各特徴量との距離の中間値が最小となる特徴量を削除するようにしてもよい。

図７は、本実施形態に係る類似キーフレームの特徴点に登録されている複数の特徴量の数が上限数に達した場合に削除する特徴量の説明に供する図である。

図７の例では、上限数が「４」であり、ある特徴点について登録済みの特徴量にインデックス１〜４が割り当てられている。そして、当該特徴点に追加する特徴量のインデックスをインデックス５とする。インデックス１について、インデックス１〜５の各々との間でハミング距離を計算する。計算結果として、一例として（０、１５、９、１３、２０）が得られる。この場合、ハミング距離の中間値は、「１３」（下線付きの値）となる。同様に、インデックス２について、計算結果として、一例として（１５、０、１０、５、２９）が得られる。この場合、ハミング距離の中間値は、「１０」となる。インデックス３について、計算結果として、一例として（９、１０、０、７、１２）が得られる。この場合、ハミング距離の中間値は、「９」となる。インデックス４について、計算結果として、一例として（１３、５、７、０、３１）が得られる。この場合、ハミング距離の中間値は、「７」となる。インデックス５について、計算結果として、一例として（２０、２９、１２、３１、０）が得られる。この場合、ハミング距離の中間値は、「２０」となる。

上記より、各インデックス１〜５のハミング距離の中間値は、（１３、１０、９、７、２０）として得られる。これより、ハミング距離の中間値の最小値は「７」となる。この場合、対応するインデックス４の特徴量を削除する。

次に、図８を参照して、本実施形態に係る自車位置推定装置１０の作用について説明する。なお、図８は、本実施形態に係る自車位置推定プログラムによる処理の流れの一例を示すフローチャートである。

本実施形態に係る自車位置推定プログラムは、実行開始の指示がなされると、図２（ｂ）に示すスタート地点ＳＰに停車した状態からゴール地点ＧＰに向けて人が車両５０を運転するのに伴って、自車位置推定処理が実行される。しかしながら、これに限られず、本自車位置推定プログラムを、例えばスタート地点ＳＰからゴール地点ＧＰまで車両５０が自動運転される場合に適用してもよい。本実施形態では、予め地図情報１６Ａが生成され、記憶部１６に格納されているものとする。

なお、本実施形態では、本自車位置推定プログラムをＲＯＭ１２Ｂ等に予め記憶させておく形態としているが、これに限られない。例えば、本自車位置推定プログラムがコンピュータにより読み取り可能な可搬型の記憶媒体に記憶された状態で提供される形態、図示しないネットワークインタフェース等による通信手段を介して配信される形態等を適用してもよい。なお、可搬型の記憶媒体としては、一例として、ＣＤ−ＲＯＭ（Compact Disk Read Only Memory）、ＤＶＤ−ＲＯＭ（Digital Versatile Disk Read Only Memory）、及びＵＳＢ（Universal Serial Bus）メモリ等が挙げられる。

図８のステップ１００では、検出部２０が、車両５０が走行された状態でカメラ１４によって撮影された走行画像を取得する。この走行画像の取得は、車両５０がスタート地点ＳＰから出発した後予め定められた時間ごと、例えば一般的なビデオレートである３３ｍｓ（ミリ秒）ごとに取得する。

ステップ１０２では、検出部２０が、走行画像から特徴点を検出する。特徴点の検出には、一例としてＦＡＳＴが用いられる。

ステップ１０４では、算出部２２が、上記ステップ１０２で検出した特徴点を示す特徴量を算出する。特徴量の算出には、一例としてＯＲＢが用いられる。

ステップ１０６では、推定部２４が、走行画像の取得が初回（１回目）か否かを判定する。本実施形態に係る自車位置推定処理では、初回か２回目以降かにより、以降の処理における類似キーフレームの探索方法を変えている。走行画像の取得が初回と判定した場合（肯定判定の場合）、ステップ１０８に移行し、走行画像の取得が初回ではない、つまり２回目以降と判定した場合（否定判定の場合）、ステップ１１０に移行する。

ステップ１０８では、推定部２４が、上記ステップ１０４で計算された特徴量から、一例として、Ｂａｇ ｏｆ Ｖｉｓｕａｌ Ｗｏｒｄｓ等を用いて走行画像に最も類似するキーフレームを類似キーフレームとして選定する。

一方、ステップ１１０では、推定部２４が、前回推定したカメラ位置に最も近いキーフレームを類似キーフレームとして選定する。

ステップ１１２では、推定部２４が、図４及び図５に示すように、類似キーフレームと走行画像との間で特徴量を比較して特徴点を対応付ける（ペアリング）。

ステップ１１４では、推定部２４が、図６に示すように、走行画像の特徴点の位置と、類似キーフレームの特徴点を走行画像に投影して得られる投影点の位置との差で表される投影誤差が最小となるようにカメラ１４の位置及び姿勢を推定する。

ステップ１１６では、推定部２４が、上記ステップ１１４で推定したカメラ１４の位置及び姿勢を、自車の位置及び姿勢に換算する。ここでいう自車の位置とは、上述したように、車両代表点Ｘの地図上における位置をいう。本実施形態では、車両代表点Ｘとカメラ１４の相対的位置は予わかっているので、カメラの位置及び姿勢を、相対的位置関係に基づいて車両代表点Ｘの位置に換算する。この換算は単純な演算であるため、走行中の自車位置が短時間で推定され、車両運動制御の遅れ時間を小さくすることができる。その結果、目標経路へ高精度に車両を誘導できるため、例えば狭所に駐車することができ、駐車空間を省スペース化できる。

ステップ１１８では、追加部２６が、上記ステップ１１２で類似キーフレームの特徴点と対応付けられた走行画像の特徴点のうち、投影誤差が所定値以下となる特徴点（インライア）の特徴量を、類似キーフレームであるキーフレームの特徴点の特徴量として地図情報１６Ａに追加登録する。

ステップ１２０では、追加部２６が、全ての走行撮影点において画像の取得が終了したか否かを判定する。なお、初回の場合には本判定は否定判定となるのでステップ１００に戻り、走行画像の取得を継続する。一方、２回目以降の場合には本判定は肯定判定となるので、本自車位置推定プログラムによる一連の処理を終了する。

このように本実施形態によれば、対応付けられた走行画像の特徴点の特徴量が前回までの基準画像の特徴点の特徴量に追加され、次回以降の走行画像の特徴点の特徴量が、基準画像の追加された特徴量とも比較される。これにより、類似と判断される特徴量の範囲が広がるため、周囲の明るさの変化により特徴量が変化した場合であっても、特徴点の対応付けを行うことができ、精度良く自車位置を推定することができる。
さらに、自動駐車システムに適用した場合、目標経路へ高精度に車両を誘導できるため、狭所に駐車することができ、駐車空間を省スペース化できる。

以上、実施形態として自車位置推定装置を例示して説明した。実施形態は、コンピュータを、位置姿勢推定装置が備える各部として機能させるためのプログラムの形態としてもよい。実施形態は、このプログラムを記憶したコンピュータが読み取り可能な記憶媒体の形態としてもよい。

その他、上記実施形態で説明した自車位置推定装置の構成は、一例であり、主旨を逸脱しない範囲内において状況に応じて変更してもよい。

また、上記実施形態で説明したプログラムの処理の流れも、一例であり、主旨を逸脱しない範囲内において不要なステップを削除したり、新たなステップを追加したり、処理順序を入れ替えたりしてもよい。

また、上記実施形態では、プログラムを実行することにより、実施形態に係る処理がコンピュータを利用してソフトウェア構成により実現される場合について説明したが、これに限らない。実施形態は、例えば、ハードウェア構成や、ハードウェア構成とソフトウェア構成との組み合わせによって実現してもよい。

１０ 自車位置推定装置
１２ 制御部
１２Ａ ＣＰＵ
１２Ｂ ＲＯＭ
１２Ｃ ＲＡＭ
１２Ｄ Ｉ／Ｏ
１２Ｅ バス
１４ カメラ
１６ 記憶部
１６Ａ 地図情報
１８ 表示部
２０ 検出部
２２ 算出部
２４ 推定部
２６ 追加部
３０ 建物
５０ 車両

Claims (8)

1. 所定の走行経路に沿った複数の位置で自車に搭載された撮影部により撮影された複数の基準画像の各々に関連付けられた、前記基準画像から検出された各特徴点と、各特徴点について登録された特徴点の特徴を示す少なくとも１つの特徴量と、前記基準画像の撮影時における前記撮影部の位置及び姿勢と、を含む地図情報を記憶した記憶部と、
   前記所定の走行経路に沿って自車を走行させた状態で前記撮影部により撮影された走行画像から特徴点を検出する検出部と、
   前記検出部により検出された特徴点の特徴を示す特徴量を算出する算出部と、
   前記算出部により算出された特徴量に基づいて、前記複数の基準画像から前記走行画像に類似する類似画像を選定し、前記走行画像の特徴点の特徴量と前記類似画像の特徴点の特徴量とを比較することにより、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行い、対応付けの結果に基づいて、前記所定の走行経路における自車の位置及び姿勢を推定する推定部と、
   前記推定部により前記類似画像の特徴点と対応付けられた前記走行画像の特徴点の特徴量を、前記類似画像である基準画像の特徴点の特徴量として前記地図情報に追加する追加部と、
   を備えた自車位置推定装置。
2. 前記推定部は、複数の特徴量が登録されている前記類似画像の特徴点との対応付けを行う際には、前記複数の特徴量の各々について、前記走行画像の特徴点の特徴量との間で距離を計算し、計算した距離の最小値が予め定められた値以下となる場合に、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行う請求項１に記載の自車位置推定装置。
3. 前記追加部は、前記類似画像の特徴点に登録されている複数の特徴量の数が上限数に達した場合に、登録済みの複数の特徴量及び追加する特徴量のうちの特徴量のペア毎に相互の距離を計算し、計算された各特徴量との距離の中間値が最小となる特徴量を削除する請求項１又は２に記載の自車位置推定装置。
4. 前記地図情報は、前記複数の基準画像の各々から検出された特徴点の前記地図情報における位置を更に含み、
   前記推定部は、対応付けた前記走行画像の特徴点の位置と前記類似画像の特徴点の位置とに基づいて、前記撮影部の位置及び姿勢を推定し、推定した前記撮影部の位置及び姿勢を自車の代表点に換算することにより、前記所定の走行経路における自車の位置及び姿勢を推定する請求項１〜３のいずれか１項に記載の自車位置推定装置。
5. 前記推定部は、前記走行画像の特徴点の位置と、前記類似画像の撮影時における前記撮影部の位置及び姿勢に基づいて前記類似画像の特徴点を前記走行画像に投影して得られる投影点の位置との差で表される投影誤差の総和が最小となる前記撮影部の位置及び姿勢を推定する請求項４に記載の自車位置推定装置。
6. 前記追加部は、前記推定部により対応付けられた前記走行画像の特徴点のうち、前記投影誤差が予め定められた値以下となる特徴点の特徴量を選択的に前記地図情報に追加する請求項５に記載の自車位置推定装置。
7. 所定の走行経路に沿った複数の位置で自車に搭載された撮影部により撮影された複数の基準画像の各々に関連付けられた、前記基準画像から検出された各特徴点と、各特徴点について登録された特徴点の特徴を示す少なくとも１つの特徴量と、前記基準画像の撮影時における前記撮影部の位置及び姿勢と、を含む地図情報を記憶した記憶部を備えた自車位置推定装置による自車位置推定方法であって、
   検出部が、前記所定の走行経路に沿って自車を走行させた状態で前記撮影部により撮影された走行画像から特徴点を検出するステップと、
   算出部が、前記検出部により検出された特徴点の特徴を示す特徴量を算出するステップと、
   推定部が、前記算出部により算出された特徴量に基づいて、前記複数の基準画像から前記走行画像に類似する類似画像を選定し、前記走行画像の特徴点の特徴量と前記類似画像の特徴点の特徴量とを比較することにより、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行い、対応付けの結果に基づいて、前記所定の走行経路における自車の位置及び姿勢を推定するステップと、
   追加部が、前記推定部により前記類似画像の特徴点と対応付けられた前記走行画像の特徴点の特徴量を、前記類似画像である基準画像の特徴点の特徴量として前記地図情報に追加するステップと、
   を含む自車位置推定方法。
8. 所定の走行経路に沿った複数の位置で自車に搭載された撮影部により撮影された複数の基準画像の各々に関連付けられた、前記基準画像から検出された各特徴点と、各特徴点について登録された特徴点の特徴を示す少なくとも１つの特徴量と、前記基準画像の撮影時における前記撮影部の位置及び姿勢と、を含む地図情報を記憶した記憶部を備えた自車位置推定装置で実行される自車位置推定プログラムであって、
   前記所定の走行経路に沿って自車を走行させた状態で前記撮影部により撮影された走行画像から特徴点を検出する検出部、
   前記検出部により検出された特徴点の特徴を示す特徴量を算出する算出部、
   前記算出部により算出された特徴量に基づいて、前記複数の基準画像から前記走行画像に類似する類似画像を選定し、前記走行画像の特徴点の特徴量と前記類似画像の特徴点の特徴量とを比較することにより、前記走行画像の特徴点と前記類似画像の特徴点との対応付けを行い、対応付けの結果に基づいて、前記所定の走行経路における自車の位置及び姿勢を推定する推定部、及び
   前記推定部により前記類似画像の特徴点と対応付けられた前記走行画像の特徴点の特徴量を、前記類似画像である基準画像の特徴点の特徴量として前記地図情報に追加する追加部、
   として機能させる自車位置推定プログラム。