JP5261842B2 - 移動体位置検出方法および移動体位置検出装置 - Google Patents

Description

本発明は、移動体の現在位置を検出する移動体位置検出方法および移動体位置検出装置に関する。

自動車車両のような移動体の現在位置を検出し、検出された現在位置を周辺の道路地図と共に表示画面に表示する移動体位置検出装置が各種開発されている。近年の移動体位置検出装置では、ＧＰＳ（Global Positioning System）と慣性センサとを組み合わせることにより移動体の現在位置を割り出す手法が一般的に用いられている。さらに、ＣＤ−ＲＯＭや、ＤＶＤ、ハードディスクなどに記録された地図情報を必要に応じて読み出し、移動体の走行経路の情報と照合することにより、移動体の現在位置を正確に特定するマップマッチングという手法も採り入れられている。このようにして得られた移動体の現在位置の情報を、地図情報に重ね合わせることにより、現在位置とその付近の所定範囲内の道路地図とを表す画像を画像表示装置の画面に表示することが可能となる（特許文献１参照）。

ＷＯ２００４／０４８８９５

ここで、ＧＰＳとは、地球上の現在位置を調べるための衛星測位システムであり、宇宙空間に打ち上げられた多数のＧＰＳ衛星から得られたＧＰＳデータに基づいて現在位置（絶対位置情報）を導出するものである。一般に、３個の衛星からの電波を捕えることができれば地球上の平面での位置がわかり(２次元測位)、４個以上の衛星からの電波を捕えることができればさらに高度の情報を得ることができる(３次元測位)。このＧＰＳの精度は、例えば、補足衛星数や、ＤＯＰ（Dilution of Precision)値、測位の次元（２次元または３次元）、マルチパス誤差、衛星自身の位置誤差、衛星に搭載された時計の誤差、衛星からの電波を受信するレシーバの雑音レベル、電離層による誤差などによって変化するが、廉価な商用ＧＰＳでは、位置（緯度および経度）の精度がおよそ２０メートル、方位角の精度がおよそ１０度となっており、ＧＰＳ衛星から１秒周期でＧＰＳデータが出力される。

また、慣性センサとは、移動体に取り付けられるジャイロスコープや速度センサなどのことであり、移動体の角速度や走行速度などを計測するものである。この慣性センサでは、１／６０秒または１／５０秒周期で移動体の角速度や走行速度が計測されるので、この計測された角速度や走行速度に基づいて移動体の移動距離や方位角など（相対位置情報）を導出することができる。そのため、慣性センサは、ＧＰＳデータを受信できないとき（例えば、トンネルの中にいるとき）や、ＧＰＳデータに大きなノイズが含まれているとき（例えば、ビルとビルの谷間にいるとき）、ＧＰＳデータを補間する目的で用いられることが多い。

ところで、従来の移動体位置検出装置では、ＧＰＳデータを取得してから移動体の現在位置を特定するまでの間に、上記した様々な誤差要素を除去して現在位置の計算を行ったり、慣性センサから得られた移動体の角速度や走行速度を用いて現在位置の補正を行ったり、さらに、マップマッチングを行うなど、種々の情報処理が必要である。そのため、移動体の現在位置を特定するまでの時間（遅延時間）が非常に長く、しかもばらつきを有している場合もあり、移動体の現在位置を特定した時には既にＧＰＳデータを取得した時の位置とは全く別の場所に移動体が移動してしまっていることもある。従って、単に移動体の走行経路の履歴を画面に表示するだけでよいような、リアルタイム性のあまり重要でない用途であれば、遅延時間が長くなったとしてもあまり問題とはならない。しかし、移動体の現在位置などに基づいて、その先の道路特性に応じた制御を移動体に対して行ったり、移動体の運転に役立つ情報を提供するような、リアルタイム性を厳しく要求される用途では、遅延時間を短縮するか、または、別の方策を用いて移動体の現在位置などを即座に推定することが求められる。

そこで、そのようなリアルタイム性を厳しく要求される用途においては、例えば、処理能力を上げて、遅延時間を短縮することが考えられる。しかし、処理能力を上げるために、高速に演算処理することの可能な部品を用いると、コストアップに繋がるので、このような方策を採ることは汎用機器においては現実的ではない。もっとも、仮に高速に演算処理することの可能な部品を用いて遅延時間を短縮した場合であっても、ＧＰＳデータは１秒周期にしか得ることができないので、ＧＰＳデータが来ない間はリアルタイムに現在位置を特定することができない。また、例えば、直近のＧＰＳデータから既に得られている位置および方位角を、慣性センサからのデータを用いて補正することにより、最新のＧＰＳデータから移動体の現在位置が特定されるまでの間、移動体の現在位置を推定することが考えられる。しかし、慣性センサでは推定時間が長くなるほど現在位置の誤差が大きくなる傾向があり、また、直近のＧＰＳデータから既に得られた位置および方位角や、最新のＧＰＳデータから得られた位置に大きな誤差が含まれている場合もある。そのため、慣性センサからのデータを用いて得られた現在位置と、最新のＧＰＳデータから得られた現在位置とが大きく食い違ってしまう可能性がある。そのときに、例えば、慣性センサからのデータを用いて得られた現在位置を、最新のＧＰＳデータから得られた現在位置で置き換えてしまうと、置き換えた瞬間に現在位置がジャンプしてしまう。このような現在位置のジャンプはリアルタイム性を厳しく要求される用途では許容することができない。

このように、従来の技術では、リアルタイムで精度良く移動体の現在位置および方位角を検出することが容易ではないという問題があった。

本発明はかかる問題点に鑑みてなされたもので、その目的は、リアルタイムで精度良く移動体の現在位置および方位角を検出することの可能な移動体位置検出方法および移動体位置検出装置を提供することにある。

本発明による第１の移動体位置検出方法は、以下の（Ａ１）〜（Ａ４）の各ステップを含むものである。
（Ａ１）ＧＰＳデータから得られた時刻ｔ_０における移動体の位置(ｘ_ｇ(ｔ_０)，ｙ_ｇ(ｔ_０))および方位角(ｏｒｉ_ｇ(ｔ_０))を、次のＧＰＳデータが得られる時刻ｔ_ａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))を推定するステップ
（Ａ２）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) と、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を導出するステップ
（Ａ３）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))を、その道路上の位置(Ｘ_ｇ(ｔ_ａ)，Ｙ_ｇ(ｔ_ａ))および方位角(ＯＲＩ_ｇ(ｔ_ａ))に移動するステップ
（Ａ４）移動体の位置(Ｘ_ｇ(ｔ_ａ)，Ｙ_ｇ(ｔ_ａ))および方位角(ＯＲＩ_ｇ(ｔ_ａ))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)とを用いて補正することにより、時刻ｔ_ａ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ)) （ｉは変数）を特定するステップ

