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

Abstract

車両周囲の路面にパターン光を投光する投光器１１と、車両に搭載され、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部１２と、撮像部１２で取得した画像におけるパターン光の形から路面に対する車両の姿勢角を算出する姿勢角算出部２２と、撮像部１２で取得した画像から、路面上の複数の特徴点を検出する特徴点検出部２３と、特徴点検出部２３により検出された路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出する姿勢変化量算出部２４と、路面に対する車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことによって車両の現在位置及び姿勢角を算出する自己位置算出部２５とを備え、投光器１１は、特徴点検出部での路面の特徴点の検出状態に応じて、複数のパターン光投光領域のうちの特定のパターン光投光領域に選択的にパターン光を投光する。

Description

本発明は、自己位置算出装置及び自己位置算出方法に関する。

車両に搭載されたカメラで車両近傍の画像を撮像して取得し、この画像の変化に基づいて車両の移動量を求める技術が知られている（特許文献１参照）。特許文献１では、車両が低速かつ微妙に移動した場合でも精度よく移動量を求められるように、画像の中から特徴点を検出し、この特徴点の位置を求め、特徴点の移動方向及び移動距離（移動量）から車両の移動量を求めている。

また、格子パターン（パターン光）を投光するレーザ投光器を用いて三次元計測を行う技術が知られている（特許文献２参照）。特許文献２では、パターン光の投光領域をカメラで撮像し、撮像した画像からパターン光を抽出して、パターン光の位置から車両の挙動を求めている。

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

しかしながら、特許文献２に記載されたパターン光の投光領域と同じ領域で、特許文献１に記載された技術により路面上の特徴点を検出しようとした場合、パターン光と特徴点の判別が難しい。一方、パターン光の投光領域から離れた領域で路面上の特徴点を検出しようとすると、特徴点の移動量の算出誤差が大きくなる。

上記問題点を鑑み、本発明は、パターン光と区別して特徴点を容易に判別することができ、車両の自己位置を精度よく算出することができる自己位置算出装置及び自己位置算出方法を提供することを目的とする。

本発明の第一の態様に係る自己位置算出装置は、車両周囲の路面にパターン光を投光し、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得し、取得した画像におけるパターン光の形から路面に対する車両の姿勢角を算出し、取得した画像から、路面上の複数の特徴点を検出し、検出された路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出し、路面に対する車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことによって車両の現在位置及び姿勢角を算出し、パターン光を投光するときには、特徴点検出部での路面の特徴点の検出状態に応じて、複数のパターン光投光領域のうちの特定のパターン光投光領域に選択的にパターン光を投光する。

図１は、第１実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図２は、投光器及びカメラを車両に搭載する方法の一例を示す外観図である。 図３（ａ）は、投光器とカメラを用いてスポット光が照射された路面上の位置を算出する様子を示す図であり、図３（ｂ）は、パターン光が照射された領域とは異なる領域で検出された特徴点の時間変化からカメラの移動方向を求める様子を示す図である。 図４は、カメラで取得した画像に二値化処理を施したパターン光の画像を示す図であり、図４（ａ）はパターン光全体を示す図であり、図４（ｂ）は１つのスポット光を拡大して示す図であり、図４（ｃ）はスポット光の重心位置を示す図である。 図５は、距離及び姿勢角の変化量を算出する方法を説明するための模式図である。 図６は、画像上に検出された特徴点を示す図であり、図６（ａ）は時刻ｔに取得した第１フレーム（画像）を示す図であり、図６（ｂ）は時刻ｔ＋Δｔに取得した第２フレームを示す図である。 図７（ａ）は、カメラで取得した画像を示す図であり、図７（ｂ）は、カメラで取得した画像に二値化処理を施したパターン光の画像を示す図であり、図７（ｃ）は、特徴点の検出結果を示す図である。 図８は、上下左右に分割されたパターン光投光領域の一例を示す概略図である。 図９（ａ）は、検出された特徴点の一例を示す概略図であり、図９（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図１０（ａ）は、選択されたパターン光投光領域の時間変化を表すグラフであり、図１０（ｂ）は、各パターン光投光領域で検出された特徴点数の時間変化を表すグラフである。 図１１は、第１実施形態に係る自己位置算出方法の一例を説明するためのフローチャートである。 図１２は、ステップＳ０１の詳細を示すフローチャートである。 図１３（ａ）は、検出された特徴点の一例を示す概略図であり、図１３（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図１４（ａ）は、各パターン光投光領域の投光フラグの時間変化を表すグラフであり、図１４（ｂ）は、各パターン光投光領域で検出された特徴点数の時間変化を表すグラフである。 図１５は、縦縞模様で交互に組み合わせたパターン光投光領域の一例を示す概略図である。 図１６（ａ）は、検出された特徴点の一例を示す概略図であり、図１６（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図１７（ａ）は、選択されたパターン光投光領域の時間変化を表すグラフであり、図１７（ｂ）は、各パターン光投光領域で検出された特徴点数の時間変化を表すグラフである。 図１８は、左右２分割したパターン光投光領域の一例を示す概略図である。 図１９（ａ）は、検出された特徴点の一例を示す概略図であり、図１９（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図２０は、スポット光の投光領域の一例を示す概略図である。 図２１（ａ）は、検出された特徴点の一例を示す概略図であり、図２１（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図２２（ａ）は、各スポット光の投光フラグの時間変化を表すグラフであり、図２２（ｂ）は、各スポット光の位置に特徴点が存在するか否かの判定結果の時間変化を表すグラフである。 図２３は、第２実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図２４（ａ）は、検出された特徴点の一例を示す概略図であり、図２４（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図２５（ａ）は、各スポット光の投光フラグの時間変化を表すグラフであり、図２５（ｂ）は、各スポット光の位置に特徴点が存在するか否かの推定結果の時間変化を表すグラフである。 図２６は、第２実施形態に係るステップＳ０１の詳細の一例を示すフローチャートである。 図２７は、車幅方向で４分割されたパターン光投光領域の一例を示す概略図である。 図２８（ａ）は、特徴点増減予測エリアの設定方法及び検出された特徴点の一例を示す概略図であり、図２８（ｂ）は、選択されたパターン光投光領域の一例を示す概略図である。 図２９（ａ）は、各スポット光の投光フラグの時間変化を表すグラフであり、図２９（ｂ）は、各特徴点増減予測エリアに特徴点が存在するか否かの判定結果の時間変化を表すグラフである。 図３０は、路面に対する距離及び姿勢を算出するか場合分けをする場合のフローチャートの一例を示す。

