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

なお、実施形態において、路面３１上の特徴点は、主に大きさが１ｃｍ以上２ｃｍ以下のアスファルト混合物の粒を想定している。この特徴点を検出するために、カメラ１２の解像度はＶＧＡ（約３０万画素）である。また、路面３１に対するカメラ１２の距離は、おおよそ７０ｃｍである。更に、カメラ１２の撮像方向は、水平面から約４５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)）を用いることができる。

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

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

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

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

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

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

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

また、図１に示す勾配変化算出部３６は、車両１０が走行する路面の勾配データを取得し、この勾配データから路面勾配の変化率を算出する。ここで、路面の勾配データは、一例としてナビゲーションシステムに登録されている路面の勾配データ、及びＧＰＳにより取得される車両１０の現在位置データに基づいて取得することができる。該勾配変化算出部３６は、取得した勾配データに対して微分演算することにより路面の勾配変化率Ｒ１を演算する。そして、勾配変化率Ｒ１の絶対値が予め設定した勾配変化率の所定の閾値Ｒthを上回った場合に、路面が急勾配であると判断する。急勾配であると判断した場合には、姿勢角算出部２２は、パターン光抽出部２１より抽出されるパターン光に基づいて、位置及び姿勢角を算出する処理を行う。

本実施形態では、路面の勾配変化に起因して生じる車両の移動量算出の誤差を５％以下に抑えるために、勾配変化率の所定の閾値Ｒthを、０．０５［rad／sec］に設定する。即ち、車両が１秒間走行する間に路面の勾配が０．０５［rad］より大きく変化した場合に、勾配変化率Ｒ１が勾配変化率の所定の閾値Ｒthに達したものと判断する。勾配変化率は、ＧＰＳデータから取得することができる。また、車載したジャイロセンサの検出値から求めることも可能である。

なお、路面の勾配変化を勾配変化率で判断しているが、これに限定されるものではなく、例えば、一定処理時間内の勾配変化の絶対値でも構わない。

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

初めに、図８のステップＳ０１において、パターン光制御部２７は、投光器１１を制御して、車両周囲の路面３１にパターン光３２ａを投光する。図８では、パターン光３２ａを連続して投光する例を説明する。なお、前述したように、必要に応じてパターン光３２ａを投光するようにしてもよい。

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

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

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

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

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

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

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

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

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

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

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

ステップＳ９０３において、勾配変化算出部３６は、車両１０に搭載されるナビゲーションシステムが有するＧＰＳの情報に基づき、路面の勾配変化率Ｒ１を検出する。この処理では、ＧＰＳより得られる路面勾配のデータを微分演算する。

ステップＳ９０５において、勾配変化算出部３６は、路面の勾配変化率Ｒ１の絶対値が予め設定した勾配変化率の所定の閾値Ｒthより大きいか否かを判断する。そして、勾配変化率の所定の閾値Ｒthより大きい場合には（ステップＳ９０５でＹＥＳ）、ステップＳ９０７に処理を進める。一方、勾配変化率の所定の閾値Ｒth以下である場合には（ステップＳ９０５でＮＯ）、ステップＳ９１３に処理を進める。ここで、本実施形態では、路面変化の影響による車両１０の移動量算出の誤差を５％以下に抑えるために、路面勾配の変化率が０．０５［rad/sec］となった時点で特徴点の初期位置を設定する。即ち、上記の勾配変化率の所定の閾値を０．０５［rad/sec］とする。

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

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

ステップＳ９１１において、ＥＣＵ１３は、車両１０の現在位置を起点とし、更に、前回の情報処理サイクルで採用した距離及び姿勢角を積分演算の起点として設定する。即ち、姿勢角算出部２２により、パターン光から車両の姿勢角を算出できない場合には、直前に算出された姿勢角を用いて、積分演算の起点とする。また、直前に算出された姿勢角の代わりに、予め設定した姿勢角の初期値を用いるようにしてもよい。この距離及び姿勢角を起点として新たな積分演算が開始される。また、車両１０の移動量算出の起点が新たに設定される。つまり、この位置を起点として新たに車両１０の移動量の積分演算を開始する。その後、図８のステップＳ１１に処理を進める。

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

