存在確率分布推定装置及びプログラム
【課題】道路情報外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定する。
【解決手段】推測航法移動量分布算出部２０で、ヨーレイトθｏｂｓ、車速Ｖｏｂｓ、各センサの推定計測誤差σθ及びσＶに基づいて、自車両の移動量分布を算出し、推測航法軌跡算出部２２で、θｏｂｓ及びＶｏｂｓの時系列データに基づいて、自車両の走行軌跡の形状を算出し、ＭＭ尤度分布算出部２４で、地図情報の全グリッドについて、道路リンク上のＭＭ尤度、またはそれより低い道路リンク外のＭＭ尤度を算出したＭＭ尤度分布を得て、第１分布算出部２６で、前タイムステップの第２存在確率分布と移動量分布とを掛け合わせて、第１存在確率分布を算出し、第２分布算出部２８で、第１存在確率分布とＭＭ尤度分布とを掛け合わせて算出される第２存在確率分布を、現在の自車位置の存在確率分布として推定する。
【解決手段】推測航法移動量分布算出部２０で、ヨーレイトθｏｂｓ、車速Ｖｏｂｓ、各センサの推定計測誤差σθ及びσＶに基づいて、自車両の移動量分布を算出し、推測航法軌跡算出部２２で、θｏｂｓ及びＶｏｂｓの時系列データに基づいて、自車両の走行軌跡の形状を算出し、ＭＭ尤度分布算出部２４で、地図情報の全グリッドについて、道路リンク上のＭＭ尤度、またはそれより低い道路リンク外のＭＭ尤度を算出したＭＭ尤度分布を得て、第１分布算出部２６で、前タイムステップの第２存在確率分布と移動量分布とを掛け合わせて、第１存在確率分布を算出し、第２分布算出部２８で、第１存在確率分布とＭＭ尤度分布とを掛け合わせて算出される第２存在確率分布を、現在の自車位置の存在確率分布として推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、存在確率分布推定装置及びプログラムに係り、特に、道路情報を含む地図上における移動体位置の存在確率分布を推定する存在確率分布推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、移動体の位置を検出するナビゲーション装置において、移動体の挙動情報を検出する自律センサと、自律センサによる検出情報を累積して移動体の慣性測位位置を検出し、時刻ｔの慣性測位位置の誤差分散、自律センサのセンサ誤差分散及び計算誤差を、移動体の移動モデルに基づいて更新式に適用して、時刻ｔ＋１の誤差分散を漸化的に算出するナビゲーション装置が提案されている（例えば、特許文献１参照）。
【０００３】
また、地図データ内の道路形状と車両の走行軌跡の形状との一致度に基づいて、車両位置を更新するナビゲーション装置が提案されている（例えば、特許文献２参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００８−２０３６５号公報
【特許文献２】特開２００９−２８７９２５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１の技術では、誤差分布の範囲が時刻と共に累積し、誤差範囲が拡大する、という問題がある。
【０００６】
この誤差分布の拡大を抑制するために、特許文献１の誤差分布の推定手法に特許文献２のマップマッチングの手法を適用した場合を考える。マップマッチングでは道路幅方向の誤差の累積を抑制できるため、誤差分布の拡大の抑制効果はあると考えられるが、道路リンク外での移動体の位置分散の算出が不可能である、という問題がある。
【０００７】
本発明は、上記の問題点を解決するためになされたもので、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定することができる存在確率分布推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するために、本発明の存在確率分布推定装置は、移動体の運動状態を検出する検出手段と、前記検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段と、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段と、前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段と、前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段と、を含んで構成されている。
【０００９】
本発明の存在確率分布推定装置によれば、検出手段が、移動体の運動状態を検出し、軌跡算出手段が、検出手段により検出された移動体の運動状態に基づいて、移動体の移動軌跡を算出する。そして、尤度分布算出手段が、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、尤度を、領域が道路情報の示す道路上に相当する場合には、道路情報と軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、領域が道路情報の示す道路外に相当する場合には、第１尤度より低い第２尤度とした分布を算出する。また、第１分布算出手段は、検出手段により検出された移動体の運動状態、及び過去に推定された領域毎の移動体の存在確率分布に基づいて、領域毎の移動体の第１存在確率分を算出する。そして、第２分布算出手段が、尤度分布算出手段により算出された尤度の分布、及び第１分布算出手段により算出された第１存在確率分布に基づいて、領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の領域毎の移動体の存在確率分布として推定する。
【００１０】
このように、検出手段により検出された移動体の移動量分布に基づく移動体の第１存在確率分布と、マップマッチングにより算出した道路上の第１尤度、及び第１尤度より低い道路外の第２尤度を領域毎に算出した尤度の分布とに基づいて、移動体の第２存在確率分布を算出し、これを移動体の存在確率分布として推定するため、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した存在確率分布を推定することができる。
【００１１】
また、本発明の存在確率分布推定プログラムは、コンピュータを、移動体の運動状態を検出する検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段、前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段、及び前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段として機能させるためのプログラムである。
【００１２】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００１３】
以上説明したように、本発明の存在確率推定装置及びプログラムによれば、検出手段により検出された移動体の移動量分布に基づく移動体の第１存在確率分布と、マップマッチングにより算出した道路上の第１尤度、及び第１尤度より低い道路外の第２尤度を領域毎に算出した尤度の分布とに基づいて、移動体の第２存在確率分布を算出し、これを移動体の存在確率分布として推定するため、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００１４】
【図１】第１の実施の形態に係る存在確率分布推定装置の機能的構成を示すブロック図である。
【図２】走行軌跡と道路リンクとのマップマッチングを説明するための図である。
【図３】第１の実施の形態に係る存在確率分布推定装置のコンピュータにおける存在確率分布推定処理ルーチンの内容を示すフローチャートである。
【図４】ＭＭ尤度分布の算出を説明するための図である。
【図５】第２の実施の形態に係る存在確率分布推定装置の機能的構成を示すブロック図である。
【図６】第２の実施の形態に係る存在確率分布推定装置のコンピュータにおける存在確率分布推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、道路情報を含む地図上における自車位置の存在確率分布を推定する存在確率分布推定装置に、本発明を適用した場合を例に説明する。
【００１６】
図１に示すように、第１の実施の形態に係る存在確率分布推定装置１０は、自車両のヨーレイトを検出するヨーレイトセンサ１２と、自車両の速度を検出する車速センサ１４と、地図情報が格納された地図データベース（地図ＤＢ）１６と、自車位置の存在確率分布を推定するコンピュータ１８とを備えている。
【００１７】
地図ＤＢ１６に格納された地図情報は、道路リンク情報を含む。また、地図情報が示す地図は、二次元グリッドで構成され、１グリッド（１ピクセル）を１単位とする。地図上の位置はグリッド座標（ｘ，ｙ）で表される。
【００１８】
コンピュータ１８は、ＣＰＵ、後述する存在確率分布推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。
【００１９】
このコンピュータ１８を以下で説明する存在確率分布推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、ヨーレイトセンサ１２及び車速センサ１４の検出値に基づいて、自車両の移動量分布を算出する推測航法移動量分布算出部２０と、ヨーレイトセンサ１２及び車速センサ１４の検出値に基づいて、自車両の走行軌跡を算出する推測航法軌跡算出部２２と、自車両の走行軌跡及び地図情報に基づいて、マップマッチング尤度（ＭＭ尤度）の分布を算出するＭＭ尤度分布算出部２４と、移動量分布及び前タイムステップの第２存在確率分布に基づいて、第１存在確率分布を算出する第１分布算出部２６と、第１存在確率分布及びＭＭ尤度分布に基づいて、第２存在確率分布を算出する第２分布算出部２８とを含んだ構成で表すことができる。
【００２０】
なお、推測航法移動量分布算出部２０及び第１分布算出部２６が本発明の第１分布算出手段の一例である。
【００２１】
推測航法移動量分布算出部２０は、ヨーレイトセンサ１２から取得したヨーレイトθｏｂｓ及び車速センサ１４から取得した車速Ｖｏｂｓに基づいて、自車両の移動量分布ｆ（△ｘ，△ｙ）を算出する。具体的には、ヨーレイトの真値がθである確率をｆｙａｗ（θ）、車速の真値がＶである確率をｆｖｅｌ（Ｖ）、移動量の真値が（Δｘ，△ｙ）である確率をｆ（△ｘ，△ｙ）と定義すると、移動量分布ｆ（△ｘ，△ｙ）は、下記（１）式で表わすことができる。
【００２２】
【数１】
【００２３】
また、ヨーレイトセンサ１２によるヨーレイトの計測値をθｏｂｓとし、その誤差分布を平均０、分散σθの正規分布と仮定した場合、下記（２）式が成立する。同様に、車速センサ１４による車速の計測値をＶｏｂｓとし、その誤差分布を平均０、分散σＶの正規分布と仮定した場合、下記（３）が成立する。
【００２４】
【数２】
【００２５】
（１）式に（２）式及び（３）式を代入すると、下記（４）式が得られる。
【００２６】
【数３】
【００２７】
従って、推測航法移動量分布算出部２０は、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１６で検出された車速Ｖｏｂｓを取得し、ヨーレイトセンサ１２及び車速センサ１６の計測精度に基づいて予め求められた推定計測誤差σθ及びσＶと共に、（４）式に代入することにより、自車両の移動量分布を算出する。
【００２８】
推測航法軌跡算出部２２は、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１４で検出された車速Ｖｏｂｓの時系列データを保持し、自車両の走行軌跡の形状を算出する。
【００２９】
ＭＭ尤度分布算出部２４は、推測航法軌跡算出部２２で算出された自車両の走行軌跡の形状及び地図ＤＢ１６に格納された道路リンク情報を含む地図情報に基づいて、地図情報を区画した領域毎のＭＭ尤度を算出する。ＭＭ尤度は、地図上のあるグリッドの道路リンクの確からしさを示す値である。本実施の形態では、１グリッドを１単位としており、グリッド毎にＭＭ尤度が算出される。このＭＭ尤度は、そのグリッドが道路リンク上か道路リンク外かで異なる。以下、道路リンク上のＭＭ尤度、及び道路リンク外のＭＭ尤度に分けて説明する。
【００３０】
道路リンク上のＭＭ尤度は、道路リンク上の全グリッドにおいて、推測航法軌跡算出部２２で算出された走行軌跡の形状と比較を行うことにより算出する。具体的には、まず、図２（ａ）に示すように、推測航法軌跡算出部２２で算出された走行軌跡の形状から自車位置を中心にＭ×Ｎピクセルの走行軌跡ウインドウを切り出す。図２（ａ）中黒く塗りつぶしたグリッドは走行軌跡を表す。次に、同図（ｂ）に示すように、地図情報から、ある道路リンク上のグリッド座標（ｘｌ，ｙｌ）を中心としてＭ×Ｎピクセルの道路リンクウインドウを切り出す。図２（ｂ）中黒く塗りつぶしたグリッドは道路リンクを表す。
【００３１】
そして、切り出した走行軌跡ウインドウと道路リンクウインドウとを比較することで、道路リンク上のグリッド座標（ｘｌ，ｙｌ）上のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を、例えば、下記（５）式に従って算出する。なお、ＭＭ尤度は回転やスケールに対し不変な特徴量を用いて算出するようにしてもよい。
【００３２】
【数４】
【００３３】
ここで、（ｉ，ｊ）は両ウインドウ中における相対的なグリッド座標を示し、Ｉ（ｉ，ｊ）は走行軌跡ウインドウ中のグリッド座標（ｉ，ｊ）が走行軌跡上であれば１、そうでなければ０となる関数を示す。Ｔ（ｉ，ｊ）も同様に、道路リンクウインドウ中のグリッド座標（ｉ，ｊ）が道路リンク上であれば１、そうでなければ０となる関数を示す。また、Ｍ及びＮはそれぞれ両ウインドウのグリッドサイズを示し，１≦ｉ≦Ｍ，１≦ｊ≦Ｎとする。（５）式に従って、道路リンク上にある全てのグリッドについてＭＭ尤度Ｗ（ｘｌ，ｙｌ）を算出する。
【００３４】
次に、道路リンク外のＭＭ尤度について説明する。道路リンク外のグリッド座標（ｘｆ，ｙｆ）におけるＭＭ尤度Ｗ（ｘｆ，ｙｆ）は、道路リンク上のグリッド座標（ｘｌ，ｙｌ）のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を用いて、下記（６）式に従って算出する。
【００３５】
【数５】
【００３６】
ただし、定積分のＬは全道路リンクを示す。また、Ｃは定数を示し、道路リンク外のＭＭ尤度Ｗ（ｘｆ，ｙｆ）が、道路リンク上のＭＭ尤度Ｗ（ｘｌ，ｙｌ）よりも十分に低い値となるように設定する。道路リンク外の全領域に所定のＭＭ尤度を与えることで、自車両が道路リンクから外れたルートを走行している場合でも、自車位置の存在確率分布の算出が可能となる。（６）式に従って、道路リンク外にある全てのグリッドについてＭＭ尤度Ｗ（ｘｆ，ｙｆ）を算出する。これにより、地図情報の全グリッドについてＭＭ尤度（Ｗ（ｘｌ，ｙｌ）またはＷ（ｘｆ，ｙｆ））が算出されたＭＭ尤度分布が得られる。
【００３７】
第１分布算出部２６は、後述する第２分布算出部２８で前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、推測航法移動量分布算出部２０で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）を、下記（７）式に従って算出する。
【００３８】
【数６】
【００３９】
ただし、ｔ＝０の時は、Ｐ２ｔ−１（Ｘ）の代わりとして、ＧＰＳ受信機からのデータなどで推定した測位結果に基づいた測位位置分布など、適当な分布を初期分布として用いことができる。
【００４０】
第２分布算出部２８は、第１分布算出部２６で算出された自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）と、ＭＭ尤度分布算出部２４で算出されたＭＭ尤度Ｗｔ（ｘ，ｙ）とを掛け合わせて、下記（８）式に従って、自車位置の第２存在確率分布を算出する。
【００４１】
【数７】
【００４２】
ＭＭ尤度Ｗｔ（ｘ，ｙ）は、グリッド座標（ｘ，ｙ）が道路リンク上であれば、（５）式により得られるＷ（ｘｌ，ｙｌ）であり、グリッド座標（ｘ，ｙ）が道路リンク外であれば、（６）式により得られるＷ（ｘｆ，ｙｆ）である。このようにして得られた第２存在確率分布Ｐ２ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力すると共に、第１分布算出部２６へ出力する。
【００４３】
次に、図３を参照して、第１の実施の形態の存在確率分布推定装置１０において実行される存在確率分布推定処理ルーチンについて説明する。
【００４４】
ステップ１００で、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１４で検出された車速Ｖｏｂｓを取得する。
【００４５】
次に、ステップ１０２で、上記ステップ１００で取得したヨーレイトθｏｂｓ、及び車速Ｖｏｂｓ、並びにヨーレイトセンサ１２及び車速センサ１６の計測精度に基づいて予め求められた推定計測誤差σθ及びσＶと共に、（４）式に代入することにより、自車両の移動量分布を算出する。
【００４６】
次に、ステップ１０４で、上記ステップ１００で取得したヨーレイトθｏｂｓ、及び車速Ｖｏｂｓの時系列データに基づいて、自車両の走行軌跡の形状を算出する。
【００４７】
次に、ステップ１０６で、図４（ａ）に示すように、上記ステップ１０４で算出された走行軌跡の形状から自車位置を中心にＭピクセル×Ｎピクセルの走行軌跡ウインドウを切り出す。また、地図情報から、ある道路リンク上のグリッド座標（ｘｌ，ｙｌ）を中心としてＭピクセル×Ｎピクセルの道路リンクウインドウを切り出す。そして、切り出した走行軌跡ウインドウと道路リンクウインドウとを比較して、（５）式により、道路リンク上のグリッド座標（ｘｌ，ｙｌ）上のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を算出する（同図（ｂ））。
【００４８】
また、道路リンク外のグリッド座標（ｘｆ，ｙｆ）におけるＭＭ尤度Ｗ（ｘｆ，ｙｆ）を、道路リンク上のグリッド座標（ｘｌ，ｙｌ）のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を用いて、（６）式に従って算出して、地図情報の全グリッドについてＭＭ尤度が算出されたＭＭ尤度分布を得る（同図（ｃ））。
【００４９】
次に、ステップ１０８で、前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、上記ステップ１０２で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐｔ（ｘ，ｙ）を、（７）式に従って算出する。
【００５０】
次に、ステップ１１０で、上記ステップ１０８で算出された自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）と、上記ステップ１０６で算出されたＭＭ尤度Ｗｔ（ｘ，ｙ）とを掛け合わせて、（８）式に従って、自車位置の第２存在確率分布を算出する。本ステップで算出された第２存在確率分布は、次のタイムステップのステップ１０８で用いられる。
【００５１】
次に、ステップ１１２で、上記ステップ１１０で算出された第２存在確率分布Ｐ２ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力して、処理を終了する。
【００５２】
以上説明したように、第１の実施の形態の存在確率分布推定装置によれば、センサの計測誤差を考慮した自車位置の第１存在確率分布と、道路リンク上のＭＭ尤度及び道路リンク上のＭＭ尤度より低い道路リンク外のＭＭ尤度を地図情報のグリッド毎に算出したＭＭ尤度分布とを掛け合わせた自車位置の第２存在確率分布を算出し、これを自車位置の存在確率分布の推定結果とすることにより、道路リンク外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した存在確率分布を推定することができる。
【００５３】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の存在確率分布推定装置の構成において、第１の実施の形態の存在確率分布推定装置１０と同一の構成については、同一符号を付して詳細な説明は省略する。
【００５４】
図５に示すように、第２の実施の形態に係る存在確率分布推定装置２１０は、ヨーレイトセンサ１２と、車速センサ１４と、ＧＰＳ衛星からの衛星信号を受信して擬似距離データを出力するＧＰＳ受信機１５と、地図ＤＢ１６と、自車位置の存在確率分布を推定するコンピュータ２１８とを備えている。
【００５５】
コンピュータ２１８を以下で説明する存在確率分布推定処理ルーチンに従って機能ブロックで表すと、図５に示すように、推測航法移動量分布算出部２０と、推測航法軌跡算出部２２と、ＭＭ尤度分布算出部２４と、擬似距離データに基づいて、ＧＰＳ測位位置分布を算出するＧＰＳ測位位置分布算出部３０と、ＧＰＳ測位位置分布及び前タイムステップの第２存在確率分布に基づいて、第３存在確率分布を算出する第３分布算出部３２と、第３存在確率分布及び移動量分布に基づいて、第１存在確率分布を算出する第１分布算出部２２６と、第２分布算出部２８とを含んだ構成で表すことができる。
【００５６】
ＧＰＳ測位位置分布算出部３０は、ＧＰＳ受信機１５から出力された擬似距離データを取得し、受信位置を測位する。また、擬似距離の残差に基づいた正規分布を誤差分布として算出し、ＧＰＳ測位位置分布を算出する。
【００５７】
また、ＧＰＳ測位位置分布算出部３０は、ＧＰＳ受信機１５から出力された擬似距離データによる測位が可能な場合には、ＧＰＳ測位位置分布を第３分布算出部３２に出力し、遮蔽などによりＧＰＳ測位が不可能な場合、またはＧＰＳ測位の信頼度が所定値以下の場合には、ＧＰＳ測位位置分布を出力しない。
【００５８】
第３分布算出部３２は、第２分布算出部２８で前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、ＧＰＳ測位位置分布算出部３０で算出されたＧＰＳ測位位置分布Ｐｔｇｐｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）を、下記（９）式に従って算出する。なお、ＧＰＳ測位位置分布算出部３０からのＧＰＳ測位位置分布の出力がなかった場合には、第３存在確率分布Ｐ３ｔ（ｘ，ｙ）は、下記（１０）式となる。
【００５９】
【数８】
【００６０】
第１分布算出部２２６は、第３分布算出部３２で算出された自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）と、推測航法移動量分布算出部２０で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）を、下記（１１）式に従って算出する。
【００６１】
【数９】
【００６２】
次に、図６を参照して、第２の実施の形態の存在確率分布推定装置２１０において実行される存在確率分布推定処理ルーチンについて説明する。なお、第１の実施の形態の存在確率分布推定処理ルーチンと同一の処理については、同一の符号を付して詳細な説明は省略する。
【００６３】
ステップ２００で、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１４で検出された車速Ｖｏｂｓ、並びにＧＰＳ受信機１５から出力された擬似距離データを取得する。
【００６４】
次に、ステップ２０２で、上記ステップ２００で取得した擬似距離データに基づいて、受信位置を測位する。また、擬似距離の残差に基づいた正規分布を誤差分布として算出し、ＧＰＳ測位位置分布を算出する。なお、遮蔽などによりＧＰＳ測位が不可能な場合、またはＧＰＳ測位の信頼度が所定値以下の場合には、ＧＰＳ測位位置分布を出力しない。
【００６５】
次に、ステップ１０２で、自車両の移動量分布を算出し、次に、ステップ１０４で、自車両の走行軌跡の形状を算出し、次に、ステップ１０６で、ＭＭ尤度分布を算出する。
【００６６】
次に、ステップ２０４で、前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、上記ステップ２０２で算出されたＧＰＳ測位位置分布Ｐｔｇｐｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）を、（９）式に従って算出する。上記ステップ２０２でＧＰＳ測位位置分布が出力されていない場合には、（１０）式に従って自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）を算出する。
【００６７】
次に、ステップ２０６で、上記ステップ２０４で算出された自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）と、上記ステップ１０２で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）を、（１１）式に従って算出する。
【００６８】
次に、ステップ１１０で、上記ステップ２０６で算出された自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）と、上記ステップ１０６で算出されたＭＭ尤度Ｗｔ（ｘ，ｙ）とを掛け合わせて、（８）式に従って、自車位置の第２存在確率分布を算出する。本ステップで算出された第２存在確率分布は、次のタイムステップのステップ２０４で用いられる。
【００６９】
次に、ステップ１１２で、上記ステップ１１０で算出された第２存在確率分布Ｐ２ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力して、処理を終了する。
【００７０】
以上説明したように、第２の実施の形態の存在確率分布推定装置によれば、ＧＰＳ測位位置分布も用いることで、より精度良く存在確率分布を推定することができる。
【００７１】
なお、上記第１及び第２の実施の形態では、道路リンク外のＭＭ尤度として、道路リンクからの距離が離れるほど低くなるようなＭＭ尤度（（６）式）を算出する場合について説明したが、この場合に限定されず、道路リンク上のＭＭ尤度より、道路リンク外のＭＭ尤度の方が低くなるような尤度であればよい。
【符号の説明】
【００７２】
１０、２１０ 存在確率分布推定装置
１２ ヨーレイトセンサ
１４ 車速センサ
１５ ＧＰＳ受信機
１６ 車速センサ
１８、２１８ コンピュータ
２０ 推測航法移動量分布算出部
２２ 推測航法軌跡算出部
２４ 尤度分布算出部
２６、２２６ 第１分布算出部
２８ 第２分布算出部
３０ ＧＰＳ測位位置分布算出部
３２ 第３分布算出部
【技術分野】
【０００１】
本発明は、存在確率分布推定装置及びプログラムに係り、特に、道路情報を含む地図上における移動体位置の存在確率分布を推定する存在確率分布推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来、移動体の位置を検出するナビゲーション装置において、移動体の挙動情報を検出する自律センサと、自律センサによる検出情報を累積して移動体の慣性測位位置を検出し、時刻ｔの慣性測位位置の誤差分散、自律センサのセンサ誤差分散及び計算誤差を、移動体の移動モデルに基づいて更新式に適用して、時刻ｔ＋１の誤差分散を漸化的に算出するナビゲーション装置が提案されている（例えば、特許文献１参照）。
【０００３】
また、地図データ内の道路形状と車両の走行軌跡の形状との一致度に基づいて、車両位置を更新するナビゲーション装置が提案されている（例えば、特許文献２参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００８−２０３６５号公報
【特許文献２】特開２００９−２８７９２５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１の技術では、誤差分布の範囲が時刻と共に累積し、誤差範囲が拡大する、という問題がある。
【０００６】
この誤差分布の拡大を抑制するために、特許文献１の誤差分布の推定手法に特許文献２のマップマッチングの手法を適用した場合を考える。マップマッチングでは道路幅方向の誤差の累積を抑制できるため、誤差分布の拡大の抑制効果はあると考えられるが、道路リンク外での移動体の位置分散の算出が不可能である、という問題がある。
【０００７】
本発明は、上記の問題点を解決するためになされたもので、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定することができる存在確率分布推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記目的を達成するために、本発明の存在確率分布推定装置は、移動体の運動状態を検出する検出手段と、前記検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段と、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段と、前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段と、前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段と、を含んで構成されている。
【０００９】
本発明の存在確率分布推定装置によれば、検出手段が、移動体の運動状態を検出し、軌跡算出手段が、検出手段により検出された移動体の運動状態に基づいて、移動体の移動軌跡を算出する。そして、尤度分布算出手段が、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、尤度を、領域が道路情報の示す道路上に相当する場合には、道路情報と軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、領域が道路情報の示す道路外に相当する場合には、第１尤度より低い第２尤度とした分布を算出する。また、第１分布算出手段は、検出手段により検出された移動体の運動状態、及び過去に推定された領域毎の移動体の存在確率分布に基づいて、領域毎の移動体の第１存在確率分を算出する。そして、第２分布算出手段が、尤度分布算出手段により算出された尤度の分布、及び第１分布算出手段により算出された第１存在確率分布に基づいて、領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の領域毎の移動体の存在確率分布として推定する。
【００１０】
このように、検出手段により検出された移動体の移動量分布に基づく移動体の第１存在確率分布と、マップマッチングにより算出した道路上の第１尤度、及び第１尤度より低い道路外の第２尤度を領域毎に算出した尤度の分布とに基づいて、移動体の第２存在確率分布を算出し、これを移動体の存在確率分布として推定するため、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した存在確率分布を推定することができる。
【００１１】
また、本発明の存在確率分布推定プログラムは、コンピュータを、移動体の運動状態を検出する検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段、前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段、及び前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段として機能させるためのプログラムである。
【００１２】
なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。
【発明の効果】
【００１３】
以上説明したように、本発明の存在確率推定装置及びプログラムによれば、検出手段により検出された移動体の移動量分布に基づく移動体の第１存在確率分布と、マップマッチングにより算出した道路上の第１尤度、及び第１尤度より低い道路外の第２尤度を領域毎に算出した尤度の分布とに基づいて、移動体の第２存在確率分布を算出し、これを移動体の存在確率分布として推定するため、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００１４】
【図１】第１の実施の形態に係る存在確率分布推定装置の機能的構成を示すブロック図である。
【図２】走行軌跡と道路リンクとのマップマッチングを説明するための図である。
【図３】第１の実施の形態に係る存在確率分布推定装置のコンピュータにおける存在確率分布推定処理ルーチンの内容を示すフローチャートである。
【図４】ＭＭ尤度分布の算出を説明するための図である。
【図５】第２の実施の形態に係る存在確率分布推定装置の機能的構成を示すブロック図である。
【図６】第２の実施の形態に係る存在確率分布推定装置のコンピュータにおける存在確率分布推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、道路情報を含む地図上における自車位置の存在確率分布を推定する存在確率分布推定装置に、本発明を適用した場合を例に説明する。
【００１６】
図１に示すように、第１の実施の形態に係る存在確率分布推定装置１０は、自車両のヨーレイトを検出するヨーレイトセンサ１２と、自車両の速度を検出する車速センサ１４と、地図情報が格納された地図データベース（地図ＤＢ）１６と、自車位置の存在確率分布を推定するコンピュータ１８とを備えている。
【００１７】
地図ＤＢ１６に格納された地図情報は、道路リンク情報を含む。また、地図情報が示す地図は、二次元グリッドで構成され、１グリッド（１ピクセル）を１単位とする。地図上の位置はグリッド座標（ｘ，ｙ）で表される。
【００１８】
コンピュータ１８は、ＣＰＵ、後述する存在確率分布推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。
【００１９】
このコンピュータ１８を以下で説明する存在確率分布推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、ヨーレイトセンサ１２及び車速センサ１４の検出値に基づいて、自車両の移動量分布を算出する推測航法移動量分布算出部２０と、ヨーレイトセンサ１２及び車速センサ１４の検出値に基づいて、自車両の走行軌跡を算出する推測航法軌跡算出部２２と、自車両の走行軌跡及び地図情報に基づいて、マップマッチング尤度（ＭＭ尤度）の分布を算出するＭＭ尤度分布算出部２４と、移動量分布及び前タイムステップの第２存在確率分布に基づいて、第１存在確率分布を算出する第１分布算出部２６と、第１存在確率分布及びＭＭ尤度分布に基づいて、第２存在確率分布を算出する第２分布算出部２８とを含んだ構成で表すことができる。
【００２０】
なお、推測航法移動量分布算出部２０及び第１分布算出部２６が本発明の第１分布算出手段の一例である。
【００２１】
推測航法移動量分布算出部２０は、ヨーレイトセンサ１２から取得したヨーレイトθｏｂｓ及び車速センサ１４から取得した車速Ｖｏｂｓに基づいて、自車両の移動量分布ｆ（△ｘ，△ｙ）を算出する。具体的には、ヨーレイトの真値がθである確率をｆｙａｗ（θ）、車速の真値がＶである確率をｆｖｅｌ（Ｖ）、移動量の真値が（Δｘ，△ｙ）である確率をｆ（△ｘ，△ｙ）と定義すると、移動量分布ｆ（△ｘ，△ｙ）は、下記（１）式で表わすことができる。
【００２２】
【数１】
【００２３】
また、ヨーレイトセンサ１２によるヨーレイトの計測値をθｏｂｓとし、その誤差分布を平均０、分散σθの正規分布と仮定した場合、下記（２）式が成立する。同様に、車速センサ１４による車速の計測値をＶｏｂｓとし、その誤差分布を平均０、分散σＶの正規分布と仮定した場合、下記（３）が成立する。
【００２４】
【数２】
【００２５】
（１）式に（２）式及び（３）式を代入すると、下記（４）式が得られる。
【００２６】
【数３】
【００２７】
従って、推測航法移動量分布算出部２０は、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１６で検出された車速Ｖｏｂｓを取得し、ヨーレイトセンサ１２及び車速センサ１６の計測精度に基づいて予め求められた推定計測誤差σθ及びσＶと共に、（４）式に代入することにより、自車両の移動量分布を算出する。
【００２８】
推測航法軌跡算出部２２は、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１４で検出された車速Ｖｏｂｓの時系列データを保持し、自車両の走行軌跡の形状を算出する。
【００２９】
ＭＭ尤度分布算出部２４は、推測航法軌跡算出部２２で算出された自車両の走行軌跡の形状及び地図ＤＢ１６に格納された道路リンク情報を含む地図情報に基づいて、地図情報を区画した領域毎のＭＭ尤度を算出する。ＭＭ尤度は、地図上のあるグリッドの道路リンクの確からしさを示す値である。本実施の形態では、１グリッドを１単位としており、グリッド毎にＭＭ尤度が算出される。このＭＭ尤度は、そのグリッドが道路リンク上か道路リンク外かで異なる。以下、道路リンク上のＭＭ尤度、及び道路リンク外のＭＭ尤度に分けて説明する。
【００３０】
道路リンク上のＭＭ尤度は、道路リンク上の全グリッドにおいて、推測航法軌跡算出部２２で算出された走行軌跡の形状と比較を行うことにより算出する。具体的には、まず、図２（ａ）に示すように、推測航法軌跡算出部２２で算出された走行軌跡の形状から自車位置を中心にＭ×Ｎピクセルの走行軌跡ウインドウを切り出す。図２（ａ）中黒く塗りつぶしたグリッドは走行軌跡を表す。次に、同図（ｂ）に示すように、地図情報から、ある道路リンク上のグリッド座標（ｘｌ，ｙｌ）を中心としてＭ×Ｎピクセルの道路リンクウインドウを切り出す。図２（ｂ）中黒く塗りつぶしたグリッドは道路リンクを表す。
【００３１】
そして、切り出した走行軌跡ウインドウと道路リンクウインドウとを比較することで、道路リンク上のグリッド座標（ｘｌ，ｙｌ）上のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を、例えば、下記（５）式に従って算出する。なお、ＭＭ尤度は回転やスケールに対し不変な特徴量を用いて算出するようにしてもよい。
【００３２】
【数４】
【００３３】
ここで、（ｉ，ｊ）は両ウインドウ中における相対的なグリッド座標を示し、Ｉ（ｉ，ｊ）は走行軌跡ウインドウ中のグリッド座標（ｉ，ｊ）が走行軌跡上であれば１、そうでなければ０となる関数を示す。Ｔ（ｉ，ｊ）も同様に、道路リンクウインドウ中のグリッド座標（ｉ，ｊ）が道路リンク上であれば１、そうでなければ０となる関数を示す。また、Ｍ及びＮはそれぞれ両ウインドウのグリッドサイズを示し，１≦ｉ≦Ｍ，１≦ｊ≦Ｎとする。（５）式に従って、道路リンク上にある全てのグリッドについてＭＭ尤度Ｗ（ｘｌ，ｙｌ）を算出する。
【００３４】
次に、道路リンク外のＭＭ尤度について説明する。道路リンク外のグリッド座標（ｘｆ，ｙｆ）におけるＭＭ尤度Ｗ（ｘｆ，ｙｆ）は、道路リンク上のグリッド座標（ｘｌ，ｙｌ）のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を用いて、下記（６）式に従って算出する。
【００３５】
【数５】
【００３６】
ただし、定積分のＬは全道路リンクを示す。また、Ｃは定数を示し、道路リンク外のＭＭ尤度Ｗ（ｘｆ，ｙｆ）が、道路リンク上のＭＭ尤度Ｗ（ｘｌ，ｙｌ）よりも十分に低い値となるように設定する。道路リンク外の全領域に所定のＭＭ尤度を与えることで、自車両が道路リンクから外れたルートを走行している場合でも、自車位置の存在確率分布の算出が可能となる。（６）式に従って、道路リンク外にある全てのグリッドについてＭＭ尤度Ｗ（ｘｆ，ｙｆ）を算出する。これにより、地図情報の全グリッドについてＭＭ尤度（Ｗ（ｘｌ，ｙｌ）またはＷ（ｘｆ，ｙｆ））が算出されたＭＭ尤度分布が得られる。
【００３７】
第１分布算出部２６は、後述する第２分布算出部２８で前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、推測航法移動量分布算出部２０で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）を、下記（７）式に従って算出する。
【００３８】
【数６】
【００３９】
ただし、ｔ＝０の時は、Ｐ２ｔ−１（Ｘ）の代わりとして、ＧＰＳ受信機からのデータなどで推定した測位結果に基づいた測位位置分布など、適当な分布を初期分布として用いことができる。
【００４０】
第２分布算出部２８は、第１分布算出部２６で算出された自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）と、ＭＭ尤度分布算出部２４で算出されたＭＭ尤度Ｗｔ（ｘ，ｙ）とを掛け合わせて、下記（８）式に従って、自車位置の第２存在確率分布を算出する。
【００４１】
【数７】
【００４２】
ＭＭ尤度Ｗｔ（ｘ，ｙ）は、グリッド座標（ｘ，ｙ）が道路リンク上であれば、（５）式により得られるＷ（ｘｌ，ｙｌ）であり、グリッド座標（ｘ，ｙ）が道路リンク外であれば、（６）式により得られるＷ（ｘｆ，ｙｆ）である。このようにして得られた第２存在確率分布Ｐ２ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力すると共に、第１分布算出部２６へ出力する。
【００４３】
次に、図３を参照して、第１の実施の形態の存在確率分布推定装置１０において実行される存在確率分布推定処理ルーチンについて説明する。
【００４４】
ステップ１００で、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１４で検出された車速Ｖｏｂｓを取得する。
【００４５】
次に、ステップ１０２で、上記ステップ１００で取得したヨーレイトθｏｂｓ、及び車速Ｖｏｂｓ、並びにヨーレイトセンサ１２及び車速センサ１６の計測精度に基づいて予め求められた推定計測誤差σθ及びσＶと共に、（４）式に代入することにより、自車両の移動量分布を算出する。
【００４６】
次に、ステップ１０４で、上記ステップ１００で取得したヨーレイトθｏｂｓ、及び車速Ｖｏｂｓの時系列データに基づいて、自車両の走行軌跡の形状を算出する。
【００４７】
次に、ステップ１０６で、図４（ａ）に示すように、上記ステップ１０４で算出された走行軌跡の形状から自車位置を中心にＭピクセル×Ｎピクセルの走行軌跡ウインドウを切り出す。また、地図情報から、ある道路リンク上のグリッド座標（ｘｌ，ｙｌ）を中心としてＭピクセル×Ｎピクセルの道路リンクウインドウを切り出す。そして、切り出した走行軌跡ウインドウと道路リンクウインドウとを比較して、（５）式により、道路リンク上のグリッド座標（ｘｌ，ｙｌ）上のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を算出する（同図（ｂ））。
【００４８】
また、道路リンク外のグリッド座標（ｘｆ，ｙｆ）におけるＭＭ尤度Ｗ（ｘｆ，ｙｆ）を、道路リンク上のグリッド座標（ｘｌ，ｙｌ）のＭＭ尤度Ｗ（ｘｌ，ｙｌ）を用いて、（６）式に従って算出して、地図情報の全グリッドについてＭＭ尤度が算出されたＭＭ尤度分布を得る（同図（ｃ））。
【００４９】
次に、ステップ１０８で、前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、上記ステップ１０２で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐｔ（ｘ，ｙ）を、（７）式に従って算出する。
【００５０】
次に、ステップ１１０で、上記ステップ１０８で算出された自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）と、上記ステップ１０６で算出されたＭＭ尤度Ｗｔ（ｘ，ｙ）とを掛け合わせて、（８）式に従って、自車位置の第２存在確率分布を算出する。本ステップで算出された第２存在確率分布は、次のタイムステップのステップ１０８で用いられる。
【００５１】
次に、ステップ１１２で、上記ステップ１１０で算出された第２存在確率分布Ｐ２ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力して、処理を終了する。
【００５２】
以上説明したように、第１の実施の形態の存在確率分布推定装置によれば、センサの計測誤差を考慮した自車位置の第１存在確率分布と、道路リンク上のＭＭ尤度及び道路リンク上のＭＭ尤度より低い道路リンク外のＭＭ尤度を地図情報のグリッド毎に算出したＭＭ尤度分布とを掛け合わせた自車位置の第２存在確率分布を算出し、これを自車位置の存在確率分布の推定結果とすることにより、道路リンク外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した存在確率分布を推定することができる。
【００５３】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の存在確率分布推定装置の構成において、第１の実施の形態の存在確率分布推定装置１０と同一の構成については、同一符号を付して詳細な説明は省略する。
【００５４】
図５に示すように、第２の実施の形態に係る存在確率分布推定装置２１０は、ヨーレイトセンサ１２と、車速センサ１４と、ＧＰＳ衛星からの衛星信号を受信して擬似距離データを出力するＧＰＳ受信機１５と、地図ＤＢ１６と、自車位置の存在確率分布を推定するコンピュータ２１８とを備えている。
【００５５】
コンピュータ２１８を以下で説明する存在確率分布推定処理ルーチンに従って機能ブロックで表すと、図５に示すように、推測航法移動量分布算出部２０と、推測航法軌跡算出部２２と、ＭＭ尤度分布算出部２４と、擬似距離データに基づいて、ＧＰＳ測位位置分布を算出するＧＰＳ測位位置分布算出部３０と、ＧＰＳ測位位置分布及び前タイムステップの第２存在確率分布に基づいて、第３存在確率分布を算出する第３分布算出部３２と、第３存在確率分布及び移動量分布に基づいて、第１存在確率分布を算出する第１分布算出部２２６と、第２分布算出部２８とを含んだ構成で表すことができる。
【００５６】
ＧＰＳ測位位置分布算出部３０は、ＧＰＳ受信機１５から出力された擬似距離データを取得し、受信位置を測位する。また、擬似距離の残差に基づいた正規分布を誤差分布として算出し、ＧＰＳ測位位置分布を算出する。
【００５７】
また、ＧＰＳ測位位置分布算出部３０は、ＧＰＳ受信機１５から出力された擬似距離データによる測位が可能な場合には、ＧＰＳ測位位置分布を第３分布算出部３２に出力し、遮蔽などによりＧＰＳ測位が不可能な場合、またはＧＰＳ測位の信頼度が所定値以下の場合には、ＧＰＳ測位位置分布を出力しない。
【００５８】
第３分布算出部３２は、第２分布算出部２８で前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、ＧＰＳ測位位置分布算出部３０で算出されたＧＰＳ測位位置分布Ｐｔｇｐｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）を、下記（９）式に従って算出する。なお、ＧＰＳ測位位置分布算出部３０からのＧＰＳ測位位置分布の出力がなかった場合には、第３存在確率分布Ｐ３ｔ（ｘ，ｙ）は、下記（１０）式となる。
【００５９】
【数８】
【００６０】
第１分布算出部２２６は、第３分布算出部３２で算出された自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）と、推測航法移動量分布算出部２０で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）を、下記（１１）式に従って算出する。
【００６１】
【数９】
【００６２】
次に、図６を参照して、第２の実施の形態の存在確率分布推定装置２１０において実行される存在確率分布推定処理ルーチンについて説明する。なお、第１の実施の形態の存在確率分布推定処理ルーチンと同一の処理については、同一の符号を付して詳細な説明は省略する。
【００６３】
ステップ２００で、ヨーレイトセンサ１２で検出されたヨーレイトθｏｂｓ、及び車速センサ１４で検出された車速Ｖｏｂｓ、並びにＧＰＳ受信機１５から出力された擬似距離データを取得する。
【００６４】
次に、ステップ２０２で、上記ステップ２００で取得した擬似距離データに基づいて、受信位置を測位する。また、擬似距離の残差に基づいた正規分布を誤差分布として算出し、ＧＰＳ測位位置分布を算出する。なお、遮蔽などによりＧＰＳ測位が不可能な場合、またはＧＰＳ測位の信頼度が所定値以下の場合には、ＧＰＳ測位位置分布を出力しない。
【００６５】
次に、ステップ１０２で、自車両の移動量分布を算出し、次に、ステップ１０４で、自車両の走行軌跡の形状を算出し、次に、ステップ１０６で、ＭＭ尤度分布を算出する。
【００６６】
次に、ステップ２０４で、前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ２ｔ−１（ｘ，ｙ）と、上記ステップ２０２で算出されたＧＰＳ測位位置分布Ｐｔｇｐｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）を、（９）式に従って算出する。上記ステップ２０２でＧＰＳ測位位置分布が出力されていない場合には、（１０）式に従って自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）を算出する。
【００６７】
次に、ステップ２０６で、上記ステップ２０４で算出された自車位置の第３存在確率分布Ｐ３ｔ（ｘ，ｙ）と、上記ステップ１０２で算出された移動量分布Ｐｔｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）を、（１１）式に従って算出する。
【００６８】
次に、ステップ１１０で、上記ステップ２０６で算出された自車位置の第１存在確率分布Ｐ１ｔ（ｘ，ｙ）と、上記ステップ１０６で算出されたＭＭ尤度Ｗｔ（ｘ，ｙ）とを掛け合わせて、（８）式に従って、自車位置の第２存在確率分布を算出する。本ステップで算出された第２存在確率分布は、次のタイムステップのステップ２０４で用いられる。
【００６９】
次に、ステップ１１２で、上記ステップ１１０で算出された第２存在確率分布Ｐ２ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力して、処理を終了する。
【００７０】
以上説明したように、第２の実施の形態の存在確率分布推定装置によれば、ＧＰＳ測位位置分布も用いることで、より精度良く存在確率分布を推定することができる。
【００７１】
なお、上記第１及び第２の実施の形態では、道路リンク外のＭＭ尤度として、道路リンクからの距離が離れるほど低くなるようなＭＭ尤度（（６）式）を算出する場合について説明したが、この場合に限定されず、道路リンク上のＭＭ尤度より、道路リンク外のＭＭ尤度の方が低くなるような尤度であればよい。
【符号の説明】
【００７２】
１０、２１０ 存在確率分布推定装置
１２ ヨーレイトセンサ
１４ 車速センサ
１５ ＧＰＳ受信機
１６ 車速センサ
１８、２１８ コンピュータ
２０ 推測航法移動量分布算出部
２２ 推測航法軌跡算出部
２４ 尤度分布算出部
２６、２２６ 第１分布算出部
２８ 第２分布算出部
３０ ＧＰＳ測位位置分布算出部
３２ 第３分布算出部
【特許請求の範囲】
【請求項１】
移動体の運動状態を検出する検出手段と、
前記検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段と、
道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段と、
前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段と、
前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段と、
を含む存在確率分布推定装置。
【請求項２】
コンピュータを、
移動体の運動状態を検出する検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段、
道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段、
前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段、及び
前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段
として機能させるための存在確率分布推定プログラム。
【請求項１】
移動体の運動状態を検出する検出手段と、
前記検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段と、
道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段と、
前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段と、
前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段と、
を含む存在確率分布推定装置。
【請求項２】
コンピュータを、
移動体の運動状態を検出する検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段、
道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段、
前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段、及び
前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段
として機能させるための存在確率分布推定プログラム。
【図１】
【図３】
【図５】
【図６】
【図２】
【図４】
【図３】
【図５】
【図６】
【図２】
【図４】
【公開番号】特開２０１２−２３３８０１（Ｐ２０１２−２３３８０１Ａ）
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願番号】特願２０１１−１０２９１６（Ｐ２０１１−１０２９１６）
【出願日】平成２３年５月２日（２０１１．５．２）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
【公開日】平成２４年１１月２９日（２０１２．１１．２９）
【国際特許分類】
【出願日】平成２３年５月２日（２０１１．５．２）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
[ Back to top ]