JP2012233801A - 存在確率分布推定装置及びプログラム - Google Patents

Abstract

【課題】道路情報外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定する。
【解決手段】推測航法移動量分布算出部２０で、ヨーレイトθ_ｏｂｓ、車速Ｖ_ｏｂｓ、各センサの推定計測誤差σ_θ及びσ_Ｖに基づいて、自車両の移動量分布を算出し、推測航法軌跡算出部２２で、θ_ｏｂｓ及びＶ_ｏｂｓの時系列データに基づいて、自車両の走行軌跡の形状を算出し、ＭＭ尤度分布算出部２４で、地図情報の全グリッドについて、道路リンク上のＭＭ尤度、またはそれより低い道路リンク外のＭＭ尤度を算出したＭＭ尤度分布を得て、第１分布算出部２６で、前タイムステップの第２存在確率分布と移動量分布とを掛け合わせて、第１存在確率分布を算出し、第２分布算出部２８で、第１存在確率分布とＭＭ尤度分布とを掛け合わせて算出される第２存在確率分布を、現在の自車位置の存在確率分布として推定する。
【選択図】図１

Description

本発明は、存在確率分布推定装置及びプログラムに係り、特に、道路情報を含む地図上における移動体位置の存在確率分布を推定する存在確率分布推定装置及びプログラムに関する。

従来、移動体の位置を検出するナビゲーション装置において、移動体の挙動情報を検出する自律センサと、自律センサによる検出情報を累積して移動体の慣性測位位置を検出し、時刻ｔの慣性測位位置の誤差分散、自律センサのセンサ誤差分散及び計算誤差を、移動体の移動モデルに基づいて更新式に適用して、時刻ｔ＋１の誤差分散を漸化的に算出するナビゲーション装置が提案されている（例えば、特許文献１参照）。

また、地図データ内の道路形状と車両の走行軌跡の形状との一致度に基づいて、車両位置を更新するナビゲーション装置が提案されている（例えば、特許文献２参照）。

特開２００８−２０３６５号公報 特開２００９−２８７９２５号公報

しかしながら、特許文献１の技術では、誤差分布の範囲が時刻と共に累積し、誤差範囲が拡大する、という問題がある。

この誤差分布の拡大を抑制するために、特許文献１の誤差分布の推定手法に特許文献２のマップマッチングの手法を適用した場合を考える。マップマッチングでは道路幅方向の誤差の累積を抑制できるため、誤差分布の拡大の抑制効果はあると考えられるが、道路リンク外での移動体の位置分散の算出が不可能である、という問題がある。

本発明は、上記の問題点を解決するためになされたもので、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定することができる存在確率分布推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の存在確率分布推定装置は、移動体の運動状態を検出する検出手段と、前記検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段と、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段と、前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段と、前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段と、を含んで構成されている。

本発明の存在確率分布推定装置によれば、検出手段が、移動体の運動状態を検出し、軌跡算出手段が、検出手段により検出された移動体の運動状態に基づいて、移動体の移動軌跡を算出する。そして、尤度分布算出手段が、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、尤度を、領域が道路情報の示す道路上に相当する場合には、道路情報と軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、領域が道路情報の示す道路外に相当する場合には、第１尤度より低い第２尤度とした分布を算出する。また、第１分布算出手段は、検出手段により検出された移動体の運動状態、及び過去に推定された領域毎の移動体の存在確率分布に基づいて、領域毎の移動体の第１存在確率分を算出する。そして、第２分布算出手段が、尤度分布算出手段により算出された尤度の分布、及び第１分布算出手段により算出された第１存在確率分布に基づいて、領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の領域毎の移動体の存在確率分布として推定する。

このように、検出手段により検出された移動体の移動量分布に基づく移動体の第１存在確率分布と、マップマッチングにより算出した道路上の第１尤度、及び第１尤度より低い道路外の第２尤度を領域毎に算出した尤度の分布とに基づいて、移動体の第２存在確率分布を算出し、これを移動体の存在確率分布として推定するため、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した存在確率分布を推定することができる。

また、本発明の存在確率分布推定プログラムは、コンピュータを、移動体の運動状態を検出する検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段、道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段、前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段、及び前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段として機能させるためのプログラムである。

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

