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

Description

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

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

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

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

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

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

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

図１は、第１実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図２は、投光器及びカメラを車両に搭載する方法の一例を示す外観図である。 図３（ａ）は、投光器とカメラを用いてスポット光が照射された路面上の位置を算出する様子を示す図であり、図３（ｂ）は、パターン光が照射された領域とは異なる領域で検出された特徴点の時間変化からカメラの移動方向を求める様子を示す図である。 図４は、カメラで取得した画像に二値化処理を施したパターン光の画像を示す図であり、図４（ａ）はパターン光全体を示す図であり、図４（ｂ）は１つのスポット光を拡大して示す図であり、図４（ｃ）はスポット光の重心位置を示す図である。 図５は、距離及び姿勢角の変化量を算出する方法を説明するための模式図である。 図６は、画像上に検出された特徴点を示す図であり、図６（ａ）は時刻ｔに取得した第１フレーム（画像）を示す図であり、図６（ｂ）は時刻ｔ＋Δｔに取得した第２フレームを示す図である。 図７（ａ）は、カメラで取得した画像を示す図であり、図７（ｂ）は、カメラで取得した画像に二値化処理を施したパターン光の画像を示す図である。図７（ｃ）は、特徴点の検出結果を示す図である。 図８は、５×５のスポット光が投光されたパターン光領域を示す概略図である。 図９は、５×５のスポット光が投光されたパターン光領域の周囲に設定された特徴点検出領域の一例を示す概略図である。 図１０は、第１実施形態に係る自己位置算出方法の一例を説明するためのフローチャートである。 図１１は、スポット光が密に投光されたパターン光領域の一例を示す概略図である。 図１２は、格子状のパターン光が投光されたパターン光領域の一例を示す概略図である。 図１３は、スポット光が密に投光されたパターン光領域の周囲に設定された特徴点検出領域の一例を示す概略図である。 図１４は、５×５のスポット光が投光されたパターン光領域の左右に設定された特徴点検出領域の一例を示す概略図である。 図１５は、スポット光が密に投光されたパターン光領域の左右に設定された特徴点検出領域の他の一例を示す概略図である。 図１６は、第２実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図１７（ａ）は、横方向速度が相対的に小さい場合の特徴点検出領域の一例を示す概略図であり、図１７（ｂ）は、横方向速度が相対的に大きい場合の特徴点検出領域の一例を示す概略図である。 図１８は、特徴点検出領域の設定方法の具体例を説明するための概略図である。 図１９（ａ）は、横方向速度の絶対値の時間変化を表すグラフであり、図１９（ｂ）は、特徴点検出領域の横幅の時間変化を表すグラフである。 図２０は、第２実施形態に係る自己位置算出方法の一例を説明するためのフローチャートである。 図２１（ａ）は、車速が相対的に小さい場合の特徴点検出領域の一例を示す概略図であり、図２１（ｂ）は、車速が相対的に大きい場合の特徴点検出領域の一例を示す概略図である。 図２２（ａ）は、車速の絶対値の時間変化を表すグラフであり、図２２（ｂ）は、特徴点検出領域の前後方向幅の時間変化を表すグラフである。 図２３（ａ）は、スポット光が密に投光されたパターン光領域の一例を示す概略図であり、図２３（ｂ）は、車速及び横方向速度に応じて設定された特徴点検出領域の一例を示す概略図である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

［第１の変形例］
次に、第１の変形例として、特徴点検出領域の設定方法の他の一例を説明する。投光器１１は、図１１に示すように、パターン光４１ａとして、３５（７×５）個のスポット光を密に投光してもよい。ここで、各パターン光４１ａに対して、特徴点を検出する際にパターン光４１ａの影響が十分小さくなる距離だけ離間した領域をパターン光領域４１ｂとする。パターン光領域４１ｂは、パターン光４１ａであるスポット光が密に投光されるため、全てのスポット光を一括して含むように形成されている。なお、図１２に示すように、パターン光４２ａとして、スポット光の代わりに格子パターンを投光した場合も同様に格子パターンの周囲を囲むようにパターン光領域４２ｂが形成される。