以下、本発明を適用した第１及び第２実施形態について図面を参照して説明する。

（第１実施形態）
［ハードウェア構成］
先ず、図１を参照して、第１実施形態に係る自己位置算出装置のハードウェア構成を説明する。自己位置算出装置は、投光器１１と、カメラ１２と、エンジンコントロールユニット（ＥＣＵ）１３とを備える。投光器１１は、車両に搭載され、車両周囲の路面にパターン光を投光する。カメラ１２は、車両に搭載され、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部の一例である。ＥＣＵ１３は、投光器１１を制御し、且つカメラ１２で取得した画像から車両の自己位置を算出する一連の情報処理サイクルを実行する制御部の一例である。

カメラ１２は、固体撮像素子、例えばＣＣＤ及びＣＭＯＳを用いたデジタルカメラであって、画像処理が可能なデジタル画像を取得する。カメラ１２の撮像対象は車両周囲の路面であって、車両周囲の路面には車両の前部、後部、側部、車両底部の路面が含まれている。例えば、図２に示すように、カメラ１２は、車両１０の前部、具体的にはフロントバンパ上に搭載することができる。車両１０の前方の路面３１上の特徴点（テクスチャ）及び投光器１１から投光されたパターン光３２ｂを撮像できるように、カメラ１２を設置する高さ及び向きが調整され、且つ、カメラ１２が備えるレンズのピント及び絞りについても自動調整される。カメラ１２は、所定の時間間隔をおいて繰り返し撮像を行い、一連の画像（フレーム）群を取得する。カメラ１２で取得した画像データは、撮像する毎にＥＣＵ１３へ転送され、ＥＣＵ１３が備えるメモリに記憶される。

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

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

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

パターン光抽出部２１は、カメラ１２で取得した画像をメモリから読み込み、この画像からパターン光の位置を抽出する。図３（ａ）に示すように、例えば、投光器１１が行列状に配列された複数のスポット光からなるパターン光３２ａを路面３１に向けて投光し、路面３１で反射したパターン光３２ａをカメラ１２で検出する。パターン光抽出部２１は、カメラ１２で取得した画像に対して二値化処理を施すことにより、図４（ａ）及び図４（ｂ）に示すように、スポット光Ｓｐの画像のみを抽出する。そして、図４（ｃ）に示すように、パターン光抽出部２１は、各スポット光Ｓｐの重心の位置Ｈｅ、即ちスポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）を算出することにより、パターン光３２ａの位置を抽出する。座標は、カメラ１２の撮像素子の画素を単位とし、５×７のスポット光Ｓｐの場合、“ｊ”は１以上３５以下の自然数である。スポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）は、パターン光３２ａの位置を示すデータとしてメモリに記憶される。

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

なお、車両１０に対するカメラ１２の搭載位置及び撮像方向は既知であるため、実施形態では、路面３１に対する車両１０の距離及び姿勢角の一例として、路面３１に対するカメラ１２の距離及び姿勢角を算出する。換言すれば、路面３１に対するカメラ１２の距離及び姿勢角を算出することにより、路面３１と車両１０との間の距離、及び路面３１に対する車両１０の姿勢角を求めることができる。

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

なお、本実施形態では、毎回の情報処理サイクルにおいて距離及び姿勢角を算出する場合について説明する。

また、カメラ１２に対する各スポット光の相対位置（Ｘｊ，Ｙｊ，Ｚｊ）は同一平面上に存在しない場合が多い。なぜなら、路面３１に表出するアスファルトの凹凸に応じて各スポット光の相対位置が変化するからである。そこで、最小二乗法を用いて、各スポット光との距離誤差の二乗和が最小となるような平面式を求めてもよい。こうして算出された距離及び姿勢角のデータは、図１に示す自己位置算出部２５で使用される。

特徴点検出部２３は、カメラ１２で取得した画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「Ｄ．Ｇ．Ｌｏｗｅ，“Ｄｉｓｔｉｎｃｔｉｖｅ Ｉｍａｇｅ Ｆｅａｔｕｒｅｓ ｆｒｏｍ Ｓｃａｌｅ−Ｉｎｖａｒｉａｎｔ Ｋｅｙｐｏｉｎｔｓ，”Ｉｎｔ．Ｊ．Ｃｏｍｐｕｔ．Ｖｉｓ．，ｖｏｌ．６０，ｎｏ．２，ｐｐ．９１−１１０，Ｎｏｖ．２００」に記載の手法を用いることができる。或いは、特徴点検出部２３は、「金澤靖，金谷健一，“コンピュータビジョンのための画像の特徴点抽出，”信学誌，ｖｏｌ．８７，ｎｏ．１２，ｐｐ．１０４３−１０４８，Ｄｅｃ．２００４」に記載の手法を用いることもできる。

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

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

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

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

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

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