以上説明したように、本発明の存在確率推定装置及びプログラムによれば、検出手段により検出された移動体の移動量分布に基づく移動体の第１存在確率分布と、マップマッチングにより算出した道路上の第１尤度、及び第１尤度より低い道路外の第２尤度を領域毎に算出した尤度の分布とに基づいて、移動体の第２存在確率分布を算出し、これを移動体の存在確率分布として推定するため、道路外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した移動体位置の存在確率分布を推定することができる、という効果が得られる。

第１の実施の形態に係る存在確率分布推定装置の機能的構成を示すブロック図である。 走行軌跡と道路リンクとのマップマッチングを説明するための図である。 第１の実施の形態に係る存在確率分布推定装置のコンピュータにおける存在確率分布推定処理ルーチンの内容を示すフローチャートである。 ＭＭ尤度分布の算出を説明するための図である。 第２の実施の形態に係る存在確率分布推定装置の機能的構成を示すブロック図である。 第２の実施の形態に係る存在確率分布推定装置のコンピュータにおける存在確率分布推定処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載され、道路情報を含む地図上における自車位置の存在確率分布を推定する存在確率分布推定装置に、本発明を適用した場合を例に説明する。

図１に示すように、第１の実施の形態に係る存在確率分布推定装置１０は、自車両のヨーレイトを検出するヨーレイトセンサ１２と、自車両の速度を検出する車速センサ１４と、地図情報が格納された地図データベース（地図ＤＢ）１６と、自車位置の存在確率分布を推定するコンピュータ１８とを備えている。

地図ＤＢ１６に格納された地図情報は、道路リンク情報を含む。また、地図情報が示す地図は、二次元グリッドで構成され、１グリッド（１ピクセル）を１単位とする。地図上の位置はグリッド座標（ｘ，ｙ）で表される。

コンピュータ１８は、ＣＰＵ、後述する存在確率分布推定処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、及びＨＤＤ等の記憶装置を含んで構成されている。

このコンピュータ１８を以下で説明する存在確率分布推定処理ルーチンに従って機能ブロックで表すと、図１に示すように、ヨーレイトセンサ１２及び車速センサ１４の検出値に基づいて、自車両の移動量分布を算出する推測航法移動量分布算出部２０と、ヨーレイトセンサ１２及び車速センサ１４の検出値に基づいて、自車両の走行軌跡を算出する推測航法軌跡算出部２２と、自車両の走行軌跡及び地図情報に基づいて、マップマッチング尤度（ＭＭ尤度）の分布を算出するＭＭ尤度分布算出部２４と、移動量分布及び前タイムステップの第２存在確率分布に基づいて、第１存在確率分布を算出する第１分布算出部２６と、第１存在確率分布及びＭＭ尤度分布に基づいて、第２存在確率分布を算出する第２分布算出部２８とを含んだ構成で表すことができる。

なお、推測航法移動量分布算出部２０及び第１分布算出部２６が本発明の第１分布算出手段の一例である。

推測航法移動量分布算出部２０は、ヨーレイトセンサ１２から取得したヨーレイトθ_ｏｂｓ及び車速センサ１４から取得した車速Ｖ_ｏｂｓに基づいて、自車両の移動量分布ｆ（△ｘ，△ｙ）を算出する。具体的には、ヨーレイトの真値がθである確率をｆ_ｙａｗ（θ）、車速の真値がＶである確率をｆ_ｖｅｌ（Ｖ）、移動量の真値が（Δｘ，△ｙ）である確率をｆ（△ｘ，△ｙ）と定義すると、移動量分布ｆ（△ｘ，△ｙ）は、下記（１）式で表わすことができる。

また、ヨーレイトセンサ１２によるヨーレイトの計測値をθ_ｏｂｓとし、その誤差分布を平均０、分散σ_θの正規分布と仮定した場合、下記（２）式が成立する。同様に、車速センサ１４による車速の計測値をＶ_ｏｂｓとし、その誤差分布を平均０、分散σ_Ｖの正規分布と仮定した場合、下記（３）が成立する。

（１）式に（２）式及び（３）式を代入すると、下記（４）式が得られる。

従って、推測航法移動量分布算出部２０は、ヨーレイトセンサ１２で検出されたヨーレイトθ_ｏｂｓ、及び車速センサ１６で検出された車速Ｖ_ｏｂｓを取得し、ヨーレイトセンサ１２及び車速センサ１６の計測精度に基づいて予め求められた推定計測誤差σ_θ及びσ_Ｖと共に、（４）式に代入することにより、自車両の移動量分布を算出する。

