(57)【要約】      （修正有）【課題】より迅速にかつより精度よく自車位置を推定することができる自車位置推定装置、自車位置推定方法、および自車位置推定プログラムを提供すること。【解決手段】複数の走行画像と、複数の基準画像の各々に関連付けられた所定の走行経路Ｓに沿った環境における環境特徴点ＦＰおよび複数の基準画像の撮影時の撮影手段の位置ＣＰおよび姿勢を含む地図情報と、を照合することによって所定の走行経路上における自車の位置Ｘおよび姿勢を推定する自車位置推定装置であって、走行経路の路面勾配差を推定するとともに推定された路面勾配差から走行画像における環境特徴点ＦＰに対応する対応特徴点の検出範囲を設定する設定手段と、検出範囲で検出された対応特徴点に基づいて複数の基準画像から走行画像と最も近似する基準画像を選択するとともに環境特徴点と対応特徴点との対応付けに基づいて自車の位置および姿勢を推定する推定手段と、を含む。【選択図】図２
2019124573.tif 000002
【特許請求の範囲】
【請求項１】
所定の走行経路上において撮影手段によって撮影した複数の走行画像と、前記所定の走行経路に沿った複数の位置で前記撮影手段によって撮影された複数の基準画像の各々に関連付けられた前記所定の走行経路に沿った環境における環境特徴点および前記複数の基準画像の撮影時の前記撮影手段の位置および姿勢を含む地図情報と、を照合することによって前記所定の走行経路上における自車の位置および姿勢を推定する自車位置推定装置であって、
前記走行経路の路面勾配差を推定するとともに推定された前記路面勾配差から前記走行画像における前記環境特徴点に対応する対応特徴点の検出範囲を設定する設定手段と、
前記検出範囲で検出された前記対応特徴点に基づいて前記複数の基準画像から前記走行画像と最も近似する基準画像を選択するとともに前記環境特徴点と前記対応特徴点との対応付けに基づいて自車の位置および姿勢を推定する推定手段と、を含む
自車位置推定装置。
【請求項２】
前記地図情報には前記環境特徴点の前記地図情報における位置座標が含まれ、
前記推定手段は、対応付けられた前記環境特徴点の位置座標と前記対応特徴点の前記走行画像上の位置とに基づいて前記撮影手段の位置および姿勢を推定するとともに、推定された前記撮影手段の位置および姿勢を自車の代表点に換算して自車の位置および姿勢を推定する
請求項１に記載の自車位置推定装置。
【請求項３】
前記地図情報には、前記環境特徴点の各々の特徴を示す特徴量が含まれ、
前記推定手段は、前回対応付けられた特徴点を最も多く含む基準画像を前記最も近似する基準画像として選択するとともに、前記対応特徴点の特徴量と前記最も近似する基準画像の環境特徴点の特徴量とを比較して前記環境特徴点と前記対応特徴点とを対応付ける
請求項２に記載の自車位置推定装置。
【請求項４】
前記撮影手段は、前記撮影手段前方の予め定められた領域を撮影するように自車の予め定められた位置に固定され、
前記設定手段は、前記複数の基準画像から前記予め定められた領域に最も近似する基準画像を選択するとともに選択された基準画像に関連付けられた前記撮影手段の位置および姿勢による路面勾配と前回撮影された走行画像について推定された前記撮影手段の位置および姿勢による路面勾配との差分から前記路面勾配差を推定する
請求項１から請求項３のいずれか１項に記載の自車位置推定装置。
【請求項５】
前記設定手段は、前記路面勾配差に応じて前記検出範囲を前記走行画像の上下方向に移動させる
請求項１から請求項４のいずれか１項に記載の自車位置推定装置。
【請求項６】
前記設定手段は、前記予め定められた領域と比べて自車位置の路面勾配が大きいほど、前記走行画像上における前記検出範囲の位置をより上部に設定する
請求項５に記載の自車位置推定装置。
【請求項７】
所定の走行経路上において撮影手段によって撮影した複数の走行画像と、前記所定の走行経路に沿った複数の位置で前記撮影手段によって撮影された複数の基準画像の各々に関連付けられた前記所定の走行経路に沿った環境における環境特徴点および前記複数の基準画像の撮影時の前記撮影手段の位置および姿勢を含む地図情報と、を照合することによって前記所定の走行経路上における自車の位置および姿勢を推定する自車位置推定装置による自車位置推定方法であって、
設定手段により、前記走行経路の路面勾配差を推定するとともに推定された前記路面勾配差から前記走行画像における前記環境特徴点に対応する対応特徴点の検出範囲を設定し、
推定手段により、前記検出範囲で検出された前記対応特徴点に基づいて前記複数の基準画像から前記走行画像と最も近似する基準画像を選択するとともに前記環境特徴点と前記対応特徴点との対応付けに基づいて自車の位置および姿勢を推定する
自車位置推定方法。
【請求項８】
所定の走行経路上において撮影手段によって撮影した複数の走行画像と、前記所定の走行経路に沿った複数の位置で前記撮影手段によって撮影された複数の基準画像の各々に関連付けられた前記所定の走行経路に沿った環境における環境特徴点および前記複数の基準画像の撮影時の前記撮影手段の位置および姿勢を含む地図情報と、を照合することによって前記所定の走行経路上における自車の位置および姿勢を推定する自車位置推定装置を制御するプログラムであって、
コンピュータを
前記走行経路の路面勾配差を推定するとともに推定された前記路面勾配差から前記走行画像における前記環境特徴点に対応する対応特徴点の検出範囲を設定する設定手段と、
前記検出範囲で検出された前記対応特徴点に基づいて前記複数の基準画像から前記走行画像と最も近似する基準画像を選択するとともに前記環境特徴点と前記対応特徴点との対応付けに基づいて自車の位置および姿勢を推定する推定手段と、として機能させる
自車位置推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自車位置推定装置、自車位置推定方法、および自車位置推定プログラムに関する。
【背景技術】
【０００２】
移動体に搭載されたカメラ等の撮影部によって撮影された撮影画像に基づいて、３次元空間上における移動体の位置を推定する技術が提案されている。
【０００３】
従来、自車位置の推定に関連する技術として、例えば特許文献１に開示された自動運転制御装置が知られている。特許文献１に係る自動運転制御装置は、車両が運転者に運転される登録モードにおいて車両の周囲環境を撮像した画像である登録時画像に基づき、車両を自動運転するための自動運転情報を生成する自動運転情報登録部と、車両が自動運転される自動運転モードにおいて車両の周囲環境を撮像した画像である自動運転時画像と、自動運転情報とに基づき、車両を自動運転する自動運転制御部と、を有し、自動運転情報登録部は、登録時画像に基づき、車両の周囲環境に存在する候補特徴点を抽出する候補特徴点抽出部と、車両の移動中に撮像された複数の登録時画像に基づき、候補特徴点のうち、車両の目的箇所の周囲に固定して配置された構造物であると判断した候補特徴点を、特徴点として選定し、所定の原点座標に対する特徴点の位置の情報である自動運転情報を生成する自動運転情報生成部と、を有し、自動運転制御部は、自動運転時画像と自動運転情報とに基づき、原点座標に対する車両の位置の情報である車両位置情報を算出する車両位置算出部と、車両位置情報に基づき、車両を目的箇所まで自動運転する自動運転実行制御部と、を有している。すなわち、特許文献１に係る自動運転制御装置では、事前に走行した際の画像から構造物の特徴点を抽出し、その３次元位置を推定して地図に登録し、自動運転時に画像から構造物の特徴点を抽出し、地図に登録された構造物の特徴点と照合することにより自車位置を推定している。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１７−１３８６６４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、車両等の移動体の移動速度に柔軟に対応しつつ自車位置の推定を行うためには、極力短時間で推定処理を行うことが求められる。しかしながら、特許文献１に係る自動運転制御装置は、画像全体から特徴点を検出するため、特徴点の検出を短時間で行うことが困難である。さらに、検出した特徴点が多くなるため、構造物の特徴点を抽出するのに時間を要する。その結果、自車位置を短時間で推定することが困難であるという課題があった。
【０００６】
また、撮影画像から抽出した特徴点に基づく自車位置の推定を行う場合には、画像全体における特徴点の分布についても配慮する必要がある。つまり、画像全体における特徴点の分布が偏った場合、画像全体について特徴点の抽出処理を行うと無駄が生じ、その結果、自車位置の推定を行う処理時間の短縮にも限界がある。特許文献１では、このような問題に関して検討されていない。
【０００７】
本発明は、上述した課題を解決するためになされたものであり、より迅速にかつより精度よく自車位置を推定することができる自車位置推定装置、自車位置推定方法、および自車位置推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するために、請求項１に記載の自車位置推定装置は、所定の走行経路上において撮影手段によって撮影した複数の走行画像と、前記所定の走行経路に沿った複数の位置で前記撮影手段によって撮影された複数の基準画像の各々に関連付けられた前記所定の走行経路に沿った環境における環境特徴点および前記複数の基準画像の撮影時の前記撮影手段の位置および姿勢を含む地図情報と、を照合することによって前記所定の走行経路上における自車の位置および姿勢を推定する自車位置推定装置であって、前記走行経路の路面勾配差を推定するとともに推定された前記路面勾配差から前記走行画像における前記環境特徴点に対応する対応特徴点の検出範囲を設定する設定手段と、前記検出範囲で検出された前記対応特徴点に基づいて前記複数の基準画像から前記走行画像と最も近似する基準画像を選択するとともに前記環境特徴点と前記対応特徴点との対応付けに基づいて自車の位置および姿勢を推定する推定手段と、を含むものである。
【０００９】
また、請求項２に記載の発明は、請求項１に記載の発明において、前記地図情報には前記環境特徴点の前記地図情報における位置座標が含まれ、前記推定手段は、対応付けられた前記環境特徴点の位置座標と前記対応特徴点の前記走行画像上の位置とに基づいて前記撮影手段の位置および姿勢を推定するとともに、推定された前記撮影手段の位置および姿勢を自車の代表点に換算して自車の位置および姿勢を推定するものである。
【００１０】
また、請求項３に記載の発明は、請求項２に記載の発明において、前記地図情報には、前記環境特徴点の各々の特徴を示す特徴量が含まれ、前記推定手段は、前回対応付けられた特徴点を最も多く含む基準画像を前記最も近似する基準画像として選択するとともに、前記対応特徴点の特徴量と前記最も近似する基準画像の環境特徴点の特徴量とを比較して前記環境特徴点と前記対応特徴点とを対応付けるものである。
【００１１】
また、請求項４に記載の発明は、請求項１から請求項３のいずれか１項に記載の発明において、前記撮影手段は、前記撮影手段前方の予め定められた領域を撮影するように自車の予め定められた位置に固定され、前記設定手段は、前記複数の基準画像から前記予め定められた領域に最も近似する基準画像を選択するとともに選択された基準画像に関連付けられた前記撮影手段の位置および姿勢による路面勾配と前回撮影された走行画像について推定された前記撮影手段の位置および姿勢による路面勾配との差分から前記路面勾配差を推定するものである。
【００１２】
また、請求項５に記載の発明は、請求項１から請求項４のいずれか１項に記載の発明において、前記設定手段は、前記路面勾配差に応じて前記検出範囲を前記走行画像の上下方向に移動させるものである。
【００１３】
また、請求項６に記載の発明は、請求項５に記載の発明において、前記設定手段は、前記予め定められた領域と比べて自車位置の路面勾配が大きいほど、前記走行画像上における前記検出範囲の位置をより上部に設定するものである。
【００１４】
上記目的を達成するために、請求項７に記載の自車位置推定方法は、所定の走行経路上において撮影手段によって撮影した複数の走行画像と、前記所定の走行経路に沿った複数の位置で前記撮影手段によって撮影された複数の基準画像の各々に関連付けられた前記所定の走行経路に沿った環境における環境特徴点および前記複数の基準画像の撮影時の前記撮影手段の位置および姿勢を含む地図情報と、を照合することによって前記所定の走行経路上における自車の位置および姿勢を推定する自車位置推定装置による自車位置推定方法であって、設定手段により、前記走行経路の路面勾配差を推定するとともに推定された前記路面勾配差から前記走行画像における前記環境特徴点に対応する対応特徴点の検出範囲を設定し、推定手段により、前記検出範囲で検出された前記対応特徴点に基づいて前記複数の基準画像から前記走行画像と最も近似する基準画像を選択するとともに前記環境特徴点と前記対応特徴点との対応付けに基づいて自車の位置および姿勢を推定するものである。
【００１５】
上記目的を達成するために、請求項８記載の自車位置推定プログラムは、所定の走行経路上において撮影手段によって撮影した複数の走行画像と、前記所定の走行経路に沿った複数の位置で前記撮影手段によって撮影された複数の基準画像の各々に関連付けられた前記所定の走行経路に沿った環境における環境特徴点および前記複数の基準画像の撮影時の前記撮影手段の位置および姿勢を含む地図情報と、を照合することによって前記所定の走行経路上における自車の位置および姿勢を推定する自車位置推定装置を制御するプログラムであって、コンピュータを前記走行経路の路面勾配差を推定するとともに推定された前記路面勾配差から前記走行画像における前記環境特徴点に対応する対応特徴点の検出範囲を設定する設定手段と、前記検出範囲で検出された前記対応特徴点に基づいて前記複数の基準画像から前記走行画像と最も近似する基準画像を選択するとともに前記環境特徴点と前記対応特徴点との対応付けに基づいて自車の位置および姿勢を推定する推定手段と、として機能させるものである。
【発明の効果】
【００１６】
本発明によれば、より迅速にかつより精度よく自車位置を推定することができる自車位置推定装置、自車位置推定方法、および自車位置推定プログラムを提供することができるという効果を奏する。
【図面の簡単な説明】
【００１７】
【図１】実施の形態に係る、（ａ）はカメラを搭載した車両を示す図、（ｂ）は自車位置推定装置の構成の一例を示すブロック図である。
【図２】（ａ）は駐車経路における特徴点を示す図、（ｂ）はキーフレームのカメラ位置と特徴点の位置を示す図である。
【図３】（ａ）は路面勾配によるカメラピッチ角の違いを説明する図、（ｂ）は特徴点検出範囲を説明する図、（ｃ）は特徴点検出範囲の上端、下端と路面勾配との関係を説明する図である。
【図４】（ａ）は初回の特徴点検出範囲を示す図、（ｂ）は路面勾配差＞０の場合の特徴点検出範囲を示す図である。
【図５】実施の形態に係る自車位置推定処理ログラムの処理の流れを示すフローチャートである。
【図６】投影誤差を説明する図である。
【発明を実施するための形態】
【００１８】
以下、図面を参照して、本発明の実施の形態について詳細に説明する。なお、以下の説明では、移動体として車両を例にとり、該車両が自車の位置を推定する際の周囲環境を駐車場とし、走行経路として駐車場外部から駐車地点までの経路を例示して説明する。
【００１９】
図１から図６を参照して、本実施の形態に係る自車位置推定装置、自車位置推定方法、および自車位置推定プログラムについて説明する。
【００２０】
図１（ａ）は、本実施の形態に係る自車位置推定装置１０を搭載した車両５０を示している。図１（ａ）に示すように、車両５０は、本実施の形態に係る自車位置の推定処理（以下、「自車位置推定処理」）において使用する、撮影手段としてのカメラ１４を備えている。本実施の形態に係るカメラ１４は、車両５０の後部のトランク等に設けられ、車両後方を撮影する。カメラ１４は、例えば、車幅方向の略中央部付近に設けられ、かつカメラ１４の光軸が水平方向より若干下側を向くように配置されている。なお、本実施の形態では、カメラ１４を車両５０の後部に設ける形態を例示して説明するが、これに限られず、環境等に応じて例えば前部に設けてもよい。また、本実施の形態では、カメラ１４として単眼カメラを例示して説明するが、これに限られず他の形態のカメラ、例えばステレオカメラ等であってもよい。
【００２１】
図１（ｂ）に示すように、本実施の形態に係る自車位置推定装置１０は、制御部１２、カメラ１４、記憶部１６、および表示部１８を含んで構成されている。
【００２２】
制御部１２は、自車位置を推定するための演算等を行う部位である。制御部１２は、一例としてＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）１２Ａ、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）１２Ｂ、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）１２Ｃ、入出力インタフェースであるＩ／Ｏ１２Ｄ等を含んで構成されている。ＣＰＵ１２Ａ、ＲＯＭ１２Ｂ、ＲＡＭ１２Ｃ、およびＩ／Ｏ１２Ｄの各々はバス１２Ｅにより相互に接続されている。
【００２３】
ＣＰＵ１２Ａは、自車位置推定装置１０の全体を統括、制御し、ＲＯＭ１２Ｂは、自車位置推定装置１０の制御プログラムや、後述する自車位置推定処理プログラム等を予め記憶する記憶手段であり、ＲＡＭ１２Ｃは、制御プログラム等のプログラムの実行時のワークエリア等として用いられる記憶手段である。制御部１２が本発明に係る「設定手段」、および「推定手段」に相当する。
【００２４】
ＲＯＭ１２Ｂには、本実施の形態で用いる地図を生成するための地図生成プログラムや、自車位置を推定する自車位置推定処理プログラムを含む各種プログラムやデータ等が記憶されている。ＲＯＭ１２Ｂに記憶されたプログラムをＲＡＭ１２Ｃに展開してＣＰＵ１２Ａが実行することにより、地図の生成や、自車位置の推定が行われる。
【００２５】
制御部１２には、Ｉ／Ｏ１２Ｄを介して、カメラ１４、記憶部１６、および表示部１８が接続されている。カメラ１４により撮影された画像は、Ｉ／Ｏ１２Ｄを介して制御部１２に取り込まれる。
【００２６】
また、記憶部１６には、本実施の形態に係る自車位置推定処理に用いる後述の地図等が格納されている。記憶部１６の形態に特に制限はないが、一例としてＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ  Ｓｔａｔｅ  Ｄｒｉｖｅ）、フラッシュメモリ等を用いることができる。なお、記憶部１６は制御部１２内に設けてもよいし、外部接続可能としてもよい。また、記憶部１６には制御部１２の制御によって生成した地図の他に、地図生成処理プログラムや自車位置推定処理プログラムを記憶してもよい。
【００２７】
表示部１８は、カメラ１４が撮影した画像等を表示する部位である。表示部１８の形態に制限はないが、例えば、液晶モニタ、ＣＲＴ（Ｃａｔｈｏｄｅ  Ｒａｙ  Ｔｕｂｅ）モニタ、ＦＰＤ（Ｆｌａｔ  Ｐａｎｅｌ  Ｄｉｓｐｌａｙ）モニタ等が用いられる。
【００２８】
図２（ａ）は本実施の形態で想定する駐車場の様子（以下、「環境」）を示す図である。本実施の形態では、車両５０の走行経路として、図２（ａ）に示す「スタート」の位置（出発地点）から、「ゴール」の位置（駐車地点）までを想定している。図２（ａ）に示す符号「ＦＰ」は、以下で説明する特徴点の位置を示している。車両５０の自車位置を推定する場合には、所定の大きさを有する車両５０内の位置（以下、「車両代表点Ｘ」）を予め決めておく必要があるが、図２（ｂ）に示すように、本実施の形態ではこの車両代表点Ｘを後輪の中央の点としている。しかしながら、代表点の位置はこれに限られず、例えば車両５０の重心の位置としてもよい。
【００２９】
図２（ｂ）は、図２（ａ）に示す環境の平面図である。図２（ｂ）に示すように、カメラ１４を搭載した車両５０は、スタート地点ＳＰからゴール地点ＧＰまで走行する。本実施の形態に係る自車位置推定処理では、実際の走行の前に、カメラ１４で環境を撮影し、撮影された画像に関連付けられた所定の付随データを地図として予め生成しておくが、符号「Ｓ」は、該生成の際の車両５０の走行軌跡を示している。実際の駐車においては、基本的に走行軌跡Ｓに沿って走行する。以下では、走行軌跡Ｓと区別して、実際の駐車時（走行時）に走行する経路を「走行経路」という場合がある。
【００３０】
図２（ｂ）に示す小円は、特徴点ＦＰの位置を示している。本実施の形態において、「特徴点」とは、建物の凹凸に基づく陰影、壁面の模様等、撮影画像において輝度の濃淡差が所定値よりも大きい点をいう。従って、特徴点は、図２（ｂ）に示すように、環境における建物３０の壁面ＷＳ、あるいは建物３０の角部に現れることが多く、また、１つの地図において複数の特徴点ＦＰが選定される。さらに、本実施の形態では、複数の特徴点ＦＰの各々を区別するために、個々の特徴点ＦＰに「特徴量」を対応付けている。本実施の形態に係る「特徴量」とは、一例として特徴点近傍における輝度の濃淡差のパターンをいう。
【００３１】
図２（ｂ）中の符号「ＣＰ」は、地図を生成する際、カメラ１４による撮影が行われた複数（図２（ｂ）の例では１２）の撮影点を示している。本実施の形態では、この複数の撮影点ＣＰで撮影された画像を「キーフレーム」といい、特徴点ＦＰおよび特徴点ＦＰに関連付けられた情報とともに地図を生成する際のデータを構成する。すなわち、キーフレームは、予め複数の撮影点ＣＰで撮影された複数の画像であり、特徴点に関する情報はこのキーフレームに関連付けられる。なお、以下の説明では、地図を生成する際の撮影点を「地図撮影点ＣＰ」といい、実際の駐車時の撮影点を「走行撮影点」という。また、走行撮影点において撮影された画像を「走行画像」という。「キーフレーム」が本発明に係る「基準画像」に相当し、キーフレームに関連付けられた特徴点ＦＰが本発明に係る「環境特徴点」に相当する。これに対し、走行画像に関連付けられた特徴点ＦＰが本発明に係る「対応特徴点」に相当する。
【００３２】
次に、本実施の形態に係る「地図」について説明する。地図には以下の情報が含まれている。
（１）各特徴点ＦＰの３次元で表示した座標（３次元位置座標（Ｘｐ、Ｙｐ、Ｚｐ））。
（２）各キーフレームにおけるカメラの３次元位置座標（Ｘｃ、Ｙｃ、Ｚｃ）と姿勢（ロール角、ピッチ角、ヨー角）。
（３）各キーフレームにおける各特徴量。
地図の生成は、予め人が運転して走行軌跡Ｓを走行し、走行の際のカメラ１４による撮影画像から特徴点の座標（３次元位置）とキーフレームにおけるカメラ１４の位置を推定する。これらの推定は、例えばＶｉｓｕａｌ  ＳＬＡＭ（Simultaneous Localization and Mapping）を用いて行うことができる。すなわち、撮影画像から、Ｖｉｓｕａｌ  ＳＬＡＭを用いて特徴点の３次元位置とキーフレームのカメラ位置を推定し、これらの推定値とキーフレームにおける特徴点の特徴量を地図に登録する。
【００３３】
より詳細には、特徴点の抽出方法としては、例えば、ＯＲＢ−ＳＬＡＭを適用することができる。ＯＲＢ−ＳＬＡＭは、特徴点としてコーナーを検出し、検出手法にＦＡＳＴ（Features from Accelerated Segment Test）を用いるものである。また、ＯＲＢ−ＳＬＡＭは、特徴量の記述にＯＲＢ（Oriented FAST and Rotated BRIEF）を用いる。ＯＲＢは、ＢＲＩＥＦ（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.なお、地図生成時には、計算時間に対する制約が厳しくないため、特徴点検出範囲を後述するように制限することなく、撮影画像全体に設定してもよい。
【００３４】
以上まとめると、地図生成時に地図撮影点ＣＰで撮影された複数の画像であるキーフレームが撮影された際の特徴点の座標、特徴量、カメラ１４の位置および姿勢が関連付けられ、地図情報として記憶部１６等の記憶手段に格納される。また、記憶部１６等に格納された地図は、後述する自車位置推定処理において参照される。
【００３５】
ここで、図３、図４を参照して本実施の形態に係る特徴点検出範囲の設定について説明する。上述したように、車両５０の走行にともなってカメラ１４で撮影される画像（キーフレーム、走行画像）における特徴点ＦＰの分布は、車両５０が走行する路面の状態等の要因により変化する場合がある。本実施の形態では、撮影画像全体における特徴点の分布に影響を及ぼす要因として、自車位置の推定の対象となる車両５０が走行する路面の勾配を想定している。すなわち、路面勾配に起因して撮影画像全体における特徴点の分布に偏りが生ずる蓋然性が高い場合には、特徴点の検出範囲を変更するように構成している。
【００３６】
図３（ａ）は、勾配のある路面Ｒを走行する車両５０を示している。図３（ａ）に示す例は、自車前方が上り坂になっている例であるが、むろん自車前方が下り坂になっている場合も考え方は同じである。図３（ａ）中、位置Ｐ０は基準位置を示し、位置Ｐ１は登攀途中の位置を示し、各々の位置に車両５０が位置している。ここで、位置Ｐ０にある車両５０のカメラ１４のピッチ角をθｆ、位置Ｐ１にある車両のカメラ１４のピッチ角をθ１とする。本実施の形態に係るピッチ角θとは、水平方向から測ったカメラ１４の光軸の傾き角度であり、水平方向から地面に近づく方向を正方向と定義する。さらに、（θ１−θｆ）で路面勾配差Δθを定義する（Δθ＝（θ１−θｆ））。この定義のもとでは、路面勾配差Δθは図３（ａ）に示す角度に等しい。なお、上述したように、本実施の形態ではカメラ１４の光軸が水平方向より若干下側を向くように配置されているが、理解のしやすさから、以下では平坦な路面Ｒにおけるカメラ１４の光軸の方向が水平方向であるとして説明する。
【００３７】
図３（ｂ）は、撮影された画像Ｇにおける特徴点検出範囲Ａｓを模式的に示している。
本実施の形態に係る特徴点検出範囲Ａｓとは、画像Ｇにおいて特徴点ＦＰの探索範囲として指定された領域をいう。すなわち、特徴点ＦＰの検出は画像Ｇの全体ではなく、指定され限定された領域で行う。図３（ｂ）に示すように、本実施の形態では、特徴点検出範囲Ａｓの指定を、画像Ｇの上下位置における上端Ｇｕおよび下端Ｇｄを指定して行う。なお、特徴点検出範囲Ａｓの指定方法はこれに限られず、例えば上端Ｇｕと下端Ｇｄとの中点Ｇｃと画像幅Ｗｓで指定してもよい。図３（ｂ）は、車両５０が略平坦な路面の基準位置にある場合の特徴点検出範囲Ａｓ０を示しており、上端がＧｕ０、下端がＧｄ０とされている。
【００３８】
図３（ｃ）は、路面勾配差Δθと、特徴点検出範囲Ａｓとの関係を示している。図３（ｃ）は自車前方が上り坂である場合の、路面勾配差ΔθがΔθ１からΔθ２の範囲の特徴点検出範囲を示している。路面勾配差Δθ＝０の点は、路面勾配差が略ゼロで路面Ｒが平坦である位置を示している。すなわち、Δθ＝０を境に路面勾配差Δθの正負が切り替わる（つまり、Δθ１＜０、Δθ２＞０である）。路面勾配差Δθ＝０における特徴点検出範囲Ａｓ０は、上端Ｇｕ０と下端Ｇｄ０との間の範囲となっている。なお、図３（ｃ）では、路面勾配差ΔθがΔθ１からΔθ２の範囲で、特徴点検出範囲Ａｓの範囲の幅が一定である場合を例示しているが、むろん特徴点検出範囲Ａｓの幅は路面勾配差Δθに応じて変えるようにしてもよい。
【００３９】
図４は、実際の画像における特徴点検出範囲Ａｓの一例を示している。図４（ａ）は、車両５０が、例えば路面勾配差Δθが略ゼロの平坦な路面Ｒに位置している場合の特徴点検出範囲の例である。図４（ａ）に示すように、この場合の特徴点検出範囲Ａｓ０は、上端Ｇｕ０が画像Ｇの上端から予め定められた長さの位置とされ、下端Ｇｄ０が画像Ｇの下端から予め定められた長さの位置とされている。換言すれば、特徴点検出範囲Ａｓ０では、画像Ｇの上端の予め定められた領域と、下端の予め定められた領域とにおいて特徴点ＦＰの検出が行われない。
【００４０】
一方、図４（ｂ）は、車両５０が、路面勾配差ΔθがΔθ＞０の路面Ｒに位置している場合の特徴点検出範囲Ａｓ１の例である。図４（ｂ）に示すように、この場合の特徴点検出範囲Ａｓ１は、上端Ｇｕ１がほぼ画像Ｇの上端とされ、下端Ｇｄ１が画像Ｇの上下方向の中央位置近辺まで上げられている。換言すれば、特徴点検出範囲Ａｓ１では、画像Ｇの上端から上端Ｇｕ１の範囲、下端Ｇｄ１から画像Ｇの下端までの範囲で特徴点ＦＰの検出が行われない。なお、特徴点検出範囲Ａｓ０とＡｓ１との関係を図３（ａ）を用いて説明すると、車両５０が位置Ｐ０にある場合の特徴点検出範囲が特徴点検出範囲Ａｓ０であり、車両５０が位置Ｐ１にある場合の特徴点検出範囲が特徴点検出範囲Ａｓ１であると考えることができる。
【００４１】
次に、図５を参照して、本実施の形態に係る自車位置推定方法について説明する。図４は、自車位置を推定する自車位置推定処理プログラムの流れを示すフローチャートである。図５に示す自車位置推定処理プログラムは、実行開始の指示がなされると、制御部１２のＣＰＵ１２ＡがＲＯＭ１２Ｂ等の記憶手段から本自車位置推定処理プログラムを読み込み、ＲＡＭ１２Ｃ等の記憶手段に展開して実行する。本実施の形態では、予め地図が生成され、記憶部１６に格納されているものとする。また、図２（ｂ）に示すスタート地点ＳＰに停車した状態からゴール地点ＧＰに向けて人が車両５０を運転するのに伴って、自車位置推定処理が実行される。しかしながら、これに限られず、本自車位置推定処理プログラムを、例えばスタート地点ＳＰからゴール地点ＧＰまで車両５０が自動運転される場合に適用してもよい。
【００４２】
なお、本実施の形態では、本自車位置推定処理プログラムをＲＯＭ１２Ｂ等に予め記憶させておく形態としているが、これに限られない。例えば、本自車位置推定処理プログラムがコンピュータにより読み取り可能な可搬型の記憶媒体に記憶された状態で提供される形態、図示しないネットワークインタフェース等による通信手段を介して配信される形態等を適用してもよい。
【００４３】
ステップＳ１００では、カメラ１４によって撮影を行い、走行画像を取得する。この走行画像の取得は、車両５０がスタート地点ＳＰから出発した後予め定められた時間ごと、例えば一般的なビデオレートである３３ｍｓ（ミリ秒）ごとに取得する。
【００４４】
ステップＳ１０２では、走行画像の取得が初回（１回目）か否か判定する。本実施の形態に係る自車位置推定処理では、初回か２回目以降かにより、以降の処理における特徴点検出範囲設定方法とキーフレーム探索方法を変えている。ステップＳ１０２が肯定判定となった場合にはステップＳ１０４に移行し、否定判定となった場合にはステップＳ１１２に移行する。
【００４５】
ステップＳ１０４では、特徴点検出範囲Ａｓを予め定められた初期特徴点検出範囲に設定する。初回の走行画像取得では、未だ路面勾配差Δθが推定されていないからである。
初期特徴点検出範囲の設定方法に制限はないが、一例として図３（ｃ）に示す、画像Ｇの上下位置中央に配置された特徴点検出範囲Ａｓ０とする。先述したように、特徴点検出範囲Ａｓ０は、画像Ｇにおける検出範囲が制限されている。初期特徴点検出範囲をこのように限定することにより、路面上等の互いに区別しにくい特徴点が検出されることを抑制し、建物上等の互いに区別しやすい特徴点をより多く検出することができる。
【００４６】
ステップＳ１０６では、特徴点の検出、検出した特徴点の特徴量の計算を実行する。特徴点の検出には先述したＦＡＳＴ、特徴量の計算にはＯＲＢを使用すればよい。
【００４７】
ステップＳ１０８では、ステップＳ１０６で計算された特徴量から、例えばＢａｇ  ｏｆ  Ｖｉｓｕａｌ  Ｗｏｒｄｓ等を用いて走行画像に最も類似するキーフレームＫＦ０を選定する。Ｂａｇ  ｏｆ  Ｖｉｓｕａｌ  Ｗｏｒｄｓは、画像中の多数の局所特徴をベクトル量子化しヒストグラムにしたものであり、画像同士の類似の程度を判定するツールである。Ｂａｇ  ｏｆ  Ｖｉｓｕａｌ  Ｗｏｒｄｓによるキーフレームの抽出には一定の時間を要するが、本処理は、スタート地点ＳＰに停車している際の初回の走行画像の取得であるため、処理時間が比較的長いＢａｇ  ｏｆ  Ｖｉｓｕａｌ  Ｗｏｒｄｓを用いても差し支えない。
【００４８】
ステップＳ１１０では、キーフレームＫＦ０と走行画像との間で特徴量を比較して特徴点を対応付ける（ペアリング）。特徴点検出範囲Ａｓ０から検出した特徴点は互いに区別しやすい特徴点を多く含むため、より正確に対応付けできる。
【００４９】
次のステップＳ１２４では、走行画像とキーフレームＫＦ０とで対応付けた特徴点の３次元位置（地図上の位置）と走行画像上の位置から、投影誤差が最小となるようにカメラの位置および姿勢を推定する。ここで、図６を参照して、本実施の形態に係る投影誤差について説明する。図６に示すａ、ｂ、ｃは走行画像上の特徴点の位置、Ａ、Ｂ、Ｃは地図上に対応付けられた地図座標系ＣＭにおける特徴点の位置、ａ’、ｂ’、ｃ’は、地図上に対応付けられた特徴点の位置Ａ、Ｂ、Ｃを走行画像に投影した位置を示している。この際位置Ａについての投影誤差εａは、位置ａと位置ａ’との差分となる。位置Ｂ、Ｃについても同様である。本実施の形態に係る特徴点の対応によれば、誤対応が少ないため、カメラの位置および姿勢がより正確に推定される。
【００５０】
次のステップＳ１２６では、ステップＳ１２４で推定したカメラの位置および姿勢を、自車の位置および姿勢に換算する。自車の位置とは、車両代表点Ｘの地図上のおける位置をいう。本実施の形態では、車両代表点Ｘとカメラ１４の相対的位置は予わかっているので、カメラの位置および姿勢を、該相対的位置関係に基づいて車両代表点Ｘの位置に換算する。該換算は単純な演算であるため、走行中の自車位置が短時間で推定され、車両運動制御の遅れ時間を小さくすることができる。その結果、目標経路へ高精度に車両を誘導できるため、例えば狭所に駐車することができ、駐車空間を省スペース化できる。
【００５１】
ステップＳ１２８では、全ての走行撮影点において画像の取得が終了したか否か判定する。初回の場合には本判定は否定判定となるのでステップＳ１００に戻り、走行画像の取得を継続する。
【００５２】
一方、ステップＳ１１２では、前回推定したカメラ１４の位置から所定の距離、例えば５ｍだけ前方の位置に最も近いキーフレームＫＦｆを探索し、選定する。より具体的には、画像中心部に映っている路面Ｒまでの距離が、カメラ１４の取付角度から予めわかっているので、その位置に最も近いキーフレームを選定する。
【００５３】
ステップＳ１１４では、キーフレームＫＦｆのカメラピッチ角θｆと前回推定したカメラピッチ角θ1との偏差（θ１-θｆ）を路面勾配差Δθｆとする。
【００５４】
ステップＳ１１６では、ステップＳ１１４で設定された路面勾配差Δθｆから、図３（ｃ）に示す図を用いて特徴点検出範囲Ａｓを設定する。すなわち、路面勾配差Δθｆの大きさに応じて特徴点検出範囲Ａｓの上端Ｇｕ、下端Ｇｄを設定する。そのため、路面勾配差Δθによらず、路面上等の互いに区別しにくい特徴点が検出されることを抑制し、建物上等の互いに区別しやすい特徴点をより多く検出することができる（図４（ｂ）参照）。
また、特徴点検出範囲Ａｓを限定することにより、特徴点の検出数が抑えられるため、特徴量の算出と特徴点の対応付けに要する計算量を低減できる。したがって、計算資源が限られているシステムにおいても、短時間で自車位置の推定が可能となる。
【００５５】
ステップＳ１１８では、ステップＳ１０６と同様の方法で、特徴点の検出、検出した特徴点の特徴量の計算を実行する。
【００５６】
ステップＳ１２０では、前回対応付けた特徴点を最も多く含むキーフレームＫＦ１を選定する。
【００５７】
ステップＳ１２２では、キーフレームＫＦ１と走行画像との間で特徴量を比較して特徴点を対応付ける（ペアリング）。本実施の形態では、路面勾配差Δθによらず、比較対象の特徴点として互いに区別しやすい特徴点を多く含むため、より正確な対応付けが可能となる。
【００５８】
次に、上記で説明したステップＳ１２４〜Ｓ１２８を実行する。ただし、ステップＳ１２４ではキーフレームＫＦ０をＫＦ１に読み替える。その後、ステップＳ１２８で肯定判定となった場合に、本自車位置推定処理プログラムを終了する。
【００５９】
以上詳述したように、本実施の形態に係る自車位置推定装置、自車位置推定方法、および自車位置推定プログラムによれば、計算資源が限られるシステムにおいても、自車位置を短時間で推定できる。本実施の形態では、画像からの特徴点の検出範囲を限定しているため、検出時間を低減できるからである。さらに、特徴点の検出数が抑えられるため、特徴量の算出と特徴点の対応付けに要する計算量を低減できる。従って、計算資源が限られているシステムにおいても、短時間で自車位置の推定が可能となっている。
【００６０】
また、本実施の形態に係る自車位置推定装置、自車位置推定方法、および自車位置推定プログラムによれば、路面勾配変化がある場合でも、自車位置を高精度に推定できる。本実施の形態では、カメラ前方と比べた自車位置の路面勾配に応じて走行画像における特徴点検出範囲を変更することにより、建物上等の互いに区別しやすい特徴点をより多く検出するとともに、路面上等の互いに区別しにくい特徴点をより少なく検出するため、キーフレームと走行画像との間で特徴点の対応付けをより正確にできるからである。従って、路面勾配変化がある場合でも、必要最小限の検出数でより高精度に自車位置を推定が可能となっている。
【００６１】
さらに、自動駐車システムに適用した場合、目標経路へ高精度に車両を誘導できるため、狭所に駐車することができ、駐車空間を省スペース化できる。本実施の形態では、走行中の自車位置を短時間で推定できるため、車両運動制御の遅れ時間を小さくできる。したがって、目標経路へ高精度に車両を誘導できるため、狭所に駐車することができ、駐車空間を省スペース化できる。
【符号の説明】
【００６２】
１０    自車位置推定装置
１２    制御部
１２Ａ  ＣＰＵ
１２Ｂ  ＲＯＭ
１２Ｃ  ＲＡＭ
１２Ｄ  Ｉ／Ｏ
１２Ｅ  バス
１４    カメラ
１６    記憶部
１８    表示部
３０    建物
５０    車両
ＣＰ    地図撮影点
ＦＰ    特徴点
Ｇ      画像
Ｒ      路面
Ｓ      走行軌跡
ＳＰ    スタート地点
ＧＰ    ゴール地点
ＷＳ    壁面
Ｘ      車両代表点
θｆ、θ  ピッチ角
Δθ    路面勾配差
【図１】
2019124573.tif 000003
【図２】
2019124573.tif 000004
【図３】
2019124573.tif 000005
【図４】
2019124573.tif 000006
【図５】
2019124573.tif 000007
【図６】
2019124573.tif 000008