特許 6237875 自己位置算出装置及び自己位置算出方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6237875

(24)【登録日】2017年11月10日

(45)【発行日】2017年11月29日

(54)【発明の名称】自己位置算出装置及び自己位置算出方法

(51)【国際特許分類】

   G08G 1/16 20060101AFI20171120BHJP

   G01C 21/28 20060101ALI20171120BHJP

【ＦＩ】

   G08G1/16 C

   G01C21/28

【請求項の数】7

【全頁数】22

(21)【出願番号】特願2016-503896(P2016-503896)

(86)(22)【出願日】2014年2月24日

(86)【国際出願番号】JP2014054311

(87)【国際公開番号】WO2015125297

(87)【国際公開日】20150827

【審査請求日】2016年8月31日

(73)【特許権者】

【識別番号】000003997

【氏名又は名称】日産自動車株式会社

(74)【代理人】

【識別番号】100083806

【弁理士】

【氏名又は名称】三好 秀和

(74)【代理人】

【識別番号】100101247

【弁理士】

【氏名又は名称】高橋 俊一

(74)【代理人】

【識別番号】100095500

【弁理士】

【氏名又は名称】伊藤 正和

(74)【代理人】

【識別番号】100098327

【弁理士】

【氏名又は名称】高松 俊雄

(72)【発明者】

【氏名】山口 一郎

(72)【発明者】

【氏名】西内 秀和

【審査官】 東 勝之

(56)【参考文献】

【文献】 特開２０１３−１４７１１４（ＪＰ，Ａ）

【文献】 特開２００７−２５６０９０（ＪＰ，Ａ）

【文献】 特開平０６−３２５２９８（ＪＰ，Ａ）

【文献】 特開２０１３−１８７８６２（ＪＰ，Ａ）

【文献】 特開２０１０−１０１６８３（ＪＰ，Ａ）

【文献】 国際公開第２０１２／１７２８７０（ＷＯ，Ａ１）

【文献】 特開２００４−１９８２１１（ＪＰ，Ａ）

【文献】 特開２００４−１９８２１２（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０８Ｇ １／００ − １／１６

Ｇ０１Ｃ ２１／００ − ２１／３６

(57)【特許請求の範囲】

【請求項1】

車両周囲の路面にパターン光を投光する投光器と、
前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
前記撮像部により取得された画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
前記撮像部により取得された画像から検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する自己位置算出部と、
前記姿勢変化量算出部での前記複数の特徴点の検出状態を判断する検出状態判断部と、
前記投光器によるパターン光の投光を制御するパターン光制御部と、を備え、
前記複数の特徴点の検出状態が第１基準を満たさないと前記検出状態判断部が判断した場合、前記パターン光制御部は、パターン光を投光する
ことを特徴とする自己位置算出装置。

【請求項2】

請求項１に記載の自己位置算出装置であって、
前記複数の特徴点の検出状態が第１基準を満たすと前記検出状態判断部が判断した場合、前記パターン光制御部は、パターン光の投光を停止する
ことを特徴とする自己位置算出装置。

【請求項3】

請求項１又は２に記載の自己位置算出装置であって、
前記複数の特徴点の検出状態が前記第１基準よりも低い第２基準を満たさないと前記検出状態判断部が判断した場合、前記姿勢角算出部は、前記パターン光の位置から前記路面に対する車両の姿勢角を算出する
ことを特徴とする自己位置算出装置。

【請求項4】

請求項１又は２に記載の自己位置算出装置であって、
前記パターン光制御部がパターン光の投光を開始してから第１所定時間が経過した後に、前記姿勢角算出部は、前記パターン光の位置から前記路面に対する車両の姿勢角を算出し始める
ことを特徴とする自己位置算出装置。

【請求項5】

請求項１〜４のいずれか一項に記載の自己位置算出装置であって、
前記複数の特徴点の検出状態が第１基準を満たさないと前記検出状態判断部が判断した場合、前記パターン光制御部は、前記検出状態判断部が判断してから第２所定時間が経過した後に、パターン光の投光を停止する
ことを特徴とする自己位置算出装置。

【請求項6】

請求項１〜４のいずれか一項に記載の自己位置算出装置であって、
前記複数の特徴点の検出状態が第１基準を満たさないと前記検出状態判断部が判断した場合、前記パターン光制御部は、前記検出状態判断部が判断してから第３所定時間が経過した後に、パターン光の投光を開始する
ことを特徴とする自己位置算出装置。

【請求項7】

車両に搭載された投光器から車両周囲の路面にパターン光を投光し、
前記車両に搭載された撮像部によって、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得し、
前記車両の制御部が、前記画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出し、
前記制御部が、前記画像から検出された前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出し、
前記制御部が、前記車両の初期位置および姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置および姿勢角を算出する、自己位置算出方法であって、
前記制御部が、前記複数の特徴点の検出状態が第１基準を満たさないと判断した場合に、前記投光器から前記パターン光を投光する
ことを特徴とする自己位置算出方法。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００８−１７５７１７号公報

【発明の概要】

【0004】

しかし、特徴点の検出状態が悪い場合、特徴点の位置から車両の移動量を精度よく求めることは難しい。