推測航法軌跡算出部２２は、ヨーレイトセンサ１２で検出されたヨーレイトθ_ｏｂｓ、及び車速センサ１４で検出された車速Ｖ_ｏｂｓの時系列データを保持し、自車両の走行軌跡の形状を算出する。

ＭＭ尤度分布算出部２４は、推測航法軌跡算出部２２で算出された自車両の走行軌跡の形状及び地図ＤＢ１６に格納された道路リンク情報を含む地図情報に基づいて、地図情報を区画した領域毎のＭＭ尤度を算出する。ＭＭ尤度は、地図上のあるグリッドの道路リンクの確からしさを示す値である。本実施の形態では、１グリッドを１単位としており、グリッド毎にＭＭ尤度が算出される。このＭＭ尤度は、そのグリッドが道路リンク上か道路リンク外かで異なる。以下、道路リンク上のＭＭ尤度、及び道路リンク外のＭＭ尤度に分けて説明する。

道路リンク上のＭＭ尤度は、道路リンク上の全グリッドにおいて、推測航法軌跡算出部２２で算出された走行軌跡の形状と比較を行うことにより算出する。具体的には、まず、図２（ａ）に示すように、推測航法軌跡算出部２２で算出された走行軌跡の形状から自車位置を中心にＭ×Ｎピクセルの走行軌跡ウインドウを切り出す。図２（ａ）中黒く塗りつぶしたグリッドは走行軌跡を表す。次に、同図（ｂ）に示すように、地図情報から、ある道路リンク上のグリッド座標（ｘ_ｌ，ｙ_ｌ）を中心としてＭ×Ｎピクセルの道路リンクウインドウを切り出す。図２（ｂ）中黒く塗りつぶしたグリッドは道路リンクを表す。

そして、切り出した走行軌跡ウインドウと道路リンクウインドウとを比較することで、道路リンク上のグリッド座標（ｘ_ｌ，ｙ_ｌ）上のＭＭ尤度Ｗ（ｘ_ｌ，ｙ_ｌ）を、例えば、下記（５）式に従って算出する。なお、ＭＭ尤度は回転やスケールに対し不変な特徴量を用いて算出するようにしてもよい。

ここで、（ｉ，ｊ）は両ウインドウ中における相対的なグリッド座標を示し、Ｉ（ｉ，ｊ）は走行軌跡ウインドウ中のグリッド座標（ｉ，ｊ）が走行軌跡上であれば１、そうでなければ０となる関数を示す。Ｔ（ｉ，ｊ）も同様に、道路リンクウインドウ中のグリッド座標（ｉ，ｊ）が道路リンク上であれば１、そうでなければ０となる関数を示す。また、Ｍ及びＮはそれぞれ両ウインドウのグリッドサイズを示し，１≦ｉ≦Ｍ，１≦ｊ≦Ｎとする。（５）式に従って、道路リンク上にある全てのグリッドについてＭＭ尤度Ｗ（ｘ_ｌ，ｙ_ｌ）を算出する。

次に、道路リンク外のＭＭ尤度について説明する。道路リンク外のグリッド座標（ｘ_ｆ，ｙ_ｆ）におけるＭＭ尤度Ｗ（ｘ_ｆ，ｙ_ｆ）は、道路リンク上のグリッド座標（ｘ_ｌ，ｙ_ｌ）のＭＭ尤度Ｗ（ｘ_ｌ，ｙ_ｌ）を用いて、下記（６）式に従って算出する。

ただし、定積分のＬは全道路リンクを示す。また、Ｃは定数を示し、道路リンク外のＭＭ尤度Ｗ（ｘ_ｆ，ｙ_ｆ）が、道路リンク上のＭＭ尤度Ｗ（ｘ_ｌ，ｙ_ｌ）よりも十分に低い値となるように設定する。道路リンク外の全領域に所定のＭＭ尤度を与えることで、自車両が道路リンクから外れたルートを走行している場合でも、自車位置の存在確率分布の算出が可能となる。（６）式に従って、道路リンク外にある全てのグリッドについてＭＭ尤度Ｗ（ｘ_ｆ，ｙ_ｆ）を算出する。これにより、地図情報の全グリッドについてＭＭ尤度（Ｗ（ｘ_ｌ，ｙ_ｌ）またはＷ（ｘ_ｆ，ｙ_ｆ））が算出されたＭＭ尤度分布が得られる。