本発明による第１の移動体位置検出装置は、以下の（Ｂ１）〜（Ｂ５）の各手段を備えたものである。
（Ｂ１）受信装置によって受信されたＧＰＳデータから時刻ｔ_ｉ（ｉは変数）における移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
（Ｂ２）ＧＰＳデータから得られた時刻ｔ_０における移動体の位置(ｘ_ｇ(ｔ_０)，ｙ_ｇ(ｔ_０))および方位角(ｏｒｉ_ｇ(ｔ_０))を、次のＧＰＳデータが得られる時刻ｔ_ａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))を推定する移動体位置推定手段
（Ｂ３）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) と、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を導出する補償量導出手段
（Ｂ４）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))を、その道路上の位置(Ｘ_ｇ(ｔ_ａ)，Ｙ_ｇ(ｔ_ａ))および方位角(ＯＲＩ_ｇ(ｔ_ａ))に移動する移動体位置移動手段
（Ｂ５）移動体の位置(Ｘ_ｇ(ｔ_ａ)，Ｙ_ｇ(ｔ_ａ))および方位角(ＯＲＩ_ｇ(ｔ_ａ))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)とを用いて補正することにより、時刻ｔ_ａ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を特定する移動体位置特定手段

本発明による第１の移動体位置検出方法および第１の移動体位置検出装置では、一のＧＰＳデータを始点として推定された時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))と、その次のＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出された補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を用いて、時刻ｔ_ａ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))が特定される。

ここで、予想誤差を、ＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) と、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))との差分、または、位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))、方位角(ｏｒｉ_ｇ’(ｔ_ａ))、位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))の少なくとも１つに対して重み付けがなされた上での、位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) と、位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))との差分とすることが可能である。

移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を、時刻ｔ_ｉにおける移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。

本発明による第２の移動体位置検出方法は、以下の（Ｃ１）〜（Ｃ５）の各ステップを含むものである。
（Ｃ１）ＧＰＳデータから得られた時刻ｔ_０における移動体の位置(ｘ_ｇ(ｔ_０)，ｙ_ｇ(ｔ_０))および方位角(ｏｒｉ_ｇ(ｔ_０))を、所定の時刻ｔ_ｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))を推定するステップ
（Ｃ２）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) を、時刻ｔ_ｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))を推定するステップ
（Ｃ３）一方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))と、他方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を導出するステップ
（Ｃ４）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))を、その道路上の位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))に移動するステップ
（Ｃ５）移動体の位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)とを用いて補正することにより、時刻ｔ_ｃ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ)) （ｉは変数）を特定するステップ

本発明による第２の移動体位置検出装置は、以下の（Ｄ１）〜（Ｄ６）の各手段を備えたものである。
（Ｄ１）受信装置によって受信されたＧＰＳデータから時刻ｔ_ｉ（ｉは変数）における移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
（Ｄ２）ＧＰＳデータから得られた時刻ｔ_０における移動体の位置(ｘ_ｇ(ｔ_０)，ｙ_ｇ(ｔ_０))および方位角(ｏｒｉ_ｇ(ｔ_０))を、所定の時刻ｔ_ｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))を推定する第１移動体位置推定手段
（Ｄ３）次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) を、時刻ｔ_ｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))を推定する第２移動体位置推定手段
（Ｄ４）一方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))と、他方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を導出する補償量導出手段
（Ｄ５）移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))を、その道路上の位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))に移動する移動体位置移動手段
（Ｄ６）移動体の位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)とを用いて補正することにより、時刻ｔ_ｃ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ)) （ｉは変数）を特定する移動体位置特定手段

本発明による第２の移動体位置検出方法および第２の移動体位置検出装置では、一のＧＰＳデータを始点として推定された時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))と、その次のＧＰＳデータを始点として推定された時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出される補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を用いて、時刻ｔ_ｃ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))が特定される。

ここで、予想誤差を、他方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))と、一方の推定により得られた時刻ｔ_ｃにおける位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))との差分、または、移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))、方位角(ｏｒｉ_ｇ’(ｔ_ｃ))、位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))の少なくとも１つに対して重み付けがなされた上での、移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))と、移動体の位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))との差分であるとすることが可能である。

また、移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を、時刻ｔ_ｉにおける移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。

本発明による第１の移動体位置検出方法および第１の移動体位置検出装置によれば、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を用いて、時刻ｔ_ａ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))が特定される。これにより、リアルタイムで精度良く時刻ｔ_ａ以降の移動体の現在位置および方位角を検出することが可能となる。

本発明による第２の移動体位置検出方法および第２の移動体位置検出装置によれば、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を用いて、時刻ｔ_ｃ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))が特定される。これにより、リアルタイムで精度良く時刻ｔ_ｃ以降の移動体の現在位置および方位角を検出することが可能となる。

ここで、本発明による第１および第２の移動体位置検出方法ならびに第１および第２の移動体位置検出装置において、移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を、時刻ｔ_ｉにおける移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ)) の前方の映像データから得られた道路に関するデータを用いて補正した場合には、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。

以下、本発明を実施するための最良の形態について図面を参照して詳細に説明する。

図１は、本発明の移動体位置検出装置１の概要構成を表したものである。なお、本発明の実施の形態に係る移動体位置検出方法は、この移動体位置検出装置１の動作によって具現化されるものであるから、以下、それらを併せて説明する。

この移動体位置検出装置１は、運転者の運転する移動体（例えば自動車）に搭載されたものであり、例えば、センサ入力部１０と、地図管理部２０と、中央処理部３０と、映像表示部４０と、制御部５０とを、その主要部として備えている。なお、センサ入力部１０、地図管理部２０、映像表示部４０および制御部５０が、移動体位置検出装置１の外部装置として設けられていてもよい。

センサ入力部１０は、ＣＣＤ（固体撮像素子）カメラ１０１と、ＧＰＳセンサ１０２と、ジャイロセンサ１０３（慣性センサ）と、車速センサ１０４（速度センサ）とを備えている。

ＣＣＤカメラ１０１は、例えば運転者がフロントガラス越しに見る視線とほぼ同様のカメラアングルで車両前方の景観を撮影（撮像）することができるように、移動体に固定されている。このＣＣＤカメラ１０１は、例えば固定焦点距離の単眼方式のものであり、道路を含んだ前方の景観を例えば１／３０秒ごとに撮像して、撮像により得られた画像データを中央処理部３０に転送するようになっている。

