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

Description

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

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

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

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

しかし、屋外環境においては、特許文献２に記載のようにパターン光を路面に投光したときに、周囲光の影響を受けるため、路面に投光したパターン光を検出することは困難である。

本発明は、上記課題に鑑みて成されたものであり、その目的は、路面に投光したパターン光を精度よく検出することができ、車両の自己位置を精度よく算出することができる自己位置算出装置及び自己位置算出方法を提供することである。

本発明の一態様に係る自己位置算出装置は、投光器から車両周囲の路面にパターン光を投光し、パターン光が投光された領域を含む車両周囲の路面を撮像部により撮像して画像を取得し、撮像部で取得した画像からパターン光の位置を抽出し、抽出されたパターン光の位置から、路面に対する車両の姿勢角を算出し、撮像部で取得した画像における路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出し、車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことによって車両の現在位置及び姿勢角を算出する。そして、パターン光の位置を抽出するときに、パターン光の検出状態が閾値以上の場合、撮像部で取得されるフレーム間の画像を重ね合わせて重畳画像を生成し、重畳画像からパターン光の位置を抽出することを特徴とする。

図１は、第１の実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図２は、車両１０への投光器１１及びカメラ１２の搭載例を示す外観図である。 図３（ａ）は、投光器１１とカメラ１２の間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕ_ｊ、Ｖ_ｊ）とから、各スポット光が照射された路面３１上の位置を算出する様子を示す図であり、図３（ｂ）は、パターン光３２ａが照射された領域とは異なる他の領域３３から検出された特徴点の時間変化から、カメラ１２の移動方向３４を求める様子を示す模式図である。 図４（ａ）及び図４（ｂ）は、カメラ１２により取得された画像に対して二値化処理を施したパターン光３２ａの画像を示す図であり、図４（ａ）はパターン光３２ａ全体を示し、図４（ｂ）は１つのスポット光Ｓ_ｐを拡大して示し、図４（ｃ）はパターン光抽出部２１により抽出された各スポット光Ｓ_ｐの重心の位置Ｈ_ｅを示す図である。 図５は、距離及び姿勢角の変化量を算出する方法を説明するための模式図である。 図６（ａ）は、時刻ｔに取得された第１フレーム（画像）３８の一例を示す。図６（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得された第２フレーム３８’を示す。 図７（ａ）は、外部環境が明るい場合に重畳画像を生成するときの車両の移動量を示す。図７（ｂ）は、外部環境が明るい場合に重畳画像を生成する様子を示す。 図８（ａ）は、外部環境が暗い場合に重畳画像を生成するときの車両の移動量を示す。図８（ｂ）は、外部環境が暗い場合に重畳画像を生成する様子を示す。 図９（ａ）〜図９（ｄ）は、第１の実施形態に係る自己位置算出装置の、リセットフラグ、重畳する画像数、特徴点検出状態、及び対応付けされた特徴点数の変化を示すタイミングチャートである。 図１０は、図１の自己位置算出装置を用いた自己位置算出方法の一例を示すフローチャートである。 図１１は、図１０のステップＳ１８の詳細な手順を示すフローチャートである。 図１２は、第２の実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図１３は、第２の実施形態に係るパターン光の位置から路面の高さの変化量を推定する方法を説明するための図である。 図１４（ａ）〜図１４（ｅ）は、第２の実施形態に係る自己位置算出装置の、リセットフラグ、ステップＳ２０１の所定の周期、重畳する画像数、路面状態の良否、及び路面の段差（凹凸）の大きさの変化を示すタイミングチャートである。 図１５は、第２の実施形態に係る自己位置算出装置による自己位置算出処理の処理手順を示すフローチャートである。 図１６は、第２の実施形態に係る自己位置算出装置による図１５のステップＳ２８の詳細な処理手順を示すフローチャートである。 図１７は、第３の実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図１８（ａ）及び図１８（ｂ）は、第３の実施形態に係る自己位置算出装置の輝度変化及び特徴点検出フラグの変化を示すタイミングチャートである。 図１９（ａ）〜図１９（ｃ）は、第３の実施形態に係る自己位置算出装置の、パターン光と特徴点を示す説明図である。 図２０（ａ）〜図２０（ｄ）は、第３の実施形態に係る自己位置算出装置の、リセットフラグ、各周期が終わるタイミング、重畳する周期数、及び投光電力の変化を示すタイミングチャートである。 図２１は、第３の実施形態に係る自己位置算出装置の処理手順を示すフローチャートである。

図面を参照して、第１〜第３の実施形態を説明する。図面の記載において同一部分には同一符号を付して説明を省略する。

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

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

車両１０の前方の路面３１上の特徴点（テクスチャ）及び投光器１１により投光されたパターン光３２ｂを撮像できるように、カメラ１２が設置される高さ及び向きが調整され、且つ、カメラ１２が備えるレンズのピント及び絞りが自動調整される。カメラ１２は、所定の時間間隔をおいて繰り返し撮像を行い、一連の画像（フレーム）群を取得する。カメラ１２により取得された画像データは、ＥＣＵ１３へ転送され、ＥＣＵ１３が備えるメモリに記憶される。

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

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

複数の情報処理部には、パターン光抽出部（重畳画像生成部）２１、姿勢角算出部２２、特徴点検出部２３、姿勢変化量算出部２４、輝度判断部（パターン光検出状態判断部）２５、自己位置算出部２６、パターン光制御部２７、検出状態判断部２８及び算出状態判断部２９が含まれる。特徴点検出部２３は、姿勢変化量算出部２４に含まれていてもよい。

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

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

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

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

特徴点検出部２３は、カメラ１２により取得された画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「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）特徴量を用いて、その周囲で輝度値がある規則性のもとで変化している点を特徴点として検出してもよい。そして、特徴点検出部２３は、１つの画像から検出した特徴点の総数Ｎを計数し、各特徴点に識別番号（i（１≦i≦Ｎ））を付す。各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）は、ＥＣＵ１３内のメモリに記憶される。図６（ａ）及び図６（ｂ）は、カメラ１２により取得された画像から検出された特徴点Ｔ_ｅの例を示す。各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）は、メモリに記憶される。

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

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

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

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

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

