JP2015184956A - 自己位置算出装置及び自己位置算出方法 - Google Patents

Abstract

【課題】車両の移動量を精度良く且つ安定して算出することができる自己位置算出装置及び自己位置算出方法を提供する。
【解決手段】投光器１１より投光するパターン光の輝度が周期的に変化するように制御するパターン光制御部２７と、カメラ１２で撮像された画像からパターン光を抽出するパターン光抽出部２１と、パターン光抽出部２１にて抽出されたパターン光の位置から、路面に対する車両の姿勢角を算出する姿勢角算出部２２を有する。また、カメラ１２にて撮像された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、車両の姿勢変化量を算出する姿勢変化量算出部２４と、車両の初期位置及び前記姿勢角に姿勢変化量を加算することで、車両の現在位置及び姿勢角を算出する自己位置算出部２６を有する。そして、姿勢変化量算出部２４は、パターン光の輝度が閾値輝度以下のときに、特徴点を検出する。
【選択図】図１

Description

本発明は、自己位置算出装置及び自己位置算出方法に係り、特に、移動量を推定する精度を向上させる技術に関する。

従来より、車両にカメラを搭載し、該カメラにて車両近傍の映像を撮像し、撮像した画像の変化に基づいて車両の移動量を求める技術が提案されている（特許文献１参照）。特許文献１では、車両の低速かつ微妙な移動においても精度よく移動量を求めるために、画像に含まれる特徴点を検出し、この特徴点の画像上での位置を求め、該特徴点の移動方向及び移動距離（移動量）から車両の移動量を求めている。

特開２００８−１７５７１７号公報

しかしながら、車両に搭載するカメラは、車両が走行することにより路面との位置関係、即ち、路面との間の距離、及び路面に対する姿勢角が変化するので、車両の移動量を高精度に求めることができない。

本発明は、このような従来の課題を解決するためになされたものであり、その目的とするところは、車両の移動量を精度良く且つ安定して算出することができる自己位置算出装置及び自己位置算出方法を提供することにある。

上記目的を達成するため、本発明は、投光器より投光するパターン光の輝度が周期的に変化するように制御するパターン光制御部と、撮像部で撮像された画像からパターン光を抽出するパターン光抽出部と、パターン光抽出部にて抽出されたパターン光の位置から、路面に対する車両の姿勢角を算出する姿勢角算出部を有する。また、撮像部にて撮像された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、車両の姿勢変化量を算出する姿勢変化量算出部と、車両の初期位置及び姿勢角に、姿勢変化量を加算してゆくことで、車両の現在位置及び姿勢角を算出する自己位置算出部を有する。そして、姿勢変化量算出部は、パターン光の輝度が閾値輝度以下のときに、特徴点を検出する。

本発明では、パターン光の輝度を周期的に変化させ、パターン光の輝度が閾値輝度以下のときに、特徴点を検出するので、特徴点の検出精度を向上させることができ、車両の移動量を安定的、且つ高精度に算出することが可能となる。

本発明の第１、第２実施形態に係る自己位置算出装置の構成を示すブロック図である。 車両に搭載される投光器により照射される投光パターン、及びカメラによる撮像範囲を示す説明図である。 （ａ）は、投光器とカメラの間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕｊ，Ｖｊ）とから、各スポット光が照射された路面上の位置を算出する様子を示す図であり、（ｂ）は、パターン光が照射された領域とは異なる他の領域から検出された特徴点の時間変化から、カメラの移動方向を求める様子を示す説明図である。 （ａ）は、カメラにより取得した画像に対して二値化処理を施したパターン光の画像を示す図、（ｂ）はその拡大図、（ｃ）はパターン光抽出部により抽出した各スポット光の重心の位置を示す図である。 距離及び姿勢角の変化量を算出する方法を説明するための模式図である。 （ａ）は、時刻ｔに取得した画像の一例を示し、（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得した画像の一例を示す。 本発明の第１実施形態に係る自己位置算出装置の処理手順を示すフローチャートである。 本発明の第１実施形態に係る自己位置算出装置の、輝度変化、及び特徴点検出フラグの変化を示すタイミングチャートである。 本発明の第１実施形態に係る自己位置算出装置の、パターン光と特徴点を示す説明図である。 本発明の第２実施形態に係る自己位置算出装置の処理手順を示すフローチャートである。 本発明の第２実施形態に係る自己位置算出装置の、輝度変化、及び特徴点検出フラグの変化を示すタイミングチャートである。 本発明の第３実施形態に係る自己位置算出装置の構成を示すブロック図である。 本発明の第３実施形態に係る自己位置算出装置の処理手順を示すフローチャートである。 本発明の第３実施形態に係る自己位置算出装置の、輝度変化、及び特徴点検出フラグの変化を示すタイミングチャートである。 本発明の第３実施形態の第１変形例に係る自己位置算出装置の処理手順を示すフローチャートである。 本発明の第３実施形態の第１変形例に係る自己位置算出装置の、輝度変化、及び特徴点検出フラグの変化を示すタイミングチャートである。 本発明の第３実施形態の第２変形例に係る自己位置算出装置の構成を示すブロック図である。

以下、本発明の実施形態について図面を参照して説明する。

［第１実施形態の説明］
図１は、本発明の実施形態に係る自己位置算出装置１００の構成を示すブロック図である。図１に示すように、自己位置算出装置１００は、投光器１１と、カメラ１２（撮像部）と、エンジンコントロールユニット（ＥＣＵ）１３と、を備える。投光器１１は、車両に搭載され、車両周囲の路面に周期的に輝度が変化するパターン光を投光する。パターン光の詳細については後述する。カメラ１２は、車両に搭載され、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する。ＥＣＵ１３は、投光器１１の作動を制御し、且つカメラ１２により取得された画像から車両の移動量を推定する一連の情報処理サイクルを実行する。

投光器１１は、図２に示すように、カメラ１２の撮像範囲内の路面３１に向けて、正方形や長方形の格子像を含む所定の形状を有するパターン光３２ｂを投光する。この際、パターン光制御部２７により、パターン光３２ｂの輝度が、周期的に変化するように制御される。

カメラ１２は、固体撮像素子、例えばＣＣＤ及びＣＭＯＳを用いたデジタルカメラであって、画像処理が可能なデジタル画像を取得する。カメラ１２の撮像対象は車両周囲の路面であって、車両周囲の路面には、車両の前部、後部、側部、車両底部の路面が含まれる。例えば、図２に示すように、カメラ１２は、車両１０の前部、具体的にはフロントバンパ上に搭載することができる。