【0005】

本発明は上記課題に鑑みて成されたものであり、その目的は、特徴点の検出状態に係わらず、車両の現在位置を精度良く且つ安定して推定することができる自己位置算出装置及び自己位置算出方法を提供することである。

【0006】

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

【図面の簡単な説明】

【0007】

【図1】図１は、実施形態に係わる自己位置算出装置の全体構成を示すブロック図である。

【図2】図２は、車両１０への投光器１１及びカメラ１２の搭載例を示す外観図である。

【図3】図３（ａ）は、投光器１１とカメラ１２の間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕ_ｊ、Ｖ_ｊ）とから、各スポット光が照射された路面３１上の位置を算出する様子を示す図であり、図３（ｂ）は、パターン光３２ａが照射された領域とは異なる他の領域３３から検出された特徴点の時間変化から、カメラ１２の移動方向３４を求める様子を示す模式図である。

【図4】図４（ａ）及び図４（ｂ）は、カメラ１２により取得された画像に対して二値化処理を施したパターン光３２ａの画像を示す図であり、図４（ａ）はパターン光３２ａ全体を示し、図４（ｂ）は１つのスポット光Ｓ_ｐを拡大して示し、図４（ｃ）はパターン光抽出部２１により抽出された各スポット光Ｓ_ｐの重心の位置Ｈ_ｅを示す図である。

【図5】図５は、距離及び姿勢角の変化量及びカメラ１２の移動量（ΔＬ）を算出する方法を説明するための模式図である。

【図6】図６（ａ）は、時刻ｔに取得された第１フレーム（画像）３８の一例を示す。図６（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得された第２フレーム３８’を示す。

【図7】図７は、図１の自己位置算出装置を用いた自己位置算出方法の一例を示すフローチャートである。

【図8】図８は、図７のステップＳ０５の詳細な手順を示すフローチャートである。

【図9】図９は、図８のフローチャートにしたがって実行される情報処理の一例を示すグラフであり、図９（ａ）は姿勢角算出部２２がパターン光３２ａから距離及び姿勢角を算出するか否かを示し、図９（ｂ）はパターン光投光実施フラグの状態を示し、図９（ｃ）は対応付けが取れる特徴点の数を示す。

【図10】図１０は、図７のステップＳ０９の詳細な手順を示すフローチャートである。

【図11】図１１（ａ）及び図１１（ｂ）は、車両１０のロール角及び車幅方向移動量の推定誤差の例を示すグラフである。

【図12】図１２は、第２実施形態に係わるステップＳ０５の詳細な手順を示すフローチャートである。

【図13】図１３は、図１２のフローチャートにしたがって実行される情報処理の一例を示すグラフであり、図１３（ａ）は姿勢角算出部２２がパターン光３２ａから距離及び姿勢角を算出するか否かを示し、図１３（ｂ）はパターン光投光実施フラグの状態を示し、図１３（ｃ）は対応付けが取れる特徴点の数を示す。

【図14】図１４は、第３実施形態に係わるステップＳ０５の詳細な手順を示すフローチャートである。

【図15】図１５は、図１４のフローチャートにしたがって実行される情報処理の一例を示すグラフであり、図１５（ａ）は姿勢角算出部２２がパターン光３２ａから距離及び姿勢角を算出するか否かを示し、図１５（ｂ）はパターン光投光実施フラグの状態を示し、図１５（ｃ）は対応付けが取れる特徴点の数を示す。

【図16】図１６は、第４実施形態に係わるステップＳ０５の詳細な手順を示すフローチャートである。

【図17】図１７は、図１６のフローチャートにしたがって実行される情報処理の一例を示すグラフであり、図１７（ａ）は姿勢角算出部２２がパターン光３２ａから距離及び姿勢角を算出するか否かを示し、図１７（ｂ）はパターン光投光実施フラグの状態を示し、図１７（ｃ）は対応付けが取れる特徴点の数を示す。

【発明を実施するための形態】

【0008】

図面を参照して、実施形態を説明する。図面の記載において同一部分には同一符号を付して説明を省略する。

【0009】

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

【0010】

カメラ１２は、固体撮像素子、例えばＣＣＤ及びＣＭＯＳを用いたデジタルカメラであって、画像処理が可能なデジタル画像を取得する。カメラ１２の撮像対象は車両周囲の路面であって、車両周囲の路面には、車両の前部、後部、側部、車両底部の路面が含まれる。例えば、図２に示すように、カメラ１２は、車両１０の前部、具体的にはフロントバンパ上に搭載することができる。

【0011】

車両１０の前方の路面３１上の特徴点（テクスチャ）及び投光器１１により投光されたパターン光３２ｂを撮像できるように、カメラ１２が設置される高さ及び向きが調整され、且つ、カメラ１２が備えるレンズのピント及び絞りが自動調整される。カメラ１２は、所定の時間間隔をおいて繰り返し撮像を行い、一連の画像（フレーム）群を取得する。カメラ１２により取得された画像データは、ＥＣＵ１３へ転送され、ＥＣＵ１３が備えるメモリに記憶される。

【0012】

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