図１１に示したパターン光４１ａを投光する場合、特徴点検出部２３は、図１３に示すように、パターン光領域４１ｂの周囲を囲むように特徴点検出領域４１ｃを設定する。特徴点検出領域４１ｃは枠状を有し、パターン光領域４１ｂに隣接している。特徴点検出部２３は、カメラ１２による撮像領域３０内のうち、パターン光領域４１ｂは特徴点の検出対象とせずに、特徴点検出領域４１ｃ内で特徴点を検出する。

［第２の変形例］
次に、第２の変形例として、特徴点検出領域の設定方法の他の一例を説明する。図８に示すように、投光器１１によりパターン光４０ａとして５×５＝２５点のスポット光を投光された場合、特徴点検出部２３は、図１４に示すように、パターン光領域４０ｂの左右（車幅方向）に特徴点検出領域４３ａ，４３ｂ，４３ｃ，４３ｄを設定してもよい。特徴点検出領域４３ａ，４３ｂ，４３ｃ，４３ｄのそれぞれは、パターン光領域４０ｂに隣接し、車幅方向の幅よりも特徴点の移動方向５２の幅が広い長方形のパターンである。車幅方向は、特徴点の移動方向５２に直交する方向であり、特徴点の移動が少ない方向である。特徴点検出部２３は、カメラ１２による撮像領域３０内のうち、特徴点検出領域４３ａ，４３ｂ，４３ｃ，４３ｄ内で特徴点を検出する。

また、図１１に示すようにパターン光４１ａとしてスポット光を密に、或いは格子パターンが投光された場合、特徴点検出部２３は、図１５に示すように、パターン光４１ａの左右（車幅方向）に特徴点検出領域４４ａ，４４ｂを設定してもよい。特徴点検出領域４４ａ，４４ｂは、パターン光領域４１ｂに隣接し、車幅方向の幅よりも特徴点の移動方向５２における幅が広い長方形のパターンである。特徴点検出部２３は、カメラ１２による撮像領域３０内のうち、特徴点検出領域４４ａ，４４ｂ内で特徴点を検出する。

［第１実施形態の効果］
第１実施形態によれば、パターン光４０ａ，４１ａを含むパターン光領域４０ｂ，４１ｂの周囲に特徴点検出領域４０ｃ，４１ｃを設定し、特徴点検出領域４０ｃ，４１ｃ内で特徴点Ｔｅを検出することにより、パターン光４０ａ，４１ａを抽出する領域とは異なり、且つ隣接する領域で特徴点を検出することができるので、パターン光４０ａ，４１ａと区別して特徴点を精度良く検出することができる。したがって、検出された特徴点を用いて、車両の自己位置を精度良く検出することができる。

また、パターン光４０ａ，４１ａを含むパターン光領域４０ｂ，４１ｂの左右（車幅方向）に特徴点検出領域４３ａ〜４３ｄ又は特徴点検出領域４４ａ，４４ｂをそれぞれ設定し、特徴点検出領域４３ａ〜４３ｄ又は特徴点検出領域４４ａ，４４ｂ内で特徴点を検出することにより、特徴点の移動方向５２の幅が車幅方向の幅よりも長く、前後フレームでの対応付けがしやすい特徴点を検出可能な領域において、特徴点を検出することができる。

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

特徴点検出部２３は、車両挙動検出部１４により検出された車両１０の挙動に応じて、特徴点検出領域を設定する。例えば、パターン光４１ａとして５×７個のスポット光が密に投光された場合、特徴点検出部２３は、車両挙動検出部１４により検出された横方向速度の絶対値が低いほど、図１７（ａ）に示すように、特徴点検出領域４４ａ，４４ｂの横幅ｗ１を狭くし、車両挙動検出部１４により検出された横方向速度の絶対値が高いほど、図１７（ｂ）に示すように、特徴点検出領域４４ａ，４４ｂの横幅ｗ１を広くする。