図３（ｂ）は、カメラ１２の撮像範囲のうち、パターン光３２ａが照射された領域とは異なる他の領域３３から検出された特徴点の時間変化から、カメラ１２の移動方向３４を求める様子を模式的に示す。また、図６（ａ）及び図６（ｂ）には、各特徴点Ｔ_ｅの位置の変化方向及び変化量を示すベクトルＤ_ｔｅを画像に重畳して示す。姿勢変化量算出部２４は、時間Δｔにおけるカメラ１２の移動量（ΔＬ）だけでなく、距離及び姿勢角の変化量も同時に算出することができる。よって、姿勢変化量算出部２４は、距離及び姿勢角の変化量を考慮して、６自由度の移動量（ΔＬ）を精度よく算出することができる。すなわち、車両１０の旋回や加減速によるロール運動或いはピッチ運動によって距離や姿勢角が変化しても、移動量（ΔＬ）の推定誤差を抑制することができる。

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

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

このように、相対位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）が算出された特徴点Ｔ_ｅ１、Ｔ_ｅ２、Ｔ_ｅ３が、後のタイミングで取得された画像３８’からも検出された場合に、姿勢変化量算出部２４は、路面上の複数の特徴点の時間変化に基づいて、「車両の姿勢変化量」を算出することができる。

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

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

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

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

検出状態判断部２８は、特徴点検出部２３による特徴点Ｔ_ｅの検出状態が第１基準を満たさず悪い状態か否かを判断する。例えば、トンネル内のコンクリート路面のように、路面にアスファルト混合物の粒などの模様や凹凸が少ない場合、画像から検出できる特徴点が少なくなる。この場合、前後フレーム間で対応関係が取れる特徴点の継続的な検出が困難となり、距離及び姿勢角の更新精度が低下してしまう。

そこで、検出状態判断部２８は、例えば、カメラ１２に対する相対位置が算出された特徴点のうち、後の情報処理サイクルで取得された画像からも検出できた特徴点の数が、所定のしきい値（例えば、３つ）以下である場合に、特徴点の検出状態が第１基準を満たさず、悪い状態と判断する。すなわち、前後フレーム間で対応付けが取れる４点以上の特徴点を検出できない場合、検出状態判断部２８は、特徴点Ｔ_ｅの検出状態が第１基準を満たさず、悪い状態と判断する。なお、距離や姿勢角の変化量を求めるには、前後フレーム間で対応付けが取れる少なくとも３つの特徴点が必要である。なぜなら、平面Ｇを特定するには３つの特徴点が必要だからである。推定精度を高めるには更に多くの特徴点が必要であるため、所定のしきい値は、４、或いは５以上であることが望ましい。

自己位置算出部２６は、特徴点の検出状態が第１基準を満たしていると検出状態判断部２８が判断した場合、積分演算の起点を維持する。一方、特徴点の検出状態が第１基準を満たさず、悪い状態と検出状態判断部２８が判断した場合、自己位置算出部２６は、同じ情報処理サイクルにおいて、姿勢角算出部２２（図１参照）が算出した距離及び姿勢角、及びその時の車両位置を新たな起点（車両の姿勢角及び初期位置）に設定し、当該起点から車両の姿勢変化量の加算を開始する。

なお、第１の実施形態で、検出状態判断部２８は、前後フレーム間で対応関係が取れる特徴点の数に基づいて、特徴点の検出状態を判断したが、１つの画像から検出された特徴点の総数Ｎに基づいて、特徴点の検出状態を判断してもよい。つまり、特徴点の総数Ｎが所定のしきい値（例えば９）以下である場合に、特徴点の検出状態が第１基準を満たさず、悪い状態と判断してもよい。検出された特徴点のうち対応付けができない特徴点があることを考慮し、所定のしきい値（３）の３倍の数（９）を、しきい値に設定すればよい。

算出状態判断部２９は、姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態か否かを判断する。例えば、路面３１上の段差にパターン光が照射された場合、路面３１上の段差はアスファルトの凹凸に比べて大きいため、距離及び姿勢角の算出精度が極端に低下してしまう。特徴点の検出状態が第１基準よりも悪く、且つ距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態の場合、距離及び姿勢角、及びその変化量を精度よく検出する手段がなくなってしまう。

そこで、算出状態判断部２９は、例えば、姿勢角算出部２２によって算出される距離及び姿勢角の標準偏差が所定のしきい値よりも大きい場合、姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態と判断する。また、３５個のスポット光のうち検出できたスポット光の数が３点未満である場合、原理的に路面３１の平面式を求めることができないため、姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態と判断する。最小二乗法を用いて平面式を求める場合、各スポット光との距離誤差の最大値の絶対値がある閾値（例えば０．０５ｍ）以上になった場合に、姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態と判断してもよい。

特徴点の検出状態が第１基準を満たさず、悪い状態と検出状態判断部２８が判断し、且つ姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態と算出状態判断部２９が判断した場合、自己位置算出部２６は、積分演算の起点として、前の情報処理サイクルにおける距離及び姿勢角、及び車両の現在位置を用いる。これにより、車両の移動量の算出誤差を抑制できる。

パターン光制御部２７は、投光器１１によるパターン光３２ａの投光を制御する。例えば、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に、パターン光制御部２７は、パターン光３２ａの投光を開始する。その後、パターン光制御部２７は、自己位置算出装置が停止するまで、パターン光３２ａを連続して投光する。或いは、所定の時間間隔をおいて、投光のオン／オフを繰り返してもよい。或いは、特徴点Ｔ_ｅの検出状態が第１基準を満たさず、悪い状態と検出状態判断部２８が判断した場合に限って、一時的にパターン光３２ａを投光してもよい。

輝度判断部（パターン光検出状態判断部）２５は、カメラ１２により取得されたパターン光の検出状態が所定の閾値以上であるか否かを判定する。例えば、輝度判断部２５は、カメラ１２により取得された画像の輝度平均値（パターン光の検出状態）が、路面輝度閾値Ｂｔｈ（所定の閾値）以上であるか否かを判断する。なお、輝度判断部２５は、カメラ１２により取得された画像の照度が閾値以上であるか否かを判断してもよい。また、輝度判断部２５の代わりに照度センサが車両に搭載されていてもよい。