【0013】

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

【0014】

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

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

特徴点検出部２３は、カメラ１２により取得された画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「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」に記載の手法を用いることができる。

【0020】

具体的には、特徴点検出部２３は、例えば、ハリス（Harris）作用素又はSUSANオペレータを用いて、物体の頂点のように周囲に比べて輝度値が大きく変化する点を特徴点として検出する。或いは、特徴点検出部２３は、SIFT（Scale-Invariant Feature Transform）特徴量を用いて、その周囲で輝度値がある規則性のもとで変化している点を特徴点として検出してもよい。そして、特徴点検出部２３は、１つの画像から検出した特徴点の総数Ｎを計数し、各特徴点に識別番号（i（１≦i≦Ｎ））を付す。各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）は、ＥＣＵ１３内のメモリに記憶される。図６（ａ）及び図６（ｂ）は、カメラ１２により取得された画像から検出された特徴点Ｔ_ｅの例を示す。各特徴点の画像上の位置（Ｕ_ｉ、Ｖ_ｉ）は、メモリに記憶される。

【0021】

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

【0022】

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

【0023】

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

【0024】

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

【0025】

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

【0026】

【数1】

【数2】

【数3】

【数4】

【0027】

図３（ｂ）は、カメラ１２の撮像範囲のうち、パターン光３２ａが照射された領域とは異なる他の領域３３から検出された特徴点の時間変化から、カメラ１２の移動方向３４を求める様子を模式的に示す。また、図６（ａ）及び図６（ｂ）には、各特徴点Ｔ_ｅの位置の変化方向及び変化量を示すベクトルＤ_ｔｅを画像に重畳して示す。姿勢変化量算出部２４は、時間Δｔにおけるカメラ１２の移動量（ΔＬ）だけでなく、距離及び姿勢角の変化量も同時に算出することができる。よって、姿勢変化量算出部２４は、距離及び姿勢角の変化量を考慮して、６自由度の移動量（ΔＬ）を精度よく算出することができる。すなわち、車両１０の旋回や加減速によるロール運動或いはピッチ運動によって距離や姿勢角が変化しても、移動量（ΔＬ）の推定誤差を抑制することができる。

【0028】

なお、姿勢変化量算出部２４は、相対位置が算出された特徴点すべてを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定してもよい。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，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)）を用いることができる。

【0029】

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

【0030】

このように、相対位置（Ｘ_ｉ，Ｙ_ｉ，Ｚ_ｉ）が算出された特徴点Ｔ_ｅ１、Ｔ_ｅ２、Ｔ_ｅ３が、後のタイミングで取得された画像３８’からも検出された場合に、姿勢変化量算出部２４は、路面上の複数の特徴点の時間変化に基づいて、「車両の姿勢変化量」を算出することができる。

【0031】

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

【0032】

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

【0033】

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

【0034】

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

【0035】

検出状態判断部３０は、特徴点検出部２３による特徴点Ｔ_ｅの検出状態が第１基準満たさず、悪い状態か否かを判断する。例えば、トンネル内のコンクリート路面のように、路面にアスファルト混合物の粒などの模様や凹凸が少ない場合、画像から検出できる特徴点が少なくなる。この場合、前後フレーム間で対応関係が取れる特徴点の継続的な検出が困難となり、距離及び姿勢角の更新精度が低下してしまう。

【0036】

そこで、検出状態判断部３０は、例えば、カメラ１２に対する相対位置が算出された特徴点のうち、後の情報処理サイクルで取得された画像からも検出できた特徴点の数が、所定のしきい値（例えば、４つ）以上でない場合に、複数の特徴点の検出状態が第１基準を満たさず、悪い状態と判断する。すなわち、前後フレーム間で対応付けが取れる４つ以上の特徴点を検出できない場合、検出状態判断部３０は、特徴点Ｔ_ｅの検出状態が第１基準を満たさず、悪い状態と判断する。なお、図６に示すように、距離や姿勢角の変化量を求めるには、前後フレーム間で対応付けが取れる少なくとも３つの特徴点が必要である。なぜなら、平面Ｇを特定するには３つの特徴点が必要だからである。推定精度を高めるには更に多くの特徴点が必要であるため、所定のしきい値は、４、或いは５以上であることが望ましい。

【0037】

パターン光制御部２７は、検出状態判断部３０の判断結果に基づき、投光器１１によるパターン光３２ａの投光を制御する。複数の特徴点の検出状態が第１基準を満たし、悪くない状態と検出状態判断部３０が判断した場合、パターン光制御部２７は、パターン光３２ａを消灯する。一方、複数の特徴点の検出状態が第１基準を満たさず、悪い状態と検出状態判断部３０が判断した場合、パターン光制御部２７は、パターン光３２ａを投光する。具体的に、パターン光制御部２７は、検出状態判断部３０の判断結果に基づいてパターン光投光実施フラグのオン／オフを切り替え、パターン光投光実施フラグを投光器１１に対して送信する。投光器１１は、パターン光投光実施フラグに基づき、次の情報処理サイクルにおいて、パターン光３２ａを投光或いは消灯する。

【0038】

