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

Abstract

【課題】路面上に凹凸があっても車両の自己位置を精度良く算出することのできる自己位置算出装置を提供する。【解決手段】本発明の自己位置算出装置は、少なくともパターン光を投光する投光器１１、１４と、パターン光が投光された路面を撮像して画像を取得するカメラ１２と、カメラ１２の画像から路面の凹凸による画像上の影の位置を検出し、影の位置に基づいて画像上に検出領域を設定する検出領域設定部２０と、検出領域におけるパターン光の位置から路面に対する車両の姿勢角を算出する姿勢角算出部２２と、検出領域から路面上の特徴点を検出する特徴点検出部２３と、検出された路面上の特徴点の時間変化に基づいて車両の姿勢変化量を算出する姿勢変化量算出部２４と、車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことで車両の現在位置及び姿勢角を算出する自己位置算出部２６とを備える。【選択図】図１

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)）を用いることができる。

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

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

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

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

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

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

また、自己位置算出部２６は、車両の初期位置及び姿勢角に姿勢変化量を加算してゆくことで、車両の現在位置及び姿勢角を算出するが、このとき予め設定された条件を満たしている場合には、積分演算の起点を維持して姿勢変化量を加算してゆく。一方、予め設定された条件を満たしていない場合には、同じ情報処理サイクルにおいて、姿勢角算出部２２で算出された距離及び姿勢角、及びその時の車両位置を新たな起点（車両の初期距離及び初期姿勢角、及び初期位置）に設定する。そして、当該起点から車両の姿勢変化量の加算を開始する。積分演算の起点を維持するための条件としては、特徴点が正常に検出されている、誤差要因を含んでいない、あるいは所定時間または所定のサイクル数を経過していない等の条件を予め設定しておけばよい。このような条件は、車両１０の移動量の誤差が積分演算によって拡大しないように設定されたものであれば、その他の条件であってもよい。

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

また、パターン光制御部２７は、検出領域設定部２０が凹凸による影の位置を検出できるように、パターン光投光器１１の投光及び路面投光器１４の点灯を制御する。具体的には、車両１０のイグニションスイッチがオン状態となって自己位置算出装置が起動するのと同時に投光及び点灯を開始してもよいし、必要に応じて投光及び点灯してもよい。
（情報処理サイクル）
次に、カメラ１２により取得された画像３８から車両１０の移動量を推定する自己位置算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図９及び図１０を参照しながら説明する。図９のフローチャートに示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に開始され、自己位置算出装置が停止するまで、繰り返し実行される。

図９のステップＳ０１において、パターン光制御部２７は、パターン光投光器１１を制御して、車両周囲の路面３１にパターン光３２ａを投光する。図９のフローチャートでは、パターン光３２ａを連続して投光する例を説明する。また、パターン光制御部２７は、路面が暗い場合等には車両周囲の路面３１に対してパターン光投光器１１から投光するか、あるいは路面投光器１４を点灯する。

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

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

ステップＳ０５において、検出領域設定部２０は、カメラ１２で取得した画像をメモリから読み込み、路面の凹凸による画像上の影の位置を検出し、この影の位置に基づいて画像上に検出領域を設定する。本実施形態では、画像上の影の位置から光源方向の領域を除いて検出領域を設定し、メモリに記憶する。

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

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

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

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

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

ステップＳ１１に進み、ＥＣＵ１３は、予め設定された条件に基づいて、積分演算の起点を設定する。詳細は、図１０を参照して後述する。

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

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

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

ステップＳ９０１において、自己位置算出部２６は、積分演算の起点を維持するために必要となる条件を満たしているか否かを判断する。この条件としては、例えば、特徴点が正常に検出されている、誤差要因を含んでいない、あるいは所定時間または所定のサイクル数を経過していない等の条件を予め設定しておけばよい。このような条件は、車両１０の移動量の誤差が積分演算によって拡大しないように設定されたものであれば、その他の条件であってもよい。そして、条件を満たしていると判断した場合（ステップＳ９０１でＹＥＳ）にはステップＳ９０３へ進み、条件を満たしていないと判断した場合（ステップＳ９０１でＮＯ）にはステップＳ９０２へ進む。

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

一方、ステップＳ９０３において、ＥＣＵ１３は、現在設定されている積分演算の起点を維持する。こうして積分演算の起点が設定されると、図９のステップＳ１１における積分演算の基点を設定する処理を終了する。