一方、図９のステップＳ９１３において、ＥＣＵ１３は、現在の設定されている起点を維持する。路面の勾配変化率Ｒ１が勾配変化率の所定の閾値Ｒth以下である場合（ステップＳ９０５でＮＯの場合）には、現在の起点を維持しても車両１０の移動量の算出に大きな誤差は発生しないものと判断されるので、起点を維持する。その後、図８のステップＳ１１に処理を進める。

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

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

そこで、本実施形態では、初回にパターン光３２ａを用いた距離及び姿勢角の算出を行ってこれを起点とし、それ以後は、特徴点の時間的な変化に基づく距離及び姿勢角の変化量を算出して、この算出結果を逐次加算することにより、距離及び姿勢角を最新なものに更新する。更に、路面の路面勾配の変化率Ｒ１が大きくなった場合には、特徴点の検出精度が低下するので、特徴点に基づいて算出される距離及び姿勢角の変化量の精度が低下する。従って、このような条件下、即ち、路面勾配の変化率Ｒ１の絶対値が勾配変化率の所定の閾値Ｒthを上回った場合にはパターン光３２ａを用いた距離及び姿勢角を用いて車両１０の移動量を算出することにより、車両１０の自己位置の算出精度を向上させるようにしている。

次に、図１０に示すタイミングチャートを参照して、路面の勾配変化と、新たな起点を設定するタイミングとの関係について説明する。図１０（ａ）は、距離及び姿勢角の起点を設定するタイミングを示す図、（ｂ）は路面の勾配を示す図、（ｃ）は路面の勾配変化率Ｒ１を示す図である。また、横軸は時間を示しており、一つの目盛りが１回の情報処理サイクルを示している。

いま、図１０（ｂ）に示すように、時刻ｔ１にて平坦な道路から上り坂に変化し、その後、時刻ｔ６にて上り坂から平坦な道路に変化する場合を想定する。時刻ｔ１にて、路面の勾配が徐々に大きくなるので、図１０（ｃ）に示すように勾配変化率Ｒ１は上昇する。その後、時刻ｔ２にてピークとなり、勾配変化率Ｒ１は下降し時刻ｔ３にてほぼ０となる。つまり、一定勾配の上り坂となる。そして、時刻ｔ４にて勾配変化率Ｒ１が負の方向に上昇し、時刻ｔ５にてピークとなり、時刻ｔ６にて勾配変化率Ｒ１はほぼ０となる。つまり、平坦な道路になる。

また、勾配変化率Ｒ１に対して、勾配変化率の所定の閾値Ｒthが設定されており、勾配変化率Ｒ１の絶対値が勾配変化率の所定の閾値Ｒthを上回った場合、即ち、「Ｒ１＞Ｒth」または「Ｒ１＜−Ｒth」となった場合に、距離及び姿勢角の起点を設定する処理を行う。つまり、図１０（ａ）に示すＴ１，Ｔ２の時間帯における各情報処理サイクルにおいては、パターン光による距離及び姿勢角を算出し、これを起点として設定する。

このようにして、本願発明の第１実施形態に係る自己位置算出装置１００では、以下の作用効果が得られる。即ち、坂道を走行することにより、路面の勾配変化率Ｒ１の絶対値が大きくなり、これに起因して車両１０の距離及び姿勢角が大きく変化する場合には、特徴点の時間変化に基づく距離及び姿勢角の積分演算を停止する。そして、パターン光を用いた距離及び姿勢角が求められている場合には、この距離及び姿勢角を新たな起点として設定する。即ち、自己位置算出部２６は、路面勾配の変化率が勾配変化率の所定の閾値よりも大きい場合、そのときの車両１０の現在位置と距離及び姿勢角を、車両１０の初期位置と距離及び姿勢角に設定し、距離及び姿勢角の変化量の加算を開始する。また、パターン光を用いた距離及び姿勢角が求められていない場合には、前回のサイクルで更新された距離及び姿勢角を新たな起点として設定する。その結果、距離及び姿勢角の算出結果に大きな誤差が生じることを防止できる。このため、車両１０の移動距離の推定精度を著しく向上させることができる。

