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

Abstract

本発明の自己位置算出装置は、車両周囲の路面にパターン光を投光し、車両周囲の路面を撮像して画像を取得する。そして、車両の走行状態を検出して車両が走行安定状態であると判定された場合に、自己位置算出装置は、所定の初期姿勢角に姿勢変化量を加算して車両の現在位置及び姿勢角を算出する。

Description

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

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

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

しかしながら、上述した従来の技術では、車両の挙動が安定していないときに車両の移動量を求めるための起点をリセットしてしまうと、車両の移動量を精度よく推定することができないという問題点があった。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、車両の挙動が安定しているときに車両の自己位置を算出するための起点をリセットして車両の自己位置を精度よく算出することのできる自己位置算出装置及びその方法を提供することを目的とする。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

走行状態判定部３０は、車両１０の走行状態を検出して車両１０が走行安定状態であるか否かを判定する。そして、車両１０が走行安定状態であると判定された場合には、自己位置算出部２６が、自己位置を算出するための起点として、車両１０が走行安定状態にある位置及び所定の初期距離及び初期姿勢角を設定する。これにより、自己位置算出部２６は、車両１０が走行安定状態にある位置及び所定の初期距離及び初期姿勢角に姿勢変化量を加算して、現在の車両１０の現在位置、路面に対する距離及び姿勢角を算出する。

一方、車両１０が走行安定状態ではないと判定された場合には、走行状態判定部３０は、姿勢角算出部２２によって今回の情報処理サイクルでパターン光３２ａから車両１０の距離及び姿勢角が算出されているか否かを判定する。そして、姿勢角算出部２２が車両１０の距離及び姿勢角を算出している場合には、自己位置算出部２６は、今回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角を、自己位置を算出するための起点として設定する。

また、姿勢角算出部２２によって今回の情報処理サイクルで車両１０の距離及び姿勢角が算出されていない場合には、自己位置算出部２６は、前回の情報処理サイクルで算出された車両１０の現在位置、路面に対する距離及び姿勢角を、自己位置を算出するための起点として設定する。

ここで、車両１０の走行状態が走行安定状態であるか否かを判定する方法について説明する。本実施形態では、車両１０が停車している場合、車両１０の速度が一定である場合、車両１０が直進している場合のいずれか１つ、またはいずれか複数、あるいはすべてを満たす場合に車両１０が走行安定状態であると判定する。

車両１０が停車しているか否かの判定方法としては、車両１０に搭載されている車輪速センサの値が０になった場合に車両１０が停車していると判定する。

また、車両１０の速度が一定であるか否かの判定方法は、車両１０に搭載されている加速度センサの前後方向の加速度が閾値以下の場合に車両１０の速度が一定であると判定する。閾値としては、車両１０が静止している状態と比較して、ピッチ角が０．０１［ｒａｄ］≒０．５７［ｄｅｇ］だけ変化する前後方向の加速度Ｘｇを予め求めておき、この値に設定する。これにより、車両１０の移動量を算出した際の誤差を１％以下に抑えることができる。尚、車両１０に加速度センサが搭載されていない場合には、車輪速センサの値を微分して加速度を求めてもよいし、姿勢変化量算出部２４で算出された移動量を時間微分して加速度を求めてもよい。

さらに、車両１０が直進しているか否かの判定方法は、車両１０に搭載されている加速度センサの車幅方向の加速度が閾値以下の場合に車両１０が直進していると判定する。閾値としては、例えば、車両１０が静止している状態と比較して、ロール角が０．０１［ｒａｄ］≒０．５７［ｄｅｇ］だけ変化する車幅方向の加速度Ｙｇを予め求めておき、この値に設定する。これにより、車両１０の移動量を算出した際の誤差を１％以下に抑えることができる。

尚、車幅方向の加速度がＹｇとなる車速Ｖに応じたヨーレートγ［ｒａｄ／ｓ］（Ｙｇ≒Ｖ×γ）を閾値としてもよいし、ハンドル操舵角を閾値としてもよい。また、車両１０に加速度センサやヨーレートセンサが搭載されていない場合には、姿勢変化量算出部２４で算出された移動量を時間微分して加速度を求めてもよい。

このようにして走行状態判定部３０によって車両１０が走行安定状態であるか否かが判定され、走行安定状態であると判定されると、自己位置算出部２６は、車両１０が走行安定状態にある位置及び所定の初期距離及び初期姿勢角に姿勢変化量を加算して、現在の車両１０の現在位置、路面に対する距離及び姿勢角を算出する。ここで、所定の初期距離及び初期姿勢角としては、車両１０のイグニッションスイッチがオンされたときの車両１０の路面に対する距離及び姿勢角を用いる。