以上、詳細に説明したように、本実施形態に係る自己位置算出装置では、カメラ１２で取得した画像から路面の凹凸による画像上の影の位置を検出し、影の位置に基づいて画像上に検出領域を設定する。そして、この検出領域におけるパターン光の位置から路面に対する車両の姿勢角を算出するとともに、検出領域から検出された特徴点の時間変化に基づいて車両の姿勢変化量を算出する。これにより、路面の凹凸の位置を考慮して画像上に検出領域を設定できるので、路面上に凹凸があっても車両の自己位置を精度良く算出することができる。

また、本実施形態に係る自己位置算出装置では、画像上の影の位置から光源方向の領域を除いて検出領域を設定する。これにより、凸部が存在する可能性のある領域を除いて検出領域を設定できるので、路面上に凹凸があっても車両の自己位置を精度良く算出することができる。

（第２実施形態）
次に、本発明の第２実施形態に係る自己位置算出装置について図面を参照して説明する。尚、ハードウェア構成は第１実施形態と同一なので、詳細な説明は省略する。

本実施形態に係る検出領域設定部２０は、画像上の影の長さ変化に基づいて路面の凸部による影を判別する。そして、路面の凸部による画像上の影の位置に基づいて、画像上に検出領域を設定するようにしたことが第１実施形態と相違している。

図１１（ａ）に示すように、光源が凸部７１から遠くに存在している場合には、路面の凸部７１によってできる影７３は長くなる。そして、図１１（ｂ）に示すように、光源が凸部７１に近づくと、影７３は短くなる。すなわち、凸部による影の形状は車両の移動に応じて大きく変化する。特に、光源方向の長さの変化が大きくなる。

一方、図１２（ａ）に示すように、路面の凹部７５によってできる影７７は、光源が凹部７５から遠くに存在している場合でもそれほど長く伸びるわけではない。そして、図１２（ｂ）に示すように、光源が凹部７５に近づいても影７３は少し短くなるだけで長さが大きく変化するわけではない。すなわち、凹部による影の形状は車両が移動しても凸部の影のように大きく変化することはない。特に、光源方向の長さの変化は小さくなる。

したがって、車両が走行している場合に、画像上にある影の領域をトラッキングし、フレーム間における影の形状の変化、特に光源方向の長さの変化が予め設定された閾値よりも大きい場合には凸部の影と判定し、閾値以下の場合には凹部の影と判定する。これにより、画像上にある影が凸部による影であるか、凹部による影であるかを判別することができる。尚、閾値については、パターン光投光器１１やカメラ１２、路面投光器１４の設置位置を考慮して、予め実験によって設定すればよい。

こうして凸部による影が判別されると、検出領域設定部２０は、路面の凸部による画像上の影の位置に基づいて検出領域を設定する。具体的に、本実施形態では凸部による影の領域を除いて検出領域を設定する。

例えば、図１３（ａ）に示す画像において、影６１ａ、６１ｄが凸部による影と判定され、影６１ｂ、６１ｃ、６１ｅが凸部以外の凹部と推定される影と判定された場合には、図１３（ｂ）に示すように除外領域８１ａ〜８１ｅを除いて検出領域を設定する。除外領域８１ａ、８１ｄは、凸部による影の領域なので、凸部による影の領域と影の位置から光源方向の領域の両方を含んでいる（破線で囲まれた範囲）。一方、除外領域８１ｂ、８１ｃ、８１ｅは、凹部による影の領域なので、影の位置から光源方向の領域だけで（破線で囲まれた範囲）、凹部による影の領域は含んでいない（破線で囲まれていない範囲）。

ここで、凸部による影の領域を検出領域から除いた理由を説明すると、凸部による影は車両の走行によって形状、特に光源方向の長さが大きく変化する。そのため、凸部による影のエッジを特徴点の検出に使用すると、車両の移動量を算出する際に誤差要因になる。そこで、凸部による影の領域を検出領域から除外することによって、特徴点を検出する際に路面の凸部による誤差を抑制し、車両の自己位置を精度良く算出できるようにしている。

また、検出領域設定部２０は、路面の凹部による画像上の影の位置に基づいて検出領域を設定してもよい。具体的には、凹部による影の領域を除いて検出領域を設定する。

例えば、上述した図１３（ｂ）では凸部による影の領域を除いて検出領域が設定されていたが、図１４では図１３（ｂ）の検出領域からさらに凹部による影の領域を含めて除外領域９１ａ〜９１ｅが設定されている（破線で囲まれた範囲）。除外領域９１ａ、９１ｄは図１３（ｂ）の除外領域８１ａ、８１ｄと同一であるが、除外領域９１ｂ、９１ｃ、９１ｅは、図１３（ｂ）の除外領域８１ｂ、８１ｃ、８１ｅに凹部による影の領域が追加されている。したがって、除外領域９１ｂ、９１ｃ、９１ｅは、凹部による影の領域と影の位置から光源方向の領域の両方を含んでいる（破線で囲まれた範囲）。

