JP2015158418A - 自己位置算出装置及び自己位置算出方法 - 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）特徴量を用いて、その周囲で輝度値がある規則性のもとで変化している点を特徴点として検出してもよい。そして、特徴点検出部２３は、１つの画像から検出した特徴点の総数Ｎを計数し、各特徴点に識別番号（ｉ（１≦ｉ≦Ｎ））を付す。各特徴点の画像上の位置（Ｕｉ，Ｖｉ）は、ＥＣＵ１３内のメモリに記憶される。図６（ａ）及び図６（ｂ）は、カメラ１２により取得された画像から検出された特徴点Ｔｅの例を示す。各特徴点の画像上の位置（Ｕｉ，Ｖｉ）は、メモリに記憶される。更に、各特徴点Ｔｅの変化方向及び変化量をベクトルＤｔｅとして示している。

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

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

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

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

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

なお、姿勢変化量算出部２４は、時刻ｔと時刻ｔ＋Δｔで検出される各画像中で相対位置が算出された特徴点の全てを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定してもよい。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，R.I. Hartley: “A linear method for reconstruction from lines and points,” Proc. 5th International Conference on Computer Vision, Cambridge, Massachusetts, pp.882-887(1995)）を用いることができる。

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

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

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

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

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

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

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

挙動量検出部３６は、車両１０に搭載される各種のセンサから車両の挙動量を取得する。挙動量の一例として、車両に搭載されるジャイロセンサの検出値を積分演算して車両のピッチ角、ロール角を算出する。そして、ピッチ角或いはロール角が車両の静止しているとき（例えば、水平状態のとき）のピッチ角、ロール角と対比して０．０５［rad］以上変化した際に、挙動量検出値が第２の閾値挙動量より大きくなったと判断する。ここで、０．０５［rad］とする理由は、路面に対する車両の姿勢角が０．０５［rad］以上変化すると、姿勢角が変化していない場合と対比して路面に存在する特徴点の移動量の算出結果に５％以上の誤差が生じるからである。換言すれば、移動量の算出結果を５％以内に抑えるためには、第２の閾値挙動量を０．０５［rad］に設定する必要がある。

挙動量予測部３７は、運転者による操作機器の操作量等に基づいて、将来の挙動量を予測し、これを挙動量予測値とする。例えば、アクセルの操作量、ブレーキの操作量、及びステアリングの操作量に対して、車両の挙動量との対応を示すマップを設定し、これをメモリ（図示省略）に記憶する。一例として、アクセル開度と車速に対応した車両の前後方向の加速度によって生じる最大ピッチ角をマップとして記憶する。或いは、ステアリング操作と車速に対応した車両の横方向加速度によって生じる最大ロール角をマップとして記憶する。該挙動量予測部３７は、運転者によりアクセルが操作された際に、この操作量に対応する最大ピッチ角を参照し、この最大ピッチ角が閾値として設定したピッチ角（第１の閾値挙動量）より大きくなるものと判断した場合に、車両の挙動量が第１の閾値挙動量より大きくなるものと予測する。

第１の閾値挙動量は、第２の閾値挙動量より小さい値に設定する。例えば、移動量の算出結果を５％以内に抑えるためには、第２の閾値挙動量を０．０５［rad］に設定する必要があるので、それに達する前に、パターン光３２ａの投光が開始されるように、第１の閾値挙動量を０．０３［rad］等、事前に判定できる値を実験的に設定しておく。

また、挙動量予測部３７は、挙動量予測値を取得する方法として、上述した運転者による操作機器の操作量を用いる場合以外で、先行車両との間の相対速度、ＧＰＳ情報に基づいて挙動量を予測することもできる。

例えば、車両の前方方向を向くように取り付けたステレオカメラ或いはレーザレンジファインダにより、自車両と先行車両との間の相対速度を算出し、車間距離が短くなった場合に、運転者がブレーキを踏むことを予測して挙動量予測値が第１閾値挙動量より大きくなると予測することもできる。更に、自車両のＧＰＳ情報と地図データを利用し、前方に急カーブが存在する場合に、ステアリング操作が行われると判断し、挙動量予測値が第１の閾値挙動量より大きくなると予測することもできる。