車両１０の前方の路面３１上の特徴点（「テクスチャ」ともいう）及び投光器１１により投光されたパターン光３２ｂを撮像できるように、カメラ１２が設置される高さ及び向きが調整され、且つ、カメラ１２が備えるレンズの焦点及び絞りが自動調整される。ここで、「特徴点」とは、アスファルト上に存在する凹凸部分である。

カメラ１２は、所定の時間間隔をおいて繰り返し撮像を行い、一連の画像（フレーム）群を取得する。例えば、パターン光３２ｂの輝度を変化させる周期を２００［Ｈｚ］とした場合、１秒間に２４００回の撮像を行う。これにより、パターン光３２ｂの輝度が最も高い時点、及び最も低い時点の画像を周期的に撮像することができる。カメラ１２により取得された画像データは、撮像周期毎にＥＣＵ１３へ転送され、ＥＣＵ１３が備えるメモリ（図示省略）に記憶される。

カメラ１２は、路面３１により照射されたパターン光を撮像する。投光器１１は、例えば、レーザポインター及び回折格子を備える。レーザポインターから射出されたレーザ光を回折格子で回折することにより、投光器１１は、図２〜図４に示すように、格子像、或いは行列状に配列された複数のスポット光からなるパターン光（３２ｂ、３２ａ）を生成する。図３及び図４に示す例では、５×７のスポット光からなるパターン光３２ａを生成している。

図１に戻り、ＥＣＵ１３は、ＣＰＵ、メモリ、及び入出力部を備えるマイクロコントローラからなり、予めインストールされたコンピュータプログラムを実行することにより、自己位置算出装置１００が備える複数の情報処理部を構成する。ＥＣＵ１３は、カメラ１２により取得された画像から車両の移動量を推定する一連の情報処理サイクルを、画像（フレーム）毎に繰り返し実行する。ＥＣＵ１３は、車両１０に関連する他の制御に用いるＥＣＵと兼用しても良い。

複数の情報処理部には、パターン光抽出部２１と、姿勢角算出部２２と、特徴点検出部２３と、姿勢変化量算出部２４と、自己位置算出部２６、及び、パターン光制御部２７が含まれる。姿勢変化量算出部２４には、特徴点検出部２３が含まれる。

パターン光制御部２７は、投光器１１によるパターン光３２ａの投光を制御する。例えば、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置１００が起動すると同時に、パターン光制御部２７は、周期的に輝度が変化するパターン光３２ａの投光を開始する。その後、パターン光制御部２７は、自己位置算出装置が停止するまで、パターン光３２ａを連続して投光する。或いは、必要に応じてパターン光３２ａを投光してもよい。本実施形態では、輝度変化の一例として、投光パターンの輝度が所定周波数の正弦波状に変化するように、投光器１１に供給する電力を制御する。例えば、後述の図８（ａ）に示すように正弦波状に輝度が変化するように制御する。また、後述する第２実施形態では、図１１（ａ）に示すように、所定の切替周波数でオン、オフを切り替えて投光パターンの輝度を制御する。

パターン光抽出部２１は、カメラ１２により取得された画像をメモリから読み込み、更に、前述した所定周波数にて同期検波処理を実施することにより、画像に含まれるパターン光３２ａを抽出する。同期検波処理については後述する。また、後述する第２実施形態では、投光パターンが投光されているとき（オン時）に撮像した画像と、投光パターンが投光されていないとき（オフ時）に撮像した画像の差分を演算することにより、パターン光３２ａを抽出する。そして、取得したパターン光３２ａから、該パターン光３２ａの位置を抽出する。図３（ａ）に示すように、例えば、投光器１１が行列状に配列された複数のスポット光からなるパターン光３２ａを路面３１に向けて投光し、路面３１で反射したパターン光３２ａを同期検波処理、或いは差分演算処理により検出する。

更に、抽出したパターン光の画像に対して二値化処理を施すことにより、図４（ａ）及びその拡大図である図４（ｂ）に示すように、スポット光Ｓｐの画像のみを抽出する。パターン光抽出部２１は、図４（ｃ）に示すように、各スポット光Ｓｐの重心の位置Ｈｅ、即ち、スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）を算出することにより、パターン光３２ａの位置を抽出する。座標は、カメラ１２の撮像素子の画素を単位とし、５×７のスポット光Ｓｐの場合、“ｊ”は１以上３５以下の整数である。スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）は、パターン光３２ａの位置を示すデータとしてメモリに記憶される。

次に、同期検波処理について説明する。カメラ１２で撮像した画像に含まれる測定信号をsin（ω０＋α）とすると、この測定信号には投光パターン以外に様々な周波数成分の太陽光や人工光が含まれている。そこで、周波数が変調周波数ωｒとなる参照信号sin（ωｒ＋β）を測定信号sin（ω０＋α）に乗じると、その結果は、cos（ω０−ωｒ＋α−β）／２−cos（ω０＋ωｒ＋α＋β）／２となる。

この測定信号をローパスフィルタに通すと、ω０≠ωｒの信号、即ち、周波数がωｒではない投光パターン以外の太陽光や人工光は除去される。その一方で、ω０＝ωｒの信号、即ち、周波数がωｒの投光パターンは、cos（α−β）／２となって抽出することができる。このように、同期検波処理を用いることにより、様々な周波数成分を含む測定信号の中から、投光パターンのみを抽出した画像を得ることができる。

即ち、本実施形態では、パターン光制御部２７は、予め設定した所定の変調周波数ωｒにてパターン光３２ａを輝度変調する。従って、路面には周波数ωｒで輝度変調された投光パターンが投光される。そして、姿勢角算出部２２は、カメラ１２で撮像される画像（測定信号）に変調周波数ωｒを乗じることにより、投光パターンのみを抽出することができる。

図１に示す姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、カメラ１２により取得された画像におけるパターン光３２ａの位置から、路面３１に対する車両１０の距離及び姿勢角を算出する。例えば、図３（ａ）に示すように、投光器１１とカメラ１２の間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕｊ、Ｖｊ）とから、三角測量の原理を用いて、各スポット光が照射された路面３１上の位置を、カメラ１２に対する相対位置として算出する。そして、姿勢角算出部２２は、カメラ１２に対する各スポット光の相対位置から、パターン光３２ａが投光された路面３１の平面式、即ち、路面３１に対するカメラ１２の距離及び姿勢角（法線ベクトル）を算出する。なお、車両１０に対するカメラ１２の搭載位置及び撮像方向は既知であるので、実施形態においては、路面３１に対する車両１０の距離及び姿勢角の一例として、路面３１に対するカメラ１２の距離及び姿勢角を算出する。以後、路面３１に対するカメラ１２の距離及び姿勢角を、「距離及び姿勢角」と略す。姿勢角算出部２２により算出された距離及び姿勢角は、メモリに記憶される。

