(57)【要約】      （修正有）【課題】車両の挙動量に拘わらず、車両の移動量を精度よく且つ安定して推定することができる自己位置算出装置及び自己位置算出方法を提供する。【解決手段】車両の挙動量を検出する挙動量検出部３６と、カメラ１２により取得されるパターン光の位置から、路面に対する車両の姿勢角を算出する姿勢角算出部２２と、路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出する姿勢変化量算出部２４と、車両の初期位置及び姿勢角に、姿勢変化量を加算してゆくことで、車両の現在位置及び姿勢角を算出する自己位置算出部２６を備える。そして、自己位置算出部２６は、車両の挙動量が閾値挙動量よりも大きい場合、そのときの車両の現在位置及び姿勢角算出部２２による車両の姿勢角に対して、姿勢変化量の加算を開始する。【選択図】図１
2015158419.tif 000002
【特許請求の範囲】
【請求項１】
車両周囲の路面にパターン光を投光する投光器と、
前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
車両の挙動量を検出する挙動量検出部と、
前記撮像部により取得された画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
前記撮像部により取得された画像の前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
を備え、
前記自己位置算出部は、車両の挙動量が閾値挙動量よりも大きい場合、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して、前記姿勢変化量の加算を開始する
ことを特徴とする自己位置算出装置。
【請求項２】
前記自己位置算出部は、車両の挙動量が閾値挙動量以下の場合、第１の周期で、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して、前記姿勢変化量の加算を開始し、
車両の挙動量が閾値挙動量よりも大きい場合、前記第１の周期よりも短い第２の周期で、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して、前記姿勢変化量の加算を開始する
ことを特徴とする請求項１に記載の自己位置算出装置。
【請求項３】
前記挙動量検出部は、車両の前後方向の加速度を前記挙動量として検出することを特徴とする請求項１または２に記載の自己位置算出装置。
置。
【請求項４】
前記挙動量検出部は、車両の左右方向の移動速度を前記挙動量として検出することを特徴とする請求項１または２に記載の自己位置算出装置。
置。
【請求項５】
前記挙動量検出部は、車両の鉛直方向の移動速度を前記挙動量として検出することを特徴とする請求項１または２に記載の自己位置算出装置。
【請求項６】
前記自己位置算出部は、前記姿勢角算出部により、パターン光から車両の姿勢角を算出できない場合には、予め設定した姿勢角初期値を用いることを特徴とする請求項１〜５のいずれか１項に記載の自己位置算出装置。
【請求項７】
前記自己位置算出部は、前記姿勢角算出部により、パターン光から車両の姿勢角を算出できない場合には、直前に算出された姿勢角を用いることを特徴とする請求項１〜５のいずれか１項に記載の自己位置算出装置。
【請求項８】
車両周囲の路面にパターン光を投光する投光手順と、
前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
車両の挙動量を検出する車両挙動検出手順と、
前記画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出手順と、
前記画像に含まれる複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出手順と、
前記車両の初期位置及び姿勢角に、前記姿勢変化量を加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
を備え、
前記自己位置算出手順は、車両の挙動量が閾値挙動量よりも大きい場合、そのときの前記車両の現在位置及び前記パターン光の位置から算出された前記車両の姿勢角に対して、前記姿勢変化量の加算を開始する
ことを特徴とする自己位置算出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置算出装置及び自己位置算出方法に係り、特に、移動量を推定する精度を向上させる技術に関する。
【背景技術】
【０００２】
従来より、車両にカメラを搭載し、該カメラにて車両近傍の映像を撮像し、撮像した画像の変化に基づいて車両の移動量を求める技術が提案されている（特許文献１参照）。特許文献１では、車両の低速かつ微妙な移動においても精度よく移動量を求めるために、画像に含まれる特徴点を検出し、この特徴点の画像上での位置を求め、該特徴点の移動方向及び移動距離（移動量）から車両の移動量を求めている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００８−１７５７１７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、車両の挙動量、例えば、車両の前後方向の加速度が大きい場合には、カメラと路面との位置関係に変化が生じるので、高精度な移動量の測定ができなくなるという問題が生じる。つまり、カメラで撮像した画像に基づいて移動量を推定する際に、急発進、急停車等、車両の挙動量が大きい場合には、車両の姿勢が変化することにより路面に対するカメラの姿勢角が変化する。従って、このカメラで撮像した画像に基づいて車両の移動量を推定すると、姿勢角の変化に起因して、移動量の推定値に大きな誤差が生じてしまい、高精度な測定ができなくなってしまう。
【０００５】
本発明は、このような従来の課題を解決するためになされたものであり、その目的とするところは、車両の挙動量が大きい場合においても、車両の移動量を精度よく且つ安定して算出することができる自己位置算出装置及び自己位置算出方法を提供することにある。
【課題を解決するための手段】
【０００６】
上記目的を達成するため、本願発明は、車両の挙動量を検出する挙動量検出部と、撮像部により取得されるパターン光の位置から、路面に対する車両の姿勢角を算出する姿勢角算出部と、路面上の複数の特徴点の時間変化に基づいて、車両の姿勢変化量を算出する姿勢変化量算出部と、車両の初期位置及び姿勢角に、姿勢変化量を加算してゆくことで、車両の現在位置及び姿勢角を算出する自己位置算出部とを備える。そして、自己位置算出部は、車両の挙動量が閾値挙動量よりも大きい場合、そのときの車両の現在位置及び姿勢角算出部による車両の姿勢角に対して、姿勢変化量の加算を開始する。
【発明の効果】
【０００７】
本発明によれば、車両の挙動量が閾値挙動量よりも大きい場合には、そのときの車両の現在位置及び姿勢角算出部による車両の姿勢角に対して姿勢角変化量の加算を開始する。従って、車両の挙動量が大きい場合でも、車両の初期位置及び姿勢角の算出精度が低下することを防止でき、車両の移動量を精度よく且つ安定して算出することが可能となる。
【図面の簡単な説明】
【０００８】
【図１】本発明の第１実施形態に係る自己位置算出装置の構成を示すブロック図である。
【図２】車両に搭載される投光器により照射される投光パターン、及びカメラによる撮像範囲を示す説明図である。
【図３】（ａ）は、投光器とカメラの間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕｊ，Ｖｊ）とから、各スポット光が照射された路面上の位置を算出する様子を示す図であり、（ｂ）は、パターン光が照射された領域とは異なる他の領域から検出された特徴点の時間変化から、カメラの移動方向を求める様子を示す説明図である。
【図４】（ａ）は、カメラにより取得した画像に対して二値化処理を施したパターン光の画像を示す図、（ｂ）はその拡大図、（ｃ）はパターン光抽出部により抽出した各スポット光の重心の位置を示す図である。
【図５】距離及び姿勢角の変化量を算出する方法を説明するための模式図である。
【図６】（ａ）は、時刻ｔに取得した画像の一例を示し、（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得した画像の一例を示す。
【図７】本発明の実施形態に係る自己位置算出装置の処理手順を示すフローチャートである。
【図８】第１実施形態に係り、図８のステップＳ０９に示す処理の詳細な処理手順を示すフローチャートである。
【図９】第１実施形態に係る自己位置算出装置の、路面の勾配変化と起点設定のタイミングを示すタイミングチャートである。
【図１０】第２実施形態に係る自己位置算出装置の、車両の加速度と起点設定のタイミングを示すタイミングチャートである。
【図１１】第３実施形態に係り、図８のステップＳ０９に示す処理の詳細な処理手順を示すフローチャートである。
【図１２】第３実施形態に係る自己位置算出装置の、車両の加速度と起点設定のタイミングを示すタイミングチャートである。
【発明を実施するための形態】
【０００９】
以下、本発明の実施形態について、図面を参照して説明する。
【００１０】
［第１実施形態の説明］
図１は、本発明の実施形態に係る自己位置算出装置の構成を示すブロック図である。図１に示すように、自己位置算出装置１００は、投光器１１と、カメラ１２（撮像部）と、エンジンコントロールユニット（ＥＣＵ）１３と、を備える。投光器１１は、車両に搭載され、車両周囲の路面にパターン光を投光する。パターン光の詳細については後述する。カメラ１２は、車両に搭載され、パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する。ＥＣＵ１３は、投光器１１の作動を制御し、且つカメラ１２により取得された画像から車両の移動量を推定する一連の情報処理サイクルを実行する。
【００１１】
カメラ１２は、固体撮像素子、例えばＣＣＤ及びＣＭＯＳを用いたデジタルカメラであって、画像処理が可能なデジタル画像を取得する。カメラ１２の撮像対象は車両周囲の路面であって、車両周囲の路面には、車両の前部、後部、側部、車両底部の路面が含まれる。例えば、図２に示すように、カメラ１２は、車両１０の前部、具体的にはフロントバンパ上に搭載することができる。
【００１２】
車両１０の前方の路面３１上の特徴点（「テクスチャ」ともいう）及び投光器１１により投光されたパターン光３２ｂを撮像できるように、カメラ１２が設置される高さ及び向きが調整され、且つ、カメラ１２が備えるレンズのピント及び絞りが自動調整される。ここで、「特徴点」とは、アスファルト上に存在する凹凸部分である。
【００１３】
カメラ１２は、所定の時間間隔をおいて繰り返し撮像を行い、一連の画像（フレーム）群を取得する。カメラ１２により取得された画像データは、撮像周期毎にＥＣＵ１３へ転送され、ＥＣＵ１３が備えるメモリ（図示省略）に記憶される。
【００１４】
投光器１１は、図２に示すように、カメラ１２の撮像範囲内の路面３１に向けて、正方形や長方形の格子像を含む所定の形状を有するパターン光３２ｂを投光する。カメラ１２は、パターン光を含む路面３１の領域を撮像する。投光器１１は、例えば、レーザポインター及び回折格子を備える。レーザポインターから射出されたレーザ光を回折格子で回折することにより、投光器１１は、図２〜図４に示すように、格子像、或いは行列状に配列された複数のスポット光からなるパターン光（３２ｂ、３２ａ）を生成する。図３及び図４に示す例では、５×７のスポット光からなるパターン光３２ａを生成している。
【００１５】
図３（ａ）は、路面３１にパターン光３２ａを照射し、この照射領域をカメラ１２で撮像する様子を示している。また、図３（ｂ）は、路面３１にパターン光３２ａを照射し、この照射領域とは異なる他の領域３３をカメラ１２で撮像して特徴点を検出し、該特徴点の時間変化からカメラ１２の移動方向３４を求める様子を示している。
【００１６】
図１に戻り、ＥＣＵ１３は、ＣＰＵ、メモリ、及び入出力部を備えるマイクロコントローラからなり、予めインストールされたコンピュータプログラムを実行することにより、自己位置算出装置１００が備える複数の情報処理部を構成する。ＥＣＵ１３は、カメラ１２により取得された画像から車両１０の移動量を推定する一連の情報処理サイクルを、画像（フレーム）毎に繰り返し実行する。ＥＣＵ１３は、車両１０に関連する他の制御に用いるＥＣＵと兼用してもよい。
【００１７】
複数の情報処理部には、パターン光抽出部２１と、姿勢角算出部２２と、特徴点検出部２３と、姿勢変化量算出部２４と、自己位置算出部２６と、パターン光制御部２７、及び挙動量検出部３６が含まれる。姿勢変化量算出部２４には、特徴点検出部２３が含まれている。
【００１８】
パターン光制御部２７は、投光器１１によるパターン光３２ａの投光を制御する。例えば、車両１０のイグニションスイッチがオン状態となり、当該自己位置算出装置１００が起動すると同時に、パターン光制御部２７は、パターン光３２ａの投光を開始する。その後、パターン光制御部２７は、自己位置算出装置１００が停止するまで、パターン光３２ａを連続して投光する。或いは、所定の時間間隔をおいて、投光のオン、オフを繰り返してもよい。或いは、挙動量検出部３６で検出される車両１０の挙動量（例えば、車両１０の前後方向の加速度）が大きくなった場合に限って、一時的にパターン光３２ａを投光してもよい。
【００１９】
パターン光抽出部２１は、カメラ１２により取得された画像をメモリから読み込み、画像からパターン光の位置を抽出する。図３（ａ）に示すように、例えば、投光器１１が行列状に配列された複数のスポット光からなるパターン光３２ａを路面３１に向けて投光し、路面３１で反射したパターン光３２ａをカメラ１２で検出する。パターン光抽出部２１は、カメラ１２により取得された画像に対して二値化処理を施すことにより、図４（ａ）及びその拡大図である図４（ｂ）に示すように、スポット光Ｓｐの画像のみを抽出する。パターン光抽出部２１は、図４（ｃ）に示すように、各スポット光Ｓｐの重心の位置Ｈｅ、即ち、スポット光Ｓｐの画像上の座標（Ｕｊ，Ｖｊ）を算出することにより、パターン光３２ａの位置を抽出する。座標は、カメラ１２の撮像素子の画素を単位とし、５×７のスポット光Ｓｐの場合、「ｊ」は１以上３５以下の自然数である。スポット光Ｓｐの画像上の座標（Ｕｊ，Ｖｊ）は、パターン光３２ａの位置を示すデータとしてメモリに記憶される。
【００２０】
姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、カメラ１２により取得された画像におけるパターン光３２ａの位置から、路面３１に対する車両１０の距離及び姿勢角を算出する。例えば、図３（ａ）に示すように、投光器１１とカメラ１２の間の基線長Ｌｂと、各スポット光の画像上の座標（Ｕｊ，Ｖｊ）とから、三角測量の原理を用いて、各スポット光が照射された路面３１上の位置を、カメラ１２に対する相対位置として算出する。
【００２１】
そして、該姿勢角算出部２２は、カメラ１２に対する各スポット光の相対位置から、パターン光３２ａが投光された路面３１の平面式、即ち、路面３１に対するカメラ１２の距離及び姿勢角（法線ベクトル）を算出する。なお、車両１０に対するカメラ１２の搭載位置及び撮像方向は既知であるので、実施形態においては、路面３１に対する車両１０の距離及び姿勢角の一例として、路面３１に対するカメラ１２の距離及び姿勢角を算出する。換言すれば、路面３１に対するカメラ１２の距離及び姿勢角を算出することにより、路面３１と車両１０との間の距離、及び路面３１に対する車両１０の姿勢角を求めることができる。以下では、路面３１に対するカメラ１２の距離及び姿勢角を、「距離及び姿勢角」と略す。姿勢角算出部２２により算出された距離及び姿勢角は、メモリに記憶される。
【００２２】
具体的に説明すると、カメラ１２及び投光器１１は車両１０にそれぞれ固定されているので、パターン光３２ａの照射方向と、カメラ１２と投光器１１との距離（図３の基線長Ｌｂ）は既知である。そこで、姿勢角算出部２２は、三角測量の原理を用いて、各スポット光の画像上の座標（Ｕｊ，Ｖｊ）から各スポット光が照射された路面３１上の位置を、カメラ１２に対する相対位置を３次元座標（Ｘｊ，Ｙｊ，Ｚｊ）として求めることができる。
【００２３】
なお、カメラ１２に対する各スポット光の相対位置（Ｘｊ，Ｙｊ，Ｚｊ）は同一平面上に存在しない場合が多い。なぜなら、路面３１に表出するアスファルトの凹凸に応じて各スポット光の相対位置が変化するからである。そこで、最小二乗法を用いて、各スポット光との距離誤差の二乗和が最小となるような平面式を求めてもよい。こうして算出される距離及び姿勢角のデータは、図１に示す自己位置算出部２６にて用いられることになる。
【００２４】
特徴点検出部２３は、カメラ１２により取得された画像をメモリから読み込み、メモリから読み込んだ画像から、路面３１上の特徴点を検出する。特徴点検出部２３は、路面３１上の特徴点を検出するために、例えば、「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だけ路面３１に向けて傾斜させる。また、カメラ１２により取得される画像をＥＣＵ１３に転送するときの輝度数値は、０〜２５５（０：最も暗い、２５５：最も明るい）の範囲内である。
【００２７】
姿勢変化量算出部２４は、一定の情報処理サイクル毎に撮像される各フレームの画像のうち、前回（時刻ｔ）フレームの画像に含まれる複数の特徴点の画像上の位置座標（Ｕｉ，Ｖｉ）をメモリから読み込み、更に、今回（時刻ｔ＋Δｔ）フレームの画像に含まれる複数の特徴点の画像上の位置座標（Ｕｉ，Ｖｉ）をメモリから読み込む。そして、複数の特徴点の画像上での位置変化に基づいて、車両１０の姿勢変化量を求める。ここで、「車両の姿勢変化量」とは、路面に対する車両１０の「距離及び姿勢角の変化量」、及び路面上での「車両の移動量」の双方を含んでいる。以下、「距離及び姿勢角の変化量」及び「車両の移動量」の算出方法について説明する。
【００２８】
距離及び姿勢角の変化量は、例えば、以下のようにして求めることができる。図６（ａ）は、時刻ｔに取得された第１フレームの画像３８の一例を示す。図５及び図６（ａ）に示すように、画像３８において、例えば３つの特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の相対位置（ｘｉ，ｙｉ，ｚｉ）がそれぞれ算出されている場合を考える。この場合、特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３によって特定される平面Ｇ（図６（ａ）参照）を路面と見なすことができる。よって、姿勢変化量算出部２４は、各特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）から、路面（平面Ｇ）に対するカメラ１２の距離及び姿勢角（法線ベクトル）を求めることができる。更に、姿勢変化量算出部２４は、既知のカメラモデルによって、各特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の間の距離（ｌ１，ｌ２，ｌ３）及び夫々の特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を結ぶ直線が成す角度を求めることができる。図５のカメラ１２は、第１フレーム（時刻ｔ）におけるカメラの位置を示す。
【００２９】
なお、カメラ１２に対する特徴点の相対位置を示す３次元座標（ｘｉ，ｙｉ，ｚｉ）として、カメラ１２の撮像方向をＺ軸に設定し、撮像方向を法線とし且つカメラ１２を含む平面内に、互いに直交するＸ軸及びＹ軸を設定する。一方、画像３８上の座標として、水平方向及び垂直方向をそれぞれＶ軸及びＵ軸に設定する。
【００３０】
図６（ｂ）は、時刻ｔから時間Δｔだけ経過した時刻（ｔ＋Δｔ）に取得された第２フレームの画像３８’を示す。図５のカメラ１２’は、第２フレームの画像３８’を撮像したときのカメラ１２の位置を示す。図５及び図６（ｂ）に示すように、画像３８’において、カメラ１２’が特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を撮像し、特徴点検出部２３が特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を検出する。この場合、姿勢変化量算出部２４は、時刻ｔにおける各特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３の相対位置（ｘｉ，ｙｉ，ｚｉ）と、各特徴点の画像３８’上の位置Ｐ１（Ｕｉ，Ｖｉ）と、カメラ１２のカメラモデルとから、時間Δｔにおけるカメラ１２の移動量ΔＬ（図５参照）を算出でき、ひいては車両１０の移動量を算出できる。更には、距離及び姿勢角の変化量も算出することができる。例えば、以下の（１）〜（４）式からなる連立方程式を解くことにより、姿勢変化量算出部２４は、カメラ１２（車両）の移動量（ΔＬ）、及び、距離及び姿勢角の変化量を算出することができる。なお、下記の（１）式はカメラ１２が歪みや光軸ずれのない理想的なピンホールカメラとしてモデル化したものであり、λｉは定数、ｆは焦点距離である。カメラモデルのパラメータは、予めキャリブレーションをしておけばよい。
【数１】
2015158419.tif 000003
【数２】
2015158419.tif 000004
【数３】
2015158419.tif 000005
【数４】
2015158419.tif 000006
【００３１】
なお、姿勢変化量算出部２４は、時刻ｔと時刻ｔ＋Δｔで検出される各画像中で相対位置が算出された特徴点の全てを用いるのではなく、特徴点同士の位置関係に基づいて最適な特徴点を選定してもよい。選定方法としては、例えば、エピポーラ幾何（エピ極線幾何，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)）を用いることができる。
【００３２】
このように、姿勢変化量算出部２４が、時刻ｔのフレーム画像３８で相対位置（ｘｉ，ｙｉ，ｚｉ）が算出された特徴点Ｔｅ１、Ｔｅ２、Ｔｅ３を、時刻ｔ＋Δｔにおけるフレーム画像３８’からも検出された場合に、該姿勢変化量算出部２４は、路面上の複数の特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）と特徴点の画像上の位置（Ｕｉ，Ｖｉ）の時間変化から、「車両の姿勢角変化量」を算出することができる。更には、車両１０の移動量を算出することができる。
【００３３】
即ち、前回フレームと今回フレームの間で対応関係が取れる３点以上の特徴点を検出し続けることができれば、距離及び姿勢角の変化量を加算する処理（即ち、積分演算）を継続することにより、パターン光３２ａを用いることなく、距離及び姿勢角を更新し続けることができる。但し、最初の情報処理サイクルにおいては、パターン光３２ａを用いて算出された距離及び姿勢角、或いは所定の初期的な距離及び姿勢角を用いてもよい。つまり、積分演算の起点となる距離及び姿勢角は、パターン光３２ａを用いて算出しても、或いは、所定の初期値を用いても構わない。所定の初期的な距離及び姿勢角は、少なくとも車両１０への乗員及び搭載物を考慮した距離及び姿勢角であることが望ましい。例えば、車両１０のイグニションスイッチがオン状態であって、且つシフトポジションがパーキングから他のポジションへ移動した時に、パターン光３２ａを投光し、パターン光３２ａから算出された距離及び姿勢角を、所定の初期的な距離及び姿勢角として用いればよい。これにより、車両１０の旋回や加減速によるロール運動或いはピッチ運動が発生していない時の距離や姿勢角を求めることができる。
【００３４】
なお、前後フレーム間で特徴点を対応付けるには、例えば、検出した特徴点の周辺の小領域の画像をメモリに記録しておき、輝度や色情報の類似度から判断すればよい。具体的には、ＥＣＵ１３は、検出した特徴点を中心とする５×５（水平×垂直）画素分の画像をメモリに記録する。姿勢変化量算出部２４は、例えば、輝度情報が２０画素以上で誤差１％以下に収まっていれば、前後フレーム間で対応関係が取れる特徴点であると判断する。そして、上記の処理で取得した姿勢変化量は、後段の自己位置算出部２６にて車両１０の自己位置を算出する際に用いられる。
【００３５】
図１に示す自己位置算出部２６は、姿勢変化量算出部２４で算出された「距離及び姿勢角の変化量」から車両１０の距離及び姿勢角を算出する。更に、姿勢変化量算出部２４で算出された「車両の移動量」から車両１０の自己位置を算出する。
【００３６】
具体的は、姿勢角算出部２２（図１参照）にて算出された距離及び姿勢角（パターン光を用いて算出された距離及び姿勢角）が起点として設定された場合について説明する。この場合には、この起点（距離及び姿勢角）に対して、自己位置算出部２６は、姿勢角算出部２２で算出された距離及び姿勢角に対して、姿勢変化量算出部２４で算出された各フレーム毎の距離及び姿勢角の変化量を逐次加算して（積分演算して）、距離及び姿勢角を最新な数値に更新する。また、自己位置算出部２６は、姿勢角算出部２２にて距離及び姿勢角が算出された際の車両位置を起点（車両の初期位置）とし、この初期位置からの車両１０の移動量を逐次加算して（積分演算して）、車両１０の自己位置を算出する。例えば、地図上の位置と照合された起点（車両の初期位置）を設定することで、地図上の車両の現在位置を逐次算出することができる。
【００３７】
従って、姿勢変化量算出部２４は、時間Δｔの間でのカメラ１２の移動量（ΔＬ）を求めることにより、車両１０の自己位置を算出することができる。更には、距離及び姿勢角の変化量も同時に算出することができるので、姿勢変化量算出部２４は、車両１０の距離及び姿勢角の変化量を考慮して、６自由度（前後、左右、上下、ヨー、ピッチ、ロール）の移動量（ΔＬ）を精度よく算出することができる。即ち、車両１０の旋回や加減速によるロール運動或いはピッチ運動によって距離や姿勢角が変化しても、移動量（ΔＬ）の推定誤差を抑制することができる。
【００３８】
なお、実施形態では、距離及び姿勢角の変化量を算出し、距離及び姿勢角を更新することにより、カメラ１２の移動量（ΔＬ）を算出した。しかし、路面３１に対するカメラ１２の姿勢角だけを変化量算出及び更新の対象としても構わない。この場合、路面３１とカメラ１２との距離は一定と仮定すればよい。これにより、姿勢角の変化量を考慮して、移動量（ΔＬ）の推定誤差を抑制しつつ、ＥＣＵ１３の演算負荷を軽減し、且つ演算速度を向上させることもできる。
【００３９】
また、図１に示す挙動量検出部３６は、車両１０に搭載される各種のセンサから車両１０の挙動量を取得する。例えば、車速センサより、車両１０の走行速度を検出し、この走行速度を微分演算して、車両１０の前後方向の加速度α１（前方の加速度、後方の加速度）を検出する。他の挙動量として、ヨーレートセンサにて検出される車両１０の左右方向の移動速度を取得し、これを挙動量とすることも可能である。或いは、ストロークセンサにて検出される車両１０の鉛直方向（上下方向）の移動速度を検出し、これを挙動量とすることも可能である。そして、加速度α１が予め設定した閾値加速度αthを上回った場合、挙動量が過多であると判断する。
【００４０】
例えば、車両１０の移動距離の算出精度を、車両１０の挙動変化による車両１０の移動量算出の誤差を５％以下に抑えるとすると、車両１０のピッチ角、或いはロール角の算出値が、実際のピッチ角に対して０．０５［rad］より大きく変化することを防止する必要がある。ピッチ角、及びロール角は車載したジャイロセンサの検出値を積分して求めることができる。
【００４１】
一例として、車両１０の前後方向の加速度を用いる場合には、車両１０のピッチ角が車両停止時のピッチ角と比較して０．０５［rad］変化するときの、前後方向の加速度を予め求めておき、この加速度を閾値加速度αthとして設定する。車両１０のロール角の算出値が、実際の車両停止時のロール角に対して０．０５［rad］より大きく変化することを防止する。
【００４２】
また他の例として、車両１０の横方向速度を用いる場合には、車両１０のロール角が、車両１０が停止している状態と比較して、０．０５［rad］変化するときの車両１０の横方向加速度Ｙｇ［ｍ/sec2］を予め算出しておき、この加速度となる車速Ｖ［ｍ/sec］に応じたヨーレートγ［rad/sec］を閾値とすることができる。
【００４３】
更に他の例として、車両１０の鉛直方向の速度を用いる場合には、路面に対する距離が１秒間に５％より大きく変化する際の鉛直方向の速度を閾値とすることができる。この場合は、車両挙動の変化の周波数が最速１［Ｈｚ］と想定しているので、カメラ１２の路面からの高さが１．０ｍであれば、０．０５［ｍ/sec］が鉛直方向の閾値速度となる。
【００４４】
そして、車両１０の挙動量が過多であると判断した場合には、姿勢角算出部２２は、パターン光抽出部２１より抽出されるパターン光に基づいて、車両の距離及び姿勢角を算出する処理を行う。
【００４５】
［第１実施形態の作用］
次に、カメラ１２により取得された画像３８（図５参照）から車両１０の自己位置、即ち、「車両の距離及び姿勢角」、及び「車両の移動量」を算出する算出方法の一例として、ＥＣＵ１３により繰り返し実行される情報処理サイクルを、図７及び図８に示すフローチャートを参照して説明する。図７に示す情報処理サイクルは、車両１０のイグニションスイッチがオン状態となり、自己位置算出装置１００が起動すると同時に開始され、該自己位置算出装置１００が停止するまで、繰り返し実行される。
【００４６】
初めに、図７のステップＳ０１において、パターン光制御部２７は、投光器１１を制御して、車両周囲の路面３１にパターン光３２ａを投光する。図７では、パターン光３２ａを連続して投光する例を説明する。なお、前述したように、必要に応じてパターン光３２ａを投光するようにしてもよい。
【００４７】
ステップＳ０３に進み、ＥＣＵ１３は、カメラ１２を制御して、パターン光３２ａが投光された領域を含む車両周囲の路面３１を撮像して画像３８を取得する。ＥＣＵ１３は、カメラ１２により取得された画像データを、メモリに記憶する。
【００４８】
なお、ＥＣＵ１３はカメラ１２の絞りを自動制御できる。前回の情報処理サイクルで取得した画像３８の平均輝度から、輝度値の最大値と最小値の中間値になるようにカメラ１２の絞りをフィードバック制御してもよい。また、パターン光３２ａが投光されている領域は輝度値が高いため、パターン光３２ａを抽出した部分を除いた領域から、平均輝度値を求めてもよい。
【００４９】
ステップＳ０５に進み、パターン光抽出部２１は、カメラ１２により取得された画像３８をメモリから読み込み、図４（ｃ）に示したように、画像３８からパターン光３２ａの位置を抽出する。パターン光抽出部２１は、パターン光３２ａの位置を示すデータとして算出されたスポット光Ｓｐの画像上の座標（Ｕｊ，Ｖｊ）をメモリに記憶する。更に、ステップＳ０５では、姿勢角算出部２２は、パターン光３２ａの位置を示すデータをメモリから読み込み、パターン光３２ａの位置から、距離及び姿勢角を算出し、メモリに記憶する。距離及び姿勢角の算出方法は、前述した通りである。なお、姿勢角算出部２２で算出した距離及び姿勢角は、後述するように、情報処理サイクルが初回の場合、及び後述する条件となった場合に、積分演算を実行する際の新たな起点として設定する。
【００５０】
ステップＳ０７に進み、ＥＣＵ１３は、画像３８から特徴点（例えば、アスファルト上に存在する凹凸部位）を検出し、前回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点を抽出し、特徴点の画像上の位置（Ｕｉ，Ｖｉ）から距離及び姿勢角の変化量を算出する。更に、車両１０の移動量を算出する。
【００５１】
具体的には、まず特徴点検出部２３は、カメラ１２により取得された画像３８をメモリから読み込み、画像３８から路面３１上の特徴点を検出し、各特徴点の画像上の位置（Ｕｉ，Ｖｉ）をメモリに記憶する。
【００５２】
姿勢変化量算出部２４は、各特徴点の画像上の位置（Ｕｉ，Ｖｉ）をメモリから読み込み、距離及び姿勢角と、特徴点の画像上の位置（Ｕｉ，Ｖｉ）とから、カメラ１２に対する特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）を算出する。なお、姿勢変化量算出部２４は、前回の情報処理サイクルのステップＳ０９（後述）において選択された距離及び姿勢角を用いる。姿勢変化量算出部２４は、カメラ１２に対する特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）をメモリに記憶する。
【００５３】
そして、姿勢変化量算出部２４は、特徴点の画像上の位置（Ｕｉ，Ｖｉ）と、前回の情報処理サイクルのステップＳ０７において算出された特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）をメモリから読み込む。姿勢変化量算出部２４は、今回の情報処理サイクルと今回の情報処理サイクルの間で対応関係が取れる特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）及び画像上の位置（Ｕｉ，Ｖｉ）を用いて、距離及び姿勢角の変化量を算出する。更に、前回の特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）と今回の特徴点の相対位置（ｘｉ，ｙｉ，ｚｉ）から、車両１０の移動量を算出する。そして、この処理で算出した「距離及び姿勢角の変化量」及び「車両の移動量」は、ステップＳ１１の処理で用いられる。
【００５４】
ステップＳ０９に進み、ＥＣＵ１３は、車両１０の走行時の挙動量に応じて、距離及び姿勢角を積分演算にて算出する際の起点を設定する。詳細な処理手順については、図８に示すフローチャートを参照して後述する。
【００５５】
ステップＳ１１に進み、自己位置算出部２６は、ステップＳ０９の処理で設定された積分演算の起点、及びステップＳ０７の処理で算出された車両１０の移動量から、車両１０の自己位置を算出する。
【００５６】
こうして、本実施形態に係る自己位置算出装置１００は、上記した一連の情報処理サイクルを繰り返し実行して車両１０の移動量を積算することにより、車両１０の自己位置を算出することができる。
【００５７】
次に、図８のフローチャートを参照して、図７のステップＳ０９の詳細な手順を説明する。ステップＳ９０１において、ＥＣＵ１３は、今回の情報処理サイクルが初回であるか否かを判断する。そして、初回である場合、即ち、前回の情報処理サイクルのデータが無い場合にはステップＳ９０９に処理を進め、初回でない場合にはステップＳ９０３に処理を進める。
【００５８】
ステップＳ９０３において、挙動量検出部３６は、車両１０に搭載される車速センサより取得される走行速度データに基づき、車両１０の前後方向の加速度α１を検出する。この処理では、走行速度データを微分演算する。
【００５９】
ステップＳ９０５において、挙動量検出部３６は、車両１０の加速度α１が予め設定した閾値加速度αthより大きいか否かを判断する。そして、閾値加速度αthより大きい場合には（ステップＳ９０５でＹＥＳ）、ステップＳ９０７に処理を進める。一方、閾値加速度αth以下である場合には（ステップＳ９０５でＮＯ）、ステップＳ９１３に処理を進める。
【００６０】
ステップＳ９０７において、姿勢角算出部２２は、パターン光抽出部２１で抽出されたパターン光３２ａに基づき、距離及び姿勢角が算出されているか否かを判断する。具体的な例として、投光器１１より照射される３５点のスポット光のうち、３点以上のスポット光が検出されている場合に距離及び姿勢角の算出が可能なので、３点以上のスポット光が検出されているか否かを判断する。そして、距離及び姿勢角が算出されている場合には（ステップＳ９０７でＹＥＳ）、ステップＳ９０９に処理を進める。一方、距離及び姿勢角が算出されていない場合には（ステップＳ９０７でＮＯ）、ステップＳ９１１に処理を進める。
【００６１】
ステップＳ９０９において、ＥＣＵ１３は、車両１０の現在位置を起点とし、更に、同じ情報処理サイクルのステップＳ０５（図７参照）で算出された距離及び姿勢角、即ち、パターン光の位置から算出された距離及び姿勢角を積分演算の起点として設定する。従って、距離及び姿勢角は、パターン光３２ａから算出された距離及び姿勢角に設定される。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両１０の移動量算出の起点が新たに設定される。つまり、この位置を起点として新たに車両１０の移動量の積分演算を開始する。
【００６２】
ステップＳ９１１において、ＥＣＵ１３は、車両１０の現在位置を起点とし、更に、前回の情報処理サイクルで採用した距離及び姿勢角を積分演算の起点として設定する。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両１０の移動量算出の起点が新たに設定される。つまり、この位置を起点として新たに車両１０の移動量の積分演算を開始する。その後、図７のステップＳ１１に処理を進める。
【００６３】
つまり、車両１０の加速度α１が閾値加速度αthより大きい場合（ステップＳ９０５でＹＥＳの場合）には、車両１０の距離及び姿勢角が大きく変動し、これに伴って、カメラ１２の距離及び姿勢角が大きく変動するので、特徴点の位置座標を正確に求められなくなる可能性が高い。従って、このような場合には姿勢変化量算出部２４は、距離及び姿勢角の変化量を算出せず、パターン光から算出した距離及び姿勢角（ステップＳ９０９）、或いは、前回の情報処理サイクルで採用した距離及び姿勢角（ステップＳ９１１）を起点として設定する。即ち、車両１０の初期位置、距離及び姿勢角を、その時点の位置、距離及び姿勢角に設定して、距離及び姿勢角の変化量の加算を開始する。こうすることにより、距離及び姿勢角に大きな誤差が生じることを防止し、車両１０の移動量の算出の精度が低下することを防止する。
【００６４】
一方、図８のステップＳ９１３において、ＥＣＵ１３は、現在の設定されている起点を維持する。車両１０の加速度α１が閾値加速度αth以下である場合（ステップＳ９０５でＮＯの場合）には、現在の起点を維持しても車両１０の移動量の算出に大きな誤差は発生しないものと判断されるので、起点を維持する。その後、図７のステップＳ１１に処理を進める。
【００６５】
上記の処理をまとめると、以下の通りである。カメラ１２を用いて路面に存在する特徴点を含む画像を撮影し、この特徴点の時間的な変化から車両１０の移動量（６自由度の移動量）を算出する場合には、カメラ１２と路面との位置関係が一定であることが条件となる。しかし、実際にはカメラ１２は車両１０に搭載されるので、該車両１０の走行状態、或いは路面環境に応じて路面との相対的な位置が時々刻々と変化する。従って、カメラ１２と路面との間の位置関係、即ち、距離及び姿勢角を時間経過に伴って更新する必要がある。
【００６６】
ここで、距離及び姿勢角の測定方法として、投光器１１より投光したパターン光３２ａを用いる方法（図７のＳ０５の処理）がある。従って、各情報処理サイクル毎に常にパターン光３２ａを用いて距離及び姿勢角を算出し、更に、特徴点の時間的な変化を検出すれば、車両１０の移動量を算出でき、ひいては車両１０の自己位置を算出できる。しかし、路面がコンクリートの場合等、路面が平坦でパターン光３２ａをよく反射する状況下であればこの方法は有効であるが、実際の路面は凹凸が多く存在するアスファルトの路面であることが多い。この場合には、パターン光３２ａを用いるよりも、特徴点の時間的な変化に基づいて、距離及び姿勢角の変化量を算出し、起点として設定されている距離及び姿勢角に対して、変化量を逐次加算する処理（積分演算）を実行する方が距離及び姿勢角を高精度に求めることができる。
【００６７】
そこで、本実施形態では、初回にパターン光３２ａを用いた距離及び姿勢角の算出を行ってこれを起点とし、それ以後は、特徴点の時間的な変化に基づく距離及び姿勢角の変化量を算出して、この算出結果を逐次加算することにより、距離及び姿勢角を最新なものに更新する。更に、車両１０の加速度α１（挙動量）が大きくなった場合には、特徴点の検出精度が低下するので、特徴点に基づいて算出される距離及び姿勢角の変化量の精度が低下する。従って、このような条件下、即ち、車両１０の加速度α１の絶対値が閾値加速度αthを上回った場合にはパターン光３２ａを用いた距離及び姿勢角を用いて車両１０の移動量を算出することにより、車両１０の自己位置の算出精度を向上させるようにしている。
【００６８】
次に、図９に示すタイミングチャートを参照して、車両１０の加速度α１の変化と、新たな起点を設定するタイミングとの関係について説明する。図９（ａ）は、距離及び姿勢角の起点を設定するタイミングを示す図、（ｂ）は車両の加速度α１を示す図である。また、横軸は時間を示しており、一つの目盛りが１回の情報処理サイクルを示している。
【００６９】
いま、図９（ｂ）に示すように、時刻ｔ１にて車両が加速を開始して一定速度に達し、その後、時刻ｔ４にて減速を開始して停止する場合を想定する。時刻ｔ１にて、車両１０の加速を開始するので、図９（ｂ）に示すように加速度α１は上昇する。その後、時刻ｔ２にてピークとなり、加速度α１は下降し時刻ｔ３にてほぼ０となる。つまり、一定速度となる。そして、時刻ｔ４にて減速を開始することにより加速度α１が負の方向に上昇し、時刻ｔ５にてピークとなり、時刻ｔ６にて加速度α１はほぼ０となる。つまり、車両１０が停止する。
【００７０】
また、車両１０の加速度α１に対して、閾値加速度αthが設定されており、加速度αが閾値加速度αthを上回った場合、即ち、「α１＞αth」または「α１＜−αth」となった場合に、距離及び姿勢角の起点を設定する処理を行う。つまり、図９（ａ）に示すＴ１，Ｔ２の時間帯における各情報処理サイクルでは、パターン光による距離及び姿勢角を算出し、これを起点として設定する。
【００７１】
このようにして、本願発明の第１実施形態に係る自己位置算出装置１００では、以下の作用効果が得られる。即ち、車両１０を加速、或いは減速することにより、加速度α１が大きくなり、これに起因して車両１０の距離及び姿勢角が大きく変化する場合には、特徴点の時間変化に基づく距離及び姿勢角の積分演算を停止する。そして、パターン光を用いた距離及び姿勢角が求められている場合には、この距離及び姿勢角を新たな起点として設定する。つまり、自己位置算出部２６は、車両１０の加速度（挙動量）が閾値加速度（閾値挙動量）よりも大きい場合、そのときの車両１０の現在位置と距離及び姿勢角を、車両１０の初期位置と距離及び姿勢角に設定し、距離及び姿勢角の変化量の加算を開始する。また、パターン光を用いた距離及び姿勢角が求められていない場合には、前回のサイクルで更新された距離及び姿勢角を新たな起点として設定する。その結果、距離及び姿勢角の算出結果に大きな誤差が生じることを防止できる。このため、車両１０の移動量の推定精度を著しく向上させることができる。
【００７２】
また、挙動量検出部３６は、車両の前後方向の加速度を挙動量として検出するので、距離及び姿勢角の算出精度を向上させることができる。更に、車両１０の移動距離の推定精度を著しく向上させることができる。
【００７３】
第１実施形態では、車両１０の挙動量の一例として、前後方向の加速度α１を用いる場合を例に挙げた。本発明は、その他の挙動量として、前述したように、横方向の移動速度、或いは鉛直方向の移動速度を挙動量として用いることができる。
【００７４】
即ち、車両１０の横方向速度を用いる場合には、車両１０のロール角が、車両１０が停止している状態と比較して、０．０５［rad］変化するときの車両１０の横方向加速度Ｙｇ［ｍ/sec2］を予め算出しておき、この加速度となる車速Ｖ［ｍ/sec］に応じたヨーレートγ［rad/sec］を閾値とすることができる。従って、挙動量検出部３６は、車両の左右方向の移動速度を前記挙動量として検出することにより、距離及び姿勢角の算出精度を向上させることができる。更に、車両１０の移動距離の推定精度を著しく向上させることができる。
【００７５】
また、鉛直方向の移動量を用いる場合には、ストロークセンサで検出される鉛直方向の移動距離データを微分演算することにより、鉛直方向の移動速度を求める。この際、車両１０の移動量算出の誤差を５％以内に抑えるために、路面からカメラ１２までの距離（路面を基準としたカメラ１２の高さ）が１秒間に５％変化する速度を閾値とすることが望ましい。例えば、路面からカメラ１２までの距離が１ｍである場合には、閾値移動速度（閾値挙動量）を０．０５［ｍ／ｓ］とすればよい。
【００７６】
こうすることにより、車両１０が鉛直方向に移動し、その移動速度が閾値移動速度に達した場合には、パターン光を用いた距離及び姿勢角を新たな基点として設定し、車両１０の移動量の演算を開始するので、車両１０の移動距離の推定精度を著しく向上させることができる。
【００７７】
また、第１実施形態では、姿勢角算出部２２による姿勢角の算出ができない場合には、直前に算出された姿勢角を用いるので、大きな誤差を含むことなく車両の位置を算出することができる。更に、予め設定した姿勢角の初期値（姿勢角初期値）を用いても同様に、大きな誤差を含むことなく車両の位置を算出することが可能となる。
【００７８】
［第２実施形態の説明］
次に、本発明に係る自己位置算出装置１００の第２実施形態について説明する。前述した第１実施形態では、情報処理サイクルの初回にパターン光を用いた距離及び姿勢角の算出を行い、その後、特徴点を用いた距離及び姿勢角の更新処理を行い、更に、車両１０の加速度α１が大きくなった場合（閾値加速度αthを超えた場合）に、パターン光を用いた距離及び姿勢角の起点を設定することについて説明した。
【００７９】
これに対して、第２実施形態では、予め設定した周期（第１の周期）で周期的にパターン光を用いた距離及び姿勢角を算出し、この距離及び姿勢角に起点を設定する点で第１実施形態と相違する。具体的には、例えば、情報処理サイクルの１０回毎にパターン光を用いた距離及び姿勢角を算出し、距離及び姿勢角の起点を設定する処理を行う。装置構成は、第１実施形態に示した図１と同様であるので、説明を省略する。
【００８０】
以下、第２実施形態に係る自己位置算出装置１００の作用を図１０に示すタイミングチャートを参照して説明する。図１０（ａ）は、パターン光を用いて距離及び姿勢角を算出するタイミング（距離及び姿勢角をリセットするタイミング）を示す図、（ｂ）は周期的に距離及び姿勢角を算出するタイミングを示す図、（ｃ）は車両１０の前後方向の加速度α１を示す図である。また、横軸は時間を示しており、一つの目盛りが１回の情報処理サイクルを示している。
【００８１】
図１０（ｂ）に示すように、車両１０の加速度α１が小さい場合（図１０（ｃ）とは異なり加速度α１が変化しない場合）には、一定の周期でパターン光を用いた距離及び姿勢角の算出を行う。即ち、図１に示した自己位置算出部２６にて演算される距離及び姿勢角は、一定の周期でリセットされ、パターン光を用いた距離及び姿勢角により起点が設定される。
【００８２】
また、図１０（ｃ）に示すように、時刻ｔ１１にて車両１０が加速を開始する場合には、時刻ｔ１１にて加速度α１が徐々に大きくなる。その後、加速度α１がピークとなり、加速度α１は下降し時刻ｔ１２にてほぼ０となる。つまり、ほぼ一定の速度となる。そして、時刻ｔ１３にて加速度α１が負の方向に上昇し、時刻ｔ１４にて加速度α１はほぼ０となる。つまり、車両１０が停止する。
【００８３】
そして、加速度α１に対して、閾値加速度αthが設定されており、加速度α１が閾値加速度αthを上回った場合、即ち、「α１＞αth」または「α１＜−αth」となった場合に、距離及び姿勢角の起点を設定する処理を行う。つまり、図１０（ａ）に示すＴ１１，Ｔ１２の時間帯において積分演算を停止して、パターン光による距離及び姿勢角の起点の設定を行う。従って、周期的に設定される時刻（図１０（ｂ）に示すタイミング）に加え、時間Ｔ１１，Ｔ１２において、距離及び姿勢角の起点を設定する処理を行う。
【００８４】
そして、上記のように距離及び姿勢角の起点を設定することにより、図１０（ｄ）に示すように、車両１０の移動量をより正確に推定することが可能になる。図１０（ｄ）において、曲線ｒ１は、車両１０の移動量の真の数値を示し、曲線ｒ２は第２実施形態を採用した場合の車両１０の移動量を示し、曲線ｒ３は第２実施形態を採用せずに、周期的な起点設定のみを用いた場合の車両１０の移動量を示している。
【００８５】
そして、図１０（ｄ）から理解されるように、第２実施形態に係る移動量の算出を採用した場合には、車両１０の移動量がより真の数値に近づいている。
【００８６】
このようにして、第２実施形態に係る自己位置算出装置１００では、前述した第１実施形態と同様の効果を達成できる。即ち、車両１０の前後方向の加速度（挙動量）が大きい場合には、距離及び姿勢角の起点を設定するので、挙動量が大きい場合でも車両１０の移動量を高精度に算出することができる。更に、第２実施形態では、第１の周期でパターン光を用いた距離及び姿勢角の算出を行い、この距離及び姿勢角にて起点を設定するので、車両１０の移動量が大きな誤差を含むことを防止できる。
【００８７】
［第３実施形態の説明］
次に、本発明の第３実施形態について説明する。前述した第２実施形態では、所定の周期でパターン光を用いた距離及び姿勢角の算出を実行し、距離及び姿勢角の起点を設定した。更に、加速度α１が閾値加速度αthを上回った場合に、パターン光を用いた距離及び姿勢角により起点を設定する例について説明した。
【００８８】
これに対して、第３実施形態では、通常時（α１＜αthの場合）には第１の周期でパターン光を用いた距離及び姿勢角の算出を実行し、加速度α１が閾値加速度αthを上回った場合には、第１の周期よりも短い第２の周期でパターン光を用いた距離及び姿勢角の算出を実行する。装置構成は、前述の第１実施形態で説明した図１と同一であるので説明を省略する。
【００８９】
第３実施形態に係る自己位置算出装置１００は、前述した図７に示した処理により実行される。図７におけるステップＳ０９の処理が相違するので、以下、移動量算出の起点設定処理について図１１に示すフローチャートを参照して説明する。
【００９０】
図１１に示すフローチャートは、図８と対比して、ステップＳ９０６ａ、Ｓ９０６ｂの処理が加えられている点で相違する。以下詳細に説明する。
【００９１】
ステップＳ９０１において、ＥＣＵ１３は、今回の情報処理サイクルが初回であるか否かを判断する。そして、初回である場合、即ち、前回の情報処理サイクルのデータが無い場合にはステップＳ９０９に処理を進め、初回でない場合にはステップＳ９０３に処理を進める。
【００９２】
ステップＳ９０３において、挙動量検出部３６は、車両１０に搭載される車速センサで検出される車両速度データを取得し、この車両速度データを微分演算することにより、車両１０の前後方向の加速度α１を検出する。
【００９３】
ステップＳ９０５において、挙動量検出部３６は、加速度α１（挙動量）が予め設定した閾値加速度αth（閾値挙動量）より大きいか否かを判定する。そして、閾値挙動量より大きい場合には（ステップＳ９０５でＹＥＳ）、ステップＳ９０６ｂに処理を進める。一方、閾値挙動量以下である場合には（ステップＳ９０５でＮＯ）、ステップＳ９０６ｂに処理を進める。
【００９４】
ステップＳ９０６ａにおいてＥＣＵ１３は、パターン光を用いた距離及び姿勢の算出周期を第１の周期に設定する。例えば、情報処理サイクル１０回分を第１の周期に設定する。その後、ステップＳ９０６ｃに処理を進める。
【００９５】
ステップＳ９０６ｂにおいてＥＣＵ１３は、パターン光を用いた距離及び姿勢の算出周期を第１の周期よりも短い第２の周期に設定する。例えば、情報処理サイクル２回分を第２の周期に設定する。その後、ステップＳ９０６ｃに処理を進める。
【００９６】
ステップＳ９０６ｃにおいて、ＥＣＵ１３は、検出のタイミングであるか否かを判断する。そして、検出のタイミングである場合にはステップＳ９０７に処理を進め、検出のタイミングでない場合にはステップＳ９１３に処理を進める。ステップＳ９０７以降の処理は、図８と同一であるので、説明を省略する。
【００９７】
次に、図１２に示すタイミングチャートを参照して、路面の勾配変化と、距離及び姿勢角の設定との関係について説明する。図１２（ａ）は、周期を変更して（第２の周期として）距離及び姿勢角の起点を設定する処理のタイミングを示す図、（ｂ）は周期を変更せずに（第１の周期として）距離及び姿勢角の起点を設定する処理のタイミングを示す図、（ｃ）は車両１０の加速度α１を示す図である。また、横軸は時間を示しており、一つの目盛りが１回の情報処理サイクルを示している。
【００９８】
いま、車両１０の加速度α１が小さい場合には、図１２（ｂ）に示すように、第１の周期、一例として、情報処理サイクルの１０回分を１周期として距離及び姿勢角のリセット処理が行われる。具体的には、時刻ｔ２１、ｔ２３、ｔ２５、ｔ２７のタイミングで距離及び姿勢角の起点設定処理が行われる。
【００９９】
いま、図１２（ｃ）に示すように、加速度α１が大きくなり、閾値加速度αthを上回ると、起点設定の周期が第１の周期から第２の周期に変更される（図１１のＳ９０６ｂの処理）。一例として、情報処理サイクルの２回分を１周期として距離及び姿勢角の起点設定処理が行われる。即ち、図１２（ｃ）に示す時刻ｔ２２〜ｔ２３の間でα１がαthを上回っている時間帯において、図１２（ａ）に示すように、起点設定の周期が第２の周期に変更される。
【０１００】
同様に、図１２（ｃ）に示す時刻ｔ２６〜ｔ２７の間で負の方向の加速度α１が閾値加速度−αthを下回っているので、この時間帯においてリセット処理の周期が第２の周期に変更される。
【０１０１】
このようにして、第３実施形態によれば、車両１０の前後方向の加速度α１が大きくならない場合には、第１の周期で距離及び姿勢角の起点設定が行われ、加速度α１が閾値加速度αthを上回った場合に、第１の周期よりも短い第２の周期で距離及び姿勢角の起点設定が行われる。従って、加速度α１が大きいという車両１０の移動距離測定に大きな誤差が生じることが予想される状況下において、パターン光を用いた距離及び姿勢角の算出周期が短くなる。即ち、距離及び算出角の起点設定の周期が短くなる。その結果、距離及び姿勢角の算出結果に大きな誤差が生じることを防止でき、車両１０の移動距離の推定精度を著しく向上させることができる。
【０１０２】
また、第３実施形態では車両１０の挙動量として車両１０の前後方向の加速度を例に挙げて説明したが、本発明はこれに限定されるものではなく、車両１０の横方向速度、或いは車両１０の鉛直方向の移動速度を挙動量として用いることも可能である。更には、前後方向の加速度、横方向の移動速度、及び鉛直方向の移動速度のうち、２以上を組み合わせて車両１０の挙動量とすることも可能である。
【０１０３】
以上、本発明の自己位置算出装置及び自己位置算出方法を図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置き換えることができる。
【０１０４】
例えば、図２では、カメラ１２と投光器１１を車両１０の前面に取り付ける例について示したが、本発明はこれに限定されず、車両１０の側方、後方、真下に向けて設置してもよい。また、上記した各実施形態では車両１０に搭載した場合について記述したが、オートバイや建設機械など、道路の路面或いは壁面上の特徴点を撮像することが可能な移動体すべてに適用可能である。
【符号の説明】
【０１０５】
１０  車両
１１  投光器
１２  カメラ（撮像部）
１３  ＥＣＵ
２１  パターン光抽出部
２２  姿勢角算出部
２３  特徴点検出部
２４  姿勢変化量算出部
２６  自己位置算出部
２７  パターン光制御部
３１  路面
３２ａ，３２ｂ  パターン光
３６  挙動量検出部
１００  自己位置算出装置
【図１】
2015158419.tif 000007
【図２】
2015158419.tif 000008
【図３】
2015158419.tif 000009
【図５】
2015158419.tif 000010
【図６】
2015158419.tif 000011
【図７】
2015158419.tif 000012
【図８】
2015158419.tif 000013
【図９】
2015158419.tif 000014
【図１０】
2015158419.tif 000015
【図１１】
2015158419.tif 000016
【図１２】
2015158419.tif 000017
【図４】
2015158419.tif 000018