そして、本実施形態では、挙動量予測部３７にて将来における車両１０の挙動量予測値が第１の閾値挙動量より大きくなると予測される場合には、投光器１１によるパターン光３２ａの投光を開始し、挙動量検出部３６にて、車両１０の挙動量検出値が第２の閾値挙動量（第１の閾値挙動量よりも大きい）より大きくなった場合に、パターン光３２ａから算出した距離及び姿勢角を起点に設定する。こうすることにより、パターン光３２ａにより求められる距離及び姿勢角が必要となった時点で、すでにパターン光３２ａは画像３８で捉えており、迅速に演算し設定することができる。

また、挙動量検出部３６にて、車両１０の挙動量検出値が第１の閾値挙動量より大きくなった場合には、投光器１１によるパターン光３２ａの投光を開始するようにした場合でも、車両１０の挙動量検出値が第２の閾値挙動量（第１の閾値挙動量よりも大きい）より大きくなった場合に、パターン光３２ａから算出した距離及び姿勢角を起点に設定されるので、パターン光３２ａにより求められる距離及び姿勢角が必要となった時点で、すでにパターン光３２ａは画像３８で捉えており、迅速に演算し設定することができる。

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

初めに、図７のステップＳ１１において、ＥＣＵ１３は、今回の情報処理サイクルが初回であるか否かを判断する。そして、初回である場合、即ち、前回の情報処理サイクルのデータが無い場合にはステップＳ１４に処理を進め、初回でない場合にはステップＳ１２に処理を進める。

ステップＳ１２において、挙動量予測部３７は、車両１０に搭載される操作機器の操作量に基づいて挙動量予測値を取得する。この処理では、前述したように、アクセル操作やブレーキ操作、或いはステアリング操作の操作量を取得し、この操作量を、車両の挙動量との対応関係を示すマップに当てはめて将来における車両の挙動量を予測する。そして、これを挙動量予測値とする。

ステップＳ１３において、挙動量予測部３７は、ステップＳ１２の処理で予測した挙動量予測値が予め設定した第１の閾値挙動量Ｑthより大きくなるか否かを判断する。そして、第１の閾値挙動量Ｑthより大きくなると判断した場合には（ステップＳ１３でＹＥＳ）、ステップＳ１４に処理を進め、そうでなければ（ステップＳ１３でＮＯ）、ステップＳ１７に処理を進める。

ステップＳ１４において、投光器１１は、路面に向けてパターン光３２ａを投光する。その結果、例えば、図４に示した如くの複数のスポット光からなるパターン光３２ａが路面上に投光される。

つまり、ステップＳ１２〜Ｓ１４の処理では、車両１０の挙動量予測値を取得し、この挙動量予測値が第１の閾値挙動量Ｑthより大きくなると判断される場合、換言すれば、数秒後に車両１０の挙動量が大きくなると予測される場合には、投光器１１よりパターン光３２ａを投光する。こうすることにより、車両１０の挙動量が増大する前に先行して路面上にパターン光３２ａを投光することができる。

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

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

ステップＳ１６に進み、パターン光抽出部２１は、カメラ１２により取得された画像３８をメモリから読み込み、図４（ｃ）に示したように、画像３８からパターン光３２ａの位置を抽出する。パターン光抽出部２１は、パターン光３２ａの位置を示すデータとして算出されたスポット光Ｓｐの画像上の座標（Ｕｊ，Ｖｊ）をメモリに記憶する。更に、ステップＳ１６では、姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から、距離及び姿勢角を算出し、メモリに記憶する。距離及び姿勢角の算出方法は、前述した通りである。なお、姿勢角算出部２２で算出した距離及び姿勢角は、後述するように、情報処理サイクルが初回の場合、及び後述する条件となった場合に、積分演算を実行する際の新たな起点として設定する。その後、ステップＳ１９に処理を進める。

一方、ステップＳ１７において、投光器１１は、スポット光を消灯する。即ち、車両の挙動量予測値が第１の閾値挙動量Ｑth以下の場合には、車両の移動量、距離及び姿勢角の積分演算は高精度に実行されるものと考えられるので、パターン光３２ａの検出による距離及び姿勢角の設定処理を行わない。

ステップＳ１８において、ＥＣＵ１３は、前述したステップＳ１５と同様の処理を実行して、パターン光３２ａを投光する領域を含む車両周囲の路面３１を撮像し画像３８を取得する。その後、ステップＳ１９に処理を進める。