路面輝度閾値Ｂｔｈは、例えば、以下の手順によって予め求めることができる。まず、車両に人や荷物や燃料等が乗っていない空載状態において、パターン光をアスファルト路面に投光した時の画像をカメラ１２により撮像する。この時、画像上のアスファルト路面の輝度はほぼ一様になるようにする。例えば、パターン光が映っていない部分の画素の９５％が輝度平均値から±２０以内に収まるように光環境を調整して撮像する。なお、カメラ１２により取得された画像の輝度値は、０〜２５５（０：最も暗い、２５５：最も明るい）の範囲内とする。更に、得られた画像において、パターン光が照射された画素の輝度平均値Ｂｐと、それ以外のアスファルト路面が映っている画素の輝度平均値Ｂａとを比較する。この一連の処理を、アスファルト路面が映っている部分の輝度平均値Ｂａを０から１０刻みに大きくしながら繰り返し、Ｂｐ×０．７＜Ｂａとなった時の輝度平均値Ｂａを路面輝度閾値Ｂｔｈとする。即ち、アスファルト路面の輝度がパターン光の輝度の約３０％となる輝度値を路面輝度閾値Ｂｔｈとする。

パターン光抽出部２１は、輝度判断部２５によりパターン光の検出状態が所定の閾値以上であると判定された場合に、カメラ１２により得られた前後フレーム間の画像を所定の枚数だけ重ね合わせる。なお、重ね合わせる画像としては、現在から過去分のメモリに蓄積された前後フレーム間の画像を用いる場合を説明する。なお、重ね合わせる画像として、現在から将来的にカメラ１２により取得される画像を含んでいてもよい。パターン光抽出部２１は更に、生成した重畳画像からパターン光の位置を抽出する。姿勢角算出部２２は、重畳画像から抽出されたパターン光の位置から路面に対する車両の姿勢角を算出することができる。また、自己位置算出部２６は、その時の車両の現在位置及び重畳画像により算出された姿勢角を、車両の初期位置及び姿勢角（起点）に設定し、姿勢変化量の加算を開始することができる。

ここで、パターン光抽出部２１は、カメラ１２で取得した画像におけるパターン光の検出状態に応じて所定の画像の数を設定する。パターン光の検出状態は、例えば周囲光の輝度値に対するパターン光の輝度値の比（Ｓ／Ｎ比）である。パターン光抽出部２１は、Ｓ／Ｎ比が小さい（外部環境が明るい）ほど、重ね合わせる画像数を多くする。

図７（ａ）に示すように、外部環境が相対的に明るい場合、必要な画像数が相対的に多いため、画像群を取得するための車両１００の移動量は相対的に大きい。また、図７（ｂ）に示すように、相対的に多い数の画像Ｉ１を重ね合わせて重畳画像Ｉ２を生成する。一方、図８（ａ）に示すように、外部環境が相対的に暗い場合、必要な画像数が相対的に少ないため、画像群を取得するための車両１００の移動量は相対的に小さい。また、図８（ｂ）に示すように、相対的に少ない数の画像Ｉ１を重ね合わせて重畳画像Ｉ２を生成する。

パターン光抽出部２１により重ね合わせるべきパターン光抽出に必要な画像の数は、例えば以下の手順により設定することができる。まず、路面輝度閾値Ｂｔｈを求める時と同様に、アスファルト路面が映っている部分の輝度平均値Ｂａを０から１０刻みに大きくしていき、各輝度平均値Ｂａにおける、パターン光が照射された画素の輝度平均値Ｂｐの比Ｒａｐ＝Ｂａ／Ｂｐを予め実験等で求めておき、ＥＣＵのメモリに記憶させておく。そして、実際に制御を行う場合には、カメラ１２により取得された画像の輝度平均値の１の位を四捨五入した時のＳ／Ｎ比Ｒａｐを参照し、以下の式（５）で求められたＳｎの小数点第一位を切り上げた枚数を、パターン光抽出に必要な画像の数に設定する。

Ｓｎ＝ｍａｘ｛（０．５÷（１−Ｒａｐ）２），５０｝ ・・・（５）

即ち、アスファルト路面の輝度平均値Ｂａがパターン光の輝度平均値Ｂｐの約２９％以下ならば１枚、７５％ならば８枚、９０％以上ならば５０枚とする。なお、パターン光が照射された部分は画像全体と比べて十分小さいので、画像全体から輝度平均値を算出してもよい。また、実際に輝度平均値Ｂａを０から１０刻みに大きくして各Ｒａｐを求める実験において、実際に重畳してスポット光の抽出を行い、パターン光の各スポット光の抽出の成功率が９５％以上となる枚数と設定してもよい。

また、以下で説明するように、パターン光抽出部２１により重畳画像が生成できない（又は生成が困難な）場合には、前回の情報処理サイクルで採用した距離及び姿勢角（以下、単に「前回値」ともいう。）、又は上述した車両の所定の初期距離及び初期姿勢角（単に「初期値」ともいう。）を起点に用いる。

まず、Ｓ／Ｎ比が小さすぎて（明るすぎて）、積分時間が長すぎる場合がある。例えば、カメラ１２により取得された画像の輝度平均値の１の位を四捨五入した時の比Ｒａｐを参照し、比Ｒａｐが０．９５以上、即ち、アスファルト路面の輝度平均がパターン光の輝度平均の９０％以上ならば、重畳画像作成に時間がかかりすぎて、その間の路面変化が小さいという仮定が成り立たなくなるか、或いはパターン光の抽出そのものが原理的に困難と判断し、前回値又は初期値を起点に用いる。

また、車両挙動が大きすぎる（車速が高すぎる）場合がある。例えば、設定された重畳画像の枚数を撮像する間に、車両の進行距離が０．２［ｍ］を超える場合には、路面変化が小さいという仮定が成り立たなくなると判断し、重畳画像が生成できないと判断する。ここで、１０００ｆｐｓのカメラを用いると、アスファルト路面の輝度平均がパターン光の輝度平均の７５％の場合、重畳画像は８枚必要である。よって、以下の式（６）の通り、９０ｋｍ／ｈ以上では、前回値又は初期値を起点に用いる。

０．２［ｍ］÷（８［枚］÷１０００［ｆｐｓ］）
＝２５［ｍ／ｓ］
＝９０［ｋｍ／ｈ］ ・・・（６）

また、路面変化（段差や凹凸）が大きすぎる場合がある。例えば、設定された重畳画像の枚数を撮像する間に、車両周囲の路面状態が閾値以上変化しているか否かを判定する。そして、車両周囲の路面状態が閾値以上変化しているという状態になった画像数が５％を超えた場合には、路面変化が小さいという仮定が成り立たなくなると判断し、前回値又は初期値を起点に用いる。なお、路面状態の変化の判定の詳細は、第２の実施形態において説明する。