なお、姿勢変化量算出部２４は、時刻ｔと時刻ｔ＋Δｔで検出される各画像中で相対位置が算出された特徴点の全てを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定しても良い。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，Ｒ．Ｉ．Ｈａｒｔｌｅｙ：“Ａ ｌｉｎｅａｒ ｍｅｔｈｏｄ ｆｏｒ ｒｅｃｏｎｓｔｒｕｃｔｉｏｎ ｆｒｏｍ ｌｉｎｅｓ ａｎｄ ｐｏｉｎｔｓ，”Ｐｒｏｃ．５ｔｈＩｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ，Ｃａｍｂｒｉｄｇｅ，Ｍａｓｓａｃｈｕｓｅｔｔｓ，ｐｐ．８８２−８８７（１９９５））を用いることができる。

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

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

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

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

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

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

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

パターン光制御部２６は、投光器１１によるパターン光３２ａの投光を制御する。例えば、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に、パターン光制御部２６は、パターン光３２ａの投光を開始する。その後、自己位置算出装置が停止するまで、パターン光制御部２６は、パターン光３２ａを連続して投光する。或いは、所定の時間間隔をおいて、投光のオン／オフを繰り返してもよい。

ここで、７×５のスポット光Ｓｐからなるパターン光を投光した場合、図７（ａ）に示すような画像がカメラ１２により取得される。この画像に対して、パターン光抽出部２１が二値化処理を施すことにより、図７（ｂ）に示すようにパターン光（スポット光）Ｓｐを抽出することができる。一方、特徴点検出部２３が、パターン光Ｓｐの投光領域と同じ領域で路面３１上の特徴点を検出しようとすると、図７（ｃ）に示すように、パターン光Ｓｐに対して路面３１上の特徴点を判別することが難しく、特徴点の検出が難しい。これに対して、特徴点検出部２３が、パターン光Ｓｐの投光領域から離れた領域で路面上の特徴点を検出しようとすると、今度は特徴点の移動量の算出誤差が大きくなる。

そこで、本実施形態では、パターン光制御部２６は、特徴点検出部２３での路面の特徴点の検出状態に応じて、複数のパターン光投光領域のうち特定のパターン光投光領域に選択的にパターン光を投光する。

パターン光制御部２６は、例えば図８に示すように、撮像領域３０内に複数（４つ）のパターン光投光領域Ａ〜Ｄを設定する。図８において、矢印４１は車両の移動方向を示し、矢印４１と逆向きの矢印４２は特徴点の移動方向を示す。パターン光投光領域Ａ〜Ｄは、車両の移動方向４１（上下）及び車両の移動方向４１に直交する車幅方向（左右）に４分割された矩形のパターンである。なお、パターン光投光領域の数は複数であれば特に限定されず、２つ又は３つのパターン光投光領域を設定してもよく、５つ以上のパターン光投光領域を設定してもよい。

図９（ａ）に示すように特徴点検出部２３により特徴点Ｔｅが検出された場合、パターン光制御部２６は、パターン光投光領域Ａ〜Ｄ毎に検出できた特徴点Ｔｅの数をカウントする。ここで、パターン光制御部２６は、パターン光投光領域Ａ〜Ｄ内に完全に収まっている特徴点数をカウントしてもよい。或いは、パターン光投光領域Ａ〜Ｄ内に完全に収まっている特徴点に加え、パターン光投光領域Ａ〜Ｄの境界上に存在し、パターン光投光領域Ａ〜Ｄ内に一部が属している特徴点Ｔｅも含めてカウントしてもよい。

図９（ａ）において、パターン光投光領域Ａ〜Ｄ内に完全に収まっている特徴点数をカウントした場合、パターン光投光領域Ａ〜Ｄで検出できた特徴点数はそれぞれ１個、４個、４個、３個となる。パターン光制御部２６は、複数のパターン光投光領域Ａ〜Ｄのうち、図９（ｂ）に示すように、特徴点数が最も少ないパターン光投光領域Ａに選択的に投光する。

図１０（ａ）に、選択されたパターン光投光領域の時間変化を示し、図１０（ｂ）に、パターン光投光領域Ａ〜Ｄで検出できた特徴点数の時間変化を示す。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。時刻ｔ０において、パターン光制御部２６が特徴点検出部２３により検出できた特徴点Ｔｅの数をカウントすると、図１０（ｂ）に示すように、パターン光投光領域Ａで検出できた特徴点数が最も少ない。そこで、次の情報処理サイクルである時刻ｔ１において、パターン光制御部２６は、図１０（ａ）に示すように、前の情報処理サイクルの時刻ｔ０において特徴点数が最も少なかったパターン光投光領域Ａに選択的にパターン光を投光させる。

同様に、時刻ｔ１〜ｔ６において、パターン光制御部２６が特徴点数をカウントする。そして、次の情報処理サイクルである時刻ｔ２〜ｔ７において、前の情報処理サイクルの時刻ｔ１〜ｔ６において特徴点数が最も少なかった領域に選択的にパターン光を投光させる。

［情報処理サイクル］
次に、図１１を参照して、ＥＣＵ１３によって繰り返し実行される情報処理サイクルを説明する。この情報処理サイクルは、カメラ１２で取得した画像３８から車両１０の自己位置を算出する自己位置算出方法の一例である。

図１１のフローチャートに示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に開始され、自己位置算出装置が停止するまで繰り返し実行される。

図１１のステップＳ０１において、パターン光制御部２６は、投光器１１を制御して、車両周囲の路面３１にパターン光を投光する。図１１のフローチャートでは、パターン光３２ａを連続して投光する例を説明する。なお、ステップＳ０１の詳細は後述する。

ステップＳ０３に進み、ＥＣＵ１３は、カメラ１２を制御して、パターン光３２ａが投光された領域を含む車両周囲の路面３１を撮像して画像３８を取得する。ＥＣＵ１３は、カメラ１２で取得した画像データを、メモリに記憶する。

なお、ＥＣＵ１３はカメラ１２の絞りを自動制御できる。前の情報処理サイクルで取得した画像３８の平均輝度から、輝度値の最大値と最小値の中間値になるようにカメラ１２の絞りをフィードバック制御してもよい。また、パターン光３２ａが投光されている領域は輝度値が高いため、ＥＣＵ１３は、パターン光３２ａを抽出した部分を除いた領域から、平均輝度値を求めてもよい。