ステップＳ１９において、ＥＣＵ１３は、画像３８から特徴点（例えば、アスファルト上に存在する凹凸部位）を検出し、前回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点を抽出し、特徴点の画像上の位置（Ｕｉ，Ｖｉ）から距離及び姿勢角の変化量を算出する。更に、車両１０の移動量を算出する。

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

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

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

ステップＳ２０に進み、ＥＣＵ１３は、車両１０の走行時の挙動量に応じて、距離及び姿勢角を積分演算にて算出する際の起点を設定する。詳細な処理手順については、図８に示すフローチャートを参照して後述する。

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

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

次に、図８のフローチャートを参照して、図７のステップＳ２０の詳細な手順を説明する。初めに、ステップＳ３１において、挙動量検出部３６は、車両１０の挙動量を検出する。例えば、車両１０に搭載されるヨーレートセンサの出力信号に基づき、車両のピッチ角、ロール角を求める。

ステップＳ３２において、挙動量検出部３６は、車両の挙動量検出値が予め設定した第２の閾値挙動量Ａより大きいか否かを判断する。例えば、ピッチ角の最大値、及びロール角の最大値を閾値挙動量として設定し、車両のピッチ角、ロール角が最大値となった際に、挙動量が第２の閾値挙動量Ａより大きいと判断し（ステップＳ３２でＹＥＳ）、ステップＳ３３に処理を進める。一方、第２の閾値挙動量Ａ以下である場合には（ステップＳ３２でＮＯ）、ステップＳ３６に処理を進める。

ステップＳ３３において、姿勢角算出部２２は、パターン光抽出部２１で抽出されたパターン光３２ａに基づき、距離及び姿勢角が算出されているか否かを判断する。具体的な例として、投光器１１より照射される３５点のスポット光のうち、３点以上のスポット光が検出されている場合に距離及び姿勢角の算出が可能なので、３点以上のスポット光が検出されているか否かを判断する。そして、距離及び姿勢角が算出されている場合には（ステップＳ３３でＹＥＳ）、ステップＳ３４に処理を進める。一方、距離及び姿勢角が算出されていない場合には（ステップＳ３３でＮＯ）、ステップＳ３５に処理を進める。

ステップＳ３４において、ＥＣＵ１３は、車両１０の現在位置を起点とし、更に、同じ情報処理サイクルのステップＳ１６（図７参照）で算出された距離及び姿勢角、即ち、パターン光３２ａの位置から算出された距離及び姿勢角を積分演算の起点として設定する。従って、距離及び姿勢角は、パターン光３２ａから算出された距離及び姿勢角に設定される。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両１０の移動量算出の起点が新たに設定される。つまり、この位置を起点として新たに車両１０の移動量の積分演算を開始する。

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

つまり、車両の挙動量検出値が第２の閾値挙動量Ａより大きい場合（ステップＳ３２でＹＥＳの場合）には、車両１０の距離及び姿勢角が大きく変動し、これに伴って、カメラ１２の距離及び姿勢角が大きく変動するので、特徴点の位置座標を正確に求められなくなる可能性が高い。従って、このような場合には姿勢変化量算出部２４は、距離及び姿勢角の変化量を算出せず、パターン光３２ａから算出した距離及び姿勢角（ステップＳ３４）、或いは、前回の情報処理サイクルで採用した距離及び姿勢角（ステップＳ３５）を起点として設定する。即ち、車両１０の初期位置、距離及び姿勢角を、その時点の位置、距離及び姿勢角に設定して、距離及び姿勢角の変化量の加算を開始する。こうすることにより、距離及び姿勢角に大きな誤差が生じることを防止し、車両１０の移動量の算出の精度が低下することを防止する。

一方、図８のステップＳ３６において、ＥＣＵ１３は、現在の設定されている起点を維持する。車両の挙動量検出値が第２の閾値挙動量Ａ以下である場合（ステップＳ３２でＮＯの場合）には、現在の起点を維持しても車両１０の移動量の算出に大きな誤差は発生しないものと判断されるので、起点を維持する。その後、図７のステップＳ２１に処理を進める。

上記の処理をまとめると、以下の通りである。カメラ１２を用いて路面に存在する特徴点を含む画像を撮影し、この特徴点の時間的な変化から車両１０の移動量（６自由度の移動量）を算出する場合には、カメラ１２と路面との位置関係が一定であることが条件となる。しかし、実際にはカメラ１２は車両１０に搭載されるので、該車両１０の走行状態、或いは路面環境に応じて路面との相対的な位置が時々刻々と変化する。従って、カメラ１２と路面との間の位置関係、即ち、距離及び姿勢角を時間経過に伴って更新する必要がある。