具体的には、カメラ１２及び投光器１１は車両１０にそれぞれ固定されているので、パターン光３２ａの照射方向と、カメラ１２と投光器１１との距離（図３の基線長Ｌｂ）は既知である。そこで、姿勢角算出部２２は、三角測量の原理を用いて、各スポット光の画像上の座標（Ｕｊ、Ｖｊ）から各スポット光が照射された路面３１上の位置を、カメラ１２に対する相対位置（Ｘｊ，Ｙｊ，Ｚｊ）として求めることができる。

なお、カメラ１２に対する各スポット光の相対位置（Ｘｊ，Ｙｊ，Ｚｊ）は同一平面上に存在しない場合が多い。なぜなら、路面３１に表出するアスファルトの凹凸に応じて各スポット光の相対位置が変化するからである。そこで、最小二乗法を用いて、各スポット光との距離誤差の二乗和が最小となるような平面式を求めても良い。

特徴点検出部２３は、カメラ１２により取得された画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。詳細には、所定周波数で正弦波状に輝度が変化するパターン光の輝度が、予め設定した閾値輝度以下となった時間帯に特徴点検出フラグを「１」に設定し、この時間帯において特徴点を検出する。また、後述する第２実施形態では、パターン光の投光がオフとされている時間帯において、特徴点を検出する。

更に、特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「D.G. Lowe，“Distinctive Image Features from Scale-Invariant Keypoints,” Int. J. Comput. Vis., vol.60, no.2, pp. 91-110, Nov. 200 」、或いは、「金澤 靖, 金谷健一, “コンピュータビジョンのための画像の特徴点抽出,” 信学誌, vol.87, no.12, pp.1043-1048, Dec. 2004」に記載の手法を用いることができる。

具体的には、特徴点検出部２３は、例えば、ハリス（Harris）作用素又はSUSANオペレータを用いて、物体の頂点のように周囲に比べて輝度値が大きく変化する点を特徴点として検出する。或いは、特徴点検出部２３は、SIFT（Scale-Invariant Feature Transform）特徴量を用いて、その周囲で輝度値がある規則性のもとで変化している点を特徴点として検出しても良い。そして、特徴点検出部２３は、１つの画像から検出した特徴点の総数Ｎを計数し、各特徴点に識別番号（ｉ（１≦ｉ≦Ｎ））を付す。各特徴点の画像上の位置（Ｕｉ、Ｖｉ）は、ＥＣＵ１３内のメモリに記憶される。図６（ａ）及び図６（ｂ）は、カメラ１２により取得された画像から検出された特徴点Ｔｅの例を示す。更に、各特徴点Ｔｅの変化方向及び変化量をベクトルＤｔｅとして示している。

なお、実施形態において、路面３１上の特徴点は、主に大きさが１ｃｍ以上２ｃｍ以下のアスファルト混合物の粒を想定している。この特徴点を検出するために、カメラ１２の解像度はＶＧＡ（約３０万画素）である。また、路面３１に対するカメラ１２の距離は、おおよそ７０ｃｍである。更に、カメラ１２の撮像方向は、水平面から約４５degだけ路面３１に向けて傾斜させる。また、カメラ１２により取得される画像をＥＣＵ１３に転送するときの輝度数値は、０〜２５５（０：最も暗い、２５５：最も明るい）の範囲内である。

姿勢変化量算出部２４は、一定の情報処理サイクル毎に撮像される各フレームの画像のうち、前回（時刻ｔ）フレームの画像に含まれる複数の特徴点の画像上の位置座標（Ｕｉ，Ｖｉ）をメモリから読み込み、更に、今回（時刻ｔ＋Δｔ）フレームの画像に含まれる複数の特徴点の画像上の位置座標（Ｕｉ，Ｖｉ）をメモリから読み込む。そして、複数の特徴点の画像上での位置変化に基づいて、車両１０の姿勢変化量を求める。ここで、「車両の姿勢変化量」とは、路面に対する車両１０の「距離及び姿勢角の変化量」、及び路面上での「車両の移動量」の双方を含んでいる。以下、「距離及び姿勢角の変化量」及び「車両の移動量」の算出方法について説明する。

距離及び姿勢角の変化量は、例えば、以下のようにして求めることができる。図６（ａ）は、時刻ｔに取得された第１フレームの画像３８の一例を示す。図５及び図６（ａ）に示すように、画像３８において、例えば３つの特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の相対位置（ｘｉ，ｙｉ，ｚｉ）がそれぞれ算出されている場合を考える。この場合、特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３によって特定される平面Ｇ（図６（ａ）参照）を路面と見なすことができる。よって、姿勢変化量算出部２４は、各特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）から、路面（平面Ｇ）に対するカメラ１２の距離及び姿勢角（法線ベクトル）を求めることができる。更に、姿勢変化量算出部２４は、既知のカメラモデルによって、各特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の間の距離（ｌ１，ｌ２，ｌ３）及び夫々の特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を結ぶ直線が成す角度を求めることができる。図５のカメラ１２は、第１フレーム（時刻ｔ）におけるカメラの位置を示す。

なお、カメラ１２に対する特徴点の相対位置を示す３次元座標（ｘｉ，ｙｉ，ｚｉ）として、カメラ１２の撮像方向をＺ軸に設定し、撮像方向を法線とし且つカメラ１２を含む平面内に、互いに直交するＸ軸及びＹ軸を設定する。一方、画像３８上の座標として、水平方向及び垂直方向をそれぞれＶ軸及びＵ軸に設定する。