ＧＰＳセンサ１０２は、例えば、地球上の現在位置を調べるための衛星測位システム（ＧＰＳ）において、宇宙空間に打ち上げられた多数のＧＰＳ衛星から地表に向けて送信されてくる電波を受信するレシーバであり、受信した電波に含まれる各種データ（ＧＰＳデータ）を中央処理部３０に転送するようになっている。なお、中央処理部３０は、ＧＰＳデータから、移動体の位置（緯度および経度）と、移動体の方位角とを算出することができる。

ＧＰＳ衛星からは、例えば１秒ごとにＧＰＳデータを含む電波が出力されているが、その電波は、地表に到達するまでの間に電離層などの影響を受けて、１秒から２秒の不確定性を持ってレシーバに到達する。ここで、廉価な商用ＧＰＳでは、位置（緯度および経度）の精度がおよそ２０メートル、方位角の精度がおよそ１０度となっている。

なお、ＧＰＳ衛星から送信されてくる電波を受信する代わりに、ＤＧＰＳ(Differential GPS)において、基準局から送信されてくる中波やＦＭ放送などの地上波を受信するようにしてもよい。このＤＧＰＳとは、位置の分かっている基準局でＧＰＳによる測量を行い、実際の位置とＧＰＳで算出された位置とのずれを中波やＦＭ放送などの地上波で送信することにより、ＧＰＳ衛星からの信号により計測した結果を補正することの可能なシステムであり、ＧＰＳよりも精度よく測位することが可能である。

ジャイロセンサ１０３は、例えば、コリオリ力による振動を電気的に検出する３次元振動ジャイロからなり、検出した振動データを中央処理部３０に転送するようになっている。なお、中央処理部３０は、得られた振動データから、例えば１／６０秒ごとに移動体の角速度を算出することが可能である。

車速センサ１０４は、例えば、車速の大きさに応じたパルス信号を出力するパルスセンサであり、出力されたパルス信号を中央処理部３０に転送するようになっている。なお、中央処理部３０は、得られたパルス信号から、例えば１／５０秒ごとに移動体の車速を算出することが可能である。

地図管理部２０は、所定の地理的領域の地図データが記録された記録媒体２０１と、この記録媒体２０１から地図データを読み出すドライバ２０２とを有している。この地図管理部２０は、中央処理部３０からの読み出し命令に従って、地図データ（例えば、所定の地域に含まれる道路に関するデータや、その道路に付随するデータなど）を記録媒体２０１から読み出して、中央処理部３０に転送するようになっている。

ここで、道路に関するデータには、例えば、ノードと呼ばれる交差点、分岐点および補間点などの位置（緯度、経度、海抜）や、ノード間を繋ぐ道路セグメントの等級（国道、県道、農道などにそれぞれ割り当てられている数値）、道路セグメントの形状（水平曲率、縦断曲率）、道路セグメントの車線数などが含まれている。また、道路に付随するデータには、例えば、ランドマーク、鉄道の駅、病院およびガソリンスタンドなどの位置、形状、大きさおよびアイコンなどが含まれている。

中央処理部３０は、例えば、測位処理モジュール３０１と、画像処理モジュール３０２と、制御モジュール３０３とをその主要部として有している。

測位処理モジュール３０１は、センサ入力部１０から入力された各種データおよびパルス信号から、移動体の位置、方位角、角速度および車速を算出し、算出した移動体の位置、方位角、角速度および車速と、地図管理部２０から読み出された道路に関するデータとに基づいて、移動体の位置および方位角を特定する演算を行うデバイスである。この測位処理モジュール３０１は本実施の形態における要部の１つであることから、これについては後に詳述する。

画像処理モジュール３０２は、例えば、地図管理部２０から読み出された地図データ上に、測位処理モジュール３０１で特定された移動体の位置および方位角を重ね合わせた画像データを生成したり、ＣＣＤカメラ１０１で撮像された画像データ上に、道路に付随するデータをアイコンで表現したものを重ね合わせた画像データを生成するするデバイスである。

制御モジュール３０３は、測位処理モジュール３０１で特定された移動体の位置および方位角と、ＣＣＤカメラ１０１から得られた画像データと、地図管理部２０から読み出された地図データとに基づいて、移動体をリアルタイムに制御する制御信号を生成するデバイスである。この制御モジュール３０３も本実施の形態における要部の１つであることから、これについても後に詳述する。

映像表示部４０は、例えば液晶表示装置からなり、画像処理モジュール３０２によって生成された画像データを画面に表示するようになっている。

制御部５０は、制御モジュール３０３によって生成された制御信号に基づいて、制御対象の機器（例えばブレーキやハンドル）を制御するデバイスである。

次に、測位処理モジュール３０１について説明する。

図２（１），（２），（３）は、各センサからデータが出力されるタイミングを模式的に表したものである。図２（１），（２），（３）に示したように、ＧＰＳデータは周期Ｔ_ｇでＧＰＳセンサ１０２から出力され、ジャイロデータは周期Ｔ_ｗでジャイロセンサ１０３から出力され、車速データは周期Ｔ_ｖで車速センサ１０４から出力される。ここで、ジャイロデータの周期Ｔ_ｗおよび車速データの周期Ｔ_ｖは、ＧＰＳデータの周期Ｔ_ｇよりも短い。そのため、測位処理モジュール３０１は、ＧＰＳデータがこない間は、ジャイロデータおよび車速データを用いて移動体の位置および方位角を推定する。また、各センサからデータが出力されるタイミングは、一般に互いに同期していない。そこで、測位処理モジュール３０１は、各センサからの出力タイミングが互いに同期していない場合には、例えば、ジャイロセンサ１０３または車速センサ１０４の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定する。なお、本実施の形態では、ジャイロセンサ１０３の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定するものとする。