また、ナビゲーションシステム等から得られる地図情報に登録されている路面の勾配データにより、路面勾配を取得するので、路面勾配の変化を高精度、且つ迅速に算出することができ、車両１０の移動量測定を高精度に行うことができる。

第１実施形態では、勾配変化算出部３６で使用する路面勾配検出の一例として、ＧＰＳ情報を用いる場合について説明したが、本発明はこれに限定されるものではなく、車載したストロークセンサ或いはジャイロセンサの検出結果に基づいて路面高さを算出し、この路面高さの変化量から路面勾配の変化を求めることも可能である。

この場合には、車両挙動変化の影響による車両１０の移動量算出の誤差を５％以下に抑えるために、車両１０が静止している状態と比較して、路面に対する距離が５％以上変化したときに、特徴点の初期位置を設定すればよい。例えば、路面からカメラ１２までの高さが１．０ｍである場合には、０．０５ｍが閾値となる。なお、路面高さの変化は、ストロークセンサ等により求めることができる。そして、ジャイロセンサやストロークセンサを用いる場合には、ＧＰＳ情報を取得することなく路面勾配を検出できるので、ＧＰＳ情報に含まれない路面勾配を検出でき、より高精度な車両１０の移動量測定が可能となる。

また、路面に投光したパターン光に含まれる行列に各スポット光を結ぶ線分の傾斜角度を用いて路面の勾配を測定することも可能である。パターン光を用いる方法として、図７に示す如くのパターン光が得られた場合には、行方向に並ぶスポット光を結ぶ線分（Ｙ軸方向）と、列方向に並ぶスポット光を結ぶ線分（Ｑ１の方向）の傾き（Ｘ軸方向を０degとしたときの傾斜角度）を算出し、この傾きが平坦な路面を走行している場合と対比して所定角度（例えば、５deg）より大きくなった場合に、路面の勾配が大きくなったものと判断することができる。つまり、路面勾配が大きくなるほど、線分Ｑ１のＸ軸に対する傾きが大きくなるので、この線分Ｑ１の傾きから路面勾配を推定できる。そして、単位時間当たりの路面勾配の変化を算出することにより、勾配変化率を求めることが可能となる。

このように、行方向に並ぶスポット光を結ぶ線分に対する、列方向に並ぶスポット光を結ぶ線分の傾斜角度を算出し、この傾斜角度に基づいて路面勾配を取得することにより、より高精度な路面勾配の変化率の算出が可能となる。

更には、スポット光の間隔ｑ１が変化した場合、例えば、平坦な路面を走行している場合と対比して５％以上変化した場合に、路面の勾配が大きくなったものと判断することができる。即ち、路面勾配が大きくなると、間隔ｑ１の変化量が大きくなるのでこの間隔ｑ１から路面勾配を推定できる。

また、第１実施形態では、姿勢角算出部２２による姿勢角の算出ができない場合には、直前に算出された姿勢角を用いるので、大きな誤差を含むことなく車両の位置を算出することができる。更に、予め設定した姿勢角の初期値（姿勢角初期値）を用いても同様に、大きな誤差を含むことなく車両の位置を算出することが可能となる。

［第２実施形態の説明］
次に、本発明に係る自己位置算出装置１００の第２実施形態について説明する。前述した第１実施形態では、情報処理サイクルの初回にパターン光を用いた距離及び姿勢角の算出を行い、その後、特徴点を用いた距離及び姿勢角の更新処理を行い、更に、路面の勾配変化率Ｒ１が大きくなった場合（勾配変化率の所定の閾値Ｒthを超えた場合）に、パターン光を用いた距離及び姿勢角の起点を設定することについて説明した。

これに対して、第２実施形態では、予め設定した周期（第１の周期）で周期的にパターン光を用いた距離及び姿勢角を算出し、この距離及び姿勢角に起点を設定する点で第１実施形態と相違する。具体的には、例えば、情報処理サイクルの１０回毎にパターン光を用いた距離及び姿勢角を算出し、距離及び姿勢角の起点を設定する処理を行う。装置構成は、第１実施形態に示した図１と同様であるので、説明を省略する。