ステップＳ０５に進み、先ず、パターン光抽出部２１は、カメラ１２で取得した画像３８をメモリから読み込み、図４（ｃ）に示すように、画像３８からパターン光３２ａの位置を抽出する。パターン光抽出部２１は、パターン光３２ａの位置を示すデータとして算出されたスポット光Ｓｐの画像上の座標（Ｕｊ、Ｖｊ）をメモリに記憶する。

さらに、ステップＳ０５において、姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から路面３１に対する車両１０の距離及び姿勢角を算出してメモリに記憶する。

ステップＳ０７に進み、ＥＣＵ１３は、画像３８から特徴点Ｔｅを検出し、前後の情報処理サイクルの間で対応関係が取れる特徴点Ｔｅを抽出し、特徴点Ｔｅの画像上の位置（Ｕｉ、Ｖｉ）から距離及び姿勢角の変化量を算出する。さらに、車両の移動量を算出する。

具体的に、先ず、特徴点検出部２３は、カメラ１２で取得した図８に対応する撮像領域３０の画像３８をメモリから読み込み、画像３８から路面３１上の特徴点Ｔｅを検出し、各特徴点Ｔｅの画像上の位置（Ｕｉ、Ｖｉ）をメモリに記憶させる。

姿勢変化量算出部２４は、各特徴点Ｔｅの画像上の位置（Ｕｉ、Ｖｉ）をメモリから読み込み、姿勢角算出部２２で算出された距離及び姿勢角と、特徴点Ｔｅの画像上の位置（Ｕｉ、Ｖｉ）とから、カメラ１２に対する特徴点Ｔｅの相対位置（Ｘｉ，Ｙｉ，Ｚｉ）を算出する。なお、姿勢変化量算出部２４は、前の情報処理サイクルのステップＳ０９において設定された距離及び姿勢角を用いる。そして、姿勢変化量算出部２４は、カメラ１２に対する特徴点Ｔｅの相対位置（Ｘｉ，Ｙｉ，Ｚｉ）を、メモリに記憶する。

この後、姿勢変化量算出部２４は、特徴点Ｔｅの画像上の位置（Ｕｉ、Ｖｉ）と、前の情報処理サイクルのステップＳ０７において算出された特徴点Ｔｅの相対位置（Ｘｉ，Ｙｉ，Ｚｉ）をメモリから読み込む。姿勢変化量算出部２４は、前後の情報処理サイクルの間で対応関係が取れる特徴点Ｔｅの相対位置（Ｘｉ，Ｙｉ，Ｚｉ）及び画像上の位置（Ｕｉ、Ｖｉ）を用いて、距離及び姿勢角の変化量を算出する。さらに、姿勢変化量算出部２４は、前回の特徴点Ｔｅの相対位置（Ｘｉ，Ｙｉ，Ｚｉ）と今回の特徴点Ｔｅの相対位置（Ｘｉ，Ｙｉ，Ｚｉ）から、車両の移動量を算出してメモリに記憶する。ステップＳ０７で算出された「距離及び姿勢角の変化量」並びに「車両の移動量」は、ステップＳ１１の処理で用いられる。

ステップＳ０９に進み、ＥＣＵ１３は、最初の情報処理サイクルでは、パターン光３２ａを用いて算出した距離及び姿勢角、或いは所定の初期位置及び初期姿勢角を用いて、自己位置を算出するための積分演算の起点を設定する。以降の情報処理サイクルでは、前回フレームと今回フレームの間で対応関係が取れる３点以上の特徴点を継続して検出できれば、距離及び姿勢角の変化量を加算する処理（積分演算）を継続することにより、パターン光３２ａを用いずに距離及び姿勢角を更新し続けることができる。

ステップＳ１１に進み、自己位置算出部２５は、ステップＳ０９で設定された積分演算の起点と、ステップＳ０７の処理で算出された車両１０の距離及び姿勢角の変化量とから、車両１０の自己位置を算出する。

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

［ステップＳ０１の処理］
次に、図１２のフローチャートを参照して、図１１のステップＳ０１の詳細な手順を説明する。図１２に示すように、ステップＳ１０１において、パターン光制御部２６は、特徴点検出部２３により検出された前の情報処理サイクルの特徴点Ｔｅの位置をメモリから読み出し、前の情報処理サイクルにおける各パターン投光領域内で検出できた特徴点数をカウントする。ステップＳ１０２において、投光器１１は、複数のパターン光投光領域のうち、特徴点の検出数が最も少なかった領域に選択的に投光する。

［第１の変形例］
次に、第１の変形例として、パターン光の投光方法の他の一例を説明する。投光器１１は、複数のパターン光投光領域のうち、特徴点が最も少ない領域を選択する代わりに、特徴点が相対的に少ない複数（例えば２つ）の領域に選択的に投光してもよい。例えば、図１３（ａ）に示すように、パターン光投光領域Ａ〜Ｄ内に完全に収まっている特徴点数をカウントした場合、パターン光投光領域Ａ〜Ｄで検出できた特徴点数はそれぞれ１個、４個、４個、３個となる。投光器１１は、図１３（ｂ）に示すように、特徴点数が相対的に少ない２つのパターン光投光領域Ａ，Ｄに選択的に投光する。

図１４（ａ）にパターン光投光領域Ａ〜Ｄの投光フラグの時間変化を示し、図１４（ｂ）にパターン光投光領域Ａ〜Ｄで検出できた特徴点数を示す。パターン光投光領域Ａ〜Ｄの投光フラグは投光する場合に“１”、投光しない場合に“０”、に設定されている。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。時刻ｔ０において、パターン光制御部２６は、パターン光投光領域Ａ〜Ｄ毎で検出できた特徴点数をカウントすると、図１４（ｂ）に示すように、パターン光投光領域Ａ，Ｄで検出できた特徴点数が相対的に少ない。したがって、次の情報処理サイクルの時刻ｔ１において、投光器１１は、図１４（ａ）に示すように、前の情報処理サイクルの時刻ｔ０において特徴点数が相対的に少なかった２つのパターン光投光領域Ａ，Ｄに選択的に投光する。