更に、検出状態判断部３０は、特徴点検出部２３による特徴点Ｔ_ｅの検出状態が第１基準よりも低い第２基準を満たさず、悪い状態か否かを判断する。例えば、前後フレーム間で対応付けが取れる３つ以上の特徴点を検出できない場合、検出状態判断部３０は、特徴点Ｔ_ｅの検出状態が第２基準を満たさず、悪い状態と判断する。

【0039】

複数の特徴点の検出状態が第２基準を満たし、悪くない状態と検出状態判断部３０が判断した場合、姿勢角算出部２２は、パターン光３２ａの位置から距離及び姿勢角を算出しない。一方、複数の特徴点の検出状態が第２基準を満たさず、悪い状態と検出状態判断部３０が判断した場合、特徴点Ｔ_ｅから平面Ｇを特定することができなくなるため、姿勢角算出部２２は、パターン光３２ａの位置から距離及び姿勢角を算出する。

【0040】

自己位置算出部２６は、複数の特徴点の検出状態が第２基準を満たしていると検出状態判断部３０が判断した場合、積分演算の起点を維持する。一方、複数の特徴点の検出状態が第２基準を満たさず、悪い状態と検出状態判断部３０が判断した場合、自己位置算出部２６は、同じ情報処理サイクルにおいて、姿勢角算出部２２（図１参照）が算出した距離及び姿勢角、及びその時の車両位置を新たな起点（車両の姿勢角及び初期位置）に設定し、当該起点から車両の姿勢変化量の加算を開始する。

【0041】

なお、第１実施形態で、検出状態判断部３０は、前後フレーム間で対応関係が取れる特徴点の数に基づいて、複数の特徴点の検出状態を判断したが、１つの画像から検出された特徴点の総数Ｎに基づいて、複数の特徴点の検出状態を判断してもよい。つまり、１つの画像から検出された特徴点の総数Ｎが所定のしきい値（例えば９）以下である場合に、複数の特徴点の検出状態が悪いと判断してもよい。検出された特徴点のうち対応付けが取れない特徴点があることを考慮し、所定のしきい値（４）の３倍の数（１２）を、しきい値に設定すればよい。

【0042】

算出状態判断部３５は、姿勢角算出部２２による距離及び姿勢角の算出状態が第３基準満たさず、悪い状態か否かを判断する。例えば、路面３１上の段差にパターン光が照射された場合、路面３１上の段差はアスファルトの凹凸に比べて大きいため、距離及び姿勢角の算出精度が極端に低下してしまう。複数の特徴点の検出状態が第２基準を満たさず、悪い状態で、且つ距離及び姿勢角の算出状態が第３基準を満たさず、悪い状態の場合、距離及び姿勢角、及びその変化量を精度よく検出する手段がなくなってしまう。

【0043】

そこで、算出状態判断部３５は、例えば、姿勢角算出部２２によって算出される距離及び姿勢角の標準偏差が所定のしきい値よりも大きい場合、姿勢角算出部２２による距離及び姿勢角の算出状態が第３基準を満たさず、悪い状態と判断する。また、３５個のスポット光のうち検出できたスポット光の数が３点未満である場合、原理的に路面３１の平面式を求めることができないため、姿勢角算出部２２による距離及び姿勢角の算出状態が第３基準を満たさず、悪い状態と判断する。最小二乗法を用いて平面式を求める場合、各スポット光との距離誤差の最大値の絶対値がある閾値（例えば０．０５ｍ）以上になった場合に、姿勢角算出部２２による距離及び姿勢角の算出状態が第３基準を満たさず、悪い状態と判断してもよい。

【0044】

複数の特徴点の検出状態が第２基準を満たさず、悪い状態と検出状態判断部３０が判断し、且つ姿勢角算出部２２による距離及び姿勢角の算出状態が第３基準を満たさず、悪い状態と算出状態判断部３５が判断した場合、自己位置算出部２６は、積分演算の起点として、前の情報処理サイクルにおける距離及び姿勢角、及び車両の現在位置を用いる。これにより、車両の移動量の算出誤差を抑制できる。

【0045】

（情報処理サイクル）
次に、カメラ１２により取得された画像３８から車両１０の移動量を推定する自己位置算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図７を参照しながら説明する。図７のフローチャートに示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置が起動すると同時に開始され、自己位置算出装置が停止するまで、繰り返し実行される。

【0046】

図７のステップＳ０１において、投光器１１は、パターン光３２ａを投光或いは消灯する。具体的に、投光器１１は、前回の情報処理サイクルのステップＳ０５においてパターン光制御部２７から送信されるパターン光投光実施フラグのオン／オフに基づき、パターン光３２ａの投光／消灯を切り替える。パターン光投光実施フラグについては、図８を参照して後述する。

【0047】

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

【0048】

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

【0049】

ステップＳ０５に進み、検出状態判断部３０は複数の特徴点の検出状態を判断する。パターン光制御部２７は、検出状態判断部３０の判断結果に基づき、パターン光投光実施フラグを切り替え、投光器１１へ送信する。そして、姿勢角算出部２２は、検出状態判断部３０の判断結果に基づき、距離及び姿勢角を算出し、或いは算出しない。ステップＳ０５の詳細は、図８及び図９を参照して後述する。