以下、第２実施形態に係る自己位置算出装置１００の作用を図１１に示すタイミングチャートを参照して説明する。図１１（ａ）は、パターン光を用いて距離及び姿勢角を算出するタイミング（距離及び姿勢角をリセットするタイミング）を示す図、（ｂ）は周期的に距離及び姿勢角を算出するタイミングを示す図、（ｃ）は路面の勾配変化率Ｒ１を示す図である。また、横軸は時間を示しており、一つの目盛りが１回の情報処理サイクルを示している。

図１１（ｂ）に示すように、路面の勾配変化率Ｒ１が低い場合（図１１（ｃ）のように勾配が変化しない場合）には、一定の周期でパターン光を用いた距離及び姿勢角の算出を行う。即ち、図１に示した姿勢変化量算出部２４にて演算される距離及び姿勢角は、一定の周期でリセットされ、パターン光を用いた距離及び姿勢角により起点が設定される。

また、図１１（ｃ）に示すように、時刻ｔ１１にて平坦な道路から上り坂に変化する場合には、時刻ｔ１１にて路面の勾配が徐々に大きくなるので、勾配変化率Ｒ１は上昇する。その後、勾配変化率Ｒ１がピークとなり、勾配変化率Ｒ１は下降し時刻ｔ１２にてほぼ０となる。つまり、一定勾配の上り坂となる。そして、時刻ｔ１３にて勾配変化率Ｒ１が負の方向に上昇し、時刻ｔ１４にて勾配変化率Ｒ１はほぼ０となる。つまり、平坦な道路になる。

そして、勾配変化率Ｒ１に対して、勾配変化率の所定の閾値Ｒthが設定されており、勾配変化率Ｒ１の絶対値が勾配変化率の所定の閾値Ｒthを上回った場合、即ち、「Ｒ１＞Ｒth」または「Ｒ１＜−Ｒth」となった場合に、距離及び姿勢角の起点を設定する処理を行う。つまり、図１１（ａ）に示すＴ１１，Ｔ１２の時間帯において積分演算を停止して、パターン光による距離及び姿勢角の起点の設定を行う。従って、周期的に設定される時刻（図１１（ｂ）に示すタイミング）に加え、時間Ｔ１１，Ｔ１２において、距離及び姿勢角の起点を設定する処理を行う。

そして、上記のように距離及び姿勢角の起点を設定することにより、図１１（ｄ）に示すように、車両１０の移動量をより正確に推定することが可能になる。図１１（ｄ）において、曲線ｒ１は、車両１０の移動量の真の数値を示し、曲線ｒ２は第２実施形態を採用した場合の車両１０の移動量を示し、曲線ｒ３は第２実施形態を採用せずに、周期的な起点設定のみを用いた場合の車両１０の移動量を示している。

そして、図１１（ｄ）から理解されるように、第２実施形態に係る移動量の算出を採用した場合には、車両１０の移動量がより真の数値に近づいている。

このようにして、第２実施形態に係る自己位置算出装置１００では、前述した第１実施形態と同様の効果を達成できる。更に、第２実施形態では、第１の周期でパターン光を用いた距離及び姿勢角の算出を行い、この距離及び姿勢角にて起点を設定するので、路面勾配の変化率が大きい場合に高精度な移動距離の推定が可能になる。更に、その他の要因により距離及び姿勢角に誤差が生じた場合でも、第１の周期で起点を設定するので、車両１０の移動距離に大きな誤差が生じることを回避できる。

［第３実施形態の説明］
次に、本発明の第３実施形態について説明する。前述した第２実施形態では、所定の周期でパターン光を用いた距離及び姿勢角の算出を実行し、距離及び姿勢角の起点を設定した。更に、路面勾配の変化率Ｒ１が勾配変化率の所定の閾値Ｒthを上回った場合に、パターン光を用いた距離及び姿勢角により起点を設定する例について説明した。

