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

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記撮像部により取得された画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部により取得された画像から検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する自己位置算出部と、
   前記パターン光が投光された領域を含む車両周囲の路面の照度を検出する照度検出部と、を備え、
   前記自己位置算出部は、前記路面の照度が所定の閾値以下のときに、その時の前記車両の現在位置および前記姿勢角算出部による車両の姿勢角を、前記車両の初期位置および姿勢角に設定し、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出装置。
2. 車両に搭載された投光器から車両周囲の路面にパターン光を投光する手順と、
   前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   前記車両の制御部が、前記画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する手順と、
   前記制御部が、前記画像から検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する手順と、
   前記制御部が、前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する自己位置算出手順と、
   前記制御部が、前記パターン光が投光された領域を含む車両周囲の路面の照度を検出する手順と、を含み、
   前記自己位置算出手順では、前記路面の照度が所定の閾値以下のときに、その時の前記車両の現在位置および前記パターン光の位置から算出された車両の姿勢角を、前記車両の初期位置および姿勢角に設定し、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出方法。