【0050】

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

【0051】

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

【0052】

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

【0053】

ステップＳ０９に進み、ＥＣＵ１３は、複数の特徴点の検出状態、及びパターン光による距離及び姿勢角の算出状態に応じて、積分演算の起点を設定する。詳細は、図１０を参照して、後述する。

【0054】

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

【0055】

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

【0056】

図８のフローチャートを参照して、図７のステップＳ０５の詳細な手順を説明する。先ず、ステップＳ５０１において、検出状態判断部３０は、前後フレーム間で対応付けが取れる特徴点を４つ以上検出しているか否かを判断する。特徴点を４つ以上検出している場合（Ｓ５０１でＹＥＳ）、特徴点の時間変化から距離及び姿勢角の変化量を算出できる。そこで、ステップＳ５０５へ進み、パターン光制御部２７は、パターン光投光実施フラグをオフ状態に設定し、投光器１１へ送信する。その後、パターン光３２ａの位置から距離及び姿勢角を算出せずに、図７のステップＳ０７へ進む。

【0057】

一方、特徴点を４つ以上検出していない場合（Ｓ５０１でＮＯ）、複数の特徴点の検出状態が第１基準を満たさず、悪い状態と判断できるので、ステップＳ５０３へ進む。そして、パターン光制御部２７は、パターン光投光実施フラグをオン状態に設定し、投光器１１へ送信する。その後、ステップＳ５０７へ進む。

【0058】

ステップＳ５０７において、検出状態判断部３０は、前後フレーム間で対応付けが取れる特徴点を３つ以上検出しているか否かを判断する。特徴点を３つ以上検出している場合（Ｓ５０７でＹＥＳ）、特徴点の時間変化から距離及び姿勢角の変化量を算出できる。そこで、パターン光３２ａの位置から距離及び姿勢角を算出せずに、図７のステップＳ０７へ進む。

【0059】

一方、特徴点を３つ以上検出していない場合（Ｓ５０７でＮＯ）、特徴点の時間変化から距離及び姿勢角の変化量を算出できない。そこで、ステップＳ５０９に進み、姿勢角算出部２２は、パターン光３２ａの位置から距離及び姿勢角を算出する。その後、図７のステップＳ０７へ進む。

【0060】

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

【0061】

姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から距離及び姿勢角を算出し、メモリに記憶する。

【0062】

図９を参照して、図８のフローチャートにしたがって実行される情報処理の一例を説明する。図９（ａ）は姿勢角算出部２２がパターン光３２ａから距離及び姿勢角を算出するか否かを示し、図９（ｂ）はパターン光投光実施フラグの状態を示し、図９（ｃ）は対応付けが取れる特徴点の数を示す。図９（ａ）〜（ｃ）の横軸は、時間の経過を示し、ｔ０、ｔ１、ｔ２、・・・は情報処理サイクル（以後、単に「サイクル」とも呼ぶ）を示す。

【0063】

サイクルｔ０〜ｔ３で、対応付けが取れる特徴点の数が４つ以上であるため、パターン光投光実施フラグはオフ状態であり、「三次元計測実施フラグ」もオフ状態である。「三次元計測実施フラグ」がオフ状態であれば、姿勢角算出部２２はパターン光３２ａから距離及び姿勢角を算出しない。サイクルｔ４〜ｔ６で、対応付けが取れる特徴点の数が３つになると、図８のステップＳ５０１はＮＯと判断され、ステップＳ５０３でパターン光投光実施フラグがオン状態となるが、ステップＳ５０７でＹＥＳと判断されるため、三次元計測実施フラグはオフ状態のままである。サイクルｔ７〜ｔ１０で対応付けが取れる特徴点の数が４つ以上になると、パターン光投光実施フラグがオフ状態となる。サイクルｔ１２〜ｔ１４で、対応付けが取れる特徴点の数が３つ未満となるため、ステップＳ５０１及びＳ５０７で共にＮＯと判断されるため、パターン光投光実施フラグ及び三次元計測実施フラグがオン状態となり、姿勢角算出部２２による距離及び姿勢角の算出が実施される。

【0064】

図１０のフローチャートを参照して、図７のステップＳ０９の詳細な手順を説明する。ステップＳ９００において、ＥＣＵ１３は、今回の情報処理サイクルが初回であるか否かを判断する。そして、初回である場合、即ち、前回の情報処理サイクルのデータが無い場合にはステップＳ９０５に処理を進め、初回でない場合にはステップＳ９０１に処理を進める。

【0065】

ステップＳ９０１において、検出状態判断部３０は、特徴点検出部２３による特徴点Ｔ_ｅの検出状態が第２基準を満たさず、悪い状態か否かを判断する。即ち、図８のステップＳ５０７と同じように、対応付けが取れる特徴点Ｔ_ｅの数が３つ以上であるか否かを判断する。３つ以上でないと判断した場合（ステップＳ９０１でＹＥＳ）、ステップＳ９０３へ進み、３つ以上であると判断した場合（ステップＳ９０１でＮＯ）、ステップＳ９０９へ進む。