ここで、距離及び姿勢角の測定方法として、投光器１１より投光したパターン光３２ａを用いる方法がある。従って、各情報処理サイクル毎に常にパターン光３２ａを用いて距離及び姿勢角を算出し、更に、特徴点の時間的な変化を検出すれば、車両１０の移動量を算出でき、ひいては車両１０の自己位置を算出できる。しかし、路面がコンクリートの場合等、路面が平坦でパターン光３２ａをよく反射する状況下であればこの方法は有効であるが、実際の路面は凹凸が多く存在するアスファルトの路面であることが多い。この場合には、パターン光３２ａを用いるよりも、特徴点の時間的な変化に基づいて、距離及び姿勢角の変化量を算出し、起点として設定されている距離及び姿勢角に対して、変化量を逐次加算する処理（積分演算）を実行する方が距離及び姿勢角を高精度に求めることができる。

そこで、本実施形態では、初回にパターン光３２ａを用いた距離及び姿勢角の算出を行ってこれを起点とし、それ以後は、特徴点の時間的な変化に基づく距離及び姿勢角の変化量を算出して、この算出結果を逐次加算することにより、距離及び姿勢角を最新なものに更新する。更に、車両のロール角、或いはピッチ角等の挙動量が大きくなった場合には、特徴点の検出精度が低下するので、本実施形態では、車両の挙動量予測値が第１の閾値挙動量より大きくなることが予測される場合には、予めパターン光３２ａを照射する。この際、挙動量の予測に、例えば、アクセル、ブレーキ、ステアリング等の操作機器の操作信号を用いることができる。

その後、実際に挙動量検出値が第２の閾値挙動量より大きくなった際に、パターン光３２ａを用いて距離及び姿勢角を算出し、更に車両１０の移動量を算出する。こうして、車両１０の自己位置の算出精度を向上させている。

次に、図９に示すタイミングチャートを参照して、車両の挙動量予測値、及び検出値の変化、パターン光３２ａの投光タイミング、及び起点を演算するタイミングの関係について説明する。図９（ａ）は、距離及び姿勢角の起点を設定するタイミングを示す図、（ｂ）は、パターン光３２ａを投光するタイミングを示す図、（ｃ）は、車両の挙動量予測値Ｑ１、及び検出値Ｑ２の変化を示す図である。

例えば、図９（ｃ）の時刻ｔ１にて、車両１０にてブレーキ操作が行われ、その操作量が過多となり、前述したブレーキ操作と車両の挙動量との対応を示すマップに基づき、車両１０の挙動量予測値Ｑ１（ピッチ角等）が第１の閾値挙動量Ｑthより大きくなるものと判断された場合（挙動量予測値Ｑ１が第１の閾値挙動量Ｑthより大きくなる場合）には、その直後の情報処理サイクルでパターン光３２ａの投光を開始する。即ち、図９（ｂ）に示すように、時刻ｔ２にてパターン光３２ａの投光を開始する。なお、車両機器の操作は、ブレーキ操作に限らず、アクセル操作、ステアリング操作等を採用し、これらの操作量に基づいて挙動量予測値を求めてもよい。その後、挙動量検出値Ｑ２が第２の閾値挙動量Ａに達しない場合（図８のステップＳ３３でＮＯの場合）には、距離及び姿勢角を起点とする処理は行われない。そして、車両１０の挙動量検出値Ｑ２が第２の閾値挙動量Ａよりも低く設定した第１の閾値挙動量Ｑth以下となった時点である時刻ｔ３にてパターン光３２ａの投光が終了する。

また、図９（ｃ）の時刻ｔ４にて、再度車両１０の挙動量予測値Ｑ１が第１の閾値挙動量Ｑthより大きくなると予測された場合には、前述と同様に図９（ｂ）に示すように、時刻ｔ５にてパターン光３２ａの投光を開始する。その後、時刻ｔ６にて挙動量検出値Ｑ２が第２の閾値挙動量Ａより大きくなった場合には、図９（ａ）に示すように距離及び姿勢角を起点として設定する処理を行う。その後、時刻ｔ７にて車両の挙動量検出値Ｑ２が低下し第１の閾値挙動量Ｑth以下となった時点でパターン光３２ａの投光は停止し、更に、起点設定の処理が終了する。