例えば、図７に示すように、走行状態判定部３０が車両１０の前後方向の加速度をモニターしている場合について説明すると、閾値はピッチ角が０．０１［ｒａｄ］だけ変化する加速度Ｘｇに設定されている。この場合に、時刻ｔ１に車両１０のイグニッションスイッチがオンされると、姿勢角算出フラグがオンされるので、このタイミングでパターン光３２ａが照射される。そして、姿勢角算出部２２が、パターン光３２ａの位置から車両１０の距離及び姿勢角を算出してメモリに記憶する。この後、時刻ｔ２では、車両１０の前後方向の加速度が閾値Ｘｇ以下となるので、走行状態判定部３０は走行安定状態であると判定してリセットフラグをオンに設定する。したがって、自己位置算出部２６は、自己位置を算出するための起点をリセットして、所定の初期距離及び初期姿勢角、すなわちイグニッションスイッチがオンされたときの車両１０の現在位置、路面に対する距離及び姿勢角を起点として用いる。

しかし、時刻ｔ３になると、車両１０の前後方向の加速度が閾値Ｘｇを越えるので、走行状態判定部３０は、走行安定状態ではないと判定してリセットフラグをオフに設定する。そこで、自己位置算出部２６は、自己位置を算出するための起点として所定の初期距離及び初期姿勢角を用いることを中止する。この後、時刻ｔ４になると、再び車両１０の前後方向の加速度が閾値Ｘｇ以下となるので、走行安定状態であると判定されてリセットフラグがオンとなり、自己位置算出部２６は所定の初期距離及び初期姿勢角を起点として用いる。

このようにして本実施形態では、車両１０が走行安定状態であると判定された場合に、自己位置を算出するための起点を所定の初期距離及び初期姿勢角に設定する。これにより、車両１０の挙動が安定しているときに自己位置を算出するための起点をリセットできるので、車両１０の自己位置を精度よく算出することができる。

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

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

図８のステップＳ０１において、パターン光制御部２７は、投光器１１を制御して、車両周囲の路面３１にパターン光３２ａを投光する。

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

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

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

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

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

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

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

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

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

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

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

［ステップＳ０９の処理］
次に、図９のフローチャートを参照して、図８のステップＳ０９の詳細な手順を説明する。図９に示すように、ステップＳ１０１において、走行状態判定部３０は、車両１０の走行状態を検出する。具体的には、車輪速センサによって車両１０の速度を検出したり、加速度センサによって車両１０の前後方向や車幅方向の加速度を検出したりする。また、ヨーレートセンサによって車両１０のヨーレートを検出してもよい。

次に、ステップＳ１０３において、走行状態判定部３０は、車両１０が走行安定状態であるか否かを判定する。具体的に、走行状態判定部３０は、車両１０が停車している場合に走行安定状態であると判定する。この場合には、車両１０に搭載されている車輪速センサの値が０になると、車両１０が停車したと判断して走行安定状態であると判定する。

また、走行状態判定部３０は、車両１０の速度が一定である場合に走行安定状態であると判定する。この場合には、車両１０に搭載されている加速度センサで検出された前後方向の加速度が閾値以下になると、車両１０の速度が一定と判断して走行安定状態であると判定する。

さらに、走行状態判定部３０は、車両１０が直進している場合に走行安定状態であると判定する。この場合には、車両１０に搭載されている加速度センサで検出された車幅方向の加速度が閾値以下になると、車両１０が直進していると判断して走行安定状態であると判定する。

このようにして、車両１０が走行安定状態であるか否かの判定が行われ、走行状態判定部３０が走行安定状態であると判定した場合（ステップＳ１０３でＹＥＳ）にはステップＳ１０５に進む。また、走行状態判定部３０が走行安定状態ではないと判定した場合（ステップＳ１０３でＮＯ）にはステップＳ１０７に進む。

ステップＳ１０５では、自己位置算出部２６が、積分演算の起点として所定の初期距離及び初期姿勢角を設定する。例えば、本実施形態では、車両１０のイグニッションスイッチがオンされたときの車両１０の路面に対する距離及び姿勢角が、所定の初期距離及び初期姿勢角として設定される。

ステップＳ１０７では、走行状態判定部３０が、姿勢角算出部２２によってパターン光３２ａから距離及び姿勢角が算出されているか否かを判定する。そして、今回の情報処理サイクルのステップＳ０５において、姿勢角算出部２２が距離及び姿勢角を算出している場合（ステップＳ１０７でＹＥＳ）にはステップＳ１０９へ進む。一方、今回の情報処理サイクルのステップＳ０５において、姿勢角算出部２２が距離及び姿勢角を算出していない場合（ステップＳ１０７でＮＯ）には、ステップＳ１１１へ進む。

ステップＳ１０９では、自己位置算出部２６が、積分演算の起点として、今回の情報処理サイクルのステップＳ０５でパターン光３２ａから算出された距離及び姿勢角を設定する。また、自己位置算出部２６は、今回の情報処理サイクルで距離及び姿勢角が算出された際の車両位置を、積分演算の起点（車両の初期位置）として設定する。

ステップＳ１１１では、自己位置算出部２６が、積分演算の起点として、前回の情報処理サイクルのステップＳ０５でパターン光３２ａから算出された距離及び姿勢角を設定する。また、自己位置算出部２６は、前回の情報処理サイクルで距離及び姿勢角が算出された際の車両位置を、積分演算の起点（車両の初期位置）として設定する。

このようにして、積分演算の起点が設定されると、ステップＳ０９の処理は終了して図８のステップＳ１１に進む。