【0066】

ステップＳ９０９で、ＥＣＵ１３は、現在設定されている積分演算の起点を維持する。

【0067】

ステップＳ９０３で、算出状態判断部３５は、姿勢角算出部２２による距離及び姿勢角の算出状態が第３基準を満たさず、悪い状態か否かを判断する。たとえば、同じ情報処理サイクルのステップＳ５０９において距離及び姿勢角の算出に成功しているか否かを判断する。成功していると判断した場合（ステップＳ９０３でＹＥＳ）、ステップＳ９０５へ進み、成功していないと判断した場合（ステップＳ９０３でＮＯ）、ステップＳ９０７へ進む。

【0068】

ステップＳ９０５で、ＥＣＵ１３は、ステップＳ５０９で姿勢角算出部２２により算出された距離及び姿勢角、及びその時の車両の現在位置を積分演算の起点として設定する。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両の現在位置を起点として新たな積分演算が開始される。

【0069】

ステップＳ９０７で、ＥＣＵ１３は、前回の情報処理サイクルで採用した距離及び姿勢角、及び前回の情報処理サイクルにおける車両の現在位置を積分演算の起点として設定する。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両の現在位置を起点として新たな積分演算が開始される。その後、図８のステップＳ１１に処理を進める。

【0070】

以上説明したように、第１実施形態によれば、以下の作用効果が得られる。

【0071】

複数の特徴点の検出状態が悪いと、姿勢変化量算出部２４による距離及び姿勢角の算出精度が低下するため、車両１０の移動量の推定誤差が大きくなる。図１１（ａ）及び図１１（ｂ）は、車両１０のロール角（姿勢角の一例）及び移動量（車幅方向）の推定誤差の例を示すグラフであって、車両１０が傾斜の無い平坦な路面上を一定の速度で直進している場合の、ロール角の推定値及び移動量の算出値の時間変化を示す。図中の「Ｐ_１」及び「Ｐ_２」は、複数の特徴点の検出状態に係わらず、移動量算出の起点、及び距離及び姿勢角の積分演算の起点を維持する比較例を示す。「Ｑ_１」及び「Ｑ_２」は真値を示す。車両１０は直進しているため、ロール角及び車幅方向の移動量の真値（Ｑ_１、Ｑ_２）は０のままである。しかし、時刻ｔ１〜ｔ２の間、複数の特徴点の検出状態が悪いコンクリート路面を走行したため、路面の平面式に誤差が発生して、ロール角に推定誤差が現れる。ロール角の誤差は、車幅方向の移動量の誤差として反映されてしまう。比較例は、複数の特徴点の検出状態に応じて、移動量算出の起点、及びロール角の積分演算の起点を、パターン光３２ａから算出された距離及び姿勢角、及びその時の車両の現在位置へリセットしない。このため、車両１０の移動量の誤差は拡大してしまう。

【0072】

第１実施形態によれば、複数の特徴点の検出状態が第１基準を満たさず、悪い状態と検出状態判断部３０が判断した場合（ステップＳ５０１でＮＯ）、パターン光制御部２７は、パターン光３２ａを投光する。これにより、パターン光３２ａの位置から距離及び姿勢角を算出可能となる。よって、パターン光３２ａから算出された誤差の小さい距離及び姿勢角を起点として新たに積分演算を開始できる。よって、車両１０の現在位置を精度良く且つ安定して推定することができる。

【0073】

また、複数の特徴点の検出状態が悪くなる路面には、例えば、特徴点の対象となる模様や凹凸が少ない路面が含まれる。模様や凹凸が少ない路面は平坦性が高いため、パターン光３２ａから算出された距離及び姿勢角に含まれるノイズ成分が少なくなる。一方、複数の特徴点の検出状態が良い路面３１では、アスファルトの凹凸によりパターン光３２ａの反射位置が安定しないため、パターン光３２ａから算出された距離及び姿勢角は多くのノイズ成分を含むことになる。第１実施形態によれば、複数の特徴点の検出状態に応じて、特徴点を用いた距離及び姿勢角の更新とパターン光３２ａを用いた距離及び姿勢角の算出とを適切に使い分けて、距離及び姿勢角を精度良く且つ安定して求めることができる。

【0074】

更に、路面３１の照度の急激な変化によって画像３８のコントラストが低下してしまい、複数の特徴点の検出状態が悪く場合がある。第１実施形態によれば、複数の特徴点の検出状態を直接判断することにより、照度計などのセンサを別途設ける必要が無くなり、コストが軽減され、装置の軽量化に寄与する。

【0075】

第１実施形態によれば、複数の特徴点の検出状態が第１基準を満たし、悪くない状態と検出状態判断部３０が判断した場合（ステップＳ５０１でＹＥＳ）、パターン光制御部２７は、パターン光３２ａの投光を停止する。パターン光３２ａを、複数の特徴点の検出状態が第１基準を満たさず、悪い状態の場合のみ、投光することができるので、パターン光３２ａを必要な場合だけ投光して、投光に必要な電力の消費を抑制することができる。

【0076】