図６（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得された第２フレームの画像３８’を示す。図５のカメラ１２’は、第２フレームの画像３８’を撮像したときのカメラ１２の位置を示す。図５及び図６（ｂ）に示すように、画像３８’において、カメラ１２’が特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を撮像し、特徴点検出部２３が特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を検出する。この場合、姿勢変化量算出部２４は、時刻ｔにおける各特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の相対位置（ｘｉ，ｙｉ，ｚｉ）と、各特徴点の画像３８’上の位置Ｐ１（Ｕｉ，Ｖｉ）と、カメラ１２のカメラモデルとから、時間Δｔにおけるカメラ１２の移動量ΔＬ（図５参照）を算出でき、ひいては車両１０の移動量を算出できる。更には、距離及び姿勢角の変化量も算出することができる。例えば、以下の（１）〜（４）式からなる連立方程式を解くことにより、姿勢変化量算出部２４は、カメラ１２（車両）の移動量（ΔＬ）、及び、距離及び姿勢角の変化量を算出することができる。なお、下記の（１）式はカメラ１２が歪みや光軸ずれのない理想的なピンホールカメラとしてモデル化したものであり、λｉは定数、ｆは焦点距離である。カメラモデルのパラメータは、予めキャリブレーションをしておけばよい。

なお、姿勢変化量算出部２４は、時刻ｔと時刻ｔ＋Δｔで検出される各画像中で相対位置が算出された特徴点の全てを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定してもよい。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，R.I. Hartley: “A linear method for reconstruction from lines and points,” Proc. 5th International Conference on Computer Vision, Cambridge, Massachusetts, pp.882-887(1995)）を用いることができる。

このように、姿勢変化量算出部２４が、時刻ｔのフレーム画像３８で相対位置（ｘｉ，ｙｉ，ｚｉ）が算出された特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を、時刻ｔ＋Δｔにおけるフレーム画像３８’からも検出された場合に、該姿勢変化量算出部２４は、路面上の複数の特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）と特徴点の画像上の位置（Ｕｉ，Ｖｉ）の時間変化から、「車両の姿勢変化量」を算出することができる。更には、車両１０の移動量を算出することができる。

即ち、前回フレームと今回フレームの間で対応関係が取れる３点以上の特徴点を検出し続けることができれば、距離及び姿勢角の変化量を加算する処理（即ち、積分演算）を継続することにより、パターン光３２ａを用いることなく、距離及び姿勢角を更新し続けることができる。但し、最初の情報処理サイクルにおいては、パターン光３２ａを用いて算出された距離及び姿勢角、或いは所定の初期的な距離及び姿勢角を用いてもよい。つまり、積分演算の起点となる距離及び姿勢角は、パターン光３２ａを用いて算出しても、或いは、所定の初期値を用いても構わない。所定の初期的な距離及び姿勢角は、少なくとも車両１０への乗員及び搭載物を考慮した距離及び姿勢角であることが望ましい。例えば、車両１０のイグニションスイッチがオン状態であって、且つシフトポジションがパーキングから他のポジションへ移動した時に、パターン光３２ａを投光し、パターン光３２ａから算出された距離及び姿勢角を、所定の初期的な距離及び姿勢角として用いればよい。これにより、車両１０の旋回や加減速によるロール運動或いはピッチ運動が発生していない時の距離や姿勢角を求めることができる。

なお、前後フレーム間で特徴点を対応付けるには、例えば、検出した特徴点の周辺の小領域の画像をメモリに記録しておき、輝度や色情報の類似度から判断すればよい。具体的には、ＥＣＵ１３は、検出した特徴点を中心とする５×５（水平×垂直）画素分の画像をメモリに記録する。姿勢変化量算出部２４は、例えば、輝度情報が２０画素以上で誤差１％以下に収まっていれば、前後フレーム間で対応関係が取れる特徴点であると判断する。そして、上記の処理で取得した姿勢変化量は、後段の自己位置算出部２６にて車両１０の自己位置を算出する際に用いられる。

図１に示す自己位置算出部２６は、姿勢変化量算出部２４で算出された「距離及び姿勢角の変化量」から車両１０の距離及び姿勢角を算出する。更に、姿勢変化量算出部２４で算出された「車両の移動量」から車両１０の自己位置を算出する。

具体的な例として、姿勢角算出部２２にて算出された距離及び姿勢角（パターン光を用いて算出された距離及び姿勢角）が起点として設定された場合について説明する。この場合には、この起点（距離及び姿勢角）に対して、自己位置算出部２６は、姿勢変化量算出部２４で算出された各フレーム毎の距離及び姿勢角の変化量を逐次加算して（積分演算して）、距離及び姿勢角を最新な数値に更新する。また、姿勢角算出部２２にて距離及び姿勢角が算出された際の車両位置を起点（車両の初期位置）とし、この初期位置からの車両１０の移動量を逐次加算して（積分演算して）、車両１０の自己位置を算出する。例えば、地図上の位置と照合された起点（車両の初期位置）を設定することで、地図上の車両の現在位置を逐次算出することができる。

従って、姿勢変化量算出部２４は、時間Δｔの間でのカメラ１２の移動量（ΔＬ）を求めることにより、車両１０の自己位置を算出することができる。更には、距離及び姿勢角の変化量も同時に算出することができるので、姿勢変化量算出部２４は、車両１０の距離及び姿勢角の変化量を考慮して、６自由度（前後、左右、上下、ヨー、ピッチ、ロール）の移動量（ΔＬ）を精度よく算出することができる。即ち、車両１０の旋回や加減速によるロール運動或いはピッチ運動によって距離や姿勢角が変化しても、移動量（ΔＬ）の推定誤差を抑制することができる。

なお、実施形態では、距離及び姿勢角の変化量を算出し、距離及び姿勢角を更新することにより、カメラ１２の移動量（ΔＬ）を算出した。しかし、路面３１に対するカメラ１２の姿勢角だけを変化量算出及び更新の対象としても構わない。この場合、路面３１とカメラ１２との距離は一定と仮定すればよい。これにより、姿勢角の変化量を考慮して、移動量（ΔＬ）の推定誤差を抑制しつつ、ＥＣＵ１３の演算負荷を軽減し、且つ演算速度を向上させることもできる。

［第１実施形態の作用］
次に、カメラ１２により取得された画像３８（図５参照）から車両１０の移動量を推定する自己位置算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図７に示すフローチャート、図８に示す特性曲線、及び図９に示す説明図を参照して説明する。

図８（ａ）は、投光器１１より投光されるパターン光３２ａの輝度変化を示す特性図、図８（ｂ）は、特徴点検出フラグの変化を示す特性図である。また、図９は路面に投光されるパターン光を示す説明図であり、図９（ａ）、（ｃ）はパターン光３２ａの輝度が高い場合、図９（ｃ）は、パターン光３２ａの輝度が低い場合の状態を示している。図７に示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置１００が起動すると同時に開始され、該自己位置算出装置１００が停止するまで、繰り返し実行される。