図３は、移動体の位置および方位角が時々刻々と変化する様子を模式的に表したものである。なお、図３には、図２（１），（２），（３）の時刻ｔ_０，ｔ_１，ｔ_２における移動体の変位を高さ方向から眺めた様子が例示されている。図２（１），（２），（３）および図３から、測位処理モジュール３０１は、時刻ｔ_０における移動体の位置（ｘ_ｇ（ｔ_０），ｙ_ｇ（ｔ_０））および方位角（ｏｒｉ_ｇ（ｔ_０））を、ＧＰＳデータから直接得ることができる。他方、時刻ｔ_１における移動体の位置（ｘ_ｇ（ｔ_１），ｙ_ｇ（ｔ_１））および方位角（ｏｒｉ_ｇ（ｔ_１））を、ＧＰＳデータから直接得ることができない。そこで、測位処理モジュール３０１は、車速データから得られた移動体の車速ｖ（ｔ_０）にΔｔ（＝時刻ｔ_１−時刻ｔ_０）を乗算することにより移動距離Δｄ（ｔ_０）を求めると共に、ジャイロデータから得られた移動体の角速度ｗ（ｔ_０）にΔｔを乗算することにより方位角の変位量Δｏｒｉ（ｔ_０）を求めたのち、移動距離Δｄ（ｔ_０）を（ｏｒｉ_ｇ（ｔ_０）＋Δｏｒｉ（ｔ_０））（＝ｏｒｉ_ｇ（ｔ_１））を用いてｘ成分とｙ成分とに分解することにより、時刻ｔ_１における移動体の位置（ｘ_ｇ（ｔ_１），ｙ_ｇ（ｔ_１））および方位角（ｏｒｉ_ｇ（ｔ_１））を推定する。これらを一般化したものを以下の数１〜３に示す。なお、数１〜３中のｉは、ゼロ以上の整数である。また、数１〜３中の（Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ）は、後述するように補償量であるが、このときは（０，０，０）とする。

このように、測位処理モジュール３０１は、ＧＰＳデータから直接得られた時刻ｔ_０における移動体の位置（ｘ_ｇ（ｔ_０），ｙ_ｇ（ｔ_０））および方位角（ｏｒｉ_ｇ（ｔ_０））を始点として、時刻ｔ_ｉにおける移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、ジャイロデータおよび車速データを利用して推定する。

次に、測位処理モジュール３０１は、図２に示したように、時刻ｔ_ａにおいて次のＧＰＳデータが得られた場合には、まず、ＧＰＳの測位品質を評価することが好ましい。具体的には、補足衛星数や、ＤＯＰ値、測位の次元（２次元または３次元）を評価することが考えられる。この場合には、例えば、補足衛星数が７個以下、ＤＯＰ値が２．０以上、測位の次元が２次元となっているときには、ＧＰＳデータから得られる時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))は大きな誤差を含んでおり、ＧＰＳの測位品質が悪いと判断する。また、ＧＰＳデータから時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))を算出したのちに、時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))と時刻ｔ_０における移動体の位置(ｘ_ｇ(ｔ_０)，ｙ_ｇ(ｔ_０))との差分と、車速データを用いて算出された時刻ｔ_０から時刻ｔ_ａまでの移動距離との誤差を評価することが考えられる。この場合には、その誤差が所定の値を超えたときには、ＧＰＳの測位品質が悪いと判断する。以上のようにしてＧＰＳの測位品質を評価した結果、ＧＰＳの測位品質が悪いと判断した場合には、その値を用いないで、引き続き、時刻ｔ_ｉにおける移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、ジャイロデータおよび車速データを利用して推定する。逆に、ＧＰＳデータから得られる時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))が大きな誤差を含んでいないと判断した場合には、以下の手順を実行する。

なお、時刻ｔ_ａに得られる予定のＧＰＳデータが得られない場合には、引き続き、時刻ｔ_ｉにおける移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、ジャイロデータおよび車速データを利用して推定する。また、上の評価において、ＧＰＳデータから時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))を算出する手順が含まれていない場合には、移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))をＧＰＳデータから算出したのち、以下の手順を実行するものとする。また、以下の手順において、ＧＰＳデータから移動体の位置および方位角を算出する場合には、上記した評価方法を実行することが好ましい。

次に、測位処理モジュール３０１は、数４に示したように、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）に、その予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を含む補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を導出する。

ここで、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）は、ＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))と、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))とを含むものである。予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）は、例えば、ＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) と、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))との差分、または、位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))、方位角(ｏｒｉ_ｇ’(ｔ_ａ))、位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))の少なくとも１つに対して重み付けがなされた上での、位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) と、位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))との差分である。なお、ＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))から、推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))に転換行列の各成分（Ｈ_ｘ，Ｈ_ｙ，Ｈ_ｏｒｉ）を重み付けとして乗算したものを減算することにより、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）を求めた例を数５に示した。

また、補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）は、拡張されたカルマンフィルタであり、例えば、数６に示したように、１から、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を減算することにより求められる。ここで、確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）は、例えば、数７に示したようなガウス分布として規定されている。

次に、図４に示したように、時刻ｔ_ａにおける推定値（（ｘ_ｇ（ｔ_ａ），ｙ_ｇ（ｔ_ａ），ｏｒｉ_ｇ（ｔ_ａ））に対してマップマッチング処理を行うことにより、道路上の移動体の位置（Ｘ_ｇ(ｔ_ａ)，Ｘ_ｇ(ｔ_ａ)) および方位角（ＯＲＩ_ｇ（ｔ_ａ））を求める。なお、図４は、時刻ｔ_ａにおける移動体の位置（（ｘ_ｇ（ｔ_ａ），ｙ_ｇ（ｔ_ａ））および方位角（ｏｒｉ_ｇ（ｔ_ａ））がマップマッチング処理により所定の位置（Ｘ_ｇ(ｔ_ａ)，Ｘ_ｇ(ｔ_ａ)) および方位角（ＯＲＩ_ｇ（ｔ_ａ））に移動する様子を模式的に表したものである。

マップマッチング処理は、例えば、生成された道路形状モデルと、移動体の種々の情報とを対比し、その対比の結果に基づいて、移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置を強制的に移動させる処理であり、例えば、以下の２つの方法により行うことが可能である。

なお、上記の道路形状モデルは、例えば、移動体の位置および方位角をベースに、移動体位置の前方の道路に関するデータを検索し、検索により得られた道路に関するデータに基づいて、道路の水平形状を表す数学モデルであるクロソイド（clothoid）曲線モデルのパラメータを推定することにより生成することが可能である。

まず、１つ目の方法では、（１）推定により得られた時刻ｔ_ａおよびその直近の時刻において移動体が走行した経路（走行経路）の形状と、推定により得られた時刻ｔ_ａにおける移動体の位置（ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))の近傍（所定の距離以下）に存在する１または複数の道路の形状とを対比し、移動体の走行経路の形状に最も近似した形状を有する道路を１つ抽出する。例えば、図５に示したように、移動体の走行経路Ｔの形状と、移動体の近傍にある道路Ｒ１，Ｒ２のそれぞれの形状とを対比し、走行経路Ｔの形状により近い道路Ｒ１を抽出する。（２）次に、抽出した道路の中央線上に移動体の位置（ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))を移動して、中央線上に移動した後の移動体の位置（Ｘ_ｇ(ｔ_ａ)，Ｘ_ｇ(ｔ_ａ))を求める。例えば、抽出した道路の中央線に対して移動体の位置（ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))から下ろした垂線が、抽出した道路の中央線と交わる位置を算出し、その位置に移動体の位置（ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))を移動する。