図７のステップＳ０５におけるパターン光３２ａの投光及び消灯（ステップＳ５０３、Ｓ５０５）の判断は、次の情報処理サイクルのステップＳ０１において反映される。よって、パターン光３２ａを用いて距離及び姿勢角を算出するためには、予め、前のサイクルにおいてパターン光投光実施フラグをオン状態に切り替えておく必要がある。そこで、第１実施形態では、複数の特徴点の検出状態が第１基準を満たさず、悪い状態であれば、パターン光投光実施フラグをオン状態に切り替える（Ｓ０５）。その次のサイクルにおいて、パターン光３２ａが投光される（Ｓ０１）。そして、同じサイクルにおいて、複数の特徴点の検出状態が第２基準を満たさず悪い状態の場合（Ｓ５０７でＮＯ）、姿勢角算出部２２は、パターン光３２ａの位置から距離及び姿勢角を算出することができる。このように、複数の特徴点の検出状態を２つの基準（第１基準、第２基準）を用いて判断することにより、パターン光３２ａの投光のタイミング、及び距離及び姿勢角の算出のタイミングを適切に制御することが可能となる。

【0077】

（第２実施形態）
第２実施形態では、パターン光３２ａを用いた距離及び姿勢角の算出タイミングを、パターン光３２ａの投光を開始してからの経過時間に応じて制御する例を説明する。第２実施形態に係わる自己位置算出装置のハードウェア構成は図１に示した通りであるため、説明を省略する。また、ＥＣＵ１３により繰り返し実行される情報処理サイクルも、ステップＳ０５を除き、図７に示した通りであるため、説明を省略する。後述する第３及び第４実施形態についても同様である。

【0078】

図１２を参照して、第２実施形態に係わるステップＳ０５の詳細な手順を説明する。図１２に示すフローチャートでは、図８のステップＳ５０７の代わりに、ステップＳ５１１を実施する。その他のステップは、図８と同じであり説明を省略する。ＥＣＵ１３は、パターン光３２ａの投光を開始してからの経過時間を計測する。

【0079】

具体的に、ＥＣＵ１３は、ステップＳ５０３でパターン光投光実施フラグをオフ状態からオン状態へ切り替えてからの経過時間を計測する。ステップＳ５１１において、ＥＣＵ１３は、パターン光投光実施フラグを切り替えてから第１所定時間が経過したか否かを判断する。第１所定時間が経過していなければ（Ｓ５１１でＮＯ）、パターン光３２ａの位置から距離及び姿勢角を算出することなく、ステップＳ０７へ進む。一方、第１所定時間が経過していれば（Ｓ５１１でＹＥＳ）、ステップＳ５０９へ進み、パターン光３２ａの位置から距離及び姿勢角を算出し、その後、ステップＳ０７へ進む。

【0080】

なお、第１所定時間は、例えば、図１３に示すように、２回分の情報処理サイクル（Ｄ１）を設定することができる。つまり、パターン光投光実施フラグをオフ状態からオン状態へ切り替えた情報処理サイクル（ｔ１１）、及びその次の情報処理サイクル（ｔ１２）では距離及び姿勢角を算出せず（Ｓ５１１でＮＯ）、またその次（２回目）の情報処理サイクル（ｔ１３）において、初めて距離及び姿勢角を算出する（Ｓ５１１でＹＥＳ、Ｓ５０９）。

【0081】

前述したように、パターン光３２ａを用いて距離及び姿勢角を算出するためには、予め、前のサイクルにおいてパターン光投光実施フラグをオン状態に切り替えておく必要がある。第２実施形態では、パターン光制御部２７がパターン光３２ａの投光を開始してから第１所定時間（Ｄ１）が経過した後に、姿勢角算出部２２は、パターン光３２ａの位置から距離及び姿勢角を算出し始める。これにより、パターン光３２ａの投光のタイミング、及び距離及び姿勢角の算出のタイミングを適切に制御することが可能となる。

【0082】

（第３実施形態）
第３実施形態では、パターン光３２ａの投光を開始した後に、複数の特徴点の検出状態が第１基準を満たし、悪くない状態と判断しても、当該判断から第２所定時間が経過するまではパターン光３２ａの投光を継続する例を説明する。

【0083】

複数の特徴点の検出状態が第１基準よりも悪くないと検出状態判断部３０が判断した場合、パターン光制御部２７は、検出状態判断部３０が判断してから第２所定時間が経過した後に、パターン光３２ａの投光を停止する。

【0084】

図１４を参照して、第３実施形態に係わるステップＳ０５の詳細な手順を説明する。図１４に示すフローチャートは、図８に対して、更にステップＳ５１３及びステップＳ５１５を備える。その他のステップは、図８と同じであり説明を省略する。

【0085】