同様に、時刻ｔ１〜ｔ６において、パターン光制御部２６は、パターン光投光領域Ａ〜Ｄ毎で検出できた特徴点数をカウントする。そして、次の情報処理サイクルの時刻ｔ２〜ｔ７において、投光器１１は、特徴点数が相対的に少ない２つの領域に選択的に投光する。

なお、投光器１１は、選択するパターン光投光領域Ａ〜Ｄの数を限定せずに、パターン光投光領域Ａ〜Ｄのうち、特徴点Ｔｅが所定の閾値以下の領域を選択してもよい。

［第２の変形例］
次に、第２の変形例として、パターン光の投光方法の他の一例を説明する。パターン光制御部２６は、図１５に示すように、特徴点の移動方向４２に延伸する縦縞模様に分割され、それぞれ交互に組み合わせた複数のパターン光投光領域Ａ，Ｂを設定してもよい。複数のパターン光投光領域Ａ，Ｂは、車幅方向にそれぞれ２つの領域を交互に有し、二者択一で選択される領域である。なお、複数のパターン光投光領域Ａ，Ｂのそれぞれの数は特に限定されず、複数のパターン光投光領域Ａ，Ｂのそれぞれが３つ以上の領域を有していてもよい。

パターン光制御部２６は、図１６（ａ）に示すように、２つのパターン光投光領域Ａで検出できた特徴点数の合計値及び２つのパターン光投光領域Ｂで検出できた特徴点数の合計値をそれぞれカウントする。パターン光投光領域Ａ，Ｂ内に完全に収まっている特徴点数をカウントした場合、パターン光投光領域Ａで検出できた特徴点数は合計４個であり、パターン光投光領域Ｂで検出できた特徴点数は合計１０個である。したがって、投光器１１は、図１６（ｂ）に示すように、特徴点数が相対的に少ないパターン光投光領域Ａに選択的に投光する。

図１７（ａ）はパターン光投光領域Ａ，Ｂのパターン光投光領域の時間変化を示し、図１７（ｂ）はパターン光投光領域Ａ，Ｂで検出できた特徴点数を示す。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。時刻ｔ０において、パターン光制御部２６は、パターン光投光領域Ａ，Ｂで検出できた特徴点数をカウントすると、図１７（ｂ）に示すように、パターン光投光領域Ａで検出できた特徴点数が相対的に少ない。このため、投光器１１は、図１７（ａ）に示すように、次の情報処理サイクルの時刻ｔ１において、前の情報処理サイクルの時刻ｔ０において特徴点数が相対的に少なかったパターン光投光領域Ａに選択的に投光する。同様に、投光器１１は、時刻ｔ２〜ｔ７において、前の情報処理サイクルの時刻ｔ１〜ｔ６において特徴点数が相対的に少なかった一方の領域に選択的に投光する。

［第３の変形例］
次に、第３の変形例として、パターン光の投光方法の他の一例を説明する。パターン光制御部２６は、図１８に示すように、車幅方向（左右）に２分割された２つのパターン光投光領域Ａ，Ｂを設定してもよい。

例えば、図１９（ａ）に示すように、パターン光投光領域Ａ，Ｂ内に完全に収まっている特徴点数をカウントした場合、パターン光投光領域Ａ，Ｂで検出できた特徴点数はそれぞれ３個、７個である。パターン光制御部２６は、図１９（ｂ）に示すように、特徴点数が相対的に少ないパターン光投光領域Ａに選択的に投光する。この場合も、図１７（ａ）及び図１７（ｂ）に示すように、投光器１１は、時刻ｔ１〜ｔ７において、前の情報処理サイクルの時刻ｔ０〜ｔ６において特徴点数が相対的に少なかった一方の領域に選択的に投光する。

［第４の変形例］
次に、第４の変形例として、特徴点Ｔｅの検出位置に応じてパターン光の位置を設定する場合を説明する。パターン光制御部２６は、図２０に示すように、複数のパターン光投光領域として、ｍ×ｎのスポット光Ｓｍｎ（ここでは５×５のスポット光Ｓ１１，Ｓ１２，…，Ｓ５５）を設定する。また、図２１（ａ）に示すように、特徴点検出部２３により特徴点Ｔｅの位置が検出される。

パターン光制御部２６は、特徴点検出部２３により検出された特徴点Ｔｅの位置が、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の位置（投光領域）と重なるか否かをそれぞれ判定する。この判定は前の情報処理サイクルで行う必要があるため、現在の情報処理サイクルでは特徴点Ｔｅが検出位置から移動していることを考慮し、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の投光領域よりも広げた領域を、特徴点Ｔｅの位置と重なるか判定してもよい。なお、消灯しているスポット光の位置は、路面に対するカメラ１２の距離及び姿勢角、並びに他のスポット光の位置から推定して求めることができる。投光器１１は、図２１（ｂ）に示すように、特徴点Ｔｅと重なると判定された位置のスポット光は消灯し、特徴点Ｔｅと重ならないと判定された位置のスポット光を選択的に投光する。