初めに、図７のステップＳ１１において、パターン光制御部２７は、投光器１１を制御して、車両周囲の路面３１にパターン光を投光する。この際、パターン光制御部２７は、図８（ａ）に示すように、パターン光の輝度Ｂ１が所定周期の正弦波状に変化するように投光電力を制御する。例えば、正弦波の周波数を２００［Ｈｚ］とする。その結果、時間経過に伴い輝度Ｂ１が正弦波状に変化するパターン光が路面３１に投光される。

ステップＳ１２において、カメラ１２は、パターン光が投光されている領域を含む路面３１を撮像して画像を取得する。ステップＳ１３において、特徴点検出部２３は、投光器１１より投光されるパターン光３２ａの輝度が予め設定した閾値輝度Ｂth以下であるか否かを判断する。

閾値輝度Ｂthより大きい場合には（ステップＳ１３でＮＯ）、ステップＳ１４において、パターン光抽出部２１は、上述した同期検波処理によりパターン光３２ａを抽出する。例えば、図９（ａ），（ｃ）に示すように、図８に示す時刻ｔ１或いは時刻ｔ３（輝度Ｂ１が閾値輝度Ｂthよりも大きい時刻）に撮影した画像から、同期検波処理によりパターン光３２ａを抽出する。そして、パターン光３２ａの位置を示すデータとして、スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）を算出し、これをメモリに記憶する。更に、姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から、距離及び姿勢角を算出し、メモリに記憶する。距離及び姿勢角の算出方法は、前述した通りである。なお、以下では、姿勢角算出部２２によりパターン光を用いて路面３１に対する車両１０の距離及び姿勢角を求め、これを積分演算の起点となる距離及び姿勢角として再設定する処理を「距離及び姿勢角をリセットする」と表現する。その後、ステップＳ１６に処理を進める。

一方、閾値輝度Ｂth以下である場合には（ステップＳ１３でＹＥＳ）、ステップＳ１５において、特徴点検出部２３は、路面３１に存在する特徴点を検出する。具体的には、パターン光３２ａの輝度Ｂ１が、図８（ａ）に示すように正弦波状に変化する場合には、輝度Ｂ１が閾値輝度Ｂth以下となる時間帯に特徴点を検出する。即ち、図８（ｂ）に示すように、輝度Ｂ１が閾値輝度Ｂth以下となる時間帯に特徴点検出フラグを「１」とし、この時間帯に特徴点を検出する。例えば、図９（ｂ）に示すように、図８に示す時刻ｔ２（輝度Ｂ１が閾値輝度Ｂth以下の時刻）に撮影した画像から特徴点を検出する。更に、姿勢変化量算出部２４は、特徴点検出部２３で検出された特徴点の画像上の位置に基づき、カメラ１２に対する特徴点の変化量を算出する。この際、特徴点を検出する領域は、パターン光３２ａを投光する領域と全部または一部が重なっている領域としている。

また、ステップＳ１５において、特徴点検出部２３は、画像３８から特徴点（例えば、アスファルト上に存在する凹凸部位）を検出し、前回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点を抽出し、特徴点の画像上の位置（Ｕｉ、Ｖｉ）から距離及び姿勢角を更新する。

具体的には、特徴点検出部２３は、特徴点検出フラグが「１」のとき、カメラ１２により取得された画像３８をメモリから読み込み、画像３８から路面３１上の特徴点を検出し、各特徴点の画像上の位置（Ｕｉ、Ｖｉ）をメモリに記憶する。姿勢変化量算出部２４は、各特徴点の画像上の位置（Ｕｉ、Ｖｉ）をメモリから読み込み、距離及び姿勢角と、特徴点の画像上の位置（Ｕｉ、Ｖｉ）とから、カメラ１２に対する特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）を算出する。姿勢変化量算出部２４は、カメラ１２に対する特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）をメモリに記憶する。

そして、姿勢変化量算出部２４は、特徴点の画像上の位置（Ｕｉ、Ｖｉ）と、前回の情報処理サイクルのステップＳ１５において算出された特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）をメモリから読み込む。姿勢変化量算出部２４は、今回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点の相対位置（Ｘｉ，Ｙｉ，Ｚｉ）及び画像上の位置（Ｕｉ、Ｖｉ）を用いて、距離及び姿勢角の変化量を算出する。姿勢変化量算出部２４は、前回の情報処理サイクルで求められた距離及び姿勢角に、上記した距離及び姿勢角の変化量を加算することにより、距離及び姿勢角を更新する。そして、更新後の距離及び姿勢角をメモリに記憶する。つまり、前回のサイクルのステップＳ１６（後述）の処理で設定された距離及び姿勢角に対して、今回の情報処理サイクル毎の距離及び姿勢角の変化量を積分演算することにより、距離及び姿勢角を更新する処理を実行する。その後、ステップＳ１６に処理を進める。

ステップＳ１６において、ＥＣＵ１３は、積分演算の起点を選択する。この処理では、最初の情報処理サイクルにおいては、パターン光３２ａより算出される距離及び姿勢角を選択して設定する。更に、予め設定した条件が成立した場合、例えば、特徴点検出部２３における特徴点の検出状態が低下し、特徴点をフラグが「１」となるタイミングで複数の特徴点を検出できなくなった場合には、移動量算出の起点をパターン光により算出する距離及び姿勢角、即ちステップＳ１４の処理で算出された距離及び姿勢角にリセットする。他方、特徴点検出部２３にて特徴点が正常に検出されている場合には、該特徴点の位置に基づいて距離及び姿勢角を更新する。

つまり、特徴点検出部２３により特徴点が正常に検出されない場合には、カメラ１２の距離及び姿勢角を高精度に設定することができず、この精度の低い距離及び姿勢角を採用して車両の移動量を算出すると、車両の移動量を高精度に検出できなくなる。従って、このような場合には、移動量算出の起点をパターン光の位置から求められる距離及び姿勢角にリセットする。こうすることで、距離及び姿勢角に大幅な誤差が生じることを防止する。

次いで、ステップＳ１７において、自己位置算出部２６は、ステップＳ１４或いはＳ１５の処理で求められた距離及び姿勢角と、積分演算の起点、及び、特徴点の画像上の位置（Ｕｉ、Ｖｉ）の変化量とから、路面３１に対するカメラ１２の移動量（ΔＬ）、即ち車両１０の移動量を算出する。

こうして、第１実施形態に係る自己位置算出装置では、上記した一連の情報処理サイクルを繰り返し実行して車両１０の移動量を積算することにより、車両１０の位置を算出することができるのである。