次に、２つ目の方法では、（１）推定により得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))と、推定により得られた移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))の近傍（所定の距離以下）に存在する１または複数の道路の中央線の位置および方位角とを対比し、移動体が最も実在しそうな位置および方位角の中央線を有する道路を１つ抽出する。例えば、図５に示したように、移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))と、移動体の近傍にある道路Ｒ１，Ｒ２のそれぞれの中央線の位置および方位角とを対比し、移動体が最も実在しそうな道路Ｒ１を抽出する。（２）次に、抽出した道路の中央線のうち、移動体が最も実在しそうな位置および方位角を有する中央線上に移動体の位置（ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))を移動して、中央線上に移動した後の移動体の位置（Ｘ_ｇ(ｔ_ａ)，Ｘ_ｇ(ｔ_ａ))を求める。例えば、移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))に最も近似した位置および方位角の中央線上に、移動体の位置（ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))を移動する。

なお、マップマッチング処理において、移動体の方位角（ｏｒｉ_ｇ（ｔ_ａ））に対して、移動先の中央線の方位角と等しくする補正をすることにより道路の央線上に移動した後の移動体の方位角（ＯＲＩ_ｇ（ｔ_ａ））を得てもよいし、全く補正しないで道路の央線上に移動した後の移動体の方位角（ＯＲＩ_ｇ（ｔ_ａ））を得てもよい。

次に、測位処理モジュール３０１は、図６に示したように、マップマッチング処理された移動体の位置(Ｘ_ｇ(ｔ_ａ)，Ｙ_ｇ(ｔ_ａ))および方位角(ＯＲＩ_ｇ(ｔ_ａ))を始点として、時刻ｔ_ｉにおける移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、ジャイロデータおよび車速データと、上で求めた補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)とを利用して推定する。具体的には、上記した数１〜４を利用して推定する。

次に、測位処理モジュール３０１は、時刻ｔ_ｂにおいて次のＧＰＳデータが得られた場合には、ＧＰＳデータから時刻ｔ_ｂにおける最新の移動体の位置(ｘ_ｇ’(ｔ_ｂ)，ｙ_ｇ’(ｔ_ｂ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))を算出する。続いて、数４に示したように、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）に、その予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を含む補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を更新する。

その後、時刻ｔ_ｂにおける推定値（（ｘ_ｇ（ｔ_ｂ），ｙ_ｇ（ｔ_ｂ），ｏｒｉ_ｇ（ｔ_ｂ））に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置（Ｘ_ｇ(ｔ_ｂ)，Ｘ_ｇ(ｔ_ｂ))および方位角（ＯＲＩ_ｇ（ｔ_ｂ））を求める。そして、時刻ｔ_ｂ以降の移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））については、上記した方法を繰り返し行うことにより求めることができる。

このように、測位処理モジュール３０１は、新たなＧＰＳデータが得られた時刻（例えばｔ_ｂ）における移動体の位置（例えば（（ｘ_ｇ（ｔ_ｂ），ｙ_ｇ（ｔ_ｂ）））および方位角（例えば（ｏｒｉ_ｇ（ｔ_ｂ））をマップマッチング処理し、それにより得られた移動体の位置（例えば（（Ｘ_ｇ（ｔ_ｂ），Ｙ_ｇ（ｔ_ｂ）））および方位角（例えば（ＯＲＩ_ｇ（ｔ_ｂ））と、新たなＧＰＳデータから得られた移動体の位置（例えば（（ｘ_ｇ’（ｔ_ｂ），ｙ_ｇ’（ｔ_ｂ）））および方位角（例えば（ｏｒｉ_ｇ’（ｔ_ｂ））とを用いることにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を求めている。しかし、本実施の形態では、測位処理モジュール３０１は、上記した処理手順に限られるものではなく、例えば、以下に示したような処理手順を行うことにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を求めることが可能である。

図７（１），（２），（３）は、各センサからデータが出力されるタイミングを模式的に表したものである。図８は、時刻ｔ_ａにおける移動体の位置（（ｘ_ｇ（ｔ_ａ），ｙ_ｇ（ｔ_ａ））がマップマッチング処理により所定の位置（Ｘ_ｇ(ｔ_ａ)，Ｘ_ｇ(ｔ_ａ))に移動する様子を模式的に表したものであり、図９は、時刻ｔ_ｄにおける移動体の位置（（ｘ_ｇ（ｔ_ｄ），ｙ_ｇ（ｔ_ｄ））がマップマッチング処理により所定の位置（Ｘ_ｇ(ｔ_ｄ)，Ｘ_ｇ(ｔ_ｄ))に移動する様子を模式的に表したものである。

測位処理モジュール３０１は、ＧＰＳデータから時刻ｔ_０における移動体の位置(ｘ_ｇ(ｔ_０)，ｙ_ｇ(ｔ_０))および方位角(ｏｒｉ_ｇ(ｔ_０))を算出したのち、算出した移動体の位置（ｘ_ｇ（ｔ_０），ｙ_ｇ（ｔ_０））および方位角（ｏｒｉ_ｇ（ｔ_０））を始点として、時刻ｔ_ｉにおける移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、数１〜３を利用して推定し、新たなＧＰＳデータが得られる時刻ｔ_ａよりも後の時刻ｔ_ｃにおける移動体の位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））も数１〜３を利用して推定する。なお、この場合には、数１〜３において、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を（０，０，０）とする。

また、測位処理モジュール３０１は、ＧＰＳデータから時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ))を算出したのち、算出した移動体の位置（ｘ_ｇ’（ｔ_ａ），ｙ_ｇ’（ｔ_ａ））および方位角（ｏｒｉ_ｇ’（ｔ_ａ））を始点として、時刻ｔ_ａよりも後の時刻ｔ_ｃにおける移動体の位置（（ｘ_ｇ’（ｔ_ｃ），ｙ_ｇ’（ｔ_ｃ））および方位角（ｏｒｉ_ｇ’（ｔ_ｃ））を、数１〜３を利用して推定する。なお、この場合には、数１〜３において、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を（０，０，０）とする。

次に、測位処理モジュール３０１は、数４に示したように、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）に、その予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を含む補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を導出する。

ここで、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）は、一方の推定により得られた時刻ｔ_ｃにおける移動体の位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））と、他方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))とを含むものである。この予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）は、例えば、他方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ)) と、一方の推定により得られた時刻ｔ_ｃにおける移動体の位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））との差分、または、位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))、方位角(ｏｒｉ_ｇ’(ｔ_ｃ))、位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））の少なくとも１つに対して重み付けがなされた上での、位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ)) と、位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））との差分である。

