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

Description

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

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

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

しかしながら、上述した従来の技術では、車両周囲の路面上に凹凸や段差があると、車両の位置を精度よく算出することができないという問題点があった。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、車両周囲の路面上に凹凸や段差がある場合でも車両の自己位置を精度よく算出することのできる自己位置算出装置及び自己位置算出方法を提供することを目的とする。

上述した課題を解決するために、本発明の一態様に係る自己位置算出装置は、パターン光が投光された車両周囲の路面を撮像して画像を取得し、この画像におけるパターン光の位置から路面に対する車両の姿勢角を算出する。また、自己位置算出装置は、取得した画像における路面上の複数の特徴点の時間変化に基づいて車両の姿勢変化量を算出し、この姿勢変化量を車両の初期位置及び姿勢角に加算することによって車両の現在位置及び姿勢角を算出する。そして、自己位置算出装置は、車両周囲の路面状態が閾値以上変化していると判定した場合には、前回の情報処理サイクルで算出した車両の現在位置及び姿勢角に姿勢変化量を加算して車両の現在位置及び姿勢角を算出する。

図１は、第１実施形態に係る自己位置算出装置の全体構成を示すブロック図である。 図２は、投光器及びカメラを車両に搭載する方法の一例を示す外観図である。 図３（ａ）は、投光器とカメラを用いてスポット光が照射された路面上の位置を算出する様子を示す図であり、図３（ｂ）は、パターン光が照射された領域とは異なる領域で検出された特徴点の時間変化からカメラの移動方向を求める様子を示す図である。 図４は、カメラで取得した画像に二値化処理を施したパターン光の画像を示す図であり、図４（ａ）はパターン光全体を示す図であり、図４（ｂ）は１つのスポット光を拡大して示す図であり、図４（ｃ）はスポット光の重心位置を示す図である。 図５は、距離及び姿勢角の変化量を算出する方法を説明するための模式図である。 図６は、画像上に検出された特徴点を示す図であり、図６（ａ）は時刻ｔに取得した第１フレーム（画像）を示す図であり、図６（ｂ）は時刻ｔ＋Δｔに取得した第２フレームを示す図である。 図７は、パターン光の位置から路面の高さの変化量を推定する方法を説明するための図である。 図８は、第１実施形態に係る自己位置算出装置による路面状態の変化を判定する処理を示すタイムチャートである。 図９は、第１実施形態に係る自己位置算出装置による自己位置算出処理の処理手順を示すフローチャートである。 図１０は、第１実施形態に係る自己位置算出装置による図９のステップＳ０９の詳細な処理手順を示すフローチャートである。 図１１は、車両のロール角及び移動量に推定誤差が発生した場合の一例を示す図である。 図１２は、第２実施形態に係る自己位置算出装置による路面状態の変化を判定する処理を示すタイムチャートである。 図１３は、第２実施形態に係る自己位置算出装置による図９のステップＳ０９の詳細な処理手順を示すフローチャートである。

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

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

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

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

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

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

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

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

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

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

なお、本実施形態では、毎回の情報処理サイクルにおいて距離及び姿勢角を算出する場合について説明する。ただし、後述するように、路面状態判定部３０によって路面状態が閾値以上変化していると判定された場合には、姿勢角算出部２２は、車両の距離及び姿勢角の算出を停止する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

具体的に、先ず、特徴点検出部２３は、カメラ１２で取得した画像３８をメモリから読み込み、画像３８から路面３１上の特徴点を検出し、各特徴点の画像上の位置（Ｕｉ、Ｖｉ）をメモリに記憶させる。

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

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

ステップＳ０９に進み、ＥＣＵ１３は、車両周囲の路面状態の変化に応じて、自己位置を算出するための積分演算の起点を設定する。詳細は、図１０を参照して後述する。

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

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