図９（ａ）〜図９（ｄ）は、第１の実施形態に係る自己位置算出装置の、リセットフラグ、重畳する画像数、特徴点検出状態、及び対応付けされた特徴点数の変化を示す。例えば、時刻ｔ１１，ｔ１２の情報処理サイクルにおいて、図９（ｄ）に示すように対応付けされた特徴点数が３点以下となり、図９（ｃ）に示すように特徴点検出状態が悪いと判断される。これに伴い、図９（ａ）に示すようにリセットフラグが「１」に設定される。

このうち、時刻ｔ１１の情報処理サイクルにおいては、図９（ｂ）に示すように、パターン光抽出部２１が、カメラ１２の画像の平均輝度値から、重畳する画像数を１枚（即ち、重畳しない）と設定する。そして、パターン光抽出部２１は、現在の時刻ｔ１１の情報処理サイクルにおいて取得される１枚の画像からパターン光を抽出する。

一方、時刻ｔ１２の情報処理サイクルでは、図９（ｂ）に示すように、パターン光抽出部２１が、カメラ１２の画像の平均輝度値から、重畳する画像数を２枚と設定する。更に、パターン光抽出部２１が、時刻ｔ１２においてカメラ１２により取得された画像と、前回の情報処理サイクルにおいてカメラ１２により取得された画像との２枚を重ね合わせて（輝度値を合算して）重畳画像を生成する。そして、パターン光抽出部２１が、重畳画像からパターン光を抽出する。

［情報処理サイクル］
次に、カメラ１２により取得された画像３８から車両１０の移動量を推定する自己位置算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図１０及び図１１を参照しながら説明する。図１０のフローチャートに示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に開始され、自己位置算出装置が停止するまで、繰り返し実行される。

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

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

ステップＳ１２に進み、輝度判断部２５は、カメラ１２により取得された画像３８をメモリから読み込み、画像の平均輝度が路面輝度閾値Ｂｔｈ以上か判断する。画像の平均輝度が路面輝度閾値Ｂｔｈ未満と判断された場合、ステップＳ１５に進む。

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

一方、ステップＳ１２において、画像の平均輝度が路面輝度閾値Ｂｔｈ以上と判断された場合、ステップＳ１３に進む。ステップＳ１３において、パターン光抽出部２１は、カメラ１２により取得された画像の平均輝度から、パターン光抽出に必要な画像数（フレーム数）を設定する。

ステップＳ１４において、パターン光抽出部２１は、カメラ１２により取得された画像３８をメモリから読み込み、設定した画像数の前後フレームの画像を重ね合わせる（輝度値を合算する）ことにより、重畳画像を生成する。更に、パターン光抽出部２１は、生成した重畳画像からパターン光３２ａの位置を抽出する。パターン光抽出部２１は、パターン光３２ａの位置を示すデータとして算出されたスポット光Ｓ_ｐの画像上の座標（Ｕ_ｊ、Ｖ_ｊ）をメモリに記憶する。

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

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

具体的に、先ず、特徴点検出部２３は、カメラ１２により取得された画像３８をメモリから読み込み、画像３８から路面３１上の特徴点を検出し、各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）をメモリに記憶する。姿勢変化量算出部２４は、各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）をメモリから読み込み、距離及び姿勢角と、特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）とから、カメラ１２に対する特徴点の相対位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）を算出する。なお、姿勢変化量算出部２４は、前の情報処理サイクルのステップＳ１６において設定された距離及び姿勢角を用いる。姿勢変化量算出部２４は、カメラ１２に対する特徴点の相対位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）を、メモリに記憶する。

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

ステップＳ１８に進み、ＥＣＵ１３は、特徴点の検出状態、及びパターン光による距離及び姿勢角の算出状態に応じて、積分演算の起点を設定する。詳細は、図１１を参照して、後述する。

ステップＳ１９に進み、自己位置算出部２６は、ステップＳ１８の処理で設定された積分演算の起点、及びステップＳ１７の処理で算出された車両の移動量から、車両の現在位置を算出する。

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

図１１のフローチャートを参照して、図１０のステップＳ１８の詳細な手順を説明する。ステップＳ１００において、ＥＣＵ１３は、今回の情報処理サイクルが初回であるか否かを判断する。そして、初回である場合、即ち、前回の情報処理サイクルのデータが無い場合にはステップＳ１０４へ進み、初回でない場合にはステップＳ１０１に進む。

ステップＳ１０１において、検出状態判断部２８は、特徴点検出部２３による特徴点Ｔ_ｅの検出状態が第１基準を満たさず、悪い状態か否かを判断する。悪いと判断した場合（ステップＳ１０１でＹＥＳ）、ステップＳ１０２へ進み、悪くなっていないと判断した場合（ステップＳ１０１でＮＯ）、ステップＳ１０６へ進む。ステップＳ１０６で、ＥＣＵ１３は、現在の設定されている積分演算の起点を維持する。

ステップＳ１０２において、ＥＣＵ１３は、重畳画像を生成できたか判断する。重畳画像を生成できない場合としては、例えば、将来的に取得される画像も含めるために所定の枚数の画像を取得するための時間がかかり、重畳画像を生成中である場合や、又は原理的に重畳画像を生成することが不可能又は困難である場合が挙げられる。重畳画像を生成できないと判断された場合（ステップＳ１０２でＮＯ）、ステップＳ１０３へ進み、重畳画像を生成できたと判断した場合（ステップＳ１０２でＹＥＳ）、ステップＳ１０４へ進む。

ステップＳ１０３で、算出状態判断部２９は、姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態か否かを判断する。たとえば、距離及び姿勢角の算出に成功しているか否かを判断する。成功していると判断した場合（ステップＳ１０３でＹＥＳ）、ステップＳ１０４へ進み、成功していないと判断した場合（ステップＳ１０３でＮＯ）、ステップＳ１０５へ進む。

ステップＳ１０４で、ＥＣＵ１３は、車両の現在位置を起点として設定し、更に、同じ情報処理サイクルのステップＳ１６で算出された距離及び姿勢角を積分演算の起点として設定する。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両の現在位置を起点として新たに車両の移動量の積分演算を開始する。