なお、数５において、時刻ｔ_ａを時刻ｔ_ｃに置き換えることにより、他方の推定により得られた時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ))から、一方の推定により得られた時刻ｔ_ａにおける移動体の位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））に転換行列の各成分（Ｈ_ｘ，Ｈ_ｙ，Ｈ_ｏｒｉ）を重み付けとして乗算したものを減算することにより、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）を求めた例を得ることができる。また、数７においても、時刻ｔ_ａを時刻ｔ_ｃに置き換えることにより、確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を得ることができる。

次に、測位処理モジュール３０１は、図８に示したように、新たなＧＰＳデータが得られた時刻ｔ_ａよりも後の時刻ｔ_ｃにおける移動体の位置（（ｘ_ｇ（ｔ_ｃ），ｙ_ｇ（ｔ_ｃ））および方位角（ｏｒｉ_ｇ（ｔ_ｃ））に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置（（Ｘ_ｇ（ｔ_ｃ），Ｙ_ｇ（ｔ_ｃ））および方位角（ＯＲＩ_ｇ（ｔ_ｃ））を求める。

次に、測位処理モジュール３０１は、図９に示したように、マップマッチング処理された移動体の位置(Ｘ_ｇ(ｔ_ｃ)，Ｙ_ｇ(ｔ_ｃ))および方位角(ＯＲＩ_ｇ(ｔ_ｃ))を始点として、時刻ｔ_ｉにおける移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、ジャイロデータおよび車速データと、上で求めた補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)とを利用して推定する。具体的には、上記した数１〜４を利用して推定する。

その後、測位処理モジュール３０１は、新たなＧＰＳデータが得られた時刻ｔ_ｂよりも後の時刻ｔ_ｄにおける移動体の位置（（ｘ_ｇ（ｔ_ｄ），ｙ_ｇ（ｔ_ｄ））および方位角（ｏｒｉ_ｇ（ｔ_ｄ））に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置（（Ｘ_ｇ（ｔ_ｄ），Ｙ_ｇ（ｔ_ｄ））および方位角（ＯＲＩ_ｇ（ｔ_ｄ））を求める。

また、測位処理モジュール３０１は、ＧＰＳデータから時刻ｔ_ｂにおける最新の移動体の位置(ｘ_ｇ’(ｔ_ｂ)，ｙ_ｇ’(ｔ_ｂ))および方位角(ｏｒｉ_ｇ’(ｔ_ｂ))を算出したのち、算出した移動体の位置（ｘ_ｇ’（ｔ_ｂ），ｙ_ｇ’（ｔ_ｂ））および方位角（ｏｒｉ_ｇ’（ｔ_ｂ））を始点として、時刻ｔ_ｂよりも後の時刻ｔ_ｄにおける移動体の位置（（ｘ_ｇ’（ｔ_ｄ），ｙ_ｇ’（ｔ_ｄ））および方位角（ｏｒｉ_ｇ’（ｔ_ｄ））を、数１〜３を利用して推定する。なお、この場合には、数１〜３において、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を（０，０，０）とする。

その後、測位処理モジュール３０１は、数４に示したように、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）に、その予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を含む補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を乗算することにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を更新する。そして、時刻ｔ_ｄ以降の移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））については、上記した方法を繰り返し行うことにより求めることができる。

このように、測位処理モジュール３０１は、新たなＧＰＳデータが得られた時刻よりも後の時刻（例えばｔ_ｂ）における移動体の位置（例えば（（ｘ_ｇ（ｔ_ｄ），ｙ_ｇ（ｔ_ｄ）））および方位角（例えば（ｏｒｉ_ｇ（ｔ_ｄ））をマップマッチング処理し、そのマップマッチング処理により得られた移動体の位置（例えば（（Ｘ_ｇ（ｔ_ｄ），Ｙ_ｇ（ｔ_ｄ）））および方位角（例えば（ＯＲＩ_ｇ（ｔ_ｄ））と、新たなＧＰＳデータから得られた移動体の位置（例えば（（ｘ_ｇ’（ｔ_ｂ），ｙ_ｇ’（ｔ_ｂ）））および方位角（例えば（ｏｒｉ_ｇ’（ｔ_ｂ））を始点としてジャイロデータおよび車速データを用いて推定された移動体の位置（例えば（（Ｘ_ｇ’（ｔ_ｄ），Ｙ_ｇ’（ｔ_ｄ）））および方位角（例えば（ＯＲＩ_ｇ’（ｔ_ｄ））とを用いることにより補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を求めることも可能である。

ところで、ＧＰＳデータから得られる移動体の位置および方位角は、電離層の影響などによるドリフトや、周期が長いことに起因するジャンプにより、ガウス分布となっていない。そのため、補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を仮に一般的な線形のカルマンフィルタで規定した場合には、数１の解が不安定となり、精度良く移動体の位置を求めることが困難となる。一方、本実施の形態では、補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を、確率密度関数を用いて規定しているので、数１の解が安定となり、精度良く移動体の位置を求めることが可能となる。

図１１は、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の値（破線）の一例と、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)の値（実線）の一例とを示したものである。ここで、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の値は、数５において転換行列（Ｈ_ｘ，Ｈ_ｙ，Ｈ_ｏｒｉ）の各成分を１としたときに得られたものであり、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)の値は、数４において数６および数７を代入したときに得られたものである。

従来のように、補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を一般的な線形のカルマンフィルタで規定した場合には、補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）が一種のローパスフィルタとして振る舞うので、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)の値が急激かつ極端にジャンプするのが抑制されるだけでなく、さらに、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の値に応じて変化することも抑制されてしまう。その結果、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができない。

一方、本実施の形態では、図１１から、補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)の値が急激かつ極端にジャンプするのが抑制されているものの、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の値に応じて変化しているのがわかる。つまり、予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができる。

次に、制御モジュール３０３について説明する。

制御モジュール３０３は、数８に示したように、測位処理モジュール３０１において求められた移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ））に、ＣＣＤカメラ１０１の画像データから得られる道路２の所定の位置からのオフセット（ｘ_ｖ（ｔ_ｉ），ｙ_ｖ（ｔ_ｉ））を加算すると共に、測位処理モジュール３０１において求められた移動体の方位角（ｏｒｉ_ｖ（ｔ_ｉ））に、方位角（ｏｒｉ_ｇ（ｔ_ｉ））と道路２と平行な線（例えば道路２の中央線３）とのなす角（ｏｒｉ_ｖ（ｔ_ｉ））を加算する。これは、主に、測位処理モジュール３０１のマップマッチング処理によって移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ）））や方位角（ｏｒｉ_ｇ（ｔ_ｉ））が道路上の所定の場所（例えば道路の中央線）に移動させられたことによる誤差を取り除くために行われるものである。従って、測位処理モジュール３０１において求められた移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ）））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、数８を用いて補正することにより、実際の移動体の位置（（ｘ（ｔ_ｉ），ｙ（ｔ_ｉ）））および方位角（ｏｒｉ（ｔ_ｉ））に戻すことが可能となる。なお、このプロセスは、測位処理モジュール３０１においてマップマッチング処理により求められた移動体の位置（（Ｘ_ｇ（ｔ_ｉ），Ｙ_ｇ（ｔ_ｉ）や方位角（ＯＲＩ_ｇ（ｔ_ｉ））に対してももちろん適用可能である。