具体的には、特徴点検出領域４４ａ，４４ｂの横幅ｗ１は、カメラ１２の撮像領域３０の位置と、フレームレートに応じて決定される。例えば図１８に示すように、カメラ１２の撮像領域３０の位置が、車両１０の後輪車軸中心ＯからＬ［ｍ］の距離にあり、フレームレートがＦ［ｆｐｓ］の場合を考える。すると車両１０が極低速で走行し、後輪車軸中心Ｏを旋回中心として走行している場合、横方向速度であるヨーレートがγ［ｒａｄ／ｓｅｃ］の時、１フレーム当たりの特徴点Ｔｅの横方向移動量はＬγ／Ｆ［ｍ］となる。前後フレームで特徴点Ｔｅを対応付けて検出するためには、原理的にこのＬγ／Ｆ以上の幅の横方向領域が必要である。本実施形態では余裕を見て、この５倍の幅（５×Ｌγ／Ｆ）を特徴点検出領域４４ａ，４４ｂの横幅ｗ１として設定する。

図１９（ａ）に、ヨーレートγの絶対値の時間変化を示し、図１９（ｂ）に、特徴点検出領域の横幅（５×Ｌγ／Ｆ）の時間変化を示す。時刻ｔ０〜ｔ７は、１情報処理サイクル毎の時刻を示す。図１９（ａ）及び図１９（ｂ）に示すように、ヨーレートγの絶対値の増減に比例して、特徴点検出領域の横幅（５×Ｌγ／Ｆ）を増減させてもよい。或いは、ヨーレートγの１又は２以上の閾値を予め設けておき、ヨーレートγが閾値以上となった場合に特徴点検出領域の横幅（５×Ｌγ／Ｆ）を段階的に増減させてもよい。

［情報処理サイクル］
次に、図２０を参照して、第２実施の形態に係る自己位置算出方法のステップＳ０７の詳細を説明する。他の手順は、第１実施の形態に係る自己位置算出方法の手順と実質的に同様であるので、重複した説明を省略する。

ステップＳ１０１において、車両挙動検出部１４は、車両１０の挙動（ここでは、ヨーレートγ）を検出する。ステップＳ１０２において、特徴点検出部２３は、カメラ１２で取得した図８に対応する撮像領域３０の画像３８をメモリから読み込み、パターン光領域４０ｂを囲むように特徴点検出領域４０ｃを設定する。このとき、車両１０のヨーレートγに応じて、特徴点検出領域４０ｃの横幅ｗ１をその値が（５×Ｌγ／Ｆ）となるように広げる。ステップＳ１０３において、特徴点検出部２３は、画像３８の特徴点検出領域４０ｃ内から路面３１上の特徴点Ｔｅを検出し、各特徴点Ｔｅの画像上の位置（Ｕｉ、Ｖｉ）をメモリに記憶させる。

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

［第１の変形例］
次に、第１の変形例として、特徴点検出領域の設定方法の他の一例を説明する。特徴点検出部２３は、車両挙動検出部１４により検出された車速Ｖの絶対値に応じて、特徴点検出領域の車両前後方向幅を変化させてもよい。例えば、特徴点検出部２３は、車両挙動検出部１４により検出された車速Ｖの絶対値が相対的に低いとき、図２１（ａ）に示すように、特徴点検出領域４４ａ，４４ｂの前後方向幅ｗ２を相対的に狭く設定する。一方、特徴点検出部２３は、車両挙動検出部１４により検出された車速Ｖの絶対値が相対的に高いとき、図２１（ｂ）に示すように、特徴点検出領域４４ａ，４４ｂの前後方向幅ｗ２を相対的に広く設定する。

具体的には、例えばカメラ１２のフレームレートがＦ［ｆｐｓ］の場合を考える。すると車速Ｖ［ｍ／ｓ］で走行している場合、１フレーム当たりの画像上の特徴点Ｔｅの車両前後方向の移動量はＶ／Ｆ［ｍ］となる。前後フレームで特徴点Ｔｅを対応付けて検出するためには、原理的にこのＶ／Ｆ以上の前後方向の領域が必要である。本実施形態では、余裕を見て、この５倍の長さの前後方向幅（５×Ｖ／Ｆ）を特徴点検出領域４４ａ，４４ｂの前後方向幅ｗ２として設定する。