このようにして、第１実施形態に係る自己位置算出装置１００では、投光器１１より所定の変調周波数（例えば、２００Ｈｚ）で正弦波状に輝度が変化するパターン光３２ａを路面に投光し、このパターン光３２ａを含む画像をカメラ１２で撮像する。この画像を同期検波処理することにより、パターン光３２ａを抽出する。従って、路面上に投光したパターン光３２ａを高精度に検出することが可能となる。その後、パターン光３２ａの位置に基づいてカメラ１２の距離及び姿勢角を算出する。この際、距離及び姿勢角の算出は、上記した所定の変調周期で設定される周期毎に実施しても良いし、複数周期毎（例えば、１０周期毎）に実施しても良い。従って、所定の周期毎、或いは複数周期毎にパターン光３２ａに基づく距離及び姿勢角が算出される。

また、パターン光３２ａの輝度が閾値輝度Ｂth以下となる時間帯に、路面上に存在する特徴点を検出する。この際、パターン光３２ａの輝度は低いので、特徴点を検出する際にパターン光３２ａの影響を受けることを防止でき、高精度な特徴点検出が可能となる。そして、検出した特徴点に基づき、上述した方法を採用することにより車両の移動量を算出することができる。その結果、車両の移動量を高精度に算出することが可能となる。

ここで、パターン光３２ａの輝度の設定について説明する。本実施形態では、最も日照量が多い夏至（６月）に近い晴天下においてもパターン光３２ａを検出できるように、該パターン光３２ａの最大輝度（図８（ａ）に示す輝度Ｂ１の上のピーク）を設定する。また、パターン光の影響が最も大きい夜間において、投光パターンを路面の特徴点と９９％以上の確率で誤検出しないように、パターン光の最小輝度（図８（ａ）に示す輝度Ｂ１の下のピーク）を設定する。

また、パターン光の輝度変調の周波数は、例えば、２００［Ｈｚ］とし、カメラ１２のフレームレート（１秒間の撮影数）を２４００［ｆｐｓ］とする。これは、本実施形態で、車両の最高速度７２Ｋｍ／ｈ（≒２０ｍ／ｓ）を想定した場合において、１周期での移動量を０．１ｍ以下に抑えるためである。これは本実施形態の原理上、パターン光３２ａが投光される領域と、特徴点が検出される領域が可能な限り近い、或いは同一であることが望ましいためである。また、同期検波の原理から、１周期の間での移動量が大きくなり、路面状態が変化してしまうと、投光パターン以外の太陽光や人工光が変化し、同期検波の前提が崩れる可能性があり、１周期での移動量を小さく抑えることにより、これを回避する。従って、より高速なカメラを用い、この一定周期を更に短くすると更なる性能向上が期待できる。

また、本実施形態では、所定の変調周波数で輝度変調したパターン光を照射し、更に、撮像した画像を同期検波処理することにより、パターン光３２ａを抽出する例について説明したが、同期画像を所定回数積算した画像を生成し、この積算した画像からパターン光３２ａを抽出するようにしてもよい。つまり、１周期の同期検波処理により得られる同期画像では、パターン光３２ａが投光された部分が十分強調されずにノイズが多く含まれてしまい、２値化処理を高精度に実施することができなくなる場合がある。そこで、２周期以上で取得した同期画像を積分し、パターン光３２ａが投光された部分を、より一層強調した同期検波画像を作成することにより、ノイズの影響をより排除して画像上の投光パターンを検出することができる。

更に、特徴点を検出する領域は、パターン光３２ａを投光する領域と重複している。従って、カメラ１２による撮像範囲を狭めることができ、画像処理に要する演算負荷を軽減することができる。

［第２実施形態の説明］
次に、本発明に係る自己位置算出装置１００の第２実施形態について説明する。装置構成は、前述した図１と同様である。前述した第１実施形態では、投光器１１より投光するパターン光の輝度を正弦波状に変化させ、カメラ１２で撮像される画像を同期検波処理することにより、パターン光３２ａを検出する例について示した。これに対して、第２実施形態では、所定の切替周期でパターン光３２ａの投光をオン、オフ制御し、パターン光３２ａの投光時（オン時）に撮像した画像と、パターン光３２ａの非投光時（オフ時）に撮像した画像の差分を演算して、パターン光３２ａを取得する。

即ち、パターン光制御部２７は、図１１（ａ）に示すように、所定の切替周期でパターン光３２ａのオン、オフを切り替える。そして、特徴点検出部２３は、図１１（ｂ）に示すように、パターン光３２ａがオフのときに特徴点検出フラグを「１」として特徴点を検出する。

以下、第２実施形態に係る自己位置算出装置１００の処理手順を、図１０に示すフローチャートを参照して説明する。

初めに、図１０のステップＳ３１において、パターン光制御部２７は、投光器１１を制御して、車両周囲の路面３１にパターン光を投光する。この際、パターン光制御部２７は、図１１（ａ）に示すように、所定の切替周期でパターン光のオン、オフが切り替えられるように投光電力を制御する。

ステップＳ３２において、カメラ１２は、パターン光が投光されている領域を含む路面３１を撮像して画像を取得し、この画像をメモリに記憶する。ステップＳ３３において、特徴点検出部２３は、投光器１１より投光されるパターン光３２ａがオフであるか否かを判断する。

パターン光３２ａがオンである場合には（ステップＳ３３でＮＯ）、ステップＳ３４において、パターン光抽出部２１は、メモリに記憶されているパターン光オン時の画像と、以前に撮像されているパターン光オフ時の画像との差分画像を生成し、該差分画像からパターン光を抽出する。即ち、パターン光３２ａの投光時に撮像した画像と、パターン光３２ａの非投光時に撮像した画像の差分を演算することにより、パターン光３２ａを抽出することができる。なお、この際、車両１０は移動しており、車両１０と路面との相対位置は変化するが、カメラ１２のフレームレートが大きいので、車両１０の移動距離を無視することができる。

そして、前述した第１実施形態と同様に、パターン光３２ａの位置を示すデータとして、スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）を算出し、これをメモリに記憶する。更に、姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から、距離及び姿勢角を算出し、メモリに記憶する。その後、ステップＳ３６に処理を進める。

