JP6492974B2 - 自己位置算出装置及び自己位置算出方法 - 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)）を用いることができる。

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

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

ここで、特徴点検出部２３は、投光器１１によるパターン光の投光領域の周囲に特徴点検出領域を設定し、この特徴点検出領域内で路面３１上の特徴点を検出する。特徴点検出領域は、路面３１上の特徴点を検出するにあたって、パターン光に対してパターン光の影響が十分小さくなる距離だけ離れた領域を意味する。パターン光の影響が十分小さくなる距離は、予め実験的に求めておくことができる。本実施形態では、パターン光の影響が十分小さくなる距離の一例として、想定される光環境下で９９％以上の確率で特徴点を誤検出しなくなる距離を求めておく。

例えば、投光器１１は、図１０に示すように、路面３１上のカメラ１２の撮像領域８０内に、パターン光３２ａとして２５（５×５）個のスポット光Ｓ_ｐを投光する。図１０において、矢印８２は車両１０の移動方向を示し、矢印８２に対して逆向きの矢印８３は特徴点の移動方向を示す。ここで、各スポット光Ｓ_ｐに対して、特徴点を検出する際にパターン光の影響が十分小さくなる距離だけ離間した領域をパターン光領域８４とする。

特徴点検出部２３は、図１１に示すように、カメラ１２の撮像領域８０内で、パターン光領域８４の周囲を囲むように特徴点検出領域８６を設定する。特徴点検出領域８６は、格子状を有し、パターン光領域８４に隣接している。特徴点検出部２３は、カメラ１２による撮像領域８０内のうち、パターン光領域８４内は特徴点の検出対象とせずに、特徴点検出領域８６内で特徴点を検出する。この結果、パターン光と特徴点とを容易に区別することができるので、特徴点を精度良く検出することができる。特徴点検出領域８６は、パターン光領域８４に隣接するのではなく、スポット光Ｓ_ｐに隣接するように設定することもできる。

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

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

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

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

パターン光制御部２７は、投光器１１によるパターン光３２ａの投光を制御する。例えば、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に、パターン光制御部２７は、パターン光３２ａの投光を開始する。その後、パターン光制御部２７は、自己位置算出装置が停止するまで、パターン光３２ａを連続して投光する。或いは、所定の時間間隔をおいて、投光のオン／オフを繰り返してもよい。さらに、パターン光制御部２７は、各スポット光Ｓ_ｐに形状を付与したり、強度を変化させる等の処理を行っているので、各スポット光Ｓ_ｐはユニークに識別される。
（情報処理サイクル）
次に、カメラ１２により取得された画像３８から車両１０の移動量を推定する自己位置算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図１２を参照しながら説明する。図１２のフローチャートに示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に開始され、自己位置算出装置が停止するまで、繰り返し実行される。

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

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

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

ステップＳ０５において、路面ペイント検出部２０は、カメラ１２で撮像された画像をメモリから読み込み、この画像に二値化処理を行って二値化画像を生成し、二値化画像のメッシュ領域の中からペイント領域を検出する。路面ペイント検出部２０は、検出されたペイント領域をメモリに記憶する。

ステップＳ０７に進み、ＥＣＵ１３は、パターン光が路面上のペイント領域に照射されているか否かを判定する。具体的には、ステップＳ０５においてペイント領域が検出されていればパターン光が路面上のペイント領域に照射されていると判定して、ステップＳ０９に進む。一方、ステップＳ０５においてペイント領域が検出されていなければパターン光は路面上のペイント領域に照射されていないと判定して、ステップＳ１１に進む。

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

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

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

具体的に、先ず、特徴点検出部２３は、カメラ１２により取得された画像３８をメモリから読み込み、図１１に示すようにパターン光領域８４を囲むように特徴点検出領域８６を設定する。更に、特徴点検出部２３は、画像３８から特徴点検出領域８６内にある路面３１上の特徴点を検出し、各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）をメモリに記憶する。

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

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

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

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

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

［変形例］
次に、変形例として、パターン光を投光する領域を変化させる場合について説明する。例えば、図１３に示すように、カメラ１２の撮像領域９０のうち一部の領域のみにパターン光９２を投光する。図１３では、カメラ１２の撮像領域９０が複数のメッシュ領域５０に区分され、その中の中央付近にある９個のメッシュ領域にスポット光Ｓ_ｐが１点ずつ投光されている。

このとき、特徴点検出部２３は、パターン光９２が投光されたメッシュ領域に隣接させて特徴点検出領域を設定する。例えば、図１４（ａ）に示す状況では、カメラ１２が撮像領域９０を撮像し、この撮像領域９０にはペイント領域９４が撮像されている。この場合に、特徴点検出部２３は、図１４（ｂ）に示すようにパターン光９２が投光されているメッシュ領域に隣接したメッシュ領域を特徴点検出領域に設定する。図１４（ｂ）では、パターン光９２の左右（車幅方向）にそれぞれ特徴点検出領域９６ａ、９６ｂが設定されている。

さらに、パターン光制御部２７は、路面ペイント検出部２０で検出されたペイント領域にパターン光が投光されるように投光位置を設定し、この投光位置にパターン光が投光されるように投光器１１を制御する。この場合、パターン光制御部２７は、前回の情報処理サイクルで検出された路面のペイント領域をメモリから取得し、取得したペイント領域に基づいてパターン光の投光位置を設定する。この後、パターン光制御部２７は、投光器１１を制御して、設定された投光位置にパターン光を投光する。これにより、路面上のペイント領域にパターン光が投光される。

［実施形態の効果］
以上、詳細に説明したように、本実施形態に係る自己位置算出装置では、カメラ１２で取得した画像から路面のペイント領域を検出し、検出したペイント領域上にあるパターン光の位置に基づいて路面に対する車両の距離及び姿勢角を算出する。路面のペイント領域は表面が多孔質ではなく、アスファルト部分に比べ、凹凸の少ない、滑らかな表面であるので、ペイント領域に投光されたパターン光は、多孔質材料であるアスファルト路面に投光されたパターン光より同一平面上に存在している可能性が高い。したがって、本実施形態に係る自己位置算出装置によれば、路面上にパターン光を投光する場合、路面に対する車両の距離及び姿勢角の誤差を抑制し、ひいては移動量推定の誤差を抑制することができる。

また、本実施形態に係る自己位置算出装置では、ペイント領域に基づいてパターン光の投光位置を設定し、設定された投光位置にパターン光が投光されるように投光器１１を制御する。これにより、確実にパターン光をペイント領域上に投光することができるので、距離及び姿勢角の誤差を抑制し、ひいては移動量推定の誤差を抑制することができる。

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

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

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
２０ 路面ペイント検出部
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
２７ パターン光制御部
３１ 路面
３２ａ、３２ｂ、９２ パターン光
５０ メッシュ領域
６０、７０、８０、９０ 撮像領域
８４ パターン光領域
８６、９６ａ、９６ｂ 特徴点検出領域
Ｔｅ 特徴点

Claims (3)

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