ここで、オフセット（ｘ_ｖ（ｔ_ｉ），ｙ_ｖ（ｔ_ｉ））は、例えば、数９に示したように、地図管理部２０から読み出された地図データから抽出された道路幅Ｗから、ＣＣＤカメラ１０１の画像データから得られた移動体の現在位置と道路２の路側線４との距離ｄ（ｔ_ｉ）を減算して得られた値を、π／２に、移動体の方位角（ｏｒｉ_ｇ（ｔ_ｉ））と、道路２と平行な線（例えば道路２の中央線３）と直交する直交線５と移動体の方位角（ｏｒｉ_ｖ（ｔ_ｉ））とのなす角（θ（ｔ_ｉ））とを加算して得られた角度を用いてｘ成分とｙ成分とに分解することにより得られる。

このように、制御モジュール３０３は、ＣＣＤカメラ１０１の画像データと、地図管理部２０から読み出された地図データとを利用して、測位処理モジュール３０１において求められた移動体の位置（（ｘ_ｇ（ｔ_ｉ），ｙ_ｇ（ｔ_ｉ）））および方位角（ｏｒｉ_ｇ（ｔ_ｉ））を、実際の移動体の位置（（ｘ（ｔ_ｉ），ｙ（ｔ_ｉ）））および方位角（ｏｒｉ（ｔ_ｉ））に戻したことにより、移動体をリアルタイムに制御することが可能となる。

例えば、移動体の現在の位置（（ｘ（ｔ_ｉ），ｙ（ｔ_ｉ）））の前方の道路形状と、移動体の現在の車速ｖ（ｔ_ｉ）および方位角（ｏｒｉ（ｔ_ｉ））とに基づいて、移動体のブレーキや、ハンドルの操舵角などを制御する制御信号を生成する。また、移動体の走行ルートがあらかじめ設定されている場合には、その走行ルートに基づいて、移動体のアクセルや、ブレーキ、ハンドル、ウインカなどを制御する制御信号を生成する。その結果、制御部５０は、その制御信号に応じた制御を制御対象の機器（例えば、移動体のアクセルや、ブレーキ、ハンドル、ウインカなど）に対して行うこととなる。

次に、本実施の形態の移動体位置検出装置１の動作について説明する。

この移動体位置検出装置１では、測位処理モジュール３０１において、例えば、一のＧＰＳデータを始点として導出された時刻ｔ_ａにおける移動体の位置(ｘ_ｇ(ｔ_ａ)，ｙ_ｇ(ｔ_ａ))および方位角(ｏｒｉ_ｇ(ｔ_ａ))と、その次のＧＰＳデータから得られた時刻ｔ_ａにおける移動体の位置(ｘ_ｇ’(ｔ_ａ)，ｙ_ｇ’(ｔ_ａ))および方位角(ｏｒｉ_ｇ’(ｔ_ａ)) とを含む予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）に、その予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を含む補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を乗算することにより導出される補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を用いて、時刻ｔ_ａ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))が特定される。

また、この移動体位置検出装置１では、測位処理モジュール３０１において、例えば、一のＧＰＳデータを始点として導出された時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ(ｔ_ｃ)，ｙ_ｇ(ｔ_ｃ))および方位角(ｏｒｉ_ｇ(ｔ_ｃ))と、その次のＧＰＳデータを始点として導出された時刻ｔ_ｃにおける移動体の位置(ｘ_ｇ’(ｔ_ｃ)，ｙ_ｇ’(ｔ_ｃ))および方位角(ｏｒｉ_ｇ’(ｔ_ｃ)) とを含む予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）に、その予想誤差（ΔＤ_ｘ，ΔＤ_ｙ，ΔＤ_ｏｒｉ）の確率密度関数（Ｐ_ｘ，Ｐ_ｙ，Ｐ_ｏｒｉ）を含む補償係数（Ｋ_ｘ，Ｋ_ｙ，Ｋ_ｏｒｉ）を乗算することにより導出される補償量(Ｃ_ｘ，Ｃ_ｙ，Ｃ_ｏｒｉ)を用いて、時刻ｔ_ｃ以降の移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))が特定される。

これにより、本実施の形態では、リアルタイムで精度良く時刻ｔ_ａ以降の移動体の現在位置を検出することが可能となる。

また、本実施の形態では、移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ))および方位角(ｏｒｉ_ｇ(ｔ_ｉ))を、時刻ｔ_ｉにおける移動体の位置(ｘ_ｇ(ｔ_ｉ)，ｙ_ｇ(ｔ_ｉ)) の前方の映像データから得られた道路に関するデータを用いて補正しているので、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。

以上、実施の形態を挙げて本発明を説明したが、本発明はこれに限定されるものではなく、種々の変形が可能である。

例えば、上記実施の形態では、測位処理モジュール３０１、画像処理モジュール３０２および制御モジュール３０３はデバイス（ハードウェア）として記述されていたが、中央処理部３０にロードされたプログラムであってもよい。また、記録媒体２に記録されている地図データについても、あらかじめ中央処理部３０にロードされていてもよい。

本発明の一実施の形態に係る移動体位置検出装置の概要構成図である。 センサ入力部からの各入力の周期およびタイミングを表す関係図である。 移動体の変位の様子の一例を表す平面図である。 移動体の位置および方位角の補正の一例について説明するための平面図である。 マップマッチング処理について説明するための平面図である。 図４に続いて行われる補正について説明するための平面図である。 センサ入力部からの各入力の周期およびタイミングを表す関係図である。 移動体の位置および方位角の補正の他の例について説明するための平面図である。 図８に続いて行われる補正について説明するための平面図である。 カメラの画像データを利用して移動体の位置および方位角を補正する方法について説明するための平面図である。 実施例における移動体の位置（ｘ，ｙ）および方位角の変位の様子を表す変位図である。

符号の説明