一方、パターン光がオフ時である（パターン光の輝度が閾値輝度以下である）場合には（ステップＳ３３でＹＥＳ）、ステップＳ３５において、特徴点検出部２３は、路面３１に存在する特徴点を検出する。即ち、図１１（ｂ）に示す特徴点検出フラグが「１」とされている時間帯に、カメラ１２で撮像された画像を用いて特徴点を検出する。更に、姿勢変化量算出部２４は、特徴点検出部２３で検出された特徴点の画像上の位置に基づき、カメラ１２に対する特徴点の変化量を算出する。特徴点を検出する領域は、パターン光３２ａを投光する領域と同一の領域としている。特徴点の位置から、カメラ１２の距離及び姿勢角の変化量を算出する手法は、前述した第１実施形態と同様である。その後、ステップＳ３６、Ｓ３７に処理を進める。ステップＳ３６、Ｓ３７の処理は、図７に示したステップＳ１６、Ｓ１７の処理と同様であるので説明を省略する。

こうして、第２実施形態に係る自己位置算出装置１００は、上記した一連の情報処理サイクルを繰り返し実行して車両１０の移動量を積算することにより、車両１０の位置を算出することができるのである。

このようにして、第２実施形態に係る自己位置算出装置１００では、投光器１１より所定の切替周期でオン、オフが切り替えられるパターン光３２ａを路面に投光し、このパターン光３２ａを含む画像をカメラ１２で撮像する。そして、パターン光３２ａをオンとしたときの画像と、パターン光３２ａをオフとしたときの画像の差分を演算して、パターン光３２ａを抽出する。従って、路面上に投光したパターン光３２ａを高精度に検出することが可能となる。そして、パターン光３２ａの位置に基づいてカメラ１２の距離及び姿勢角を算出するので、距離及び姿勢角を高精度に算出することができる。

また、パターン光３２ａがオフとされている時間帯に、路面上に存在する特徴点を検出する。この際、パターン光３２ａは投光されていないので、特徴点の検出にパターン光３２ａの影響を受けること無く、高精度な特徴点検出が可能となる。そして、上述した方法を採用することにより、車両の移動量を算出することができる。その結果、車両の移動量を高精度に算出することが可能となる。

なお、第２実施形態に係る自己位置算出装置１００についても、前述した第１実施形態と同様に、パターン光のオン、オフを切り替える際の切替周波数を、例えば２００［Ｈｚ］とし、カメラ１２のフレームレートを２４００［ｆｐｓ］とすることが望ましい。

［第３実施形態の説明］
次に、本発明の第３実施形態について説明する。図１２は、第３実施形態に係る自己位置算出装置１００ａの構成を示すブロック図である。第３実施形態では、図１に示した自己位置算出装置１００と対比して、特徴点移動速度算出部３７が設けられている点で相違する。

特徴点移動速度算出部３７は、特徴点検出部２３で検出された特徴点の位置座標の変化に基づいて、特徴点の移動速度を算出する。特徴点を取得する周期が既知であるから、特徴点の移動量を求めることにより特徴点の移動速度を算出することができる。この特徴点の移動速度データを姿勢変化量算出部２４に出力する。そして、姿勢変化量算出部２４は、特徴点の移動速度データの大きさに基づいて、姿勢変化量の算出周期を変更する。一例として、予め閾値速度を設定しておき、特徴点移動速度算出部３７より取得される特徴点の移動速度が閾値速度よりも高い場合には、姿勢変化量の算出周期が短くなるように設定する。

図１２における特徴点移動速度算出部３７以外の構成は、前述した図１と同様であるので、同一符号を付して構成説明を省略する。

次に、第３実施形態に係る自己位置算出装置１００ａの作用を、図１３に示すフローチャート、及び図１４に示すタイミングチャートを参照して説明する。図１３に示すフローチャートは、前述した図７のフローチャートに対して、ステップＳ１３１、Ｓ１３２、Ｓ１３３の処理が追加されている点で相違する。それ以外は図７と同様であるので、同一のステップ番号を付している。ステップＳ１１，Ｓ１２の後、ステップＳ１３の処理において、パターン光３２ａの輝度が閾値輝度Ｂth以下であると判断された場合には、ステップＳ１３１において、姿勢変化量算出部２４は、特徴点移動速度算出部３７にて取得されるの特徴点移動速度が予め設定した閾値速度より大きいか否かを判断する。

そして、閾値速度以下である場合には（ステップＳ１３１でＮＯ）、ステップＳ１３２において、姿勢変化量算出部２４は、第１の演算周期で車両１０の姿勢変化量を算出する。例えば、図１４（ｂ）に示す周期τ２で姿勢変化量を算出する。一方、閾値速度より大きい場合には（ステップＳ１３１でＹＥＳ）、ステップＳ１３３において、姿勢変化量算出部２４は、第１の演算周期よりも短い第２の演算周期で車両１０の姿勢変化量を算出する。例えば、図１４（ｂ）に示す周期τ１（τ１＜τ２）で姿勢変化量を算出する。その後、ステップＳ１５〜Ｓ１７の処理を実行する。なお、図１４では、一例としてτ１の２倍をτ２とする例を示しているが、本発明はこれに限定されるものではない。また、一例として、閾値速度により特徴点の検出周期を切り替える例を示しているが、特徴点移動速度に応じて特徴点の検出周期を逐次変化させる構成としてもよい。

路面に存在する特徴点をトラッキングするためには、原理的には、特徴点を検出する前後のフレーム間で、車両の移動量が撮像範囲以内であればよい。また、前後フレームでの特徴点の対応付けに間違いが起こらないようにするためには、移動量はなるべく小さい方がよい。しかし、前後フレーム間の時間が短すぎる場合には、路面上での特徴点が殆ど変化しなくなり、移動量の計算が難しくなる。つまり、特徴点をトラッキングして移動量を算出する場合には、前後フレーム間で、特徴点がある程度移動している必要がある。

第３実施形態では、例えば、路面に存在する特徴点が、１周期で０．０５〜０．１［ｍ］の範囲となるように、特徴点の検出周期を設定する。従って、パターン光３２ａを投光する際の最大の周波数を２００［Ｈｚ］とした場合、本実施形態で特徴点の移動速度［２０ｍ／ｓ］（≒車速７２［Ｋｍ／ｈ］）においては、この最大周波数２００［Ｈｚ］で制御を行うと目標範囲内の０．１ｍとなる。これに対し、特徴点の移動速度１［ｍ／ｓ］（≒車速３．６［Ｋｍ／ｈ］）では、周波数を２０［Ｈｚ］とすることで、目標範囲内の０．０５ｍとすることができる。なお、この計算方法によると停車中は路面テクスチャの検出を中止してしまうため、周波数は最低で５［Ｈｚ］とする。