［ステップＳ０９の処理］
次に、図１０のフローチャートを参照して、図９のステップＳ０９の詳細な手順を説明する。図１０に示すように、ステップＳ１０１において、路面状態判定部３０は、車両周囲の路面状態の変化を検出する。具体的に、路面状態判定部３０は、パターン光３２ａのスポット光の数を検出したり、各車輪に取り付けられているストロークセンサの検出値の振動を検出したりする。また、路面状態判定部３０は、車両の鉛直方向の加速度を計測できる加速度センサの検出値を積分して鉛直方向の速度を算出したり、パターン光３２ａの位置を検出したりしてもよい。

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

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

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

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

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

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

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

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

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

［第１実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置算出装置では、車両周囲の路面状態が閾値以上変化していると判定した場合に、前回の情報処理サイクルで算出した車両の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して、現在の車両の現在位置、路面に対する距離及び姿勢角を算出する。これにより、車両周囲の路面上に凹凸や段差がある場合でも車両の自己位置を精度よく算出することができる。

従来では、車両周囲の路面上に凹凸や段差があると、車両の移動量の誤差が大きくなっていた。例えば、図１１は、車両１０のロール角（姿勢角の一例）及び移動量（車幅方向）の推定誤差の一例を示している。図１１（ａ）では、車両１０がロール角の生じない直進走行している場合のロール角の算出値の時間変化を示しており、図１１（ｂ）では、同じ場合の車両１０の移動量の算出値の時間変化を示している。図１１において、「Ｐ１」及び「Ｐ２」は、従来の方法で車両の移動量を算出した場合の値を示しており、「Ｑ１」及び「Ｑ２」は実際の値である真値を示している。

車両１０は直進しているため、ロール角及び車幅方向の移動量の真値（Ｑ１、Ｑ２）は０のままで移動しない。しかし、時刻ｔ１〜ｔ２の間に、車両１０が凹凸や段差のある路面上を走行したため、路面の平面式に誤差が発生して、ロール角に算出誤差が現れる。このロール角の誤差は、車幅方向の移動量の誤差として反映され、しかも積分するので、車幅方向の移動量の誤差は拡大してしまう。これは、車両周囲の路面状態が段差等によって変化して誤差があるにもかかわらず、毎回の情報処理サイクルで距離及び姿勢角を算出していることが原因である。

そこで、本実施形態に係る自己位置算出装置では、車両周囲の路面状態が閾値以上変化していると判定した場合には、車両１０の現在位置、路面に対する距離及び姿勢角の算出を停止し、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角を起点に用いる。

したがって、今回の情報処理サイクルで車両周囲の路面状態に変化が生じていたとしても、前回の情報処理サイクルの値を用いることにより、図１１のＰ１、Ｐ２に示すように誤差が拡大することはなく、車両１０の自己位置を精度よく算出することができる。

また、本実施形態に係る自己位置算出装置では、路面状態の変化を路面の高さの変化量から推定する。これにより、路面上にある凹凸や段差を確実に検出することができるので、車両の自己位置を精度よく算出することができる。

さらに、本実施形態に係る自己位置算出装置では、路面の高さの変化量をパターン光の位置から推定する。これにより、車両に搭載されたセンサを用いることなく、路面状態の変化を検出できるので、簡単な手法で精度よく車両の自己位置を算出することができる。
［第２実施形態］
次に、本発明の第２実施形態に係る自己位置算出装置について図面を参照して説明する。ただし、本実施形態に係る自己位置算出装置の構成は、図１に示した第１実施形態と同一なので、詳細な説明は省略する。

上述した第１実施形態では、毎回の情報処理サイクルで車両１０の距離及び姿勢角を算出していたが、本実施形態では一定周期で車両１０の距離及び姿勢角を算出するようにしたことが相違している。

したがって、図１２に示すように、例えば８フレーム等の一定の周期で周期カウントパルスが発生するように設定しておき、そのタイミングで姿勢角算出フラグを「ＯＮ」に設定する。これにより、一定周期で車両１０の距離及び姿勢角を算出できるようになる。