［第１実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置算出装置では、車両１０が走行安定状態であると判定された場合に、所定の初期距離及び初期姿勢角に姿勢変化量を加算して、現在の車両１０の現在位置、路面に対する距離及び姿勢角を算出する。これにより、車両１０の挙動が安定しているときに、車両１０の自己位置を算出するための起点を初期距離及び初期姿勢角にリセットできるので、車両１０の自己位置を精度よく算出することができる。さらに、車両１０の自己位置を算出するための起点を定期的に初期距離及び初期姿勢角にリセットできるので、誤差が拡大してしまうことを防止することができる。

また、本実施形態に係る自己位置算出装置では、車両１０が停車したときに走行安定状態であると判定するので、車両１０の挙動が安定している状態を確実に検出することができ、これによって車両１０の自己位置を精度よく算出することができる。

さらに、本実施形態に係る自己位置算出装置では、車両１０の速度が一定のときに走行安定状態であると判定するので、車両１０の挙動が安定している状態を確実に検出することができ、これによって車両１０の自己位置を精度よく算出することができる。

また、本実施形態に係る自己位置算出装置では、車両１０が直進しているときに走行安定状態であると判定するので、車両１０の挙動が安定している状態を確実に検出することができ、これによって車両１０の自己位置を精度よく算出することができる。

さらに、本実施形態に係る自己位置算出装置では、所定の初期距離及び初期姿勢角として車両１０のイグニッションスイッチがオンされたときの車両１０の路面に対する距離及び姿勢角を用いる。これにより、車両１０の乗員数や積載量を反映した値を初期距離及び初期姿勢角とすることができるので、より正確に車両１０の自己位置を算出することができる。

［第２実施形態］
次に、本発明の第２実施形態に係る自己位置算出装置について図面を参照して説明する。ただし、本実施形態に係る自己位置算出装置の構成は、図１に示した第１実施形態と同一なので、詳細な説明は省略する。

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

［ステップＳ０９の処理］
次に、図１０のフローチャートを参照して、図８のステップＳ０９の詳細な手順を説明する。上述した第１実施形態では、ステップＳ１０３において車両１０が走行安定状態であるか否かを判定した後に、ステップＳ１０７でパターン光３２ａから距離及び姿勢角が算出されているか否かを判定していた。しかし、本実施形態では、図１０に示すようにステップＳ２０３においてパターン光３２ａから距離及び姿勢角が算出されているか否かを判定した後に、ステップＳ２０５において車両１０が走行安定状態であるか否かを判定している。

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

［第２実施形態の効果］
上述したように、本実施形態に係る自己位置算出装置では、今回の情報処理サイクルでパターン光３２ａから車両１０の距離及び姿勢角が算出できなかった場合に限って、車両１０が走行安定状態であるか否かを判定する。これにより、車両が走行安定状態であるか否かを判定する処理を行う頻度を少なくできるので、ＥＣＵ１３の演算負荷を軽減し、且つ演算速度を向上させることができる。

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

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

Claims (6)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部で取得した画像における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部で取得した画像における前記路面上の複数の特徴点の時間変化に基づいて前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
   前記車両の走行状態を検出して前記車両が走行安定状態であるか否かを判定する走行状態判定部とを備え、
   前記走行状態判定部によって前記車両が走行安定状態であると判定された場合には、前記自己位置算出部は前記車両が走行安定状態にあるときの前記車両の現在位置及び所定の初期姿勢角に前記姿勢変化量を加算して前記車両の現在位置及び姿勢角を算出することを特徴とする自己位置算出装置。
2. 前記走行状態判定部は、前記車両が停車したときに走行安定状態であると判定することを特徴とする請求項１に記載の自己位置算出装置。
3. 前記走行状態判定部は、前記車両の速度が一定のときに走行安定状態であると判定することを特徴とする請求項１または２に記載の自己位置算出装置。
4. 前記走行状態判定部は、前記車両が直進しているときに走行安定状態であると判定することを特徴とする請求項１〜３のいずれか１項に記載の自己位置算出装置。
5. 前記自己位置算出部は、前記所定の初期姿勢角として、前記車両のイグニッションスイッチがオンされたときの前記車両の現在位置及び姿勢角を用いることを特徴とする請求項１〜４のいずれか１項に記載の自己位置算出装置。
6. 車両周囲の路面にパターン光を投光する手順と、
   前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記画像における前記パターン光の位置から前記路面に対する車両の姿勢角を算出する手順と、
   前記画像における前記路面上の複数の特徴点の時間変化に基づいて前記車両の姿勢変化量を算出する手順と、
   前記車両の初期位置及び姿勢角に前記姿勢変化量を加算してゆくことによって前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
   前記車両の走行状態を検出して前記車両が走行安定状態であるか否かを判定する走行状態判定手順とを含み、
   前記走行状態判定手順によって前記車両が走行安定状態であると判定された場合に、前記自己位置算出手順では前記車両が走行安定状態にあるときの前記車両の現在位置及び所定の初期姿勢角に前記姿勢変化量を加算して前記車両の現在位置及び姿勢角を算出することを特徴とする自己位置算出方法。