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

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部により取得された画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部により取得された画像から検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する自己位置算出部と、
   前記姿勢角変化量算出部での前記複数の特徴点の検出状態を判断する検出状態判断部と、を備え、
   前記自己位置算出部は、前記複数の特徴点の検出状態が第１基準を満たさない場合、その時の前記車両の現在位置および前記姿勢角算出部による車両の姿勢角を、前記車両の初期位置および姿勢角に設定し、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出装置。
2. 請求項１に記載の自己位置算出装置であって、
   前記検出状態判断部は、前記姿勢変化量算出部での前記複数の特徴点の数が、所定のしきい値以下である場合に、第１基準を満たさないと判断することを特徴とする自己位置算出装置。
3. 請求項１に記載の自己位置算出装置であって、
   前記検出状態判断部は、前記画像から検出された前記特徴点の密度が所定のしきい密度以下である場合に、第１基準を満たさないと判断することを特徴とする自己位置算出装置。
4. 請求項１に記載の自己位置算出装置であって、
   前記画像から検出された前記複数の特徴点の相対位置から、前記複数の特徴点を含む平面を算出する平面算出部を更に備え、
   前記平面算出部による前記平面の算出精度が所定のしきい精度以下である場合に、前記検出状態判断部は、第１基準を満たさないと判断することを特徴とする自己位置算出装置。
5. 請求項１に記載の自己位置算出装置であって、
   車両周囲の照度の変化量を検出する照度検出部を更に備え、
   前記照度検出部により検出された前記照度の変化量が所定のしきい照度以上である場合に、前記検出状態判断部は、第１基準を満たさないと判断することを特徴とする自己位置算出装置。
6. 請求項１〜５のいずれか一項に記載の自己位置算出装置であって、
   前記姿勢角算出部による前記姿勢角の算出状態を判断する算出状態判断部を更に備え、
   前記複数の特徴点の検出状態が第１基準を満たさないと前記検出状態判断部が判断し、且つ前記姿勢角の算出状態が第２基準を満たさないと前記算出状態判断部が判断した場合、前記自己位置算出部は、少なくとも前記車両への乗員及び搭載物を考慮した所定の初期姿勢角に対して、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出装置。
7. 請求項１〜５のいずれか一項に記載の自己位置算出装置であって、
   前記姿勢角算出部による前記姿勢角の算出状態を判断する算出状態判断部を更に備え、
   前記複数の特徴点の検出状態が第１基準を満たさないと前記検出状態判断部が判断し、且つ前記姿勢角の算出状態が第２基準を満たさないと前記算出状態判断部が判断した場合に、前記自己位置算出部は、前記複数の特徴点の検出状態が第１基準を満たさないと前記検出状態判断部が判断する前に算出していた姿勢角に対して、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出装置。
8. 車両に搭載された投光器から車両周囲の路面にパターン光を投光する手順と、
   前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記車両の制御部が、前記画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する手順と、
   前記制御部が、前記画像から検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢角変化量算出手順と、
   前記制御部が、前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する自己位置算出手順と、
   前記制御部が、前記姿勢角変化量算出手順での前記複数の特徴点の検出状態を判断する手順と、を備え、
   前記自己位置算出手順では、前記複数の特徴点の検出状態が第１基準を満たさない場合、その時の前記車両の現在位置および前記パターン光の位置から算出された車両の姿勢角を、前記車両の初期位置および姿勢角に設定し、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出方法。