この際、図９（ｂ）、（ｃ）から理解されるように、時刻ｔ５にて先行してパターン光３２ａが投光され、その後、所定時間が経過した時刻ｔ６にてパターン光３２ａに基づく距離及び姿勢角の算出が行われる。換言すれば、挙動量検出値Ｑ２が第２の閾値挙動量Ａより大きくなった時点では、既にパターン光３２ａの投光が開始されているので、パターン光３２ａを用いた距離及び姿勢角の算出を迅速に行うことができる。

このようにして、本願発明の第１実施形態に係る自己位置算出装置１００では、以下の作用効果が得られる。即ち、ブレーキ操作、アクセルの操作、或いはステアリング操作等の車両機器の操作により、将来の車両の挙動量が予め設定した第１の閾値挙動量Ｑthより大きくなると予測される場合（挙動量予測値Ｑ１が第１の閾値挙動量Ｑthより大きくなる場合）には、投光器１１よりパターン光３２ａを投光する。従って、車両の挙動量検出値Ｑ２が第２の閾値挙動量Ａより大きくなる前の時点で先行してパターン光３２ａを投光することができ、パターン光３２ａを用いた距離及び姿勢角の算出を迅速に行うことができる。

その後、車両１０の挙動量検出値Ｑ２が第２の閾値挙動量Ａより大きくなった場合には、特徴点の時間変化に基づく距離及び姿勢角の積分演算を停止する。そして、パターン光３２ａを用いた距離及び姿勢角が求められている場合には、この距離及び姿勢角を新たな起点として設定する。即ち、自己位置算出部２６は、車両の挙動量検出値Ｑ２が第１の閾値挙動量Ｑthよりも大きい場合、そのときの車両１０の現在位置と距離及び姿勢角を、車両１０の初期位置と距離及び姿勢角に設定し、再び、距離及び姿勢角の変化量の加算を開始する。また、パターン光３２ａを用いた距離及び姿勢角が求められていない場合には、前回のサイクルで更新された距離及び姿勢角を新たな起点として設定する。従って、車両の挙動量が増大した場合においても、車両の移動量を高精度に算出することが可能となる。

更に、挙動量予測値Ｑ１が第１の閾値挙動量Ｑth以下となった場合、及び挙動量検出値Ｑ２が閾値挙動量Ｑth以下となった場合には、パターン光３２ａの投光を停止するので、不要なパターン光３２ａの投光を省略でき、制御負荷の軽減し消費電力を低減することができる。

また、第１実施形態に係る自己位置算出装置１００では、挙動量予測部３７で車両１０の挙動量Ｑ１が第１の閾値挙動量Ｑthより大きくなるものと予測された場合に、パターン光３２ａを照射する例について説明したが、挙動量予測値Ｑ１が第１の閾値挙動量Ｑthより大きくなった場合、予め設定した第１の所定時間の経過後に、パターン光３２ａを照射するようにしてもよい。こうすることにより、挙動量予測値Ｑ１が第１の閾値挙動量Ｑthの付近で上下して変動するような場合に、パターン光３２ａの投光、停止が頻繁に繰り返されることを防止することができる。

更に、運転者によるブレーキ、アクセル、ステアリング等の操作機器の操作量に基づいて、将来における車両の挙動量を予測するので、より高精度に挙動量を予測することができる。

［第２実施形態の説明］
次に、本発明に係る自己位置算出装置１００の第２実施形態について説明する。前述した第１実施形態では、パターン光３２ａが投光された後、車両１０の挙動量検出値Ｑ２が第１の閾値挙動量Ｑth以下となった場合（図９の時刻ｔ７）に、パターン光３２ａの投光を停止した。これに対して、第２実施形態では、車両１０の挙動量検出値Ｑ２が第１の閾値挙動量Ｑth以下となった後、所定時間（第２の所定時間）だけ継続してパターン光３２ａの投光を継続する。以下詳細に説明する。

