JP2016183889A - 自己位置算出装置及び自己位置算出方法 - 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 (5)

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