(57)【要約】【課題】撮影部の位置及び姿勢を安定的に推定可能な自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、並びに自車位置推定プログラムを提供することを目的とする。【解決手段】車両の舵角を検出する舵角センサの検出結果を用いて、カメラによって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出し（１００〜１０４）、抽出した特徴点と、前時刻の撮影画像の特徴点との対応付けに基づいて、車両の位置及び姿勢を推定し（１０６〜１１２）、推定結果に基づいて、特徴点の３次元位置を推定して地図に登録することにより環境地図を生成する（１１４）。【選択図】図５
2018205950.tif 000002
【特許請求の範囲】
【請求項１】
車両の移動方向を撮影する撮影部と、
車両の舵角を検出する検出部の検出結果を用いて、前記撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出すると共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくする抽出部と、
前記抽出部が抽出した特徴点と、他の撮影画像の特徴点との対応付けに基づいて、車両の位置及び姿勢を推定し、推定結果に基づいて、特徴点の３次元位置を求めて、局所的な環境地図を生成する生成部と、
を含む自車位置推定用環境地図生成装置。
【請求項２】
前記生成部は、局所的な環境地図として、車両の走行経路に制約がある局所空間の環境地図を生成する請求項１に記載の自車位置推定用環境地図生成装置。
【請求項３】
車両の移動方向を撮影する撮影部と、
車両の舵角を検出する検出部の検出結果を用いて、前記撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出すると共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくする抽出部と、
前記抽出部が抽出した特徴点と、予め生成された環境地図上の対応する特徴点とに基づいて、車両の位置及び姿勢を推定し、推定結果に基づいて、自車両の位置を推定する推定部と、
を含む自車位置推定装置。
【請求項４】
車両の舵角を検出する検出部の検出結果を用いて、車両の移動方向を撮影する撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出する共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくし、
抽出した特徴点と、他の撮影画像の特徴点との対応付けに基づいて、車両の位置及び姿勢を推定し、
推定結果に基づいて、特徴点の３次元位置を求めて、局所的な環境地図を生成する処理をコンピュータに実行させるための自車位置推定用環境地図生成プログラム。
【請求項５】
車両の舵角を検出する検出部の検出結果を用いて、車両の移動方向を撮影する撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出すると共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくし、
抽出した特徴点と、予め生成された環境地図上の対応する特徴点とに基づいて、車両の位置及び姿勢を推定し、
推定結果に基づいて、自車両の位置を推定する処理をコンピュータに実行させるための自車位置推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自車の位置を推定するための地図を生成す自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、及び自車位置推定プログラムに関する。
【背景技術】
【０００２】
移動体に搭載されたカメラ等の撮影部によって撮影された撮影画像に基づいて、３次元空間上における移動体の位置を推定する技術が提案されている。
【０００３】
例えば、特許文献１では、第１画像に関する第１データと、移動体の移動に伴う前記第１画像との差異を含む第２画像に関する第２データと、を含む情報を取得する取得部と、前記情報に基づいて移動体の移動に伴う回転の方向を推定する方向推定処理と、回転の方向に基づいて、第１画像内の第１領域内の複数の第１特徴点と、第１画像内の第２領域内の複数の第２特徴点と、を検出し、前記複数の第１特徴点の第１領域内における第１密度は、複数の第２特徴点の第２領域内における第２密度よりも高い検出処理と、第２画像内の第３領域内の複数の第１対応点であって、複数の第１対応点のそれぞれは複数の第１特徴点の少なくともいずれかのそれぞれに対応する複数の第１対応点を求め、複数の第１特徴点のそれぞれと、複数の第１対応点のそれぞれと、に基づいて、移動体の位置の変化を推定する位置推定処理と、を実施する処理部と、を備えた移動体位置推定装置が提案されている。
【０００４】
また、特許文献１では、移動体の移動に伴う回転方向の推定を、取得して情報から推定する以外に、速度センサ、加速度センサと、角速度センサ、及び角加速度センサを用いて推定することが記載されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１６−６１６０４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１の技術のように、２つの撮影画像に基づいて移動体の移動に伴う移動体に搭載された撮影部の回転方向を推定する場合、回転方向を検出するために２つの撮影画像の特徴点等を比較する等の処理負荷が高くなるため、改善の余地がある。また、速度センサ、加速度センサと、角速度センサ、及び角加速度センサの検出結果に基づいて移動体の移動に伴う撮影部の回転方向を推定する場合には、加速度センサ、角速度センサ、及び角加速度センサはノイズが大きいため、移動体の移動に伴う撮影部の回転方向を安定的に推定する上で、改善の余地がある。特に、低速域では、移動体の移動に伴う撮影部の回転方向を安定的に推定できない。
【０００７】
また、環境地図を生成する場合に、移動体の移動に伴う撮影部の回転方向を安定的に推定できないと、特徴点を抽出する画像領域を適切に設定できないため地図精度が低下し、移動体の位置の推定精度も低下する。
【０００８】
本発明は、上記事実を考慮して成されたもので、撮影部の位置及び姿勢を安定的に推定可能な自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、並びに自車位置推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記目的を達成するために本発明に係る自車位置推定用環境地図生成装置は、車両の移動方向を撮影する撮影部と、車両の舵角を検出する検出部の検出結果を用いて、前記撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出すると共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくする抽出部と、前記抽出部が抽出した特徴点と、他の撮影画像の特徴点との対応付けに基づいて、車両の位置及び姿勢を推定し、推定結果に基づいて、特徴点の３次元位置を求めて、局所的な環境地図を生成する生成部と、を含む。
【００１０】
なお、前記生成部は、局所的な環境地図として、車両の走行経路に制約がある局所空間の環境地図を生成してもよい。
【００１１】
また、上記目的を達成するために本発明に係る自車位置推定装置は、車両の移動方向を撮影する撮影部と、車両の舵角を検出する検出部の検出結果を用いて、前記撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出すると共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくする抽出部と、前記抽出部が抽出した特徴点と、予め生成された環境地図上の対応する特徴点とに基づいて、車両の位置及び姿勢を推定し、推定結果に基づいて、自車両の位置を推定する推定部とを含む。
【００１２】
また、上記目的を達成するために本発明に係る自車位置推定用環境地図生成プログラムは、車両の舵角を検出する検出部の検出結果を用いて、車両の移動方向を撮影する撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出する共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくし、抽出した特徴点と、他の撮影画像の特徴点との対応付けに基づいて、車両の位置及び姿勢を推定し、推定結果に基づいて、特徴点の３次元位置を求めて、局所的な環境地図を生成する処理をコンピュータに実行させる。
【００１３】
また、上記目的を達成するために本発明に係る自車位置推定プログラムは、車両の舵角を検出する検出部の検出結果を用いて、車両の移動方向を撮影する撮影部によって撮影された撮影画像における車両の移動方向に対応する予め定めた範囲の領域以外の領域から特徴点を抽出するか、又は前記撮影画像の全域から特徴点を抽出すると共に前記撮影画像における車両の移動方向に対応する予め定めた範囲の領域の重み付けより他の領域の重み付けを大きくし、抽出した特徴点と、予め生成された環境地図上の対応する特徴点とに基づいて、車両の位置及び姿勢を推定し、推定結果に基づいて、自車両の位置を推定する処理をコンピュータに実行させる。
【発明の効果】
【００１４】
以上説明したように本発明によれば、撮影部の位置及び姿勢を安定的に推定することが可能となる、という効果がある。
【図面の簡単な説明】
【００１５】
【図１】本実施形態に係る自車位置推定装置を搭載する車両を示す図である。
【図２】本実施形態に係る自車位置推定装置の概略構成を示すブロック図である。
【図３】２時刻の撮影画像及び当該撮影画像の進行方向の領域を示す図である。
【図４】（Ａ）は現時刻の撮影画像の一例を示す図であり、（Ｂ）は前時刻の撮影画像の一例を示す図である。
【図５】本実施形態に係る自車位置推定装置において環境地図生成プログラムを実行することで行われる処理の流れの一例を示すフローチャートである。
【図６】本実施形態に係る自車位置推定装置において自車位置推定プログラムを実行することで行われる処理の流れの一例を示すフローチャートである。
【発明を実施するための形態】
【００１６】
以下、図面を参照して本発明の実施の形態の一例を詳細に説明する。図１は、本実施形態に係る自車位置推定装置を搭載する車両２０を示す図であり、図２は、本実施形態に係る自車位置推定装置の概略構成を示すブロック図である。
【００１７】
自車位置推定装置１０は、撮影部としてのカメラ１４を備えている。カメラ１４は、車両後部のトランク等に設けられ、車両後方を撮影する。カメラ１４は、例えば、図１に示すように、車幅方向の略中央部付近に設けられ、かつカメラ１４の光軸が水平方向より若干下側を向くように配置されている。
【００１８】
また、自車位置推定装置１０は、自車位置を推定するための演算等を行う制御部１２を備えている。制御部１２は、例えば、図２に示すように、ＣＰＵ１２Ａ、ＲＯＭ１２Ｂ、ＲＡＭ１２Ｃ、及びＩ／Ｏ（入出力インターフェース）１２Ｄがバス１２Ｅに接続されたコンピュータで構成される。
【００１９】
ＲＯＭ１２Ｂには、環境地図を生成するための環境地図生成プログラムや、自車位置を推定する自車位置推定プログラムを含む各種プログラムやデータ等が記憶されている。ＲＯＭ１２Ｂに記憶されたプログラムをＲＡＭ１２Ｃに展開してＣＰＵ１２Ａが実行することにより、環境地図の生成や、自車位置の推定が行われる。
【００２０】
Ｉ／Ｏ１２Ｄには、カメラ１４が接続されていると共に、車両２０の操舵角度を検出する舵角センサ１６が接続されている。そして、カメラ１４の撮影画像及び舵角センサ１６によって検出された操舵角度（以下、舵角という。）が制御部１２に入力される。
【００２１】
また、Ｉ／Ｏ１２Ｄには、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）、及びフラッシュメモリ等の記憶部１８が接続されている。なお、記憶部１８は制御部１２内に設けてもよいし、外部接続可能としてもよい。また、記憶部１８には制御部１２の制御によって生成した環境地図を記憶するが、環境地図生成プログラムや自車位置推定プログラムを記憶してもよい。
【００２２】
制御部１２は、環境地図生成プログラムを実行することにより、カメラ１４によって撮影された撮影画像と、舵角センサ１６によって検出された舵角と、に基づいて、３次元の点群の環境地図を生成して記憶部１８に記憶する。また、制御部１２は、自車位置推定プログラムを実行することにより、記憶部１８に記憶された環境地図に基づいて自車位置を推定する。本実施形態では、制御部１２が、生成する環境地図としては、局所的な環境地図を生成するが、より具体的には、車両２０の走行経路に制約がある局所空間（例えば、駐車空間等）の環境地図を生成する。
【００２３】
ここで、本実施形態に係る自車位置推定装置１０による環境地図の作成方法について説明する。
【００２４】
本実施形態に係る自車位置推定装置１０の制御部１２は、異なる２位置でカメラ１４によって撮影された各撮影画像から特徴点をそれぞれ抽出して、対応する特徴点同士を対応付ける。そして、２つの撮影画像の対応する特徴点に基づいて、カメラ１４の位置及び姿勢を推定し、特徴点の３次元座標を算出して、環境地図を生成する。
【００２５】
具体的には、制御部１２が、現時刻の撮影画像を取得し、前時刻の撮影画像とのペアを生成する。そして、舵角センサ１６の検出結果に基づいて、図３に示すように、現時刻の撮影画像の進行方向の領域を特定する。そして、制御部１２が、現時刻における撮影画像の進行方向の領域（図３の点線丸で示す領域）以外の特徴点を抽出する。前時刻における撮影画像についても、同様に、進行方向の領域以外の特徴点が既に抽出されているものとする。
【００２６】
ここで、進行方向の領域以外の領域は、本実施形態では、舵角センサ１６によって検出された舵角に基づいて撮影画像の進行方向の領域を導出することにより特定する。すなわち、車両２０の舵角が分かれば、進行方向は決定できるので、舵角センサ１６によって検出された舵角から、撮影画像の進行方向の領域を特定することができる。これにより、２つの撮影画像を用いて進行方向の領域を決定する場合より、車両２０の進行方向の領域を検出する際の処理負荷を低減することができる。また、加速度センサ、角速度センサ、及び角加速度センサの検出結果を用いて車両２０の進行方向の領域を検出する場合より、安定した進行方向の検出が可能となる。特に、車両２０が低速で移動している場合には、加速度センサ、角速度センサ、及び角加速度センサから車両２０の進行方向を検出することが困難となるが、舵角センサ１６の検出結果を用いることにより、安定した進行方向の検出が可能となる。なお、進行方向の領域は、舵角から検出した進行方向を含む予め定めた範囲の領域を特定する。例えば、進行方向の領域は、撮影画像を複数分割しておき、舵角センサ１６によって検出された舵角に基づいて、撮影画像の複数分割してある領域の中から対応付けることで特定する。
【００２７】
特徴点の抽出方法としては、例えば、ＯＲＢ−ＳＬＡＭを適用することができる。ＯＲＢ−ＳＬＡＭは、特徴点としてコーナーを検出し、検出手法にＦＡＳＴ（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.
【００２８】
例えば、現時刻の撮影画像の一例を図４（Ａ）に示し、前時刻の撮影画像の一例を図４（Ｂ）に示す。この例の場合には、舵角センサ１６の検出結果から特定したそれぞれの進行方向の領域は、図中の点線丸で示す領域とされ、この領域以外の領域の特徴点を抽出した結果を実線の丸で示す。
【００２９】
また、特徴点を抽出する際には、撮影画像における車両２０の進行方向に対応する領域では、進行方向の領域以外の領域に比べて、特徴点の移動量が少ないため、進行方向の予め定めた領域以外の領域における特徴点を抽出する。これにより、撮影画像の全面の特徴点を抽出する場合に比べて特徴点を抽出する処理負荷を低減することができる。また、特徴点の移動量が大きくなる領域から特徴点を抽出することにより、以降の処理において特徴点の３次元位置を推定する際の視差に対応する撮影画像間の対応する特徴点のずれが大きくなるため、より正確な３次元位置の推定が可能となる。特に、駐車空間等のように車両２０の走行経路に制約がある局所空間では、進行方向の予め定めた領域以外の領域における特徴点を抽出することにより、正確な３次元位置の推定が可能となる。
【００３０】
また、現時刻と前時刻のそれぞれの撮影画像間の特徴点の対応付けは、例えば、上述のＯＲＢ等の特徴量を用いて現時刻の特徴点と前時刻の特徴点とを対応付ける。
【００３１】
また、カメラ１４の位置及び姿勢の推定と特徴点の３次元座標の算出とでは、初期化段階において、対応付けられた複数組の特徴点のペアから、例えば、５点法及び７点法等を用いて、前時刻の撮影画像を基準とする基本行列を推定する。なお、現時刻の撮影画像を基準とする基本行列を推定してもよい。
【００３２】
また、特異値分解等によって、推定して得られた基本行列から並進ベクトル及び回転行列を算出する。この並進ベクトル及び回転行列の算出により、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を基準とする現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を推定する。なお、前時刻の撮影画像の撮影時のカメラ１４の位置を原点とし、前時刻の撮影画像の撮影時のカメラ１４の姿勢を基準として、３次元の地図座標系を設定する。
【００３３】
また、対応付けられた特徴点のペアの前時刻の撮影画像内の位置及び現時刻の撮影画像内の位置と、推定した、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に対する現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢とを用いて、三角測量の原理によって、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に対する、特徴点の三次元座標を算出する。そして、この三次元座標を、地図座標系の三次元座標に変換する。
【００３４】
そして、各特徴点について求めた地図座標系の三次元座標及び特徴量を含めた環境地図情報を生成する。また、地図座標系における前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢と、現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢とを求め、画像情報、及び画像に含まれる特徴点情報と共に、環境地図情報に格納する。
【００３５】
一方、初期化段階ではない場合、環境地図情報の各特徴点と、現時刻の撮影画像の特徴点とを照合して、地図座標系における現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を求める。
【００３６】
また、対応付けられた特徴点のうち、環境地図情報に登録されていない新しい特徴点が存在する場合には、当該新しい特徴点の現時刻の撮影画像内の位置及び他方の画像内の位置と、環境地図情報から得られる、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢と、求められた現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢とを用いて、三角測量の原理によって、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に対する、特徴点の三次元座標を算出する。そして、この三次元座標を、地図座標系の三次元座標に変換する。
【００３７】
そして、新しい特徴点について求めた地図座標系の三次元座標及び特徴量を環境地図情報に格納する。また、求められた現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を、画像情報、及び画像に含まれる特徴点情報と共に、環境地図情報に格納する。
【００３８】
また、本実施形態に係る自車位置推定装置１０の制御部１２は、カメラ１４によって撮影された撮影画像から特徴点を抽出し、環境地図情報に含まれる特徴点と照合して、地図座標系における撮影画像の撮影時のカメラ１４の位置及び姿勢を求め、車両２０の自己位置及び姿勢とする。このとき、環境地図を作成するときの同様に、舵角センサ１６の検出結果に基づいて、現時刻の撮影画像の進行方向の領域を特定し、現時刻における撮影画像の進行方向の領域以外の特徴点を抽出する。
【００３９】
続いて、上述のように構成された本実施形態に係る自車位置推定装置１０で行われる環境地図を生成する処理について説明する。図５は、本実施形態に係る自車位置推定装置１０において環境地図生成プログラムを実行することで行われる処理の流れの一例を示すフローチャートであり、当該処理は、各時刻の撮影画像が取得される毎に繰り返し行われる。
【００４０】
ステップ１００では、ＣＰＵ１２Ａが、舵角センサ１６の検出結果を取得してステップ１０２へ移行する。
【００４１】
ステップ１０２では、ＣＰＵ１２Ａが、取得した舵角センサ１６の検出結果に基づいて、車両２０（カメラ１４）の進行方向を算出してステップ１０４へ移行する。
【００４２】
ステップ１０４では、ＣＰＵ１２Ａが、特徴点抽出領域を決定してステップ１０６へ移行する。特徴点抽出領域は、カメラ１４の撮影画像における、ステップ１０２で算出した車両２０の進行方向に対応する予め定めた範囲の領域以外の領域を特徴点の抽出領域として決定する。
【００４３】
ステップ１０６では、ＣＰＵ１２Ａが、現時刻の撮影画像から特徴点を抽出してステップ１０８へ移行する。すなわち、ステップ１０４で決定した車両２０の進行方向に対応する予め定めた範囲の領域以外の領域における特徴点を抽出する。例えば、ＦＡＳＴ等を用いて車両２０（カメラ１４）の移動方向以外の領域から特徴点を抽出する。
【００４４】
ステップ１０８では、ＣＰＵ１２Ａが、上記ステップ１０６で抽出された特徴点と、前時刻のステップ１０６で抽出された特徴点とに基づいて、前時刻の撮影画像と現時刻の撮影画像の画像間の特徴点を対応付けてステップ１１０へ移行する。すなわち、時刻の異なる撮影画像間の特徴点を対応付ける。例えば、ＯＲＢ等の特徴量を用いて現時刻の特徴点と前時刻の特徴点とを対応付ける。なお、前時刻の撮影画像がない場合には、ステップ１０６以降の処理を行わずに終了する。
【００４５】
ステップ１１０では、ＣＰＵ１２Ａが、初期化段階であるか否かを判定する。該判定は、地図上に特徴点が未登録であるか否かを判定する。該判定が肯定された場合にはステップ１１２へ移行し、否定された場合にはステップ１１６へ移行する。
【００４６】
ステップ１１２では、ＣＰＵ１２Ａが、対応付けた特徴点からカメラ１４の位置及び姿勢を推定してステップ１１４へ移行する。例えば、前時刻の撮影画像を基準とする基本行列を推定する。また、特異値分解等によって、推定して得られた基本行列から並進ベクトル及び回転行列を算出し、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を基準とする現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を推定する。
【００４７】
ステップ１１４では、ＣＰＵ１２Ａが、カメラ１４の位置及び姿勢に基づいて、特徴点の３次元位置を推定して地図に登録する。具体的には、対応付けられた特徴点のペアの前時刻の画撮影像内の位置及び現時刻の撮影画像内の位置と、推定した、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に対する現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢とを用いて、三角測量の原理によって、現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に対する、特徴点の三次元座標を算出する。そして、この三次元座標を、地図座標系の三次元座標に変換して環境地図を生成し、生成した環境地図を記憶部１８に記憶する。
【００４８】
一方、ステップ１１６では、ＣＰＵ１２Ａが、地図上に既に登録された特徴点と、撮影画像上の特徴点から、現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢を推定してステップ１１８へ移行する。
【００４９】
ステップ１１８では、ＣＰＵ１２Ａが、現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に基づいて、新しい特徴点の３次元位置を推定して地図に登録することにより環境地図を生成する。すなわち、当該新しい特徴点の現時刻の撮影画像内の位置及び前時刻の撮影画像内の位置と、地図登録された、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢と、ステップ１１６で求めた現時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢とを用いて、三角測量の原理によって、前時刻の撮影画像の撮影時のカメラ１４の位置及び姿勢に対する、特徴点の三次元座標を算出する。そして、この三次元座標を、地図座標系の三次元座標に変換して環境地図を生成し、生成した環境地図を記憶部１８に記憶する。
【００５０】
なお、図５の処理におけるステップ１００〜１０６が自車位置推定用環境地図生成装置及び自車位置推定用環境地図生成プログラムの抽出部に対応し、ステップ１０８〜１１８が生成部に対応する。
【００５１】
次に、本実施形態に係る自車位置推定装置１０で行われる自車位置を推定する処理について説明する。図６は、本実施形態に係る自車位置推定装置１０において自車位置推定プログラムを実行することで行われる処理の流れの一例を示すフローチャートである。
【００５２】
ステップ２００では、ＣＰＵ１２Ａが、舵角センサ１６の検出結果を取得してステップ２０２へ移行する。
【００５３】
ステップ２０２では、ＣＰＵ１２Ａが、取得した舵角センサ１６の検出結果に基づいて、車両２０（カメラ１４）の進行方向を算出してステップ２０４へ移行する。
【００５４】
ステップ２０４では、ＣＰＵ１２Ａが、特徴点抽出領域を決定してステップ１０６へ移行する。特徴点抽出領域は、カメラ１４の撮影画像における、ステップ２０２で算出した車両２０の進行方向に対応する予め定めた範囲の領域以外の領域を特徴点の抽出領域として決定する。
【００５５】
ステップ２０６では、ＣＰＵ１２Ａが、現時刻の撮影画像から特徴点を抽出してステップ２０８へ移行する。すなわち、ステップ２０４で決定した車両２０の進行方向に対応する予め定めた範囲の領域以外の領域における特徴点を抽出する。例えば、ＦＡＳＴ等を用いて車両２０（カメラ１４）の移動方向以外の領域から特徴点を抽出する。
【００５６】
ステップ２０８では、ＣＰＵ１２Ａが、登録された環境地図に含まれる特徴点と、ステップ２０６で抽出された特徴点とを照合して、地図座標系における撮影画像の撮影時のカメラ１４の位置及び姿勢を推定してステップ２１０へ移行する。
【００５７】
ステップ２１０では、ＣＰＵ１２Ａが、推定したカメラ１４の位置及び姿勢から自車位置を推定して処理を終了する。すなわち、車両２０に対するカメラ１４の位置が分かっているので、カメラ１４の位置及び姿勢から自車位置を推定する。
【００５８】
なお、図６の処理におけるステップ２００〜２０６が自車位置推定装置及び自車位置推定プログラムの抽出部に対応し、ステップ２０８〜２１０が推定部に対応する。
【００５９】
以上説明したように、本実施形態では、車両２０に搭載されたカメラ１４によって撮影した撮影画像から進行方向の領域以外の領域の特徴点を抽出する際に、舵角センサ１６の検出結果に基づいて撮影画像の進行方向を特定する。これにより、２つの撮影画像を用いて進行方向の領域を決定する場合より、車両２０の進行方向の領域を検出する際の処理負荷を低減することができる。また、加速度センサ、角速度センサ、及び角加速度センサの検出結果を用いて車両２０の進行方向の領域を検出する場合より、安定した進行方向の検出が可能となる。特に、車両２０が低速で移動している場合には、加速度センサ、角速度センサ、及び角加速度センサから車両２０の進行方向を検出することが困難となるが、舵角センサ１６の検出結果を用いることにより、安定した進行方向の検出が可能となる。
【００６０】
また、特徴点を抽出する際に、進行方向の領域以外の領域を抽出するので、撮影画像の全域から特徴点の抽出を抽出するよりも処理負荷を軽減することができる。
【００６１】
さらに、特徴点の移動量が大きくなる進行方向以外の領域から特徴点を抽出するので、特徴点の３次元位置を推定する際の視差に対応する撮影画像間の特徴点のずれが大きくなるため、特徴点の３次元位置の位置精度が高い環境地図を生成することができる。特に、車両２０の走行経路に制約がある駐車空間等のような局所空間における環境地図を生成する際の地図精度を向上できる。
【００６２】
そして、このように生成した環境地図を自動駐車システム等に適用した場合には、高精度の環境地図を用いて車両２０を誘導できるので、狭い場所に駐車することが可能となる。
【００６３】
なお、上記の実施形態では、撮影画像における車両２０の進行方向に対応する領域以外の領域における特徴点を抽出してカメラ１４の位置及び姿勢を求めるようにしたが、これに限るものではない。例えば、撮影画像の全域の特徴点を抽出して、車両２０の進行方向に対応する領域以外の領域の重み付けを高くしてカメラ１４の位置及び姿勢を推定してもよい。例えば、前時刻の撮影画像における特徴点の位置から推定した現時刻の特徴点の位置と、現時刻における特徴点の位置との誤差を評価関数として、この評価関数が最小となるカメラ１４の位置及び姿勢を推定する際に、評価関数に重みをかけてカメラ１４の位置及び姿勢を推定する。
【００６４】
また、上記の実施形態における制御部１２で行われる図５、６の処理は、コンピュータがプログラムを実行することにより行われるソフトウエア処理として説明したが、ハードウエアで行う処理としてもよい。或いは、ソフトウエア及びハードウエアの双方を組み合わせた処理としてもよい。また、ソフトウエアで行う処理とする場合のプログラムは、各種記憶媒体に記憶して流通させるようにしてもよい。
【００６５】
また、上記の実施形態では、カメラ１４が車両２０の後部に設けて、車両後方を撮影する形態を説明したが、これに限るものではない。例えば、車両２０の前部に設けて、車両前方を撮影する形態や、車両２０の側方に設けて、車両前側方及び車両後側方の少なくとも一方を撮影する形態としてもよい。
【００６６】
また、上記の実施形態では、環境地図生成プログラム及び自車位置推定プログラムを予め制御部１２のＲＯＭ１２Ｂに記憶した形態として説明したが、これに限るものではない。例えば、環境地図生成プログラム及び自車位置推定プログラムは、ＣＤ−ＲＯＭ（Compact Disk Read Only Memory）、ＤＶＤ−ＲＯＭ（Digital Versatile Disk Read Only Memory）、及びＵＳＢ（Universal Serial Bus）メモリ等の記録媒体に記録された形態で提供されてもよい。また、環境地図生成プログラム及び自車位置推定プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。
【００６７】
さらに、本発明は、上記に限定されるものでなく、上記以外にも、その主旨を逸脱しない範囲内において種々変形して実施可能であることは勿論である。
【符号の説明】
【００６８】
１０    自車位置推定装置
１２    制御部
１４    カメラ
１６    舵角センサ
１８    記憶部
２０    車両
【図１】
2018205950.tif 000003
【図２】
2018205950.tif 000004
【図３】
2018205950.tif 000005
【図４】
2018205950.tif 000006
【図５】
2018205950.tif 000007
【図６】
2018205950.tif 000008