ステップＳ１０５で、ＥＣＵ１３は、車両の現在位置を起点として設定し、更に、前回の情報処理サイクルで採用した距離及び姿勢角を積分演算の起点として設定する。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両の現在位置を起点として新たに車両の移動量の積分演算を開始する。その後、図１０のステップＳ１９に進む。

［第１の実施形態の効果］
以上説明したように、第１の実施形態によれば、輝度判断部２５がパターン光の検出状態を判断し、パターン光の検出状態が閾値以上の場合、パターン光抽出部２１が前後フレーム間の画像を重ね合わせて重畳画像を生成し、重畳画像からパターン光を抽出することにより、外部環境が明るい場合であっても路面に投光したパターン光を精度よく検出することができ、車両の自己位置を精度よく算出することができる。

また、カメラ１２により取得された画像の平均輝度等のパターン光検出状態に応じて、パターン光抽出部２１が重畳画像を生成するために必要な画像数を設定することにより、外部環境の明るさに応じて検出するパターン光の輝度値を調整することができ、パターン光を精度よく検出することができる。

また、図１１のステップＳ１０２のように重畳画像の生成中は、前回の情報処理サイクルで採用された距離及び姿勢角、又は所定の初期距離及び初期姿勢角を起点として用いることにより、自己位置を算出するときの誤差を抑制することができる。

なお、図１１のステップＳ１０５で、ＥＣＵ１３は、前回の情報処理サイクルで採用した距離及び姿勢角の代わりに、所定の初期距離及び初期姿勢角を積分演算の起点として設定してもよい。詳細には、特徴点の検出状態が第１基準を満たさず、悪い状態と検出状態判断部２８が判断し、且つ姿勢角算出部２２による距離及び姿勢角の算出状態が第２基準を満たさず、悪い状態と算出状態判断部２９が判断した場合、自己位置算出部２６は、積分演算の起点として、少なくとも車両１０への乗員及び搭載物を考慮した所定の初期距離及び初期姿勢角を設定してもよい。例えば、自己位置算出装置を起動した直後の情報処理サイクルのステップＳ１６で算出した距離及び姿勢角を用いることができる。これにより、車両１０の旋回や加減速によるロール運動或いはピッチ運動が発生していない時の距離や姿勢角を起点として、距離及び姿勢角の更新及び移動量の算出を行うことができる。

（第２の実施形態）
［ハードウェア構成］
本発明の第２の実施形態として、車両周囲の路面状態の変化に基づいて自己位置を算出する場合を説明する。本発明の第２の実施形態に係る自己位置算出装置は、図１２に示すように、検出状態判断部２８及び算出状態判断部２９の代わりに、路面状態判定部３０を備える点が、第１の実施形態と異なる。他の構成は、第１の実施形態と同様であるので説明を省略する。

路面状態判定部３０は、車両周囲の路面状態の変化を検出して路面状態が閾値以上変化しているか否かを判定する。そして、路面状態が閾値以上変化していると判定した場合には、自己位置算出部２６は、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に固定する。これにより、姿勢角算出部２２は路面に対する車両１０の距離及び姿勢角の算出を停止する。また、自己位置算出部２６は、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して、現在の車両の現在位置、路面に対する距離及び姿勢角を算出する。

ここで、路面状態の変化を判定する方法について説明する。本実施形態では、パターン光３２ａのスポット光が路面に３５（５×７）個投影されている。そこで、例えば、３５個のスポット光のうちカメラ１２の画像上において８０％以下、すなわち２８個以下しか検出できない場合には、路面状態判定部３０は、路面の段差や凹凸が激しくなり、路面状態が閾値以上変化していると判定する。

このとき、路面状態の変化を路面の高さの変化量から推定してもよい。路面の高さの変化量は、車両の各車輪のサスペンションに取り付けられているストロークセンサの検出値の振動から検出することができる。例えば、ストロークセンサの検出値の振動が１Ｈｚ以上となった場合に、路面状態判定部３０は、路面の段差や凹凸が激しくなったと推定し、路面状態が閾値以上変化したと判定する。また、鉛直方向の加速度を計測する加速度センサの検出値を積分して鉛直方向の速度を算出し、その速度の向きの変化が１Ｈｚ以上となった場合に、路面状態判定部３０は、路面の段差や凹凸が激しくなり、路面状態が閾値以上変化していると判定してもよい。

さらに、路面の高さの変化量は、カメラ１２で撮像した画像におけるパターン光３２ａの位置から推定してもよい。本実施形態では、図１３に示すようなパターン光３２ａが路面３１に投影されている。そこで、パターン光３２ａのスポット光をＸ方向に結んだ線分７１と、Ｙ方向に結んだ線分７３を引く。そして、これらの線分の傾きが点７５に示すように途中で１５ｄｅｇ以上変化している場合に、路面状態判定部３０は、路面の段差や凹凸が激しくなったと推定し、路面状態が閾値以上変化していると判定する。また、図１３に示すように、隣り合うスポット光の間隔ｄ１、ｄ２の差が５０％以上変化した場合に、路面状態判定部３０は、路面状態が閾値以上変化していると判定してもよい。

このようにして路面状態が閾値以上変化していると判定すると、自己位置算出部２６は前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に固定する。したがって、姿勢角算出部２２は路面に対する車両１０の距離及び姿勢角の算出を停止し、自己位置算出部２６は、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して、現在の車両１０の現在位置、路面に対する距離及び姿勢角を算出する。

例えば、図１４（ｅ）に示すように、路面状態判定部３０は、検出されたスポット光の数をモニターしており、閾値を３５個のスポット光の８０％にあたる２８個に設定している。この場合に、路面状態判定部３０は、スポット光を２８個より多く検出できている間は姿勢角算出フラグを「１」に設定する。これにより、姿勢角算出部２２は路面に対する車両１０の距離及び姿勢角の算出を行い、自己位置算出部２６は姿勢角算出部２２で算出された車両の距離及び姿勢角を用いて現在の距離及び姿勢角を算出し、前回の情報処理サイクルで算出された車両１０の現在位置に、車両の移動量を加算することで（積分演算を継続して）、現在の車両の自己位置を算出する。

しかし、時刻ｔ２１において、スポット光の検出個数が閾値を下回ると、自己位置算出部２６は姿勢角算出フラグを「０」に切り替える。これにより、車両１０の現在位置、路面に対する距離及び姿勢角は前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に固定され、姿勢角算出部２２は車両１０の距離及び姿勢角の算出を停止する。したがって、自己位置算出部２６は、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して、現在の車両の現在位置、路面に対する距離及び姿勢角を算出する。