これに対して、第３実施形態では、通常時（Ｒ１＜Ｒthの場合）には第１の周期でパターン光を用いた距離及び姿勢角の算出を実行し、路面の勾配変化率Ｒ１の絶対値が勾配変化率の所定の閾値Ｒthを上回った場合には、第１の周期よりも短い第２の周期でパターン光を用いた距離及び姿勢角の算出を実行する。装置構成は、前述の第１実施形態で説明した図１と同一であるので説明を省略する。

第３実施形態に係る自己位置算出装置１００は、前述した図８に示した処理により実行される。図８におけるステップＳ０９の処理が相違するので、以下、移動量算出の起点設定処理について図１２に示すフローチャートを参照して説明する。

図１２に示すフローチャートは、図９と対比して、ステップＳ９０６ａ、Ｓ９０６ｂの処理が加えられている点で相違する。以下詳細に説明する。

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

ステップＳ９０３において、勾配変化算出部３６は、車両１０に搭載されるナビゲーションシステムが有するＧＰＳの情報に基づき、路面の勾配変化率Ｒ１を検出する。この処理では、ＧＰＳより得られる路面勾配のデータを微分演算する。

ステップＳ９０５において、勾配変化算出部３６は、路面の勾配変化率Ｒ１の絶対値が予め設定した勾配変化率の所定の閾値Ｒｔｈより大きいか否かを判定する。そして、勾配変化率の所定の閾値Ｒｔｈより大きい場合には（ステップＳ９０５でＹＥＳ）、ステップＳ９０６ａに処理を進める。一方、勾配変化率の所定の閾値Ｒｔｈ以下である場合には（ステップＳ９０５でＮＯ）、ステップＳ９０６ｂに処理を進める。

ステップＳ９０６ａにおいてＥＣＵ１３は、パターン光を用いた距離及び姿勢の算出周期を第１の周期に設定する。例えば、情報処理サイクル１０回分を第１の周期に設定する。その後、ステップＳ９０６ｃに処理を進める。

ステップＳ９０６ｂにおいてＥＣＵ１３は、パターン光を用いた距離及び姿勢の算出周期を第１の周期よりも短い第２の周期に設定する。例えば、情報処理サイクル２回分を第２の周期に設定する。その後、ステップＳ９０６ｃに処理を進める。

ステップＳ９０６ｃにおいて、ＥＣＵ１３は、検出のタイミングであるか否かを判断する。そして、検出のタイミングである場合にはステップＳ９０７に処理を進め、検出のタイミングでない場合にはステップＳ９１３に処理を進める。ステップＳ９０７以降の処理は、図９と同一であるので、説明を省略する。

次に、図１３に示すタイミングチャートを参照して、路面の勾配変化と、距離及び姿勢角の設定との関係について説明する。図１３（ａ）は、周期を変更して（第２の周期として）距離及び姿勢角の起点を設定する処理のタイミングを示す図、（ｂ）は周期を変更せずに（第１の周期として）距離及び姿勢角の起点を設定する処理のタイミングを示す図、（ｃ）は路面の勾配変化率Ｒ１を示す図である。また、横軸は時間を示しており、一つの目盛りが１回の情報処理サイクルを示している。

いま、路面の勾配変化率Ｒ１が大きく変化しない場合には、図１３（ｂ）に示すように、第１の周期、一例として、情報処理サイクルの１０回分を１周期として距離及び姿勢角のリセット処理が行われる。具体的には、時刻ｔ２１、ｔ２３、ｔ２５、ｔ２７のタイミングで距離及び姿勢角の起点設定処理が行われる。

いま、図１３（ｃ）に示すように、路面の勾配変化率Ｒ１が増加し、その絶対値が勾配変化率の所定の閾値Ｒthを上回ると、起点設定の周期が第１の周期から第２の周期に変更される（図１２のＳ９０６ｂの処理）。一例として、情報処理サイクルの２回分を１周期として距離及び姿勢角の起点設定処理が行われる。即ち、図１３（ｃ）に示す時刻ｔ２２〜ｔ２３の間でＲ１がＲthを上回っている時間帯において、図１３（ａ）に示すように、起点設定の周期が第２の周期に変更される。