図２２（ａ）に、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の投光フラグの時間変化を示し、図２２（ｂ）に、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の位置に特徴点Ｔｅが重なるかの判定結果の時間変化を示す。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。図２２（ｂ）に示すように、時刻ｔ０において、パターン光制御部２６は、例えばスポット光Ｓ１１，Ｓ５５の位置に特徴点Ｔｅが重ならないと判定し、スポット光Ｓ１２の位置に特徴点Ｔｅが重なると判定している。投光部１１は、図２２（ａ）に示すように、次の情報処理サイクルの時刻ｔ１において、前の情報処理サイクルで特徴点Ｔｅが重なると判定された位置のスポット光Ｓ１２を消灯し、特徴点Ｔｅが重ならないと判定された位置のスポット光Ｓ１１，Ｓ５５を選択的に点灯する。同様に、時刻ｔ２〜ｔ７において、投光部１１は、前の情報処理サイクルの時刻ｔ１〜ｔ６で特徴点Ｔｅが重なると判定された位置のスポット光を消灯し、特徴点Ｔｅと重ならないと判定された位置のスポット光を選択的に投光する。

［第１実施形態の効果］
第１実施形態によれば、特徴点Ｔｅの検出状態に応じて、図９（ａ）及び図９（ｂ）に示すように、複数のパターン光投光領域Ａ〜Ｄのうち特定のパターン光投光領域Ａに選択的に投光することにより、特徴点Ｔｅが相対的に多い領域ではパターン光が投光されないため、パターン光と区別して特徴点Ｔｅを容易に判別することができる。したがって、車両１０の自己位置を精度よく算出することができる。

また、図１５に示すように、複数のパターン光投光領域Ａ，Ｂが特徴点Ｔｅの移動方向に延伸する縦縞模様のポジとネガに分割されており、投光器１１が、複数のパターン光投光領域Ａ，Ｂのうち特徴点Ｔｅの検出数が相対的に少ない領域にパターン光を投光することにより、より簡単な機構で選択的にパターン光を投光することができる。

また、図１８に示すように、複数のパターン光投光領域Ａ，Ｂが車幅方向（左右）に２分割されており、投光器１１が、複数のパターン光投光領域Ａ，Ｂのうち、特徴点Ｔｅの検出数が相対的に少ない一方に投光することにより、より簡単な機構で選択的にパターン光を投光することができる。

また、図２１（ｂ）に示すように、特徴点Ｔｅの検出位置に応じてパターン光投光領域（スポット光）Ｓ１１，Ｓ１２，…，Ｓ５５の位置を設定することにより、パターン光投光領域が比較的細かい場合でもパターン光と区別して特徴点Ｔｅを容易に判別することができる。

（第２の実施の形態）
［ハードウェア構成］
第２実施形態に係る自己位置算出装置は、図２３に示すように、車両挙動検出部１４を更に備える点が、第１実施形態に係る自己位置算出装置の構成と異なる。車両挙動検出部１４としては、車速を検出するための車輪速センサや横方向速度を検出するためのヨーレートセンサが使用可能である。第２実施形態に係る自己位置算出装置の他の構成は、第１実施形態に係る自己位置算出装置と実質的に同様であるので、重複した説明を省略する。

パターン光制御部２６は、特徴点Ｔｅの移動方向及び移動量を推定し、推定した特徴点Ｔｅの移動方向及び移動量に応じてパターン光の位置を設定する。各特徴点Ｔｅの移動方向と移動量は、前後の２つの情報処理サイクルから求めた車両の移動量から推定することができる。或いは、車両挙動検出部１４により検出された車速やヨーレートから推定することもできる。

図２４（ａ）に、１情報処理サイクル前に検出された特徴点Ｔｅを示し、各特徴点Ｔｅについて推定された移動方向及び移動量をベクトルＤｔｅで示す。パターン光制御部２６は、各特徴点Ｔｅについて推定された移動方向及び移動量から各特徴点Ｔｅの現在の情報処理サイクルにおける位置を推定し、特徴点Ｔｅの位置が、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の位置に重なるか推定する。図２４（ｂ）に示すように、現在の情報処理サイクルにおいて特徴点Ｔｅが重なると推定された位置のスポット光を消灯し、特徴点Ｔｅが重ならないと推定された位置のスポット光を選択的に点灯する。

図２５（ａ）に、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の投光フラグの時間変化を示し、図２５（ｂ）に、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の位置に特徴点Ｔｅが存在するか否かの推定結果の時間変化を示す。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。図２５（ｂ）に示すように、時刻ｔ０において、次の情報処理サイクルである時刻ｔとなった場合にスポット光Ｓ１１，Ｓ５５の位置に特徴点Ｔｅがないと推定され、スポット光Ｓ１２の位置には特徴点Ｔｅがあると推定されている。投光器１１は、図２５（ｂ）に示すように、次の現サイクルの時刻ｔ１において、特徴点Ｔｅがあると推定された位置のスポット光Ｓ１１，Ｓ５５を消灯し、特徴点Ｔｅがないと推定された位置のスポット光Ｓ１２を選択的に点灯する。同様に、時刻ｔ１〜ｔ６において次の投光器１１は、次の情報処理サイクルの時刻ｔ２〜ｔ７における特徴点Ｔｅがスポット光の位置にあるか推定する。そして、次の情報処理サイクルの時刻ｔ２〜ｔ７において、特徴点Ｔｅがあると推定された位置のスポットを消灯し、特徴点Ｔｅがないと推定された位置のスポット光を選択的に点灯する。

次に、図２６を参照して第２実施形態に係る自己位置算出方法のステップＳ０１の詳細を説明する。ステップＳ２０１において、車両挙動検出部１４が、車速及びヨーレートを検出する。ステップＳ２０２において、パターン光制御部２６が、車両挙動検出部１４により検出された車速及びヨーレートに基づいて、前の情報処理サイクルで検出できた特徴点Ｔｅの移動量及び移動方向を推定し、更には現在の情報処理サイクルにおける特徴点Ｔｅの位置を推定する。ステップＳ２０３において、パターン光制御部２６が、特徴点Ｔｅの推定位置とスポット光Ｓ１１，Ｓ１２，…，Ｓ５５の位置が重なるか判定する。投光器１１は、パターン光制御部２６による判定の結果、特徴点Ｔｅの推定位置と重なると判定された位置のスポット光を消灯し、特徴点Ｔｅの推定位置とは重ならないと判定された位置のスポット光のみ選択的に投光する。