この後、時刻ｔ２２においてスポット光の検出個数が再び閾値を上回ると、姿勢角算出フラグが「１」に設定され、姿勢角算出部２２は車両１０の距離及び姿勢角の算出を再開する。そして、自己位置算出部２６は、姿勢角算出部２２で算出された車両１０の距離及び姿勢角を用いて車両１０の現在の距離及び姿勢角を算出する。このように本実施形態に係る自己位置算出装置は、路面状態が大きく変化した場合には前回の情報処理サイクルで算出した車両１０の現在位置、路面に対する距離及び姿勢角を用いるので、路面状態が大きく変化しても車両１０の自己位置を精度よく、且つ安定して算出することができる。

パターン光抽出部２１は、第１の実施形態と同様に、パターン光の検出状態が所定の閾値以上の場合、所定の枚数の画像を重ね合わせて重畳画像を生成する。パターン光抽出部２１は、生成した重畳画像からパターン光を抽出する。

また、路面変化が大きいために重畳画像が生成できない場合には、前回値又は初期値を起点に用いる。例えば、設定された重畳画像の枚数を撮像する間に、路面状態判定部３０が、車両周囲の路面状態が閾値以上変化しているか否かを判定する。変化していると判定された画像数が５％を超えた場合には、路面変化が小さいという仮定が成り立たなくなると判断し、前回値又は初期値を起点に用いる。

図１４（ａ）〜図１４（ｅ）は、第２の実施形態に係る自己位置算出装置の、リセットフラグ、重畳する画像数、路面状態の良否、及び路面の段差や凹凸の大きさの変化を示す。図１４（ｂ）に示すように、所定の周期でリセットするか判断される。ここでは、所定の周期を１０フレームとしているが、例えば所定の周期を１０秒に設定してもよい。

時刻ｔ２１〜ｔ２２、時刻ｔ２４〜ｔ２５においては、図１４（ｅ）に示すように、路面の段差や凹凸が閾値以下となり、図１４（ｄ）に示すように路面状態が悪いと判断される。よって、図１４（ｂ）に示すように所定の周期のリセットするタイミングであっても、図１４（ａ）に示すようにリセットフラグは「０」のままであり、重畳画像も生成されない。

一方、時刻ｔ２３においては、図１４（ｂ）に示すように所定の周期のリセットタイミングであり、図１４（ｄ）に示すように路面状態が良いと判断されているので、図１４（ａ）に示すようにリセットフラグは「１」に設定される。図１４（ｃ）に示すように、パターン光抽出部２１は、カメラ１２の画像の平均輝度値から、重畳する画像数を３枚と設定する。更に、パターン光抽出部２１は、時刻ｔ２３においてカメラ１２により取得される１枚の画像と、前回フレーム及び前々回フレームにおいてカメラ１２により取得される２枚の画像とを重ね合わせて重畳画像を生成する。そして、パターン光抽出部２１は、生成した重畳画像からパターン光を抽出する。

［情報処理サイクル］
次に、図１５及び図１６を用いて、本発明の第２の実施形態に係る自己位置算出方法を説明する。図１５に示すステップＳ２０〜Ｓ２７、Ｓ２９の手順は、図１０に示すステップＳ１０〜Ｓ１７、Ｓ１９の手順と同様であるので、説明を省略する。

ステップＳ２８において、ＥＣＵ１３は、車両周囲の路面状態の変化に応じて、自己位置を算出するための積分演算の起点を設定する。図１５のステップＳ２８の詳細な手順を図１６のフローチャートを参照して、説明する。

図１６に示すように、ステップＳ２０１において、路面状態判定部３０は、所定の周期が経過したか否かを判定する。所定の周期は、例えば、パターン光抽出部２１による重畳画像の生成に必要な数の画像を十分取得できる間隔に設定すればよい。図１４で説明したように、路面状態判定部３０は、周期カウントパルスが発生しているか否かを監視し、発生している場合には所定の周期が経過したと判定してステップＳ２０２に進む。一方、周期カウントパルスが発生していない場合には、所定の周期が経過していないと判定してステップＳ２０５に進む。

ステップＳ２０２において、路面状態判定部３０は、車両周囲の路面状態の変化を検出する。具体的に、路面状態判定部３０は、パターン光３２ａのスポット光の数を検出したり、各車輪に取り付けられているストロークセンサの検出値の振動を検出したりする。また、路面状態判定部３０は、車両の鉛直方向の加速度を計測できる加速度センサの検出値を積分して鉛直方向の速度を算出したり、パターン光３２ａの位置を検出したりしてもよい。

次に、ステップＳ２０３において、路面状態判定部３０は、車両周囲の路面状態が閾値以上変化しているか否かを判定する。例えば、パターン光３２ａのスポット光の数を検出する場合には、３５個のスポット光のうちカメラの画像上において２８個以下しか検出できない場合に、路面状態判定部３０は、路面の段差や凹凸が激しくなり、路面状態が閾値以上変化していると判定する。

また、ストロークセンサを用いる場合には、検出値の振動が１Ｈｚ以上となった場合に、路面状態判定部３０は路面状態が閾値以上変化していると判定する。さらに、加速度センサを用いる場合には、加速度センサの検出値を積分して鉛直方向の速度を算出し、その速度の向きの変化が１Ｈｚ以上となった場合に、路面状態判定部３０は路面状態が閾値以上変化していると判定する。

また、パターン光３２ａの位置を用いる場合には、スポット光を結んだ線分の傾きが途中で１５ｄｅｇ以上変化している場合に、路面状態判定部３０は路面状態が閾値以上変化していると判定する。或いは、隣り合うスポット光の間隔の差が５０％以上変化している場合に、路面状態判定部３０は路面状態が閾値以上変化していると判定してもよい。

このようにして、車両周囲の路面状態が閾値以上変化しているか否かの判定が行われ、路面状態判定部３０が閾値以上変化していると判定した場合（ステップＳ２０３でＹＥＳ）にはステップＳ２０４に進む。一方、路面状態判定部３０が閾値以上変化していないと判定した場合（ステップＳ２０３でＮＯ）にはステップＳ２０５に進む。