同様に、図１３（ｃ）に示す時刻ｔ２６〜ｔ２７の間で勾配変化率Ｒ１の絶対値が勾配変化率の所定の閾値Ｒthを上回っているので、この時間帯においてリセット処理の周期が第２の周期に変更される。

このようにして、第３実施形態によれば、路面の勾配変化率Ｒ１が大きくならない場合には、第１の周期で距離及び姿勢角の起点設定が行われ、勾配変化率Ｒ１の絶対値が勾配変化率の所定の閾値Ｒthを上回った場合に、第１の周期よりも短い第２の周期で距離及び姿勢角の起点設定が行われる。従って、勾配変化率Ｒ１が大きいという車両１０の移動距離測定に大きな誤差が生じることが予想される状況下において、パターン光を用いた距離及び姿勢角の算出周期が短くなる。即ち、距離及び算出角の起点設定の周期が短くなる。従って、距離及び姿勢角の算出結果に大きな誤差が生じることを防止でき、車両１０の移動距離の推定精度を著しく向上させることができる。

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

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

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

Claims (7)

1. 車両周囲の路面にパターン光を投光する投光器と、
   前記車両に搭載され、前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する撮像部と、
   路面の勾配を取得して路面勾配の変化を算出する勾配変化算出部と、
   前記撮像部により取得された画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出部と、
   前記撮像部により取得された画像の前記路面上の複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出部と、
   前記車両の初期位置及び、前記車両の初期位置にて前記姿勢角算出部で検出された姿勢角に、前記姿勢変化量を継続して加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出部と、
   を備え、
   前記自己位置算出部は、路面勾配の変化が所定の閾値よりも大きい場合、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出装置。
2. 前記自己位置算出部は、路面勾配の変化が所定の閾値以下の場合、第１の周期で、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して、前記姿勢変化量の加算を開始し、
   路面勾配の変化が所定の閾値よりも大きい場合、前記第１の周期よりも短い第２の周期で、そのときの前記車両の現在位置及び前記姿勢角算出部による前記車両の姿勢角に対して、前記姿勢変化量の加算を開始する
   ことを特徴とする請求項１に記載の自己位置算出装置。
3. 前記勾配変化算出部は、路面高さの変化量から路面の勾配変化を求めることを特徴とする請求項１または２に記載の自己位置算出装置。
4. 前記パターン光は、行列方向に並ぶ光からなり、前記勾配変化算出部は、行方向に並ぶ光の線分に対する、列方向に並ぶ光の線分の傾斜度合から、路面の勾配変化を求めることを特徴とする請求項１または２に記載の自己位置算出装置。
5. 前記自己位置算出部は、前記姿勢角算出部により、パターン光から車両の姿勢角を算出できない場合には、予め設定した姿勢角初期値を用いることを特徴とする請求項１〜４のいずれか１項に記載の自己位置算出装置。
6. 前記自己位置算出部は、前記姿勢角算出部により、パターン光から車両の姿勢角を算出できない場合には、直前に算出された姿勢角を用いることを特徴とする請求項１〜４のいずれか１項に記載の自己位置算出装置。
7. 車両周囲の路面にパターン光を投光する投光手順と、
   前記パターン光が投光された領域を含む車両周囲の路面を撮像して画像を取得する手順と、
   路面の勾配を取得して路面勾配の変化を算出する勾配変化算出手順と、
   前記画像における前記パターン光の位置から、前記路面に対する車両の姿勢角を算出する姿勢角算出手順と、
   前記画像に含まれる複数の特徴点の時間変化に基づいて、前記車両の姿勢変化量を算出する姿勢変化量算出手順と、
   前記車両の初期位置及び、前記車両の初期位置にて前記姿勢角算出部で検出された姿勢角に、前記姿勢変化量を継続して加算してゆくことで、前記車両の現在位置及び姿勢角を算出する自己位置算出手順と、
   を備え、
   前記自己位置算出手順は、路面勾配の変化が所定の閾値よりも大きい場合、そのときの前記車両の現在位置及び前記パターン光の位置から算出された前記車両の姿勢角に対して、前記姿勢変化量の加算を開始する
   ことを特徴とする自己位置算出方法。