第１分布算出部２６は、後述する第２分布算出部２８で前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ_２ ^ｔ−１（ｘ，ｙ）と、推測航法移動量分布算出部２０で算出された移動量分布Ｐ^ｔ _ｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ_１ ^ｔ（ｘ，ｙ）を、下記（７）式に従って算出する。

ただし、ｔ＝０の時は、Ｐ_２ ^ｔ−１（Ｘ）の代わりとして、ＧＰＳ受信機からのデータなどで推定した測位結果に基づいた測位位置分布など、適当な分布を初期分布として用いことができる。

第２分布算出部２８は、第１分布算出部２６で算出された自車位置の第１存在確率分布Ｐ_１ ^ｔ（ｘ，ｙ）と、ＭＭ尤度分布算出部２４で算出されたＭＭ尤度Ｗ^ｔ（ｘ，ｙ）とを掛け合わせて、下記（８）式に従って、自車位置の第２存在確率分布を算出する。

ＭＭ尤度Ｗ^ｔ（ｘ，ｙ）は、グリッド座標（ｘ，ｙ）が道路リンク上であれば、（５）式により得られるＷ（ｘ_ｌ，ｙ_ｌ）であり、グリッド座標（ｘ，ｙ）が道路リンク外であれば、（６）式により得られるＷ（ｘ_ｆ，ｙ_ｆ）である。このようにして得られた第２存在確率分布Ｐ_２ ^ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力すると共に、第１分布算出部２６へ出力する。

次に、図３を参照して、第１の実施の形態の存在確率分布推定装置１０において実行される存在確率分布推定処理ルーチンについて説明する。

ステップ１００で、ヨーレイトセンサ１２で検出されたヨーレイトθ_ｏｂｓ、及び車速センサ１４で検出された車速Ｖ_ｏｂｓを取得する。

次に、ステップ１０２で、上記ステップ１００で取得したヨーレイトθ_ｏｂｓ、及び車速Ｖ_ｏｂｓ、並びにヨーレイトセンサ１２及び車速センサ１６の計測精度に基づいて予め求められた推定計測誤差σ_θ及びσ_Ｖと共に、（４）式に代入することにより、自車両の移動量分布を算出する。

次に、ステップ１０４で、上記ステップ１００で取得したヨーレイトθ_ｏｂｓ、及び車速Ｖ_ｏｂｓの時系列データに基づいて、自車両の走行軌跡の形状を算出する。

次に、ステップ１０６で、図４（ａ）に示すように、上記ステップ１０４で算出された走行軌跡の形状から自車位置を中心にＭピクセル×Ｎピクセルの走行軌跡ウインドウを切り出す。また、地図情報から、ある道路リンク上のグリッド座標（ｘ_ｌ，ｙ_ｌ）を中心としてＭピクセル×Ｎピクセルの道路リンクウインドウを切り出す。そして、切り出した走行軌跡ウインドウと道路リンクウインドウとを比較して、（５）式により、道路リンク上のグリッド座標（ｘ_ｌ，ｙ_ｌ）上のＭＭ尤度Ｗ（ｘ_ｌ，ｙ_ｌ）を算出する（同図（ｂ））。

また、道路リンク外のグリッド座標（ｘ_ｆ，ｙ_ｆ）におけるＭＭ尤度Ｗ（ｘ_ｆ，ｙ_ｆ）を、道路リンク上のグリッド座標（ｘ_ｌ，ｙ_ｌ）のＭＭ尤度Ｗ（ｘ_ｌ，ｙ_ｌ）を用いて、（６）式に従って算出して、地図情報の全グリッドについてＭＭ尤度が算出されたＭＭ尤度分布を得る（同図（ｃ））。

次に、ステップ１０８で、前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ_２ ^ｔ−１（ｘ，ｙ）と、上記ステップ１０２で算出された移動量分布Ｐ^ｔ _ｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ^ｔ（ｘ，ｙ）を、（７）式に従って算出する。

次に、ステップ１１０で、上記ステップ１０８で算出された自車位置の第１存在確率分布Ｐ_１ ^ｔ（ｘ，ｙ）と、上記ステップ１０６で算出されたＭＭ尤度Ｗ^ｔ（ｘ，ｙ）とを掛け合わせて、（８）式に従って、自車位置の第２存在確率分布を算出する。本ステップで算出された第２存在確率分布は、次のタイムステップのステップ１０８で用いられる。

次に、ステップ１１２で、上記ステップ１１０で算出された第２存在確率分布Ｐ_２ ^ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力して、処理を終了する。