ここで、凹部による影の領域を検出領域から除いた理由を説明する。移動量を算出する技術では、パターン光の位置から距離及び姿勢角を算出する場合も路面が平坦であることが前提となっている。したがって、アスファルトの小さな亀裂や欠け部分のような凹部に照射されたパターン光から距離及び姿勢角を算出することは望ましくない。路面の凹部には影ができることから、凹部である可能性の高い影の領域を検出領域から除けば、パターン光から距離及び姿勢角を算出する際に路面の凹部による誤差を抑制することができる。そこで、路面の凹部による影の領域を検出領域から除いている。

尚、本実施形態では、路面の凸部を判別し、まず凸部による影の領域を検出領域から除外し、その後さらに凸部以外の凹部と推定される影の領域を除外した。しかし、路面の凸部を判別せずに、影の領域をすべて検出領域から除外するようにしてもよい。

以上、詳細に説明したように、本実施形態に係る自己位置算出装置では、画像上の影の長さ変化に基づいて路面の凸部による影を判別する。これにより、必要に応じて凸部による影の領域と凸部以外の凹部と推定される影の領域を別々に検出領域から除外できるので、車両の自己位置を精度良く算出することができる。

また、本実施形態に係る自己位置算出装置では、路面の凸部による画像上の影の位置に基づいて画像上に検出領域を設定する。これにより、凸部による影の領域を検出領域から除外できるので、特徴点を検出する際に路面の凸部による誤差を抑制し、車両の自己位置を精度良く算出することができる。

さらに、本実施形態に係る自己位置算出装置では、路面の凹部による画像上の影の位置に基づいて画像上に検出領域を設定する。これにより、凹部による影の領域を検出領域から除外できるので、パターン光から距離及び姿勢角を算出する際に路面の凹部による誤差を抑制し、車両の自己位置を精度良く算出することができる。

（第３実施形態）
次に、本発明の第３実施形態に係る自己位置算出装置について図面を参照して説明する。尚、ハードウェア構成は第１実施形態と同一なので、詳細な説明は省略する。

本実施形態に係る検出領域設定部２０は、路面の凹凸による画像上の影の位置に基づいて画像上に第１の検出領域を設定し、路面の凸部による画像上の影の位置に基づいて画像上に第１の検出領域を設定する。そして、姿勢角算出部２２は第１の検出領域におけるパターン光の位置から路面に対する車両の距離及び姿勢角を算出し、特徴点検出部２３は第２の検出領域から路面上の複数の特徴点を検出する。すなわち、本実施形態では、車両の距離及び姿勢角を算出するための第１の検出領域と、特徴点を検出するための第２の検出領域とを別々に設定したことが第１及び第２実施形態と相違している。

例えば、車両の距離及び姿勢角を算出するためのパターン光を抽出する第１の検出領域は、図１５（ａ）に示すように、除外領域１０１ａ〜１０１ｅを除いた領域に設定される。パターン光から距離及び姿勢角を算出する場合には路面が平坦であることが前提となっている。そこで、除外領域１０１ａ、１０１ｄは凸部による影と判定された領域なので、凸部が存在する可能性のある領域を除外するために、影の位置から光源方向の領域が設定されている。また、除外領域１０１ｂ、１０１ｃ、１０１ｅは凹部による影と判定された領域なので、凹部が存在する領域を除外するために、凹部による影の領域が設定されている。このとき、凹部の外縁は明るくなるので、除外領域１０１ｂ、１０１ｃ、１０１ｅは実際の影の領域よりも広く設定することが好ましい。こうして除外領域１０１ａ〜１０１ｅが設定されると、第１の検出領域は除外領域１０１ａ〜１０１ｅを除いた領域に設定される。

一方、特徴点を検出するための第２の検出領域は、図１５（ｂ）に示すように、除外領域１０３ａ、１０３ｂを除いた領域に設定される。除外領域１０３ａ、１０３ｂは凸部による影と判定された領域なので、凸部が存在する可能性のある領域を除外するために、影の位置から光源方向の領域が設定されている。さらに、凸部による影は車両の走行によって形状の変化が大きいので、凸部による影の領域も除外領域１０３ａ、１０３ｂに含まれている。ただし、凹部による影と判定された領域は、特徴点の検出に利用できるので、除外せずに検出領域としている。こうして除外領域１０３ａ、１０３ｂが設定されると、第２の検出領域は除外領域１０３ａ、１０３ｂを除いた領域に設定される。