ステップＳ２０４では、自己位置算出部２６が、車両１０の現在位置、路面に対する距離及び姿勢角を前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に固定する。すなわち、自己位置算出部２６は、積分演算の起点として、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角を設定する。

これにより、姿勢角算出部２２は路面に対する車両１０の距離及び姿勢角の算出を停止し、自己位置算出部２６は、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して、現在の車両１０の現在位置、路面に対する距離及び姿勢角を算出する。

一方、ステップＳ２０５では、自己位置算出部２６が、今回の情報処理サイクルのステップＳ１５で算出された車両１０の現在位置、路面に対する距離及び姿勢角を積分演算の起点として設定する。これにより、自己位置算出部２６は、今回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して車両１０の現在位置、路面に対する距離及び姿勢角を算出する。

このようにして、現在の車両１０の現在位置、路面に対する距離及び姿勢角を算出するための積分演算の起点が設定されると、ステップＳ２８の処理は終了して図１５のステップＳ２９に進む。

［第２の実施形態の効果］
以上説明したように、第２の実施形態によれば、輝度判断部２５がパターン光の検出状態を判断し、パターン光の検出状態が閾値以上の場合、パターン光抽出部２１がフレーム間の画像を重ね合わせて重畳画像を生成することにより、外部環境が明るい場合であっても路面に投光したパターン光を精度よく検出することができ、車両の自己位置を精度よく算出することができる。

また、カメラ１２により取得された画像の平均輝度等のパターン検出状態に応じて、パターン光抽出部２１が重畳画像を生成するために必要な画像数を設定することにより、外部環境の明るさに応じて検出するパターン光の輝度値を調整することができ、パターン光を精度よく検出することができる。

（第３の実施形態）
［ハードウェア構成］
本発明の第３の実施形態として、同期検波により得られた画像を、所定周期分重ね合わせて重畳画像を生成する場合を説明する。本発明の第３の実施形態に係る自己位置算出装置は、図１７に示すように、検出状態判断部２８及び算出状態判断部２９がない点が、第１の実施形態と異なる。他の構成は、第１の実施形態と同様であるので説明を省略する。

パターン光制御部２７は、周期的に輝度が変化するパターン光３２ａの投光を開始する。その後、パターン光制御部２７は、自己位置算出装置が停止するまで、パターン光３２ａを連続して投光する。或いは、必要に応じてパターン光３２ａを投光してもよい。本実施形態では、輝度変化の一例として、投光パターンの輝度が所定周波数の正弦波状に変化するように、投光器１１に供給する電力を制御する。

パターン光抽出部２１は、カメラ１２により取得された画像をメモリから読み込み、更に、前述した所定周波数にて同期検波処理を実施することにより、画像に含まれるパターン光３２ａを抽出する。

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

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

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

図１８（ａ）は、投光器１１より投光されるパターン光３２ａの輝度変化を示す特性図、図１８（ｂ）は、特徴点検出フラグの変化を示す特性図である。例えば、図１８（ａ）に示すように正弦波状に輝度が変化するように制御する。

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

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

特徴点検出部２３は、投光器１１より投光されるパターン光３２ａの輝度が予め設定した閾値輝度Ｂth以下であるか否かを判断する。閾値輝度Ｂthより大きい場合には、パターン光抽出部２１が、上述した同期検波処理によりパターン光３２ａを抽出する。例えば、図１９（ａ），（ｃ）に示すように、図１８（ｂ）に示す時刻ｔ１或いは時刻ｔ３（輝度Ｂ１が閾値輝度Ｂthよりも大きい時刻）に撮影した画像から、同期検波処理によりパターン光３２ａを抽出する。

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

本実施形態では、パターン光抽出部２１は、輝度判断部２５によりパターン検出状態が所定の閾値以上と判断された場合、カメラ１２で撮像された画像を所定の変調周波数で同期検波した同期画像を所定周期分、重ね合わせて（輝度値を合算して）、重畳画像を生成する。投光パターンのみを抽出した画像は、参照信号１周期中に撮像された画像から求められる。２周期以上行う場合には、それぞれの周期で抽出した画像の各画素について輝度値を合算して重ね合わせる。なお、重ね合わせた画像からスポット光を抽出する場合、各画素の輝度値を重ね合わせた画像の枚数で除して正規化してから、二値化処理によって抽出してもよい。

次に、重畳する所定周期の設定方法の一例を説明する。まず、カメラ１２により取得された画像の輝度平均値の１の位を四捨五入した時の比Ｒａｐを参照し、以下の式（７）で求められたＦｎの小数点第一位を切り上げた周期を所定の周期の数とする。即ち、アスファルト路面の輝度平均がパターン光の輝度平均の５０％未満ならば１周期、７５％ならば２周期、９０％以上ならば５周期とする。

Ｆｎ＝ｍａｘ｛（０．５÷（１−Ｒａｐ）），１０｝ ・・・（７）

なお、実際に輝度平均値Ｂａを０から１０刻みに大きくして各Ｒａｐを求める実験において、実際に同期検波による抽出を行い、パターン光の各スポット光の抽出の成功率が９５％以上となる周期と設定してもよい。

ここで、車速が高すぎて重畳画像が生成できない場合には、前回値又は初期値を起点に用いる。例えば、アスファルト路面の輝度平均がパターン光の輝度平均の７５％の場合、同期検波の周期は２周期必要である。また、同期検波の１周期あたりの画像数を４枚とすると、以下の式（８）の通り、９０ｋｍ／ｈ以上では、前回値又は初期値を起点に用いる。

０．２［ｍ］÷｛（４［枚］×２［周期］）÷１０００［ｆｐｓ］｝
＝２５［ｍ／ｓ］
＝９０［ｋｍ／ｈ］ ・・・（８）

また、路面変化が大きく、重畳画像が生成できない場合、前回値又は初期値を起点に用いる。設定された同期検波の周期中に、車両周囲の路面状態が閾値以上変化しているか否かを判定し、変化しているという状態になった画像数が５％を超えた場合には、路面変化が小さいという仮定が成り立たなくなると判断し、前回値又は初期値を起点に用いる。同期検波を用いる場合、ストロークセンサを用いて判定する手法を用いることができる。

図２０（ａ）〜図２０（ｄ）は、第３の実施形態に係る自己位置算出装置の、リセットフラグ、各周期が終わるタイミング、重畳する周期数、及び投光電力の変化を示す。時刻ｔ３１，ｔ３２，ｔ３３は、図２０（ｂ）に示すように参照信号の投光が１周期終わったタイミングであり、図２０（ｄ）に示すように周期的な投光電力が小さくなり、図２０（ａ）に示すようにリセットフラグが「１」に設定される。