ＥＣＵ１３は、対応付けが取れる特徴点の数が４つ以上であると判断されてからの経過時間を計測する。対応付けが取れる特徴点の数が４つ以上であると判断された場合（Ｓ５０１でＹＥＳ）、ステップＳ５１３へ進み、ＥＣＵ１３は、対応付けが取れる特徴点の数が４つ以上であると判断されてから第２所定時間が経過したか否かを判断する。第２所定時間が経過していれば（Ｓ５１３でＹＥＳ）、ステップＳ５０５へ進み、パターン光制御部２７は、パターン光投光実施フラグをオン状態からオフ状態へ切り替える。一方、第２所定時間が経過していなければ（Ｓ５１３でＮＯ）、ステップＳ５１５へ進み、パターン光制御部２７は、パターン光投光実施フラグをオン状態に維持する。

【0086】

なお、第２所定時間は、例えば、図１５に示すように、２回分の情報処理サイクル（Ｄ２）を設定することができる。つまり、対応付けが取れる特徴点の数が４つ以上となった情報処理サイクル（ｔ６、ｔ１４）、及びその次の情報処理サイクル（ｔ７、ｔ１５）ではパターン光投光実施フラグをオン状態に維持し（Ｓ５１３でＮＯ、Ｓ５１５）、またその次（２回目）の情報処理サイクル（ｔ８、ｔ１６）において、初めてパターン光投光実施フラグをオフ状態に切り替える（Ｓ５１３でＹＥＳ、Ｓ５０５）。

【0087】

これにより、一旦、パターン光３２ａの投光を開始したら、その後、複数の特徴点の検出状態が改善しても直ぐにはパターン光３２ａが消灯することがなくなる。つまり、パターン光３２ａの投光状態にヒステリシス（履歴効果）を持たせることができるので、パターン光３２ａのちらつきを抑制し、ちらつきによる煩わしさを軽減することができる。

【0088】

（第４実施形態）
第４実施形態では、第３実施形態とは対称的に、パターン光３２ａの消灯状態にヒステリシスを持たせる例について説明する。パターン光３２ａを消灯した後に、複数の特徴点の検出状態が第１基準を満たさず、悪くなっても、パターン光制御部２７は、当該判断から第３所定時間が経過するまではパターン光３２ａの消灯を継続する。複数の特徴点の検出状態が第１基準を満たさず、悪い状態と検出状態判断部３０が判断した場合、パターン光制御部２７は、検出状態判断部３０が判断してから第３所定時間が経過した後に、パターン光３２ａの投光を開始する。

【0089】

図１６を参照して、第４実施形態に関わるステップＳ０５の詳細な手順を説明する。図１６に示すフローチャートは、図８に対して、更にステップＳ５１７を備える。また、ステップＳ５０１において、対応付けが取れる特徴点の数が５つ以上であるか否かを判断する。つまり、対応付けが取れる特徴点の数が５つ以上でない場合、検出状態判断部３０は、複数の特徴点の検出状態が第１基準を満たさず、悪い状態、と判断する。その他のステップは、図８と同じであり説明を省略する。

【0090】

ＥＣＵ１３は、対応付けが取れる特徴点の数が５つ以上でないと判断されてからの経過時間を計測する。対応付けが取れる特徴点の数が５つ以上でないと判断された場合（Ｓ５０１でＮＯ）、ステップＳ５１７へ進み、ＥＣＵ１３は、対応付けが取れる特徴点の数が５つ以上でないと判断されてから第３所定時間が経過したか否かを判断する。第３所定時間が経過していれば（Ｓ５１７でＹＥＳ）、ステップＳ５０３へ進み、パターン光制御部２７は、パターン光投光実施フラグをオン状態に切り替える。一方、第３所定時間が経過していなければ（Ｓ５１７でＮＯ）、ステップＳ５０５へ進み、パターン光制御部２７は、パターン光投光実施フラグをオフ状態に維持する。

【0091】

なお、第３所定時間は、例えば、図１７に示すように、２回分の情報処理サイクル（Ｄ３）を設定することができる。つまり、対応付けが取れる特徴点の数が５つ未満となった情報処理サイクル（ｔ２、ｔ９）、及びその次の情報処理サイクル（ｔ３、ｔ１０）ではパターン光投光実施フラグをオフ状態に維持し（Ｓ５１７でＮＯ、Ｓ５０５）、またその次（２回目）の情報処理サイクル（ｔ４、ｔ１１）において、初めてパターン光投光実施フラグをオン状態に切り替える（Ｓ５１７でＹＥＳ、Ｓ５０３）。

【0092】

これにより、一旦、パターン光３２ａの投光を停止したら、その後、複数の特徴点の検出状態が悪化しても直ぐにはパターン光３２ａの投光を開始することがなくなる。つまり、パターン光３２ａの消灯状態にヒステリシス（履歴効果）を持たせることができるので、パターン光３２ａのちらつきを抑制し、ちらつきによる煩わしさを軽減することができる。

【0093】

上記のように、本発明の第１乃至第４実施形態を記載したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。

【0094】

図２は、カメラ１２と投光器１１を車両１０の前面に取り付けた例を示したが、車両１０の側方，後方，真下に向けて設置してもよい。また、本実施形態では車両１０の一例として、四輪の乗用自動車を図２に示したが、オートバイ、貨物自動車、或いは例えば建設機械を運搬する特殊車両など、道路の路面或いは壁面上の特徴点を撮像することが可能な移動体（車両）すべてに適用可能である。

【符号の説明】

【0095】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】