このように、第３実施形態に係る自己位置算出装置１００ａでは、前述した第１実施形態と同様の効果を達成できる。更に、特徴点の移動速度に応じて、姿勢変化量を算出する周期を変更するので、特徴点の移動速度に適した姿勢変化量の算出が可能となり、車両１０の移動量の検出をより高精度に行うことができる。

［第３実施形態の第１変形例の説明］
次に、第３実施形態の第１変形例について説明する。装置構成は、図１２と同一であるので、説明を省略する。以下、第１変形例に係る自己位置算出装置１００ａの作用を図１５に示すフローチャート、及び図１６に示すタイミングチャートを参照して説明する。なお、図５に示すフローチャートは、前述した図１０に示したフローチャートに対して、ステップＳ３０１〜３０３を追加しており、それ以外の構成は図１０と同一であるので、同一のステップ番号を付している。

初めに、ステップＳ３０１において、特徴点移動速度算出部３７は、特徴点の移動速度を算出し、この特徴点の移動速度が予め設定した閾値速度よりも大きいか否かを判断する。そして、閾値速度以下の場合には（ステップＳ３０１でＮＯ）、ステップＳ３０２に処理を進め、閾値速度よりも大きい場合には（ステップＳ３０１でＹＥＳ）、ステップＳ３０３に処理を進める。

ステップＳ３０２において、姿勢変化量算出部２４は、パターン光のオン、オフ周期を第１の切替周期に設定する。具体的には、図１６（ａ）に示すように、時間τ１１だけパターン光が投光され、時間τ１２の経過後、再度パターン光が投光されるように周期を設定する。従って、図１６（ｂ）に示すように、時間τ１２の間に特徴点が検出される。

ステップＳ３０３において、姿勢変化量算出部２４は、パターン光のオン、オフ周期を第２の切替周期に設定する。具体的には、図１６（ｃ）に示すように、時間τ１３だけパターン光が投光され、時間τ１４の経過後、再度パターン光が投光されるように周期を設定する。従って、図１６（ｄ）に示すように、時間τ１４の間に特徴点が検出される。この際、（τ１１＋τ１２）が第１の切替周期であり、（τ１３＋τ１４）が第２の切替周期である。そして、（τ１１＋τ１２）＞（τ１３＋τ１４）である。つまり、第２の切替周期は第１の切替周期よりも短くなるように設定される。

次いで、図１５のステップＳ３１において、パターン光制御部２７は、投光器１１を制御して、第１の切替周期、或いは第２の切替周期で車両周囲の路面３１にパターン光を投光する。ステップＳ３２以降の処理は、前述した図１０と同一であるので説明を省略する。

このように、第３実施形態の第１変形例に係る自己位置算出装置１００ａでは、前述した第３実施形態と同様に、特徴点の移動速度に応じて、姿勢変化量を算出する周期を変更するので、特徴点の移動速度に適した姿勢変化量の算出が可能となり、車両１０の移動量の検出をより高精度に行うことができる。

［第３実施形態の第２変形例の説明］
次に、第３実施形態の第２変形例について説明する。前述した第３実施形態では、特徴点検出部２３で検出される特徴点の移動量に基づき、特徴点移動速度算出部３７により特徴点の移動速度を算出する例について説明した。第２変形例では、特徴点の移動速度に代わり、車両に搭載される車速センサの検出信号から車速を取得し、車速に応じて姿勢変化量を算出する周期を変更する。即ち、図１７に示すように、車速センサより出力される車速データを取得する車速取得部３６を設け、該車速取得部３６で取得した車速データを姿勢角算出部２４に出力して、姿勢変化量を算出する周期を変更するようにしてもよい。このような構成においても、前述した第３実施形態と同様の効果を達成することができる。

以上、本発明の自己位置算出装置、及び自己位置算出方法を図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置き換えることができる。

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
１３ ＥＣＵ
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
２７ パターン光制御部
３２ａ，３２ｂ パターン光
３６ 車速取得部
３７ 特徴点移動速度算出部
１００，１００ａ，１００ｂ 自己位置算出装置

Claims (8)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記パターン光の輝度が周期的に変化するように制御するパターン光制御部と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得された画像からパターン光を抽出するパターン光抽出部と、
   前記パターン光抽出部にて抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
   を備え、
   前記姿勢変化量算出部は、前記パターン光の輝度が閾値輝度以下のときに、前記特徴点を検出すること
   を特徴とする自己位置算出装置。
2. 前記パターン光制御部は、前記パターン光を所定の変調周波数で輝度変調し、前記パターン光抽出部は、前記撮像部で撮像された画像を前記所定の変調周波数で同期検波して、前記パターン光を抽出することを特徴とする請求項１に記載の自己位置算出装置。
3. 前記姿勢角算出部は、撮像部で撮像される２周期以上の画像を同期検波して得られるパターン光に基づいて、前記路面に対する車両の姿勢角を算出することを特徴とする請求項２に記載の自己位置算出装置。
4. 前記パターン光制御部は、前記パターン光を所定の切替周波数でオン、オフ制御し、前記パターン光抽出部は、前記パターン光のオン時に撮像された画像と、前記パターン光がオフ時に撮像された画像の差分を演算して前記パターン光を抽出することを特徴とする請求項１に記載の自己位置算出装置。
5. 前記姿勢変化量算出部は、前記複数の特徴点の移動速度を求め、該移動速度が高いほど、姿勢変化量の算出周期を短く設定することを特徴とする請求項２〜４のいずれか１項に記載の自己位置算出装置。
6. 車両に搭載される車速センサにて検出される車速データを取得する車速取得部を更に備え、前記姿勢変化量算出部は、車両の速度が高いほど、姿勢変化量の算出周期を短く設定することを特徴とする請求項２〜４のいずれか１項に記載の自己位置算出装置。
7. 前記路面上の各特徴点を検出する領域は、前記パターン光を投光する領域と全部または一部が重なっていることを特徴とする請求項１〜６のいずれか１項に記載の自己位置算出装置。
8. 車両周囲の路面にパターン光を投光する投光手順と、
   前記パターン光の輝度が周期的に変化するように制御するパターン光制御手順と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像手順と、
   前記撮像手順で取得された画像からパターン光を抽出するパターン光抽出手順と、
   前記パターン光抽出手順にて抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出手順と、
   前記撮像手順により取得された画像から路面上の複数の特徴点を検出し、各特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出手順と、
   前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
   を備え、
   前記パターン光の輝度が閾値輝度以下のときに、前記特徴点を検出すること
   を特徴とする自己位置算出方法。