図２２（ａ）に車速Ｖの絶対値の時間変化を示し、図２２（ｂ）に特徴点検出領域の前後方向幅（５×Ｖ／Ｆ）の時間変化を示す。時刻ｔ１〜ｔ７は１情報処理サイクル毎の時刻を示す。図２２（ａ）及び図２２（ｂ）に示すように、車速Ｖの絶対値の増減に応じて、特徴点検出領域４４ａ，４４ｂの前後方向幅（５×Ｖ／Ｆ）も増減させる。或いは、車速Ｖの１又は２以上の閾値を予め設けておき、車速Ｖが閾値以上となった場合に特徴点検出領域の前後方向幅（５×Ｖ／Ｆ）を段階的に増減させてもよい。

［第２の変形例］
次に、第２の変形例として、特徴点検出領域の設定方法の他の一例を説明する。図２３（ａ）に示すように、パターン光４１ａとしてスポット光を密に照射する場合、特徴点検出部２３は、車両挙動検出部１４により検出された横方向速度γ及び車速Ｖのそれぞれに応じて、特徴点検出領域４４ａ，４４ｂの横幅ｗ４及び前後方向幅ｗ３を設定してもよい。例えば、車両１０の移動方向５１が右斜め前方である場合には、図２３（ｂ）に示すように、特徴点検出領域４４ａ，４４ｂの横幅ｗ４を（５×Ｌγ／Ｆ）まで広げ、且つ特徴点検出領域４４ａ，４４ｂの前後方向幅ｗ３を（５×Ｖ／Ｆ）まで広げる。

［第２実施形態の効果］
第２実施形態に係る自己位置算出装置によれば、パターン光４１ａを含むパターン光領域４１ｂの周囲に特徴点検出領域４４ａ，４４ｂを設定し、特徴点検出領域４４ａ，４４ｂ内で特徴点を検出することにより、パターン光４１ａと区別して特徴点を精度良く検出することができ、車両の自己位置を精度良く検出することができる。

また、パターン光４１ａを含むパターン光領域４１ｂの左右（車幅方向）に特徴点検出領域４４ａ，４４ｂを設定し、特徴点検出領域４４ａ，４４ｂ内で特徴点を検出することにより、前後フレームで対応付けしやすい特徴点を検出できる。

また、車両挙動検出部１４により検出された横方向速度に応じて特徴点検出領域４４ａ，４４ｂの横幅ｗ１を広げることにより、車両１０の移動に追従して前後フレームで対応付けが可能な特徴点を効率よく検出することができる。

更に、車両挙動検出部１４により検出された車速Ｖに応じて特徴点検出領域４４ａ，４４ｂの前後方向幅ｗ２を広げることにより、車両１０の移動に追従して前後フレームで対応付けが可能な特徴点を効率よく検出することができる。

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

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

Claims (6)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得した画像における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部で取得した画像から、前記パターン光が投光された領域の周囲に特徴点検出領域を設定し、当該特徴点検出領域内で前記路面上の複数の特徴点を検出する特徴点検出部と、
   前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記路面に対する車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出部
   とを備えることを特徴とする自己位置算出装置。
2. 前記パターン光は、複数のスポット光からなり、
   前記特徴点検出部は、前記スポット光が投光された領域のそれぞれの周囲に特徴点検出領域を設定することを特徴とする請求項１に記載の自己位置算出装置。
3. 前記特徴点検出部が、前記パターン光が投光された領域に対して車幅方向に前記特徴点検出領域を設定することを特徴とする請求項１または２に記載の自己位置算出装置。
4. 前記特徴点検出部が、前記車両の横方向速度に応じて、前記特徴点検出領域を車幅方向に広げることを特徴とする請求項１〜３のいずれか１項に記載の自己位置算出装置。
5. 前記特徴点検出部が、車速に応じて、前記特徴点検出領域を車両前後方向に広げることを特徴とする請求項１〜４のいずれか１項に記載の自己位置算出装置。
6. 車両に搭載された投光器から車両周囲の路面にパターン光を投光する手順と、
   前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記車両の制御部が、前記取得した画像における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する手順と、
   前記制御部が、前記取得した画像から、前記パターン光が投光された領域の周囲に特徴点検出部を設定し、当該特徴点検出部内で前記路面上の複数の特徴点を検出する手順と、
   前記制御部が、前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する手順と、
   前記制御部が、前記路面に対する車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する手順
   とを含むことを特徴とする自己位置算出方法。