以上説明したように、第１の実施の形態の存在確率分布推定装置によれば、センサの計測誤差を考慮した自車位置の第１存在確率分布と、道路リンク上のＭＭ尤度及び道路リンク上のＭＭ尤度より低い道路リンク外のＭＭ尤度を地図情報のグリッド毎に算出したＭＭ尤度分布とを掛け合わせた自車位置の第２存在確率分布を算出し、これを自車位置の存在確率分布の推定結果とすることにより、道路リンク外においてもマップマッチングの手法を適用して、誤差分布の拡大を抑制した存在確率分布を推定することができる。

次に、第２の実施の形態について説明する。なお、第２の実施の形態の存在確率分布推定装置の構成において、第１の実施の形態の存在確率分布推定装置１０と同一の構成については、同一符号を付して詳細な説明は省略する。

図５に示すように、第２の実施の形態に係る存在確率分布推定装置２１０は、ヨーレイトセンサ１２と、車速センサ１４と、ＧＰＳ衛星からの衛星信号を受信して擬似距離データを出力するＧＰＳ受信機１５と、地図ＤＢ１６と、自車位置の存在確率分布を推定するコンピュータ２１８とを備えている。

コンピュータ２１８を以下で説明する存在確率分布推定処理ルーチンに従って機能ブロックで表すと、図５に示すように、推測航法移動量分布算出部２０と、推測航法軌跡算出部２２と、ＭＭ尤度分布算出部２４と、擬似距離データに基づいて、ＧＰＳ測位位置分布を算出するＧＰＳ測位位置分布算出部３０と、ＧＰＳ測位位置分布及び前タイムステップの第２存在確率分布に基づいて、第３存在確率分布を算出する第３分布算出部３２と、第３存在確率分布及び移動量分布に基づいて、第１存在確率分布を算出する第１分布算出部２２６と、第２分布算出部２８とを含んだ構成で表すことができる。

ＧＰＳ測位位置分布算出部３０は、ＧＰＳ受信機１５から出力された擬似距離データを取得し、受信位置を測位する。また、擬似距離の残差に基づいた正規分布を誤差分布として算出し、ＧＰＳ測位位置分布を算出する。

また、ＧＰＳ測位位置分布算出部３０は、ＧＰＳ受信機１５から出力された擬似距離データによる測位が可能な場合には、ＧＰＳ測位位置分布を第３分布算出部３２に出力し、遮蔽などによりＧＰＳ測位が不可能な場合、またはＧＰＳ測位の信頼度が所定値以下の場合には、ＧＰＳ測位位置分布を出力しない。

第３分布算出部３２は、第２分布算出部２８で前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ_２ ^ｔ−１（ｘ，ｙ）と、ＧＰＳ測位位置分布算出部３０で算出されたＧＰＳ測位位置分布Ｐ^ｔ _ｇｐｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第３存在確率分布Ｐ_３ ^ｔ（ｘ，ｙ）を、下記（９）式に従って算出する。なお、ＧＰＳ測位位置分布算出部３０からのＧＰＳ測位位置分布の出力がなかった場合には、第３存在確率分布Ｐ_３ ^ｔ（ｘ，ｙ）は、下記（１０）式となる。

第１分布算出部２２６は、第３分布算出部３２で算出された自車位置の第３存在確率分布Ｐ_３ ^ｔ（ｘ，ｙ）と、推測航法移動量分布算出部２０で算出された移動量分布Ｐ^ｔ _ｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ_１ ^ｔ（ｘ，ｙ）を、下記（１１）式に従って算出する。

次に、図６を参照して、第２の実施の形態の存在確率分布推定装置２１０において実行される存在確率分布推定処理ルーチンについて説明する。なお、第１の実施の形態の存在確率分布推定処理ルーチンと同一の処理については、同一の符号を付して詳細な説明は省略する。

ステップ２００で、ヨーレイトセンサ１２で検出されたヨーレイトθ_ｏｂｓ、及び車速センサ１４で検出された車速Ｖ_ｏｂｓ、並びにＧＰＳ受信機１５から出力された擬似距離データを取得する。

次に、ステップ２０２で、上記ステップ２００で取得した擬似距離データに基づいて、受信位置を測位する。また、擬似距離の残差に基づいた正規分布を誤差分布として算出し、ＧＰＳ測位位置分布を算出する。なお、遮蔽などによりＧＰＳ測位が不可能な場合、またはＧＰＳ測位の信頼度が所定値以下の場合には、ＧＰＳ測位位置分布を出力しない。