［変形例］
次に、特徴点Ｔｅの移動方向に応じて、パターン光の位置を設定する場合の変形例を説明する。パターン光制御部２６は、図２７に示すように、車幅方向（左右）に４分割され、特徴点Ｔｅの移動方向４２にそれぞれ延伸するパターン光投光領域Ａ，Ｂ，Ｃ，Ｄを設定する。図２７に特徴点Ｔｅの移動方向４２を示すように、特徴点Ｔｅはカメラ１２により取得された画像に映り込んだ後、主に一方向に移動して画像から外れていく。そこで、特徴点Ｔｅが画像に映り込む側に特徴点増減予測エリアを設け、前の情報処理サイクルにおいて特徴点増減予測エリアに特徴点が存在するか否かを判断することにより、現在の情報処理サイクルにおいて特徴点Ｔｅが減少しそうな領域を狙って投光する。

例えば、パターン光制御部２６は、図２８（ａ）に示すように、撮像領域３０内の上部に特徴点増減予測エリアａ〜ｄを設定する。特徴点増減予測エリアａ〜ｄは、パターン光投光領域Ａ〜Ｄにそれぞれ対応しており、車幅方向においてパターン光投光領域Ａ〜Ｄのそれぞれと同じ位置にある。パターン光制御部２６は、各特徴点増減予測エリアａ〜ｄに特徴点Ｔｅが存在するか判定し、特徴点増減予測エリアａ，ｃには特徴点Ｔｅが存在しないと判定する。投光器１１は、図２８（ｂ）に示すように、特徴点増減予測エリアａ，ｃと同列のパターン光選択領域Ａ，Ｃに選択的に投光する。なお、各特徴点増減予測エリアａ〜ｄ内の特徴点Ｔｅの有無を判定する代わりに、各特徴点増減予測エリアａ〜ｄ毎に特徴点Ｔｅをカウントし、特徴点Ｔｅの相対的に少ない領域に投光してもよい。

図２９（ａ）に、パターン光選択領域Ａ〜Ｄの投光フラグの時間変化を示し、図２９（ｂ）に、特徴点増減予測エリアａ〜ｄに特徴点Ｔｅが存在するかの判定結果の時間変化を示す。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。図２９（ｂ）に示すように、時刻ｔ０において、特徴点増減予測エリアａ，ｄには特徴点Ｔｅが存在せず、特徴点増減予測エリアｂ，ｃに特徴点Ｔｅが存在したと判定されている。図２９（ａ）に示すように、次の情報処理サイクルの時刻ｔ１において、投光器１１は、前の情報処理サイクルにおいて特徴点Ｔｅが存在しないと判定された特徴点増減予測エリアａ，ｄと同列のパターン光投光領域Ａ，Ｄに選択的に投光する。同様に、パターン光制御部２６は、時刻ｔ２〜ｔ７において、前の情報処理サイクルの時刻ｔ１〜ｔ６において特徴点Ｔｅが存在しないと判定された特徴点増減予測エリアに対応するパターン光投光領域に選択的に投光する。

［第２実施形態の効果］
第２実施形態によれば、複数のパターン光投光領域（スポット光）Ｓ１１，Ｓ１２，…，Ｓ５５のうち、特徴点Ｔｅの検出状態に応じて特定のパターン光投光領域に選択的に投光することにより、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５と区別して特徴点Ｔｅを容易に判別することができる。したがって、車両１０の自己位置を精度よく算出することができる。

また、特徴点Ｔｅの移動方向に基づいて特徴点Ｔｅの現在の位置を推定し、パターン光投光領域（スポット光）Ｓ１１，Ｓ１２，…，Ｓ５５の位置を設定することにより、現在の情報処理サイクルにおけるスポット光Ｓ１１，Ｓ１２，…，Ｓ５５と特徴点Ｔｅとの重なりをより低減することができる。

（その他の実施の形態）
以上、第１及び第２実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。

例えば、図３０を参照して、ステップＳ０５のパターン光から路面に対する距離及び姿勢を算出する処理を必要な場合のみ実行する一例を説明する。図３０のステップＳ３０１において、ＥＣＵ１３が、ステップＳ０５の処理を実行するか否か、即ち、路面に対する距離及び姿勢を算出するか否かを場合分けする。ステップＳ０５のパターン光の位置から路面に対する距離及び姿勢角を算出する処理を実行する場合には、ステップＳ３０１に進み、パターン制御部２６が、前の情報処理サイクルにおいて、各パターン光投光領域で検出できた特徴点Ｔｅの数をカウントする。そして、ステップＳ３０２において、投光器１１が、例えば、検出できた特徴点Ｔｅが最も少ないパターン光投光領域に選択的にパターン光を投光する。その後、図１１のステップＳ０３に進む。一方、ステップＳ３０１でステップＳ０５のパターン光の位置から路面に対する距離及び姿勢角を算出する処理を実行しない場合には、ステップＳ３０４に進み、投光器１１はパターン光を消灯する。そして、ステップＳ３０５において、カメラ１２が画像を取得した後、図１１のステップＳ０７の手順に進む。

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
１３ ＥＣＵ
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２５ 自己位置算出部
２６ パターン光制御部
３１ 路面
３２ａ、３２ｂ パターン光
Ａ，Ｂ，Ｃ，Ｄ，Ｓ１１〜Ｓ５５ パターン光投光領域
Ｔｅ 特徴点

