(57)【要約】【課題】リアルタイムで精度良く移動体の現在位置および方位角を検出することの可能な移動体位置検出方法を提供する。【解決手段】一のＧＰＳデータを始点として導出された時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))と、その次のＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) とを含む予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより導出される補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を特定する。【選択図】図４
2008175786.tif 000002
【特許請求の範囲】
【請求項１】
ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、次のＧＰＳデータが得られる時刻ｔａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を推定し、
次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出し、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を、その道路上の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))に移動し、
移動体の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ)) （ｉは変数）を特定する
ことを特徴とする移動体位置検出方法。
【請求項２】
前記予想誤差は、ＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分、または、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))、方位角(ｏｒｉｇ’(ｔａ))、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))の少なくとも１つに対して重み付けがなされた上での、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分である
ことを特徴とする請求項１に記載の移動体位置検出方法。
【請求項３】
移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項１に記載の移動体位置検出方法。
【請求項４】
ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、所定の時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を推定し、
次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) を、時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))を推定し、
一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))と、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出し、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を、その道路上の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))に移動し、
移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ)) （ｉは変数）を特定する
ことを特徴とする移動体位置検出方法。
【請求項５】
前記予想誤差は、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))と、一方の推定により得られた時刻ｔｃにおける位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))との差分、または、移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))、方位角(ｏｒｉｇ’(ｔｃ))、位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))の少なくとも１つに対して重み付けがなされた上での、移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))と、移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))との差分である
ことを特徴とする請求項４に記載の移動体位置検出方法。
【請求項６】
移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項４に記載の移動体位置検出方法。
【請求項７】
受信装置によって受信されたＧＰＳデータから時刻ｔｉ（ｉは変数）における移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、次のＧＰＳデータが得られる時刻ｔａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を推定する移動体位置推定手段と、
次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出する補償量導出手段と、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を、その道路上の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))に移動する移動体位置移動手段と、
移動体の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を特定する移動体位置特定手段と
を備えたことを特徴とする移動体位置検出装置。
【請求項８】
前記予想誤差は、ＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分、または、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))、方位角(ｏｒｉｇ’(ｔａ))、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))の少なくとも１つに対して重み付けがなされた上での、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分である
ことを特徴とする請求項７に記載の移動体位置検出装置。
【請求項９】
前記移動体位置特定手段は、移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項７に記載の移動体位置検出装置。
【請求項１０】
受信装置によって受信されたＧＰＳデータから時刻ｔｉ（ｉは変数）における移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、所定の時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を推定する第１移動体位置推定手段と、
次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) を、時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))を推定する第２移動体位置推定手段と、
一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))と、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出する補償量導出手段と、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を、その道路上の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))に移動する移動体位置移動手段と、
移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ)) （ｉは変数）を特定する移動体位置特定手段と
を備えたことを特徴とする移動体位置検出装置。
【請求項１１】
前記予想誤差は、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))と、一方の推定により得られた時刻ｔｃにおける位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))との差分、または、移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))、方位角(ｏｒｉｇ’(ｔｃ))、位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))の少なくとも１つに対して重み付けがなされた上での、移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))と、移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))との差分である
ことを特徴とする請求項１０に記載の移動体位置検出装置。
【請求項１２】
前記移動体位置特定手段は、移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項１０に記載の移動体位置検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の現在位置を検出する移動体位置検出方法および移動体位置検出装置に関する。
【背景技術】
【０００２】
自動車車両のような移動体の現在位置を検出し、検出された現在位置を周辺の道路地図と共に表示画面に表示する移動体位置検出装置が各種開発されている。近年の移動体位置検出装置では、ＧＰＳ（Global Positioning System）と慣性センサとを組み合わせることにより移動体の現在位置を割り出す手法が一般的に用いられている。さらに、ＣＤ−ＲＯＭや、ＤＶＤ、ハードディスクなどに記録された地図情報を必要に応じて読み出し、移動体の走行経路の情報と照合することにより、移動体の現在位置を正確に特定するマップマッチングという手法も採り入れられている。このようにして得られた移動体の現在位置の情報を、地図情報に重ね合わせることにより、現在位置とその付近の所定範囲内の道路地図とを表す画像を画像表示装置の画面に表示することが可能となる（特許文献１参照）。
【０００３】
【特許文献１】ＷＯ２００４／０４８８９５
【０００４】
ここで、ＧＰＳとは、地球上の現在位置を調べるための衛星測位システムであり、宇宙空間に打ち上げられた多数のＧＰＳ衛星から得られたＧＰＳデータに基づいて現在位置（絶対位置情報）を導出するものである。一般に、３個の衛星からの電波を捕えることができれば地球上の平面での位置がわかり(２次元測位)、４個以上の衛星からの電波を捕えることができればさらに高度の情報を得ることができる(３次元測位)。このＧＰＳの精度は、例えば、補足衛星数や、ＤＯＰ（Dilution of Precision)値、測位の次元（２次元または３次元）、マルチパス誤差、衛星自身の位置誤差、衛星に搭載された時計の誤差、衛星からの電波を受信するレシーバの雑音レベル、電離層による誤差などによって変化するが、廉価な商用ＧＰＳでは、位置（緯度および経度）の精度がおよそ２０メートル、方位角の精度がおよそ１０度となっており、ＧＰＳ衛星から１秒周期でＧＰＳデータが出力される。
【０００５】
また、慣性センサとは、移動体に取り付けられるジャイロスコープや速度センサなどのことであり、移動体の角速度や走行速度などを計測するものである。この慣性センサでは、１／６０秒または１／５０秒周期で移動体の角速度や走行速度が計測されるので、この計測された角速度や走行速度に基づいて移動体の移動距離や方位角など（相対位置情報）を導出することができる。そのため、慣性センサは、ＧＰＳデータを受信できないとき（例えば、トンネルの中にいるとき）や、ＧＰＳデータに大きなノイズが含まれているとき（例えば、ビルとビルの谷間にいるとき）、ＧＰＳデータを補間する目的で用いられることが多い。
【発明の開示】
【発明が解決しようとする課題】
【０００６】
ところで、従来の移動体位置検出装置では、ＧＰＳデータを取得してから移動体の現在位置を特定するまでの間に、上記した様々な誤差要素を除去して現在位置の計算を行ったり、慣性センサから得られた移動体の角速度や走行速度を用いて現在位置の補正を行ったり、さらに、マップマッチングを行うなど、種々の情報処理が必要である。そのため、移動体の現在位置を特定するまでの時間（遅延時間）が非常に長く、しかもばらつきを有している場合もあり、移動体の現在位置を特定した時には既にＧＰＳデータを取得した時の位置とは全く別の場所に移動体が移動してしまっていることもある。従って、単に移動体の走行経路の履歴を画面に表示するだけでよいような、リアルタイム性のあまり重要でない用途であれば、遅延時間が長くなったとしてもあまり問題とはならない。しかし、移動体の現在位置などに基づいて、その先の道路特性に応じた制御を移動体に対して行ったり、移動体の運転に役立つ情報を提供するような、リアルタイム性を厳しく要求される用途では、遅延時間を短縮するか、または、別の方策を用いて移動体の現在位置などを即座に推定することが求められる。
【０００７】
そこで、そのようなリアルタイム性を厳しく要求される用途においては、例えば、処理能力を上げて、遅延時間を短縮することが考えられる。しかし、処理能力を上げるために、高速に演算処理することの可能な部品を用いると、コストアップに繋がるので、このような方策を採ることは汎用機器においては現実的ではない。もっとも、仮に高速に演算処理することの可能な部品を用いて遅延時間を短縮した場合であっても、ＧＰＳデータは１秒周期にしか得ることができないので、ＧＰＳデータが来ない間はリアルタイムに現在位置を特定することができない。また、例えば、直近のＧＰＳデータから既に得られている位置および方位角を、慣性センサからのデータを用いて補正することにより、最新のＧＰＳデータから移動体の現在位置が特定されるまでの間、移動体の現在位置を推定することが考えられる。しかし、慣性センサでは推定時間が長くなるほど現在位置の誤差が大きくなる傾向があり、また、直近のＧＰＳデータから既に得られた位置および方位角や、最新のＧＰＳデータから得られた位置に大きな誤差が含まれている場合もある。そのため、慣性センサからのデータを用いて得られた現在位置と、最新のＧＰＳデータから得られた現在位置とが大きく食い違ってしまう可能性がある。そのときに、例えば、慣性センサからのデータを用いて得られた現在位置を、最新のＧＰＳデータから得られた現在位置で置き換えてしまうと、置き換えた瞬間に現在位置がジャンプしてしまう。このような現在位置のジャンプはリアルタイム性を厳しく要求される用途では許容することができない。
【０００８】
このように、従来の技術では、リアルタイムで精度良く移動体の現在位置および方位角を検出することが容易ではないという問題があった。
【０００９】
本発明はかかる問題点に鑑みてなされたもので、その目的は、リアルタイムで精度良く移動体の現在位置および方位角を検出することの可能な移動体位置検出方法および移動体位置検出装置を提供することにある。
【課題を解決するための手段】
【００１０】
本発明による第１の移動体位置検出方法は、以下の（Ａ１）〜（Ａ４）の各ステップを含むものである。
（Ａ１）ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、次のＧＰＳデータが得られる時刻ｔａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を推定するステップ
（Ａ２）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出するステップ
（Ａ３）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を、その道路上の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))に移動するステップ
（Ａ４）移動体の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ)) （ｉは変数）を特定するステップ
【００１１】
本発明による第１の移動体位置検出装置は、以下の（Ｂ１）〜（Ｂ５）の各手段を備えたものである。
（Ｂ１）受信装置によって受信されたＧＰＳデータから時刻ｔｉ（ｉは変数）における移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
（Ｂ２）ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、次のＧＰＳデータが得られる時刻ｔａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を推定する移動体位置推定手段
（Ｂ３）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出する補償量導出手段
（Ｂ４）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))を、その道路上の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))に移動する移動体位置移動手段
（Ｂ５）移動体の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を特定する移動体位置特定手段
【００１２】
本発明による第１の移動体位置検出方法および第１の移動体位置検出装置では、一のＧＰＳデータを始点として推定された時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))と、その次のＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出された補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))が特定される。
【００１３】
ここで、予想誤差を、ＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分、または、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))、方位角(ｏｒｉｇ’(ｔａ))、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))の少なくとも１つに対して重み付けがなされた上での、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分とすることが可能である。
【００１４】
移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。
【００１５】
本発明による第２の移動体位置検出方法は、以下の（Ｃ１）〜（Ｃ５）の各ステップを含むものである。
（Ｃ１）ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、所定の時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を推定するステップ
（Ｃ２）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) を、時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))を推定するステップ
（Ｃ３）一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))と、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出するステップ
（Ｃ４）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を、その道路上の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))に移動するステップ
（Ｃ５）移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ)) （ｉは変数）を特定するステップ
【００１６】
本発明による第２の移動体位置検出装置は、以下の（Ｄ１）〜（Ｄ６）の各手段を備えたものである。
（Ｄ１）受信装置によって受信されたＧＰＳデータから時刻ｔｉ（ｉは変数）における移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
（Ｄ２）ＧＰＳデータから得られた時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を、所定の時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を推定する第１移動体位置推定手段
（Ｄ３）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) を、時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))を推定する第２移動体位置推定手段
（Ｄ４）一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))と、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出する補償量導出手段
（Ｄ５）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))を、その道路上の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))に移動する移動体位置移動手段
（Ｄ６）移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを用いて補正することにより、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ)) （ｉは変数）を特定する移動体位置特定手段
【００１７】
本発明による第２の移動体位置検出方法および第２の移動体位置検出装置では、一のＧＰＳデータを始点として推定された時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))と、その次のＧＰＳデータを始点として推定された時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出される補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))が特定される。
【００１８】
ここで、予想誤差を、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))と、一方の推定により得られた時刻ｔｃにおける位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))との差分、または、移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))、方位角(ｏｒｉｇ’(ｔｃ))、位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))の少なくとも１つに対して重み付けがなされた上での、移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))と、移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))との差分であるとすることが可能である。
【００１９】
また、移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。
【発明の効果】
【００２０】
本発明による第１の移動体位置検出方法および第１の移動体位置検出装置によれば、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))が特定される。これにより、リアルタイムで精度良く時刻ｔａ以降の移動体の現在位置および方位角を検出することが可能となる。
【００２１】
本発明による第２の移動体位置検出方法および第２の移動体位置検出装置によれば、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))が特定される。これにより、リアルタイムで精度良く時刻ｔｃ以降の移動体の現在位置および方位角を検出することが可能となる。
【００２２】
ここで、本発明による第１および第２の移動体位置検出方法ならびに第１および第２の移動体位置検出装置において、移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正した場合には、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。
【発明を実施するための最良の形態】
【００２３】
以下、本発明を実施するための最良の形態について図面を参照して詳細に説明する。
【００２４】
図１は、本発明の移動体位置検出装置１の概要構成を表したものである。なお、本発明の実施の形態に係る移動体位置検出方法は、この移動体位置検出装置１の動作によって具現化されるものであるから、以下、それらを併せて説明する。
【００２５】
この移動体位置検出装置１は、運転者の運転する移動体（例えば自動車）に搭載されたものであり、例えば、センサ入力部１０と、地図管理部２０と、中央処理部３０と、映像表示部４０と、制御部５０とを、その主要部として備えている。なお、センサ入力部１０、地図管理部２０、映像表示部４０および制御部５０が、移動体位置検出装置１の外部装置として設けられていてもよい。
【００２６】
センサ入力部１０は、ＣＣＤ（固体撮像素子）カメラ１０１と、ＧＰＳセンサ１０２と、ジャイロセンサ１０３（慣性センサ）と、車速センサ１０４（速度センサ）とを備えている。
【００２７】
ＣＣＤカメラ１０１は、例えば運転者がフロントガラス越しに見る視線とほぼ同様のカメラアングルで車両前方の景観を撮影（撮像）することができるように、移動体に固定されている。このＣＣＤカメラ１０１は、例えば固定焦点距離の単眼方式のものであり、道路を含んだ前方の景観を例えば１／３０秒ごとに撮像して、撮像により得られた画像データを中央処理部３０に転送するようになっている。
【００２８】
ＧＰＳセンサ１０２は、例えば、地球上の現在位置を調べるための衛星測位システム（ＧＰＳ）において、宇宙空間に打ち上げられた多数のＧＰＳ衛星から地表に向けて送信されてくる電波を受信するレシーバであり、受信した電波に含まれる各種データ（ＧＰＳデータ）を中央処理部３０に転送するようになっている。なお、中央処理部３０は、ＧＰＳデータから、移動体の位置（緯度および経度）と、移動体の方位角とを算出することができる。
【００２９】
ＧＰＳ衛星からは、例えば１秒ごとにＧＰＳデータを含む電波が出力されているが、その電波は、地表に到達するまでの間に電離層などの影響を受けて、１秒から２秒の不確定性を持ってレシーバに到達する。ここで、廉価な商用ＧＰＳでは、位置（緯度および経度）の精度がおよそ２０メートル、方位角の精度がおよそ１０度となっている。
【００３０】
なお、ＧＰＳ衛星から送信されてくる電波を受信する代わりに、ＤＧＰＳ(Differential GPS)において、基準局から送信されてくる中波やＦＭ放送などの地上波を受信するようにしてもよい。このＤＧＰＳとは、位置の分かっている基準局でＧＰＳによる測量を行い、実際の位置とＧＰＳで算出された位置とのずれを中波やＦＭ放送などの地上波で送信することにより、ＧＰＳ衛星からの信号により計測した結果を補正することの可能なシステムであり、ＧＰＳよりも精度よく測位することが可能である。
【００３１】
ジャイロセンサ１０３は、例えば、コリオリ力による振動を電気的に検出する３次元振動ジャイロからなり、検出した振動データを中央処理部３０に転送するようになっている。なお、中央処理部３０は、得られた振動データから、例えば１／６０秒ごとに移動体の角速度を算出することが可能である。
【００３２】
車速センサ１０４は、例えば、車速の大きさに応じたパルス信号を出力するパルスセンサであり、出力されたパルス信号を中央処理部３０に転送するようになっている。なお、中央処理部３０は、得られたパルス信号から、例えば１／５０秒ごとに移動体の車速を算出することが可能である。
【００３３】
地図管理部２０は、所定の地理的領域の地図データが記録された記録媒体２０１と、この記録媒体２０１から地図データを読み出すドライバ２０２とを有している。この地図管理部２０は、中央処理部３０からの読み出し命令に従って、地図データ（例えば、所定の地域に含まれる道路に関するデータや、その道路に付随するデータなど）を記録媒体２０１から読み出して、中央処理部３０に転送するようになっている。
【００３４】
ここで、道路に関するデータには、例えば、ノードと呼ばれる交差点、分岐点および補間点などの位置（緯度、経度、海抜）や、ノード間を繋ぐ道路セグメントの等級（国道、県道、農道などにそれぞれ割り当てられている数値）、道路セグメントの形状（水平曲率、縦断曲率）、道路セグメントの車線数などが含まれている。また、道路に付随するデータには、例えば、ランドマーク、鉄道の駅、病院およびガソリンスタンドなどの位置、形状、大きさおよびアイコンなどが含まれている。
【００３５】
中央処理部３０は、例えば、測位処理モジュール３０１と、画像処理モジュール３０２と、制御モジュール３０３とをその主要部として有している。
【００３６】
測位処理モジュール３０１は、センサ入力部１０から入力された各種データおよびパルス信号から、移動体の位置、方位角、角速度および車速を算出し、算出した移動体の位置、方位角、角速度および車速と、地図管理部２０から読み出された道路に関するデータとに基づいて、移動体の位置および方位角を特定する演算を行うデバイスである。この測位処理モジュール３０１は本実施の形態における要部の１つであることから、これについては後に詳述する。
【００３７】
画像処理モジュール３０２は、例えば、地図管理部２０から読み出された地図データ上に、測位処理モジュール３０１で特定された移動体の位置および方位角を重ね合わせた画像データを生成したり、ＣＣＤカメラ１０１で撮像された画像データ上に、道路に付随するデータをアイコンで表現したものを重ね合わせた画像データを生成するするデバイスである。
【００３８】
制御モジュール３０３は、測位処理モジュール３０１で特定された移動体の位置および方位角と、ＣＣＤカメラ１０１から得られた画像データと、地図管理部２０から読み出された地図データとに基づいて、移動体をリアルタイムに制御する制御信号を生成するデバイスである。この制御モジュール３０３も本実施の形態における要部の１つであることから、これについても後に詳述する。
【００３９】
映像表示部４０は、例えば液晶表示装置からなり、画像処理モジュール３０２によって生成された画像データを画面に表示するようになっている。
【００４０】
制御部５０は、制御モジュール３０３によって生成された制御信号に基づいて、制御対象の機器（例えばブレーキやハンドル）を制御するデバイスである。
【００４１】
次に、測位処理モジュール３０１について説明する。
【００４２】
図２（１），（２），（３）は、各センサからデータが出力されるタイミングを模式的に表したものである。図２（１），（２），（３）に示したように、ＧＰＳデータは周期ＴｇでＧＰＳセンサ１０２から出力され、ジャイロデータは周期Ｔｗでジャイロセンサ１０３から出力され、車速データは周期Ｔｖで車速センサ１０４から出力される。ここで、ジャイロデータの周期Ｔｗおよび車速データの周期Ｔｖは、ＧＰＳデータの周期Ｔｇよりも短い。そのため、測位処理モジュール３０１は、ＧＰＳデータがこない間は、ジャイロデータおよび車速データを用いて移動体の位置および方位角を推定する。また、各センサからデータが出力されるタイミングは、一般に互いに同期していない。そこで、測位処理モジュール３０１は、各センサからの出力タイミングが互いに同期していない場合には、例えば、ジャイロセンサ１０３または車速センサ１０４の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定する。なお、本実施の形態では、ジャイロセンサ１０３の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定するものとする。
【００４３】
図３は、移動体の位置および方位角が時々刻々と変化する様子を模式的に表したものである。なお、図３には、図２（１），（２），（３）の時刻ｔ０，ｔ１，ｔ２における移動体の変位を高さ方向から眺めた様子が例示されている。図２（１），（２），（３）および図３から、測位処理モジュール３０１は、時刻ｔ０における移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を、ＧＰＳデータから直接得ることができる。他方、時刻ｔ１における移動体の位置（ｘｇ（ｔ１），ｙｇ（ｔ１））および方位角（ｏｒｉｇ（ｔ１））を、ＧＰＳデータから直接得ることができない。そこで、測位処理モジュール３０１は、車速データから得られた移動体の車速ｖ（ｔ０）にΔｔ（＝時刻ｔ１−時刻ｔ０）を乗算することにより移動距離Δｄ（ｔ０）を求めると共に、ジャイロデータから得られた移動体の角速度ｗ（ｔ０）にΔｔを乗算することにより方位角の変位量Δｏｒｉ（ｔ０）を求めたのち、移動距離Δｄ（ｔ０）を（ｏｒｉｇ（ｔ０）＋Δｏｒｉ（ｔ０））（＝ｏｒｉｇ（ｔ１））を用いてｘ成分とｙ成分とに分解することにより、時刻ｔ１における移動体の位置（ｘｇ（ｔ１），ｙｇ（ｔ１））および方位角（ｏｒｉｇ（ｔ１））を推定する。これらを一般化したものを以下の数１〜３に示す。なお、数１〜３中のｉは、ゼロ以上の整数である。また、数１〜３中の（Ｃｘ，Ｃｙ，Ｃｏｒｉ）は、後述するように補償量であるが、このときは（０，０，０）とする。
【００４４】
【数１】
2008175786.tif 000003
【数２】
2008175786.tif 000004
【数３】
2008175786.tif 000005
【００４５】
このように、測位処理モジュール３０１は、ＧＰＳデータから直接得られた時刻ｔ０における移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を始点として、時刻ｔｉにおける移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、ジャイロデータおよび車速データを利用して推定する。
【００４６】
次に、測位処理モジュール３０１は、図２に示したように、時刻ｔａにおいて次のＧＰＳデータが得られた場合には、まず、ＧＰＳの測位品質を評価することが好ましい。具体的には、補足衛星数や、ＤＯＰ値、測位の次元（２次元または３次元）を評価することが考えられる。この場合には、例えば、補足衛星数が７個以下、ＤＯＰ値が２．０以上、測位の次元が２次元となっているときには、ＧＰＳデータから得られる時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))は大きな誤差を含んでおり、ＧＰＳの測位品質が悪いと判断する。また、ＧＰＳデータから時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ))を算出したのちに、時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))と時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))との差分と、車速データを用いて算出された時刻ｔ０から時刻ｔａまでの移動距離との誤差を評価することが考えられる。この場合には、その誤差が所定の値を超えたときには、ＧＰＳの測位品質が悪いと判断する。以上のようにしてＧＰＳの測位品質を評価した結果、ＧＰＳの測位品質が悪いと判断した場合には、その値を用いないで、引き続き、時刻ｔｉにおける移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、ジャイロデータおよび車速データを利用して推定する。逆に、ＧＰＳデータから得られる時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))が大きな誤差を含んでいないと判断した場合には、以下の手順を実行する。
【００４７】
なお、時刻ｔａに得られる予定のＧＰＳデータが得られない場合には、引き続き、時刻ｔｉにおける移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、ジャイロデータおよび車速データを利用して推定する。また、上の評価において、ＧＰＳデータから時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ))を算出する手順が含まれていない場合には、移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ))をＧＰＳデータから算出したのち、以下の手順を実行するものとする。また、以下の手順において、ＧＰＳデータから移動体の位置および方位角を算出する場合には、上記した評価方法を実行することが好ましい。
【００４８】
次に、測位処理モジュール３０１は、数４に示したように、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出する。
【数４】
2008175786.tif 000006
【００４９】
ここで、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）は、ＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ))と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))とを含むものである。予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）は、例えば、ＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分、または、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))、方位角(ｏｒｉｇ’(ｔａ))、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))の少なくとも１つに対して重み付けがなされた上での、位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) と、位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))との差分である。なお、ＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ))から、推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))に転換行列の各成分（Ｈｘ，Ｈｙ，Ｈｏｒｉ）を重み付けとして乗算したものを減算することにより、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）を求めた例を数５に示した。
【数５】
2008175786.tif 000007
【００５０】
また、補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）は、拡張されたカルマンフィルタであり、例えば、数６に示したように、１から、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を減算することにより求められる。ここで、確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）は、例えば、数７に示したようなガウス分布として規定されている。
【数６】
2008175786.tif 000008
【数７】
2008175786.tif 000009
【００５１】
次に、図４に示したように、時刻ｔａにおける推定値（（ｘｇ（ｔａ），ｙｇ（ｔａ），ｏｒｉｇ（ｔａ））に対してマップマッチング処理を行うことにより、道路上の移動体の位置（Ｘｇ(ｔａ)，Ｘｇ(ｔａ)) および方位角（ＯＲＩｇ（ｔａ））を求める。なお、図４は、時刻ｔａにおける移動体の位置（（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））がマップマッチング処理により所定の位置（Ｘｇ(ｔａ)，Ｘｇ(ｔａ)) および方位角（ＯＲＩｇ（ｔａ））に移動する様子を模式的に表したものである。
【００５２】
マップマッチング処理は、例えば、生成された道路形状モデルと、移動体の種々の情報とを対比し、その対比の結果に基づいて、移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置を強制的に移動させる処理であり、例えば、以下の２つの方法により行うことが可能である。
【００５３】
なお、上記の道路形状モデルは、例えば、移動体の位置および方位角をベースに、移動体位置の前方の道路に関するデータを検索し、検索により得られた道路に関するデータに基づいて、道路の水平形状を表す数学モデルであるクロソイド（clothoid）曲線モデルのパラメータを推定することにより生成することが可能である。
【００５４】
まず、１つ目の方法では、（１）推定により得られた時刻ｔａおよびその直近の時刻において移動体が走行した経路（走行経路）の形状と、推定により得られた時刻ｔａにおける移動体の位置（ｘｇ(ｔａ)，ｙｇ(ｔａ))の近傍（所定の距離以下）に存在する１または複数の道路の形状とを対比し、移動体の走行経路の形状に最も近似した形状を有する道路を１つ抽出する。例えば、図５に示したように、移動体の走行経路Ｔの形状と、移動体の近傍にある道路Ｒ１，Ｒ２のそれぞれの形状とを対比し、走行経路Ｔの形状により近い道路Ｒ１を抽出する。（２）次に、抽出した道路の中央線上に移動体の位置（ｘｇ(ｔａ)，ｙｇ(ｔａ))を移動して、中央線上に移動した後の移動体の位置（Ｘｇ(ｔａ)，Ｘｇ(ｔａ))を求める。例えば、抽出した道路の中央線に対して移動体の位置（ｘｇ(ｔａ)，ｙｇ(ｔａ))から下ろした垂線が、抽出した道路の中央線と交わる位置を算出し、その位置に移動体の位置（ｘｇ(ｔａ)，ｙｇ(ｔａ))を移動する。
【００５５】
次に、２つ目の方法では、（１）推定により得られた時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))と、推定により得られた移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))の近傍（所定の距離以下）に存在する１または複数の道路の中央線の位置および方位角とを対比し、移動体が最も実在しそうな位置および方位角の中央線を有する道路を１つ抽出する。例えば、図５に示したように、移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))と、移動体の近傍にある道路Ｒ１，Ｒ２のそれぞれの中央線の位置および方位角とを対比し、移動体が最も実在しそうな道路Ｒ１を抽出する。（２）次に、抽出した道路の中央線のうち、移動体が最も実在しそうな位置および方位角を有する中央線上に移動体の位置（ｘｇ(ｔａ)，ｙｇ(ｔａ))を移動して、中央線上に移動した後の移動体の位置（Ｘｇ(ｔａ)，Ｘｇ(ｔａ))を求める。例えば、移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))に最も近似した位置および方位角の中央線上に、移動体の位置（ｘｇ(ｔａ)，ｙｇ(ｔａ))を移動する。
【００５６】
なお、マップマッチング処理において、移動体の方位角（ｏｒｉｇ（ｔａ））に対して、移動先の中央線の方位角と等しくする補正をすることにより道路の央線上に移動した後の移動体の方位角（ＯＲＩｇ（ｔａ））を得てもよいし、全く補正しないで道路の央線上に移動した後の移動体の方位角（ＯＲＩｇ（ｔａ））を得てもよい。
【００５７】
次に、測位処理モジュール３０１は、図６に示したように、マップマッチング処理された移動体の位置(Ｘｇ(ｔａ)，Ｙｇ(ｔａ))および方位角(ＯＲＩｇ(ｔａ))を始点として、時刻ｔｉにおける移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、ジャイロデータおよび車速データと、上で求めた補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを利用して推定する。具体的には、上記した数１〜４を利用して推定する。
【００５８】
次に、測位処理モジュール３０１は、時刻ｔｂにおいて次のＧＰＳデータが得られた場合には、ＧＰＳデータから時刻ｔｂにおける最新の移動体の位置(ｘｇ’(ｔｂ)，ｙｇ’(ｔｂ))および方位角(ｏｒｉｇ’(ｔａ))を算出する。続いて、数４に示したように、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を更新する。
【００５９】
その後、時刻ｔｂにおける推定値（（ｘｇ（ｔｂ），ｙｇ（ｔｂ），ｏｒｉｇ（ｔｂ））に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置（Ｘｇ(ｔｂ)，Ｘｇ(ｔｂ))および方位角（ＯＲＩｇ（ｔｂ））を求める。そして、時刻ｔｂ以降の移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））については、上記した方法を繰り返し行うことにより求めることができる。
【００６０】
このように、測位処理モジュール３０１は、新たなＧＰＳデータが得られた時刻（例えばｔｂ）における移動体の位置（例えば（（ｘｇ（ｔｂ），ｙｇ（ｔｂ）））および方位角（例えば（ｏｒｉｇ（ｔｂ））をマップマッチング処理し、それにより得られた移動体の位置（例えば（（Ｘｇ（ｔｂ），Ｙｇ（ｔｂ）））および方位角（例えば（ＯＲＩｇ（ｔｂ））と、新たなＧＰＳデータから得られた移動体の位置（例えば（（ｘｇ’（ｔｂ），ｙｇ’（ｔｂ）））および方位角（例えば（ｏｒｉｇ’（ｔｂ））とを用いることにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を求めている。しかし、本実施の形態では、測位処理モジュール３０１は、上記した処理手順に限られるものではなく、例えば、以下に示したような処理手順を行うことにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を求めることが可能である。
【００６１】
図７（１），（２），（３）は、各センサからデータが出力されるタイミングを模式的に表したものである。図８は、時刻ｔａにおける移動体の位置（（ｘｇ（ｔａ），ｙｇ（ｔａ））がマップマッチング処理により所定の位置（Ｘｇ(ｔａ)，Ｘｇ(ｔａ))に移動する様子を模式的に表したものであり、図９は、時刻ｔｄにおける移動体の位置（（ｘｇ（ｔｄ），ｙｇ（ｔｄ））がマップマッチング処理により所定の位置（Ｘｇ(ｔｄ)，Ｘｇ(ｔｄ))に移動する様子を模式的に表したものである。
【００６２】
測位処理モジュール３０１は、ＧＰＳデータから時刻ｔ０における移動体の位置(ｘｇ(ｔ０)，ｙｇ(ｔ０))および方位角(ｏｒｉｇ(ｔ０))を算出したのち、算出した移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を始点として、時刻ｔｉにおける移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、数１〜３を利用して推定し、新たなＧＰＳデータが得られる時刻ｔａよりも後の時刻ｔｃにおける移動体の位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））も数１〜３を利用して推定する。なお、この場合には、数１〜３において、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を（０，０，０）とする。
【００６３】
また、測位処理モジュール３０１は、ＧＰＳデータから時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ))を算出したのち、算出した移動体の位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））を始点として、時刻ｔａよりも後の時刻ｔｃにおける移動体の位置（（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））を、数１〜３を利用して推定する。なお、この場合には、数１〜３において、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を（０，０，０）とする。
【００６４】
次に、測位処理モジュール３０１は、数４に示したように、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を導出する。
【００６５】
ここで、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）は、一方の推定により得られた時刻ｔｃにおける移動体の位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））と、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))とを含むものである。この予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）は、例えば、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ)) と、一方の推定により得られた時刻ｔｃにおける移動体の位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））との差分、または、位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))、方位角(ｏｒｉｇ’(ｔｃ))、位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））の少なくとも１つに対して重み付けがなされた上での、位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ)) と、位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））との差分である。
【００６６】
なお、数５において、時刻ｔａを時刻ｔｃに置き換えることにより、他方の推定により得られた時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ))から、一方の推定により得られた時刻ｔａにおける移動体の位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））に転換行列の各成分（Ｈｘ，Ｈｙ，Ｈｏｒｉ）を重み付けとして乗算したものを減算することにより、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）を求めた例を得ることができる。また、数７においても、時刻ｔａを時刻ｔｃに置き換えることにより、確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を得ることができる。
【００６７】
次に、測位処理モジュール３０１は、図８に示したように、新たなＧＰＳデータが得られた時刻ｔａよりも後の時刻ｔｃにおける移動体の位置（（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置（（Ｘｇ（ｔｃ），Ｙｇ（ｔｃ））および方位角（ＯＲＩｇ（ｔｃ））を求める。
【００６８】
次に、測位処理モジュール３０１は、図９に示したように、マップマッチング処理された移動体の位置(Ｘｇ(ｔｃ)，Ｙｇ(ｔｃ))および方位角(ＯＲＩｇ(ｔｃ))を始点として、時刻ｔｉにおける移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、ジャイロデータおよび車速データと、上で求めた補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)とを利用して推定する。具体的には、上記した数１〜４を利用して推定する。
【００６９】
その後、測位処理モジュール３０１は、新たなＧＰＳデータが得られた時刻ｔｂよりも後の時刻ｔｄにおける移動体の位置（（ｘｇ（ｔｄ），ｙｇ（ｔｄ））および方位角（ｏｒｉｇ（ｔｄ））に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置（（Ｘｇ（ｔｄ），Ｙｇ（ｔｄ））および方位角（ＯＲＩｇ（ｔｄ））を求める。
【００７０】
また、測位処理モジュール３０１は、ＧＰＳデータから時刻ｔｂにおける最新の移動体の位置(ｘｇ’(ｔｂ)，ｙｇ’(ｔｂ))および方位角(ｏｒｉｇ’(ｔｂ))を算出したのち、算出した移動体の位置（ｘｇ’（ｔｂ），ｙｇ’（ｔｂ））および方位角（ｏｒｉｇ’（ｔｂ））を始点として、時刻ｔｂよりも後の時刻ｔｄにおける移動体の位置（（ｘｇ’（ｔｄ），ｙｇ’（ｔｄ））および方位角（ｏｒｉｇ’（ｔｄ））を、数１〜３を利用して推定する。なお、この場合には、数１〜３において、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を（０，０，０）とする。
【００７１】
その後、測位処理モジュール３０１は、数４に示したように、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を更新する。そして、時刻ｔｄ以降の移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））については、上記した方法を繰り返し行うことにより求めることができる。
【００７２】
このように、測位処理モジュール３０１は、新たなＧＰＳデータが得られた時刻よりも後の時刻（例えばｔｂ）における移動体の位置（例えば（（ｘｇ（ｔｄ），ｙｇ（ｔｄ）））および方位角（例えば（ｏｒｉｇ（ｔｄ））をマップマッチング処理し、そのマップマッチング処理により得られた移動体の位置（例えば（（Ｘｇ（ｔｄ），Ｙｇ（ｔｄ）））および方位角（例えば（ＯＲＩｇ（ｔｄ））と、新たなＧＰＳデータから得られた移動体の位置（例えば（（ｘｇ’（ｔｂ），ｙｇ’（ｔｂ）））および方位角（例えば（ｏｒｉｇ’（ｔｂ））を始点としてジャイロデータおよび車速データを用いて推定された移動体の位置（例えば（（Ｘｇ’（ｔｄ），Ｙｇ’（ｔｄ）））および方位角（例えば（ＯＲＩｇ’（ｔｄ））とを用いることにより補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を求めることも可能である。
【００７３】
ところで、ＧＰＳデータから得られる移動体の位置および方位角は、電離層の影響などによるドリフトや、周期が長いことに起因するジャンプにより、ガウス分布となっていない。そのため、補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を仮に一般的な線形のカルマンフィルタで規定した場合には、数１の解が不安定となり、精度良く移動体の位置を求めることが困難となる。一方、本実施の形態では、補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を、確率密度関数を用いて規定しているので、数１の解が安定となり、精度良く移動体の位置を求めることが可能となる。
【００７４】
図１１は、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の値（破線）の一例と、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)の値（実線）の一例とを示したものである。ここで、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の値は、数５において転換行列（Ｈｘ，Ｈｙ，Ｈｏｒｉ）の各成分を１としたときに得られたものであり、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)の値は、数４において数６および数７を代入したときに得られたものである。
【００７５】
従来のように、補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を一般的な線形のカルマンフィルタで規定した場合には、補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）が一種のローパスフィルタとして振る舞うので、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)の値が急激かつ極端にジャンプするのが抑制されるだけでなく、さらに、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の値に応じて変化することも抑制されてしまう。その結果、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができない。
【００７６】
一方、本実施の形態では、図１１から、補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)の値が急激かつ極端にジャンプするのが抑制されているものの、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の値に応じて変化しているのがわかる。つまり、予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができる。
【００７７】
次に、制御モジュール３０３について説明する。
【００７８】
制御モジュール３０３は、数８に示したように、測位処理モジュール３０１において求められた移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ））に、ＣＣＤカメラ１０１の画像データから得られる道路２の所定の位置からのオフセット（ｘｖ（ｔｉ），ｙｖ（ｔｉ））を加算すると共に、測位処理モジュール３０１において求められた移動体の方位角（ｏｒｉｖ（ｔｉ））に、方位角（ｏｒｉｇ（ｔｉ））と道路２と平行な線（例えば道路２の中央線３）とのなす角（ｏｒｉｖ（ｔｉ））を加算する。これは、主に、測位処理モジュール３０１のマップマッチング処理によって移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ）））や方位角（ｏｒｉｇ（ｔｉ））が道路上の所定の場所（例えば道路の中央線）に移動させられたことによる誤差を取り除くために行われるものである。従って、測位処理モジュール３０１において求められた移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ）））および方位角（ｏｒｉｇ（ｔｉ））を、数８を用いて補正することにより、実際の移動体の位置（（ｘ（ｔｉ），ｙ（ｔｉ）））および方位角（ｏｒｉ（ｔｉ））に戻すことが可能となる。なお、このプロセスは、測位処理モジュール３０１においてマップマッチング処理により求められた移動体の位置（（Ｘｇ（ｔｉ），Ｙｇ（ｔｉ）や方位角（ＯＲＩｇ（ｔｉ））に対してももちろん適用可能である。
【数８】
2008175786.tif 000010
【００７９】
ここで、オフセット（ｘｖ（ｔｉ），ｙｖ（ｔｉ））は、例えば、数９に示したように、地図管理部２０から読み出された地図データから抽出された道路幅Ｗから、ＣＣＤカメラ１０１の画像データから得られた移動体の現在位置と道路２の路側線４との距離ｄ（ｔｉ）を減算して得られた値を、π／２に、移動体の方位角（ｏｒｉｇ（ｔｉ））と、道路２と平行な線（例えば道路２の中央線３）と直交する直交線５と移動体の方位角（ｏｒｉｖ（ｔｉ））とのなす角（θ（ｔｉ））とを加算して得られた角度を用いてｘ成分とｙ成分とに分解することにより得られる。
【数９】
2008175786.tif 000011
【００８０】
このように、制御モジュール３０３は、ＣＣＤカメラ１０１の画像データと、地図管理部２０から読み出された地図データとを利用して、測位処理モジュール３０１において求められた移動体の位置（（ｘｇ（ｔｉ），ｙｇ（ｔｉ）））および方位角（ｏｒｉｇ（ｔｉ））を、実際の移動体の位置（（ｘ（ｔｉ），ｙ（ｔｉ）））および方位角（ｏｒｉ（ｔｉ））に戻したことにより、移動体をリアルタイムに制御することが可能となる。
【００８１】
例えば、移動体の現在の位置（（ｘ（ｔｉ），ｙ（ｔｉ）））の前方の道路形状と、移動体の現在の車速ｖ（ｔｉ）および方位角（ｏｒｉ（ｔｉ））とに基づいて、移動体のブレーキや、ハンドルの操舵角などを制御する制御信号を生成する。また、移動体の走行ルートがあらかじめ設定されている場合には、その走行ルートに基づいて、移動体のアクセルや、ブレーキ、ハンドル、ウインカなどを制御する制御信号を生成する。その結果、制御部５０は、その制御信号に応じた制御を制御対象の機器（例えば、移動体のアクセルや、ブレーキ、ハンドル、ウインカなど）に対して行うこととなる。
【００８２】
次に、本実施の形態の移動体位置検出装置１の動作について説明する。
【００８３】
この移動体位置検出装置１では、測位処理モジュール３０１において、例えば、一のＧＰＳデータを始点として導出された時刻ｔａにおける移動体の位置(ｘｇ(ｔａ)，ｙｇ(ｔａ))および方位角(ｏｒｉｇ(ｔａ))と、その次のＧＰＳデータから得られた時刻ｔａにおける移動体の位置(ｘｇ’(ｔａ)，ｙｇ’(ｔａ))および方位角(ｏｒｉｇ’(ｔａ)) とを含む予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより導出される補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔａ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))が特定される。
【００８４】
また、この移動体位置検出装置１では、測位処理モジュール３０１において、例えば、一のＧＰＳデータを始点として導出された時刻ｔｃにおける移動体の位置(ｘｇ(ｔｃ)，ｙｇ(ｔｃ))および方位角(ｏｒｉｇ(ｔｃ))と、その次のＧＰＳデータを始点として導出された時刻ｔｃにおける移動体の位置(ｘｇ’(ｔｃ)，ｙｇ’(ｔｃ))および方位角(ｏｒｉｇ’(ｔｃ)) とを含む予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）に、その予想誤差（ΔＤｘ，ΔＤｙ，ΔＤｏｒｉ）の確率密度関数（Ｐｘ，Ｐｙ，Ｐｏｒｉ）を含む補償係数（Ｋｘ，Ｋｙ，Ｋｏｒｉ）を乗算することにより導出される補償量(Ｃｘ，Ｃｙ，Ｃｏｒｉ)を用いて、時刻ｔｃ以降の移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))が特定される。
【００８５】
これにより、本実施の形態では、リアルタイムで精度良く時刻ｔａ以降の移動体の現在位置を検出することが可能となる。
【００８６】
また、本実施の形態では、移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ))および方位角(ｏｒｉｇ(ｔｉ))を、時刻ｔｉにおける移動体の位置(ｘｇ(ｔｉ)，ｙｇ(ｔｉ)) の前方の映像データから得られた道路に関するデータを用いて補正しているので、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。
【００８７】
以上、実施の形態を挙げて本発明を説明したが、本発明はこれに限定されるものではなく、種々の変形が可能である。
【００８８】
例えば、上記実施の形態では、測位処理モジュール３０１、画像処理モジュール３０２および制御モジュール３０３はデバイス（ハードウェア）として記述されていたが、中央処理部３０にロードされたプログラムであってもよい。また、記録媒体２に記録されている地図データについても、あらかじめ中央処理部３０にロードされていてもよい。
【図面の簡単な説明】
【００８９】
【図１】本発明の一実施の形態に係る移動体位置検出装置の概要構成図である。
【図２】センサ入力部からの各入力の周期およびタイミングを表す関係図である。
【図３】移動体の変位の様子の一例を表す平面図である。
【図４】移動体の位置および方位角の補正の一例について説明するための平面図である。
【図５】マップマッチング処理について説明するための平面図である。
【図６】図４に続いて行われる補正について説明するための平面図である。
【図７】センサ入力部からの各入力の周期およびタイミングを表す関係図である。
【図８】移動体の位置および方位角の補正の他の例について説明するための平面図である。
【図９】図８に続いて行われる補正について説明するための平面図である。
【図１０】カメラの画像データを利用して移動体の位置および方位角を補正する方法について説明するための平面図である。
【図１１】実施例における移動体の位置（ｘ，ｙ）および方位角の変位の様子を表す変位図である。
【符号の説明】
【００９０】
１…移動体位置検出装置、２…道路、３…道路中央線、４…路側線、５…直交線、１０…センサ入力部、２０…地図管理部、３０…中央処理部、４０…映像表示部、５０…制御部、１０１…ＣＣＤカメラ、１０２…ＧＰＳセンサ、１０３…ジャイロセンサ、１０４…車速センサ、２０１…記録媒体、２０２…ドライバ、３０１…測位処理モジュール、３０２…画像処理モジュール、３０３…制御モジュール。
【図１】
2008175786.tif 000012
【図２】
2008175786.tif 000013
【図３】
2008175786.tif 000014
【図４】
2008175786.tif 000015
【図５】
2008175786.tif 000016
【図６】
2008175786.tif 000017
【図７】
2008175786.tif 000018
【図８】
2008175786.tif 000019
【図９】
2008175786.tif 000020
【図１０】
2008175786.tif 000021
【図１１】
2008175786.tif 000022