時刻ｔ３１,ｔ３３においては、図２０（ｃ）に示すように重畳する周期数が１周期と設定される。パターン光抽出部２１は、時刻ｔ３１,ｔ３３以前の１周期の画像を重ね合わせて重畳画像を生成する。

一方、時刻ｔ３２において、図２０（ｃ）に示すように重畳する周期数が２周期と設定される。パターン光抽出部２１が、時刻ｔ３２以前の２周期Ｔ１，Ｔ２分の画像を重ね合わせて重畳画像を生成する。

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

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

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

ステップＳ３２において、カメラ１２は、パターン光が投光されている領域を含む路面３１を撮像して画像を取得する。

ステップＳ３３において、ＥＣＵ１３が、同期検波の参照信号の投光が１周期完了したか判断する。同期検波の参照信号の投光が１周期完了したと判断された場合にステップＳ３５に進み、完了していないと判断された場合にステップＳ３４に進む。

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

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

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

一方、ステップＳ３５において、パターン光抽出部２１は、カメラ１２により取得された画像の輝度平均から、パターン光抽出に必要な同期検波の周波数を設定する。

ステップＳ３６において、パターン光抽出部２１は、同期検波により現在の参照信号の周期で取得した画像群からパターン光を抽出する。

ステップＳ３７において、パターン光抽出部２１は、過去の周期で同期検波を用いて抽出したパターン光の画像を、ステップＳ３５において設定した周期数だけ遡って重ね合わせて重畳画像を生成する。パターン光抽出部２１は更に、生成した重畳画像からパターン光の位置を抽出する。姿勢角算出部２２は、このパターン光の位置に基づいて距離及び姿勢角を算出する。

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

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

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

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

［第３の実施形態の効果］
以上説明したように、第３の実施形態によれば、輝度判断部２５がパターン光の検出状態を判断し、パターン光の検出状態が閾値以上の場合、パターン光抽出部２１がフレーム間の画像を所定の周期分、重ね合わせて重畳画像を生成することにより、外部環境が明るい場合であっても路面に投光したパターン光を精度よく検出することができ、車両の自己位置を精度よく算出することができる。

また、カメラ１２により取得された画像の平均輝度等のパターン検出状態に応じて、パターン光抽出部２１が重畳画像を生成するために必要な周期数を設定することにより、外部環境の明るさに応じて検出するパターン光の輝度値を調整することができ、パターン光を精度よく検出することができる。

（その他の実施の形態）
上記のように、本発明の第１〜第３の実施形態を記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。

本発明の第１〜第３の実施の形態では、主に、過去から現在においてカメラ１２により取得された画像を重ね合わせて重畳画像を生成する場合を説明したが、カメラ１２により将来的に取得される画像を一枚以上含めて重ね合わせて重畳画像を生成してもよい。将来的に取得される画像を含める場合としては、例えば、パターン光が常時点灯せずに間欠的に点灯している場合等が想定される。この場合、パターン光抽出部２１は、パターン光抽出に必要な枚数が取得された時点で、重畳画像を生成する。パターン光抽出に必要な枚数が取得されるまでの間（重畳画像生成中）は、自己位置算出部は、前回値又は初期値を起点として、積分演算を開始してもよい。

また、図２では、カメラ１２と投光器１１を車両１０の前面に取り付けた例を示したが、車両１０の側方，後方，真下に向けて設置してもよい。また、本実施形態では車両１０の一例として、四輪の乗用自動車を図２に示したが、オートバイ、貨物自動車、或いは例えば建設機械を運搬する特殊車両など、道路の路面或いは壁面上の特徴点を撮像することが可能な移動体（車両）すべてに適用可能である。

１３ ＥＣＵ
１０ 車両
１１ 投光器
１２ カメラ（撮像部）
２１ パターン光抽出部（重畳画像生成部）
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２５ 輝度判断部（パターン光検出状態判断部）
２６ 自己位置算出部
２８ 検出状態判断部
２９ 算出状態判断部
３０ 路面状態判定部
３１ 路面
３２ａ、３２ｂ パターン光
Ｔｅ 特徴点

Claims (5)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得した画像から、前記パターン光の位置を抽出するパターン光抽出部と、
   前記抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部で取得した画像における前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出部
   とを備え、
   前記パターン光抽出部が、前記パターン光の検出状態が閾値以上の場合、前記撮像部で取得されるフレーム間の画像を重ね合わせて重畳画像を生成し、前記重畳画像から前記パターン光の位置を抽出することを特徴とする自己位置算出装置。
2. 前記パターン光抽出部が、前記撮像部で取得された画像の輝度値に応じて、前記重畳画像として重ね合わせる画像の数を設定することを特徴とする請求項１に記載の自己位置算出装置。
3. 前記自己位置算出部は、前記パターン光抽出部による前記重畳画像の生成中は、前回の情報処理サイクルで採用した姿勢角又は初期姿勢角を起点として用いて、前記姿勢変化量の加算を開始することを特徴とする請求項１又は２に記載の自己位置算出装置。
4. 前記パターン光を所定の変調周波数で輝度変調するパターン光制御部を更に備え、
   前記パターン光抽出部が、前記撮像部で取得される画像を前記所定の変調周波数で同期検波することにより得られる同期画像を所定周期分、重ね合わせることにより前記重畳画像を生成することを特徴とする請求項１〜３のいずれか１項に記載の自己位置算出装置。
5. 投光器から車両周囲の路面にパターン光を投光するステップと、
   前記パターン光が投光された領域を含む車両周囲の路面を撮像部により撮像して画像を取得するステップと、
   前記撮像部で取得した画像から、前記パターン光の位置を抽出するステップと、
   前記抽出されたパターン光の位置から、前記路面に対する車両の姿勢角を算出するステップと、
   前記撮像部で取得した画像における前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出するステップと、
   前記車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出するステップ
   とを含み、
   前記パターン光の位置を抽出するステップは、前記パターン光の検出状態が閾値以上の場合、前記撮像部で取得されるフレーム間の画像を重ね合わせて重畳画像を生成し、前記重畳画像から前記パターン光の位置を抽出することを特徴とする自己位置算出方法。