装置構成は、第１実施形態で示した図１と同一である。但し、パターン光制御部２７は、挙動量予測値が予め設定した閾値挙動量Ｑthより大きくなると予測された場合に、パターン光３２ａを投光し、更に、該挙動量予測値が第１の閾値挙動量Ｑth以下となった場合には、その後、第２の所定時間だけパターン光３２ａの投光を継続する点で相違する。それ以外の構成は、図１と同一であるので、同一符号を付して構成説明を省略する。

以下、図１０に示すフローチャートを参照して、第２実施形態に係る自己位置算出装置１００の作用について説明する。図１０に示すように、第２実施形態では第１実施形態で説明した図７と対比して、ステップＳ１７ａを追加している点で相違する。以下、詳細に説明する。

初めに、図１０のステップＳ１１において、ＥＣＵ１３は、今回の情報処理サイクルが初回であるか否かを判断する。そして、初回である場合、即ち、前回の情報処理サイクルのデータが無い場合にはステップＳ１４に処理を進め、初回でない場合にはステップＳ１２に処理を進める。

ステップＳ１２において、挙動量予測部３７は、車両の挙動量予測値を取得する。この処理では、一例として前述したように、アクセル操作やブレーキ操作、或いはステアリング操作に基づいて、挙動量予測値を求める。

ステップＳ１３において、挙動量予測部３７は、ステップＳ１２の処理で求めた挙動量予測値が予め設定した第１の閾値挙動量Ｑthより大きくなるか否かを判断する。そして、第１の閾値挙動量Ｑthより大きくなると判断した場合には（ステップＳ１３でＹＥＳ）、ステップＳ１４に処理を進め、そうでなければ（ステップＳ１３でＮＯ）、ステップＳ１７ａに処理を進める。

ステップＳ１４以降の処理は、前述した第１実施形態と同様であるので、同一符号を付して説明を省略する。

ステップＳ１７ａにおいて、挙動量予測部３７は、挙動量予測値が第１の閾値挙動量Ｑth以下となってから、第２の所定時間が経過したか否かを判断する。そして、経過していない場合には（ステップＳ１７ａでＮＯ）、ステップＳ１４に処理を進める。一方、経過している場合には（ステップＳ１７ａでＹＥＳ）、ステップＳ１７に処理を進める。ステップＳ１７以降の処理は、前述した第１実施形態と同様であるので、同一符号を付して説明を省略する。

次に、図１１に示すタイミングチャートを参照して、車両１０の挙動量の変化、パターン光３２ａの投光タイミング、及び起点を演算するタイミングの関係について説明する。図１１（ａ）は、距離及び姿勢角の起点を設定するタイミングを示す図、（ｂ）は、パターン光３２ａを投光するタイミングを示す図、（ｃ）は、車両１０の挙動量予測値Ｑ１、及び挙動量検出値Ｑ２の変化を示す図である。

いま、図１１（ｃ）の時刻ｔ１１にて、例えば、ブレーキの操作量、アクセルの操作量、或いはステアリングの操作量に基づき、挙動量予測値Ｑ１が第１の閾値挙動量Ｑthより大きくなるものと予測された場合には、パターン光３２ａの投光を開始する。即ち、図１１（ｂ）に示すように、時刻ｔ１２にてパターン光３２ａの投光を開始する。その後、車両の挙動量検出値Ｑ２が第２の閾値挙動量Ａに達しない場合には、距離及び姿勢角を起点とする処理は行われない。そして、車両１０の挙動量検出値Ｑ２が時刻ｔ１３にて第１の閾値挙動量Ｑth以下となった場合にパターン光３２ａの投光を停止する。

また、時刻ｔ１４で、再度、挙動量予測値Ｑ１が第１の閾値挙動量Ｑthより大きくなるものと予測された場合には、時刻ｔ１５にてパターン光３２ａの投光を開始する。その後、時刻ｔ１６にて車両１０の挙動量検出値Ｑ２が第１の閾値挙動量Ｑth以下となった場合、第２の所定時間の経過後にパターン光３２ａの投光を継続する。即ち、時刻ｔ１７までパターン光３２ａの投光を継続する。

即ち、投光器１１は、車両１０の挙動量検出値Ｑ２が第１の閾値挙動量Ｑthより大きくなった状態から、挙動量検出値Ｑ２が低下して、第１の閾値挙動量Ｑthを以下となった場合、第２の所定時間の経過後にパターン光３２ａの投光を停止する。