【０００２】
算出装置及び自己位置算出方法を提供することを目的とする。
［０００７］
本発明の第一の態様に係る自己位置算出装置は、車両周囲の路面にパターン光を投光し、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得し、取得した画像におけるパターン光の位置から路面に対する車両の姿勢角を算出し、取得した画像から、路面上の複数の特徴点を検出し、検出された路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出し、路面に対する車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことによって車両の現在位置及び姿勢角を算出し、パターン光を投光するときには、特徴点検出部での路面の特徴点の検出状態に応じて、複数のパターン光投光領域のうちの特定のパターン光投光領域に選択的にパターン光を投光する。
図面の簡単な説明
［０００８］
［図１］図１は、第１実施形態に係る自己位置算出装置の全体構成を示すブロック図である。
［図２］図２は、投光器及びカメラを車両に搭載する方法の一例を示す外観図である。
［図３］図３（ａ）は、投光器とカメラを用いてスポット光が照射された路面上の位置を算出する様子を示す図であり、図３（ｂ）は、パターン光が照射された領域とは異なる領域で検出された特徴点の時間変化からカメラの移動方向を求める様子を示す図である。
［図４］図４は、カメラで取得した画像に二値化処理を施したパターン光の画像を示す図であり、図４（ａ）はパターン光全体を示す図であり、図４（ｂ）は１つのスポット光を拡大して示す図であり、図４（ｃ）はスポット光の重心位置を示す図である。
［図５］図５は、距離及び姿勢角の変化量を算出する方法を説明するための模式図である。
［図６］図６は、画像上に検出された特徴点を示す図であり、図６（ａ）は時刻ｔに取得した第１フレーム（画像）を示す図であり、図６（ｂ）は時刻ｔ＋

図２５（ａ）に、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の投光フラグの時間変化を示し、図２５（ｂ）に、スポット光Ｓ１１，Ｓ１２，…，Ｓ５５の位置に特徴点Ｔｅが存在するか否かの推定結果の時間変化を示す。時刻ｔ０〜ｔ７は１情報処理サイクル毎の時刻を示す。図２５（ｂ）に示すように、時刻ｔ０において、次の情報処理サイクルである時刻ｔ１となった場合にスポット光Ｓ１１，Ｓ５５の位置に特徴点Ｔｅがないと推定され、スポット光Ｓ１２の位置には特徴点Ｔｅがあると推定されている。投光器１１は、図２５（ａ）に示すように、次の情報処理サイクルの時刻ｔ１において、特徴点Ｔｅがあると推定された位置のスポット光Ｓ１２を消灯し、特徴点Ｔｅがないと推定された位置のスポット光Ｓ１１，Ｓ５５を選択的に点灯する。同様に、時刻ｔ１〜ｔ６においてパターン制御部２６は、次の情報処理サイクルの時刻ｔ２〜ｔ７における特徴点Ｔｅがスポット光の位置にあるか推定する。そして、次の情報処理サイクルの時刻ｔ２〜ｔ７において、特徴点Ｔｅがあると推定された位置のスポットを消灯し、特徴点Ｔｅがないと推定された位置のスポット光を選択的に点灯する。

例えば、図３０を参照して、ステップＳ０５のパターン光から路面に対する距離及び姿勢を算出する処理を必要な場合のみ実行する一例を説明する。図３０のステップＳ３０１において、ＥＣＵ１３が、ステップＳ０５の処理を実行するか否か、即ち、路面に対する距離及び姿勢を算出するか否かを場合分けする。ステップＳ０５のパターン光の位置から路面に対する距離及び姿勢角を算出する処理を実行する場合には、ステップＳ３０２に進み、パターン制御部２６が、前の情報処理サイクルにおいて、各パターン光投光領域で検出できた特徴点Ｔｅの数をカウントする。そして、ステップＳ３０３において、投光器１１が、例えば、検出できた特徴点Ｔｅが最も少ないパターン光投光領域に選択的にパターン光を投光する。その後、図１１のステップＳ０３に進む。一方、ステップＳ３０１でステップＳ０５のパターン光の位置から路面に対する距離及び姿勢角を算出する処理を実行しない場合には、ステップＳ３０４に進み、投光器１１はパターン光を消灯する。そして、ステップＳ３０５において、カメラ１２が画像を取得した後、図１１のステップＳ０７の手順に進む。

Claims (6)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得した画像における前記パターン光の形から前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部で取得した画像から、前記路面上の複数の特徴点を検出する特徴点検出部と、
   前記特徴点検出部により検出された路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記路面に対する車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出部
   とを備え、
   前記投光器は、前記特徴点検出部での路面の特徴点の検出状態に応じて、複数のパターン光投光領域のうちの特定のパターン光投光領域に選択的にパターン光を投光することを特徴とする自己位置算出装置。
2. 前記複数のパターン光投光領域が、前記特徴点の移動方向に延伸する縦縞模様に分割され、それぞれ交互に組み合わせており、
   前記投光器は、前記複数のパターン光投光領域のうち、前記特徴点の検出数が相対的に少ない領域にパターン光を投光する
   ことを特徴とする請求項１に記載の自己位置算出装置。
3. 前記複数のパターン光投光領域が車幅方向で２分割されており、
   前記投光器は、前記複数のパターン光投光領域のうち、前記特徴点の検出数が相対的に少ない一方に投光する
   ことを特徴とする請求項１に記載の自己位置算出装置。
4. 前記投光器は、前記特徴点の検出位置に応じた位置にパターン光を投光することを特徴とする請求項１〜３のいずれか１項に記載の自己位置算出装置。
5. 前記投光器は、前記特徴点の移動方向に応じた位置にパターン光を投光することを特徴とする請求項１に記載の自己位置算出装置。
6. 車両周囲の路面にパターン光を投光する手順と、
   前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記取得した画像における前記パターン光の形から前記路面に対する車両の姿勢角を算出する手順と、
   前記取得した画像から、前記路面上の複数の特徴点を検出する手順と、
   前記検出された路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する手順と、
   前記路面に対する車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する手順
   とを含み、
   前記パターン光を投光する手順は、前記路面の特徴点の検出状態に応じて、複数のパターン光投光領域のうちの特定のパターン光投光領域に選択的にパターン光を投光することを特徴とする自己位置算出方法。