１…移動体位置検出装置、２…道路、３…道路中央線、４…路側線、５…直交線、１０…センサ入力部、２０…地図管理部、３０…中央処理部、４０…映像表示部、５０…制御部、１０１…ＣＣＤカメラ、１０２…ＧＰＳセンサ、１０３…ジャイロセンサ、１０４…車速センサ、２０１…記録媒体、２０２…ドライバ、３０１…測位処理モジュール、３０２…画像処理モジュール、３０３…制御モジュール。

Claims (8)

1. ＧＰＳデータから得られた時刻ｔ０における移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を、次のＧＰＳデータが得られる時刻ｔａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔａにおける移動体の位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））を推定し、
   次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））と、推定により得られた時刻ｔａにおける移動体の位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））との差分、または、位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））、方位角（ｏｒｉｇ’（ｔａ））、位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））の少なくとも１つに対して重み付けがなされた上での、位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））と、位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））との差分である予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）を導出し、
   移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔａにおける移動体の位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））を、その道路上の位置（Ｘｇ（ｔａ），Ｙｇ（ｔａ））および方位角（ＯＲＩｇ（ｔａ））に移動し、
   移動体の位置（Ｘｇ（ｔａ），Ｙｇ（ｔａ））および方位角（ＯＲＩｇ（ｔａ））を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）とを用いて補正することにより、時刻ｔａ以降の移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））（ｉは変数）を特定する
   ことを特徴とする移動体位置検出方法。
2. 移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、時刻ｔｉにおける移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））の前方の映像データから得られた道路に関するデータを用いて補正する
   ことを特徴とする請求項１に記載の移動体位置検出方法。
3. ＧＰＳデータから得られた時刻ｔ０における移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を、所定の時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））を推定し、
   次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））を、時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））を推定し、
   一方の推定により得られた時刻ｔｃにおける移動体の位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））と、他方の推定により得られた時刻ｔｃにおける移動体の位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））との差分、または、位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））、方位角（ｏｒｉｇ’（ｔｃ））、位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））の少なくとも１つに対して重み付けがなされた上での、位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））と、位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））との差分である予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）を導出し、
   移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔｃにおける移動体の位置（ｘｇ，（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））を、その道路上の位置（Ｘｇ（ｔｃ），Ｙｇ（ｔｃ））および方位角（ＯＲＩｇ（ｔｃ））に移動し、
   移動体の位置（Ｘｇ（ｔｃ），Ｙｇ（ｔｃ））および方位角（ＯＲＩｇ（ｔｃ））を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）とを用いて補正することにより、時刻ｔｃ以降の移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））（ｉは変数）を特定する
   ことを特徴とする移動体位置検出方法。
4. 移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、時刻ｔｉにおける移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））の前方の映像データから得られた道路に関するデータを用いて補正する
   ことを特徴とする請求項３に記載の移動体位置検出方法。
5. 受信装置によって受信されたＧＰＳデータから時刻ｔｉ（ｉは変数）における移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
   ＧＰＳデータから得られた時刻ｔ０における移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を、次のＧＰＳデータが得られる時刻ｔａまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔａにおける移動体の位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））を推定する移動体位置推定手段と、
   次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））と、推定により得られた時刻ｔａにおける移動体の位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））との差分、または、位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））、方位角（ｏｒｉｇ’（ｔａ））、位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））の少なくとも１つに対して重み付けがなされた上での、位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））と、位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））との差分である予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）を導出する補償量導出手段と、
   移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻ｔａにおける移動体の位置（ｘｇ（ｔａ），ｙｇ（ｔａ））および方位角（ｏｒｉｇ（ｔａ））を、その道路上の位置（Ｘｇ（ｔａ），Ｙｇ（ｔａ））および方位角（ＯＲＩｇ（ｔａ））に移動する移動体位置移動手段と、
   移動体の位置（Ｘｇ（ｔａ），Ｙｇ（ｔａ））および方位角（ＯＲＩｇ（ｔａ））を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）とを用いて補正することにより、時刻ｔａ以降の移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を特定する移動体位置特定手段と
   を備えたことを特徴とする移動体位置検出装置。
6. 前記移動体位置特定手段は、移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、時刻ｔｉにおける移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））の前方の映像データから得られた道路に関するデータを用いて補正する
   ことを特徴とする請求項５に記載の移動体位置検出装置。
7. 受信装置によって受信されたＧＰＳデータから時刻ｔｉ（ｉは変数）における移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
   ＧＰＳデータから得られた時刻ｔ０における移動体の位置（ｘｇ（ｔ０），ｙｇ（ｔ０））および方位角（ｏｒｉｇ（ｔ０））を、所定の時刻ｔｃまでの間に慣性センサおよび速度ンサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻ｔｃにおける移動体の位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））を推定する第１移動体位置推定手段と、
   次のＧＰＳデータが得られたとき、そのＧＰＳデータから得られた時刻ｔａにおける移動体の位置（ｘｇ’（ｔａ），ｙｇ’（ｔａ））および方位角（ｏｒｉｇ’（ｔａ））を、時刻ｔｃまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻ｔｃにおける移動体の位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））を推定する第２移動体位置推定手段と、
   一方の推定により得られた時刻ｔｃにおける移動体の位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））と、他方の推定により得られた時刻ｔｃにおける移動体の位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））との差分、または、位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））、方位角（ｏｒｉｇ’（ｔｃ））、位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））の少なくとも１つに対して重み付けがなされた上での、位置（ｘｇ’（ｔｃ），ｙｇ’（ｔｃ））および方位角（ｏｒｉｇ’（ｔｃ））と、位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））との差分である予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）を導出する補償量導出手段と、
   移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻ｔｃにおける移動体の位置（ｘｇ（ｔｃ），ｙｇ（ｔｃ））および方位角（ｏｒｉｇ（ｔｃ））を、その道路上の位置（Ｘｇ（ｔｃ），Ｙｇ（ｔｃ））および方位角（ＯＲＩｇ（ｔｃ））に移動する移動体位置移動手段と、
   移動体の位置（Ｘｇ（ｔｃ），Ｙｇ（ｔｃ））および方位角（ＯＲＩｇ（ｔｃ））を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量（Ｃｘ，Ｃｙ，Ｃｏｒｉ）とを用いて補正することにより、時刻ｔｃ以降の移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））（ｉは変数）を特定する移動体位置特定手段と
   を備えたことを特徴とする移動体位置検出装置。
8. 前記移動体位置特定手段は、移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））および方位角（ｏｒｉｇ（ｔｉ））を、時刻ｔｉにおける移動体の位置（ｘｇ（ｔｉ），ｙｇ（ｔｉ））の前方の映像データから得られた道路に関するデータを用いて補正する
   ことを特徴とする請求項７に記載の移動体位置検出装置。

Images