この後、姿勢角算出部２２は第１の検出領域におけるパターン光の位置から路面に対する車両の距離及び姿勢角を算出し、特徴点検出部２３は第２の検出領域から路面上の複数の特徴点を検出する。

以上、詳細に説明したように、本実施形態に係る自己位置算出装置では、路面の凹凸による画像上の影の位置に基づいて画像上に第１の検出領域を設定し、路面の凸部による画像上の影の位置に基づいて画像上に第２の検出領域を設定する。そして、第１の検出領域におけるパターン光の位置から路面に対する車両の姿勢角を算出し、第２の検出領域から路面上の複数の特徴点を検出する。これにより、姿勢角の算出に適した検出領域と特徴点の検出に適した検出領域とをそれぞれ設定することができるので、路面の凹部による誤差と凸部による誤差の両方を抑制し、車両の自己位置を精度良く算出することができる。特に、画像からより多くのパターン光や特徴点を検出できるので、特徴が少ない路面や路面の凹凸変化に対して、よりロバストな移動量の算出が可能となる。

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

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

１０ 車両
１１ パターン光投光器（投光器）
１２ カメラ（撮像部）
１４ 路面投光器（投光器）
２０ 検出領域設定部
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
３１ 路面
３２ａ、３２ｂ パターン光
５１、５３、７１ 凸部
５５、６１ａ、６１ｂ、６１ｃ、６１ｄ、６１ｅ、７３、７７ 影の領域
６３ａ〜６３ｅ、８１ａ〜８１ｅ、９１ａ〜９１ｅ、１０１ａ〜１０１ｅ、１０３ａ〜１０３ｅ 除外領域
７５ 凹部
Ｔｅ 特徴点

Claims (7)

1. 車両周囲の路面に少なくともパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得した画像から、前記路面の凹凸による前記画像上の影の位置を検出し、前記影の位置に基づいて前記画像上に検出領域を設定する検出領域設定部と、
   前記撮像部で取得した画像の前記検出領域における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部で取得した画像の前記検出領域から、前記路面上の複数の特徴点を検出する特徴点検出部と、
   前記特徴点検出部により検出された路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する自己位置算出部と、
   を備えたことを特徴とする自己位置算出装置。
2. 請求項１に記載の自己位置算出装置であって、
   前記検出領域設定部は、前記画像上の影の位置から前記投光器の方向の領域を除いて検出領域を設定することを特徴とする自己位置算出装置。
3. 請求項１または２に記載の自己位置算出装置であって、
   前記検出領域設定部は、前記路面の凸部による前記画像上の影の位置に基づいて、前記画像上に検出領域を設定することを特徴とする自己位置算出装置。
4. 請求項３に記載の自己位置算出装置であって、
   前記検出領域設定部は、前記路面の凹部による前記画像上の影の位置に基づいて、前記画像上に検出領域を設定することを特徴とする自己位置算出装置。
5. 請求項３または４に記載の自己位置算出装置であって、
   前記検出領域設定部は、前記路面の凹凸による前記画像上の影の位置に基づいて前記画像上に第１の検出領域を設定し、前記路面の凸部による前記画像上の影の位置に基づいて前記画像上に第２の検出領域を設定し、
   前記姿勢角算出部は、前記第１の検出領域における前記パターン光の位置から前記路面に対する車両の姿勢角を算出し、
   前記特徴点検出部は、前記第２の検出領域から前記路面上の複数の特徴点を検出することを特徴とする自己位置算出装置。
6. 請求項１または２に記載の自己位置算出装置であって、
   前記検出領域設定部は、前記画像上の影の長さ変化に基づいて前記路面の凸部による影を判別することを特徴とする自己位置算出装置。
7. 車両に搭載された投光器から車両周囲の路面に少なくともパターン光を投光する手順と、
   前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記車両の制御部が、前記撮像部で取得した画像から前記路面の凹凸による前記画像上の影の位置を検出し、前記影の位置に基づいて前記画像上に検出領域を設定する手順と、
   前記制御部が、前記撮像部で取得した画像の前記検出領域における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する手順と、
   前記制御部が、前記撮像部で取得した画像の前記検出領域から、前記路面上の複数の特徴点を検出する手順と、
   前記制御部が、検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する手順と、
   前記制御部が、前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する手順と、
   を含むことを特徴とする自己位置算出方法。