このようにして、第２実施形態に係る自己位置算出装置１００では、前述した第１実施形態と同様に、車両１０の挙動量予測値Ｑ１が増大する場合には、パターン光３２ａを先行して投光することができる。従って、パターン光３２ａを用いた距離及び姿勢角の算出を迅速に行うことができる。また、車両１０の挙動量検出値Ｑ２が低下して、第１の閾値挙動量Ｑth以下となった場合には、その後、第２の所定時間Ｔ１だけパターン光３２ａの投光が継続される。従って、車両１０の挙動量予測値Ｑ１、及び挙動量検出値Ｑ２が上下に変動する場合において、パターン光３２ａの投光、停止が頻繁に繰り返されることを防止できる。その結果、車両の移動量の算出精度の向上に寄与することができる。

なお、上述した第１、第２実施形態では、挙動量予測値を取得する際に、アクセル操作、ブレーキ操作、或いはステアリング操作等、運転者による操作機器の操作量を用いる例について説明した。本願発明は、これに限定されるものではなく、例えば、ＧＰＳ情報と地図情報に基づいて、走行路の前方にカーブ路が存在する場合や、先行車両との間の相対速度が大きくなった場合等、運転者が操作機器を操作すると考えられる場合に、挙動量予測値が増大すると予測することも可能である。具体的には、走行路前方のカーブ路の曲率半径が所定値以上である場合、先行車両との間の相対速度が所定値以上である場合に、将来の挙動量が閾値挙動量以上になるものと予測することが可能である。

以上、本発明の自己位置算出装置及び自己位置算出方法を図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置き換えることができる。

例えば、図２では、カメラ１２と投光器１１を車両１０の前面に取り付ける例について示したが、本発明はこれに限定されず、車両１０の側方、後方、真下に向けて設置してもよい。また、上記した各実施形態では車両１０に搭載した場合について記述したが、オートバイや建設機械など、道路の路面或いは壁面上の特徴点を撮像することが可能な移動体すべてに適用可能である。

１０ 車両
１１ 投光器
１２ カメラ（撮像部）
１３ ＥＣＵ
２１ パターン光抽出部
２２ 姿勢角算出部
２３ 特徴点検出部
２４ 姿勢変化量算出部
２６ 自己位置算出部
２７ パターン光制御部
３１ 路面
３２ａ，３２ｂ パターン光
３６ 挙動量検出部
３７ 挙動量予測部
１００ 自己位置算出装置

Claims (7)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   前記車両の挙動量を検出する挙動量検出部と、
   前記撮像部により取得された画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部により取得された画像の前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び前記姿勢角を算出する自己位置算出部と、
   を備え、
   前記投光器は、車両の挙動量が第１の閾値挙動量より大きい場合に前記パターン光を投光することを特徴とする自己位置算出装置。
2. 将来における車両の挙動量を予測する挙動量予測部を更に備え、
   前記投光器は、前記予測された車両の挙動量が第１の閾値挙動量より大きい場合に前記パターン光を投光することを特徴とする請求項１に記載の自己位置算出装置。
3. 前記投光器は、前記車両の挙動量または前記予測された車両の挙動量が第１の閾値挙動量より大きくなった後、第１の所定時間の経過後に前記パターン光を投光することを特徴とする請求項１または２に記載の自己位置算出装置。
4. 前記自己位置算出部は、前記車両の挙動量が第２の閾値挙動量より大きい場合には、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して、前記姿勢変化量の加算を開始すること
   を特徴とする請求項１〜３のいずれか１項に記載の自己位置算出装置。
5. 前記投光器は、前記パターン光の投光を開始した後、前記車両の挙動量が第１の閾値挙動量以下となった場合、パターン光の投光を停止することを特徴とする請求項１〜４のいずれか１項に記載の自己位置算出装置。
6. 前記投光器は、前記パターン光の投光を開始した後、前記車両の挙動量が第１の閾値挙動量以下となった場合には、第２の所定時間の経過後にパターン光の投光を停止することを特徴とする請求項１〜５のいずれか１項に記載の自己位置算出装置。
7. 車両周囲の路面にパターン光を投光する投光手順と、
   前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   車両の挙動量を検出する挙動量検出手順と、
   前記画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出手順と、
   前記画像に含まれる複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出手順と、
   前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
   を備え、
   前記投光手順は、車両の挙動量が第１の閾値挙動量より大きい場合に前記パターン光を投光することを特徴とする自己位置算出方法。