ただし、周期カウントパルスが発生した際に車両周囲の路面状態が閾値以上変化している場合、すなわち検出されたスポット光の数が閾値以下の場合には姿勢角算出フラグを「ＯＮ」にせずに「ＯＦＦ」のままとする。例えば、時刻ｔ１〜ｔ２の間では、検出されたスポット光の数が閾値以下なので、周期カウントパルスは発生しているが、姿勢角算出フラグは「ＯＮ」に設定されずに「ＯＦＦ」のままである。

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

［情報処理サイクル］
本実施形態に係る自己位置算出装置による情報処理サイクルは、図９に示す第１実施形態と同一なので、詳細な説明は省略する。本実施形態では、図９のステップＳ０９の積分演算 の起点を設定する処理が第１実施形態と相違している。

［ステップＳ０９の処理］
次に、図１３のフローチャートを参照して、図９のステップＳ０９の詳細な手順を説明する。図１３に示すように、ステップＳ２０１において、路面状態判定部３０は、所定の周期が経過したか否かを判定する。図１２で説明したように、路面状態判定部３０は、周期カウントパルスが発生しているか否かを監視し、発生している場合には所定の周期が経過したと判定してステップＳ２０３に進む。一方、周期カウントパルスが発生していない場合には、所定の周期が経過していないと判定してステップＳ２０７に進む。

この後のステップＳ２０３〜Ｓ２０９の処理は、図１０のステップＳ１０１〜Ｓ１０７の処理とそれぞれ同一なので、詳細な説明は省略する。

［第２実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置算出装置では、一定の周期で車両の距離及び姿勢角の算出を行う。そして、その際に車両周囲の路面状態が閾値以上変化していると判定された場合に、自己位置算出装置は、前回の情報処理サイクルで算出された車両の現在位置、路面に対する距離及び姿勢角に姿勢変化量を加算して、現在の車両の現在位置、路面に対する距離及び姿勢角を算出する。これにより、姿勢角算出部２２が、車両の距離及び姿勢角を算出する頻度を少なくできるので、ＥＣＵ１３の演算負荷を軽減し、且つ演算速度を向上させることができる。

なお、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
１３ ＥＣＵ
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
２７ パターン光制御部
３０ 路面状態判定部
３１ 路面
３２ａ、３２ｂ パターン光
Ｔｅ 特徴点

Claims (4)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得した画像における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部で取得した画像における前記路面上の複数の特徴点の時間変化に基づいて前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
   前記車両周囲の路面状態の変化を検出して前記路面状態が閾値以上変化しているか否かを判定する路面状態判定部とを備え、
   前記路面状態判定部によって前記路面状態が閾値以上変化していると判定された場合には、前記自己位置算出部は、前回の情報処理サイクルで算出された前記車両の現在位置及び姿勢角に前記姿勢変化量を加算して前記車両の現在位置及び姿勢角を算出することを特徴とする自己位置算出装置。
2. 前記路面状態判定部は、前記路面状態の変化を路面の高さの変化量から推定することを特徴とする請求項１に記載の自己位置算出装置。
3. 前記路面状態判定部は、前記路面の高さの変化量を前記パターン光の位置から推定することを特徴とする請求項２に記載の自己位置算出装置。
4. 車両に搭載された投光器から車両周囲の路面にパターン光を投光する手順と、
   前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記車両の制御部が、前記画像における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する手順と、
   前記制御部が、前記画像における前記路面上の複数の特徴点の時間変化に基づいて前記車両の姿勢変化量を算出する手順と、
   前記制御部が、前記車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
   前記制御部が、前記車両周囲の路面状態の変化を検出して前記路面状態が閾値以上変化しているか否かを判定する路面状態判定手順とを含み、
   前記路面状態判定手順によって前記路面状態が閾値以上変化していると判定された場合に、前記自己位置算出手順では前回の情報処理サイクルで算出された前記車両の現在位置及び姿勢角に前記姿勢変化量を加算して前記車両の現在位置及び姿勢角を算出することを特徴とする自己位置算出方法。