次に、ステップ１０２で、自車両の移動量分布を算出し、次に、ステップ１０４で、自車両の走行軌跡の形状を算出し、次に、ステップ１０６で、ＭＭ尤度分布を算出する。

次に、ステップ２０４で、前タイムステップにおいて算出された自車位置の第２存在確率分布Ｐ_２ ^ｔ−１（ｘ，ｙ）と、上記ステップ２０２で算出されたＧＰＳ測位位置分布Ｐ^ｔ _ｇｐｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第３存在確率分布Ｐ_３ ^ｔ（ｘ，ｙ）を、（９）式に従って算出する。上記ステップ２０２でＧＰＳ測位位置分布が出力されていない場合には、（１０）式に従って自車位置の第３存在確率分布Ｐ_３ ^ｔ（ｘ，ｙ）を算出する。

次に、ステップ２０６で、上記ステップ２０４で算出された自車位置の第３存在確率分布Ｐ_３ ^ｔ（ｘ，ｙ）と、上記ステップ１０２で算出された移動量分布Ｐ^ｔ _ｉｎｓ（ｘ，ｙ）とを掛け合わせて、地図上の任意のグリッド座標（ｘ，ｙ）について、現タイムステップにおける自車位置の第１存在確率分布Ｐ_１ ^ｔ（ｘ，ｙ）を、（１１）式に従って算出する。

次に、ステップ１１０で、上記ステップ２０６で算出された自車位置の第１存在確率分布Ｐ_１ ^ｔ（ｘ，ｙ）と、上記ステップ１０６で算出されたＭＭ尤度Ｗ^ｔ（ｘ，ｙ）とを掛け合わせて、（８）式に従って、自車位置の第２存在確率分布を算出する。本ステップで算出された第２存在確率分布は、次のタイムステップのステップ２０４で用いられる。

次に、ステップ１１２で、上記ステップ１１０で算出された第２存在確率分布Ｐ_２ ^ｔ（ｘ，ｙ）を自車位置の存在確率分布の推定結果として出力して、処理を終了する。

以上説明したように、第２の実施の形態の存在確率分布推定装置によれば、ＧＰＳ測位位置分布も用いることで、より精度良く存在確率分布を推定することができる。

なお、上記第１及び第２の実施の形態では、道路リンク外のＭＭ尤度として、道路リンクからの距離が離れるほど低くなるようなＭＭ尤度（（６）式）を算出する場合について説明したが、この場合に限定されず、道路リンク上のＭＭ尤度より、道路リンク外のＭＭ尤度の方が低くなるような尤度であればよい。

１０、２１０ 存在確率分布推定装置
１２ ヨーレイトセンサ
１４ 車速センサ
１５ ＧＰＳ受信機
１６ 車速センサ
１８、２１８ コンピュータ
２０ 推測航法移動量分布算出部
２２ 推測航法軌跡算出部
２４ 尤度分布算出部
２６、２２６ 第１分布算出部
２８ 第２分布算出部
３０ ＧＰＳ測位位置分布算出部
３２ 第３分布算出部

Claims (2)

1. 移動体の運動状態を検出する検出手段と、
   前記検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段と、
   道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段と、
   前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段と、
   前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段と、
   を含む存在確率分布推定装置。
2. コンピュータを、
   移動体の運動状態を検出する検出手段により検出された移動体の運動状態に基づいて、前記移動体の移動軌跡を算出する軌跡算出手段、
   道路情報を含む地図を複数に区画した領域毎の道路情報の確からしさを示す尤度の分布であって、前記尤度を、前記領域が前記道路情報の示す道路上に相当する場合には、前記道路情報と前記軌跡算出手段により算出された移動軌跡とのマップマッチングにより算出した第１尤度、前記領域が前記道路情報の示す道路外に相当する場合には、前記第１尤度より低い第２尤度とした分布を算出する尤度分布算出手段、
   前記検出手段により検出された移動体の運動状態、及び過去に推定された前記領域毎の移動体の存在確率分布に基づいて、前記領域毎の移動体の第１存在確率分を算出する第１分布算出手段、及び
   前記尤度分布算出手段により算出された尤度の分布、及び前記第１分布算出手段により算出された第１存在確率分布に基づいて、前記領域毎の移動体の第２存在確率を算出し、該第２存在確率分布を、現在の前記領域毎の移動体の存在確率分布として推定する第２分布算出手段
   として機能させるための存在確率分布推定プログラム。