移動体位置検出方法および移動体位置検出装置
【課題】リアルタイムで精度良く移動体の現在位置および方位角を検出することの可能な移動体位置検出方法を提供する。
【解決手段】一のGPSデータを始点として導出された時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、その次のGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) とを含む予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を特定する。
【解決手段】一のGPSデータを始点として導出された時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、その次のGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) とを含む予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を特定する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、移動体の現在位置を検出する移動体位置検出方法および移動体位置検出装置に関する。
【背景技術】
【0002】
自動車車両のような移動体の現在位置を検出し、検出された現在位置を周辺の道路地図と共に表示画面に表示する移動体位置検出装置が各種開発されている。近年の移動体位置検出装置では、GPS(Global Positioning System)と慣性センサとを組み合わせることにより移動体の現在位置を割り出す手法が一般的に用いられている。さらに、CD−ROMや、DVD、ハードディスクなどに記録された地図情報を必要に応じて読み出し、移動体の走行経路の情報と照合することにより、移動体の現在位置を正確に特定するマップマッチングという手法も採り入れられている。このようにして得られた移動体の現在位置の情報を、地図情報に重ね合わせることにより、現在位置とその付近の所定範囲内の道路地図とを表す画像を画像表示装置の画面に表示することが可能となる(特許文献1参照)。
【0003】
【特許文献1】WO2004/048895
【0004】
ここで、GPSとは、地球上の現在位置を調べるための衛星測位システムであり、宇宙空間に打ち上げられた多数のGPS衛星から得られたGPSデータに基づいて現在位置(絶対位置情報)を導出するものである。一般に、3個の衛星からの電波を捕えることができれば地球上の平面での位置がわかり(2次元測位)、4個以上の衛星からの電波を捕えることができればさらに高度の情報を得ることができる(3次元測位)。このGPSの精度は、例えば、補足衛星数や、DOP(Dilution of Precision)値、測位の次元(2次元または3次元)、マルチパス誤差、衛星自身の位置誤差、衛星に搭載された時計の誤差、衛星からの電波を受信するレシーバの雑音レベル、電離層による誤差などによって変化するが、廉価な商用GPSでは、位置(緯度および経度)の精度がおよそ20メートル、方位角の精度がおよそ10度となっており、GPS衛星から1秒周期でGPSデータが出力される。
【0005】
また、慣性センサとは、移動体に取り付けられるジャイロスコープや速度センサなどのことであり、移動体の角速度や走行速度などを計測するものである。この慣性センサでは、1/60秒または1/50秒周期で移動体の角速度や走行速度が計測されるので、この計測された角速度や走行速度に基づいて移動体の移動距離や方位角など(相対位置情報)を導出することができる。そのため、慣性センサは、GPSデータを受信できないとき(例えば、トンネルの中にいるとき)や、GPSデータに大きなノイズが含まれているとき(例えば、ビルとビルの谷間にいるとき)、GPSデータを補間する目的で用いられることが多い。
【発明の開示】
【発明が解決しようとする課題】
【0006】
ところで、従来の移動体位置検出装置では、GPSデータを取得してから移動体の現在位置を特定するまでの間に、上記した様々な誤差要素を除去して現在位置の計算を行ったり、慣性センサから得られた移動体の角速度や走行速度を用いて現在位置の補正を行ったり、さらに、マップマッチングを行うなど、種々の情報処理が必要である。そのため、移動体の現在位置を特定するまでの時間(遅延時間)が非常に長く、しかもばらつきを有している場合もあり、移動体の現在位置を特定した時には既にGPSデータを取得した時の位置とは全く別の場所に移動体が移動してしまっていることもある。従って、単に移動体の走行経路の履歴を画面に表示するだけでよいような、リアルタイム性のあまり重要でない用途であれば、遅延時間が長くなったとしてもあまり問題とはならない。しかし、移動体の現在位置などに基づいて、その先の道路特性に応じた制御を移動体に対して行ったり、移動体の運転に役立つ情報を提供するような、リアルタイム性を厳しく要求される用途では、遅延時間を短縮するか、または、別の方策を用いて移動体の現在位置などを即座に推定することが求められる。
【0007】
そこで、そのようなリアルタイム性を厳しく要求される用途においては、例えば、処理能力を上げて、遅延時間を短縮することが考えられる。しかし、処理能力を上げるために、高速に演算処理することの可能な部品を用いると、コストアップに繋がるので、このような方策を採ることは汎用機器においては現実的ではない。もっとも、仮に高速に演算処理することの可能な部品を用いて遅延時間を短縮した場合であっても、GPSデータは1秒周期にしか得ることができないので、GPSデータが来ない間はリアルタイムに現在位置を特定することができない。また、例えば、直近のGPSデータから既に得られている位置および方位角を、慣性センサからのデータを用いて補正することにより、最新のGPSデータから移動体の現在位置が特定されるまでの間、移動体の現在位置を推定することが考えられる。しかし、慣性センサでは推定時間が長くなるほど現在位置の誤差が大きくなる傾向があり、また、直近のGPSデータから既に得られた位置および方位角や、最新のGPSデータから得られた位置に大きな誤差が含まれている場合もある。そのため、慣性センサからのデータを用いて得られた現在位置と、最新のGPSデータから得られた現在位置とが大きく食い違ってしまう可能性がある。そのときに、例えば、慣性センサからのデータを用いて得られた現在位置を、最新のGPSデータから得られた現在位置で置き換えてしまうと、置き換えた瞬間に現在位置がジャンプしてしまう。このような現在位置のジャンプはリアルタイム性を厳しく要求される用途では許容することができない。
【0008】
このように、従来の技術では、リアルタイムで精度良く移動体の現在位置および方位角を検出することが容易ではないという問題があった。
【0009】
本発明はかかる問題点に鑑みてなされたもので、その目的は、リアルタイムで精度良く移動体の現在位置および方位角を検出することの可能な移動体位置検出方法および移動体位置検出装置を提供することにある。
【課題を解決するための手段】
【0010】
本発明による第1の移動体位置検出方法は、以下の(A1)〜(A4)の各ステップを含むものである。
(A1)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定するステップ
(A2)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出するステップ
(A3)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動するステップ
(A4)移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定するステップ
【0011】
本発明による第1の移動体位置検出装置は、以下の(B1)〜(B5)の各手段を備えたものである。
(B1)受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
(B2)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定する移動体位置推定手段
(B3)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段
(B4)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動する移動体位置移動手段
(B5)移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を特定する移動体位置特定手段
【0012】
本発明による第1の移動体位置検出方法および第1の移動体位置検出装置では、一のGPSデータを始点として推定された時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、その次のGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出された補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0013】
ここで、予想誤差を、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分とすることが可能である。
【0014】
移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。
【0015】
本発明による第2の移動体位置検出方法は、以下の(C1)〜(C5)の各ステップを含むものである。
(C1)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定するステップ
(C2)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定するステップ
(C3)一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出するステップ
(C4)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動するステップ
(C5)移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定するステップ
【0016】
本発明による第2の移動体位置検出装置は、以下の(D1)〜(D6)の各手段を備えたものである。
(D1)受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
(D2)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定する第1移動体位置推定手段
(D3)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定する第2移動体位置推定手段
(D4)一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段
(D5)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動する移動体位置移動手段
(D6)移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する移動体位置特定手段
【0017】
本発明による第2の移動体位置検出方法および第2の移動体位置検出装置では、一のGPSデータを始点として推定された時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、その次のGPSデータを始点として推定された時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0018】
ここで、予想誤差を、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、一方の推定により得られた時刻tcにおける位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分、または、移動体の位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))の少なくとも1つに対して重み付けがなされた上での、移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分であるとすることが可能である。
【0019】
また、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。
【発明の効果】
【0020】
本発明による第1の移動体位置検出方法および第1の移動体位置検出装置によれば、補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。これにより、リアルタイムで精度良く時刻ta以降の移動体の現在位置および方位角を検出することが可能となる。
【0021】
本発明による第2の移動体位置検出方法および第2の移動体位置検出装置によれば、補償量(Cx,Cy,Cori)を用いて、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。これにより、リアルタイムで精度良く時刻tc以降の移動体の現在位置および方位角を検出することが可能となる。
【0022】
ここで、本発明による第1および第2の移動体位置検出方法ならびに第1および第2の移動体位置検出装置において、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正した場合には、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。
【発明を実施するための最良の形態】
【0023】
以下、本発明を実施するための最良の形態について図面を参照して詳細に説明する。
【0024】
図1は、本発明の移動体位置検出装置1の概要構成を表したものである。なお、本発明の実施の形態に係る移動体位置検出方法は、この移動体位置検出装置1の動作によって具現化されるものであるから、以下、それらを併せて説明する。
【0025】
この移動体位置検出装置1は、運転者の運転する移動体(例えば自動車)に搭載されたものであり、例えば、センサ入力部10と、地図管理部20と、中央処理部30と、映像表示部40と、制御部50とを、その主要部として備えている。なお、センサ入力部10、地図管理部20、映像表示部40および制御部50が、移動体位置検出装置1の外部装置として設けられていてもよい。
【0026】
センサ入力部10は、CCD(固体撮像素子)カメラ101と、GPSセンサ102と、ジャイロセンサ103(慣性センサ)と、車速センサ104(速度センサ)とを備えている。
【0027】
CCDカメラ101は、例えば運転者がフロントガラス越しに見る視線とほぼ同様のカメラアングルで車両前方の景観を撮影(撮像)することができるように、移動体に固定されている。このCCDカメラ101は、例えば固定焦点距離の単眼方式のものであり、道路を含んだ前方の景観を例えば1/30秒ごとに撮像して、撮像により得られた画像データを中央処理部30に転送するようになっている。
【0028】
GPSセンサ102は、例えば、地球上の現在位置を調べるための衛星測位システム(GPS)において、宇宙空間に打ち上げられた多数のGPS衛星から地表に向けて送信されてくる電波を受信するレシーバであり、受信した電波に含まれる各種データ(GPSデータ)を中央処理部30に転送するようになっている。なお、中央処理部30は、GPSデータから、移動体の位置(緯度および経度)と、移動体の方位角とを算出することができる。
【0029】
GPS衛星からは、例えば1秒ごとにGPSデータを含む電波が出力されているが、その電波は、地表に到達するまでの間に電離層などの影響を受けて、1秒から2秒の不確定性を持ってレシーバに到達する。ここで、廉価な商用GPSでは、位置(緯度および経度)の精度がおよそ20メートル、方位角の精度がおよそ10度となっている。
【0030】
なお、GPS衛星から送信されてくる電波を受信する代わりに、DGPS(Differential GPS)において、基準局から送信されてくる中波やFM放送などの地上波を受信するようにしてもよい。このDGPSとは、位置の分かっている基準局でGPSによる測量を行い、実際の位置とGPSで算出された位置とのずれを中波やFM放送などの地上波で送信することにより、GPS衛星からの信号により計測した結果を補正することの可能なシステムであり、GPSよりも精度よく測位することが可能である。
【0031】
ジャイロセンサ103は、例えば、コリオリ力による振動を電気的に検出する3次元振動ジャイロからなり、検出した振動データを中央処理部30に転送するようになっている。なお、中央処理部30は、得られた振動データから、例えば1/60秒ごとに移動体の角速度を算出することが可能である。
【0032】
車速センサ104は、例えば、車速の大きさに応じたパルス信号を出力するパルスセンサであり、出力されたパルス信号を中央処理部30に転送するようになっている。なお、中央処理部30は、得られたパルス信号から、例えば1/50秒ごとに移動体の車速を算出することが可能である。
【0033】
地図管理部20は、所定の地理的領域の地図データが記録された記録媒体201と、この記録媒体201から地図データを読み出すドライバ202とを有している。この地図管理部20は、中央処理部30からの読み出し命令に従って、地図データ(例えば、所定の地域に含まれる道路に関するデータや、その道路に付随するデータなど)を記録媒体201から読み出して、中央処理部30に転送するようになっている。
【0034】
ここで、道路に関するデータには、例えば、ノードと呼ばれる交差点、分岐点および補間点などの位置(緯度、経度、海抜)や、ノード間を繋ぐ道路セグメントの等級(国道、県道、農道などにそれぞれ割り当てられている数値)、道路セグメントの形状(水平曲率、縦断曲率)、道路セグメントの車線数などが含まれている。また、道路に付随するデータには、例えば、ランドマーク、鉄道の駅、病院およびガソリンスタンドなどの位置、形状、大きさおよびアイコンなどが含まれている。
【0035】
中央処理部30は、例えば、測位処理モジュール301と、画像処理モジュール302と、制御モジュール303とをその主要部として有している。
【0036】
測位処理モジュール301は、センサ入力部10から入力された各種データおよびパルス信号から、移動体の位置、方位角、角速度および車速を算出し、算出した移動体の位置、方位角、角速度および車速と、地図管理部20から読み出された道路に関するデータとに基づいて、移動体の位置および方位角を特定する演算を行うデバイスである。この測位処理モジュール301は本実施の形態における要部の1つであることから、これについては後に詳述する。
【0037】
画像処理モジュール302は、例えば、地図管理部20から読み出された地図データ上に、測位処理モジュール301で特定された移動体の位置および方位角を重ね合わせた画像データを生成したり、CCDカメラ101で撮像された画像データ上に、道路に付随するデータをアイコンで表現したものを重ね合わせた画像データを生成するするデバイスである。
【0038】
制御モジュール303は、測位処理モジュール301で特定された移動体の位置および方位角と、CCDカメラ101から得られた画像データと、地図管理部20から読み出された地図データとに基づいて、移動体をリアルタイムに制御する制御信号を生成するデバイスである。この制御モジュール303も本実施の形態における要部の1つであることから、これについても後に詳述する。
【0039】
映像表示部40は、例えば液晶表示装置からなり、画像処理モジュール302によって生成された画像データを画面に表示するようになっている。
【0040】
制御部50は、制御モジュール303によって生成された制御信号に基づいて、制御対象の機器(例えばブレーキやハンドル)を制御するデバイスである。
【0041】
次に、測位処理モジュール301について説明する。
【0042】
図2(1),(2),(3)は、各センサからデータが出力されるタイミングを模式的に表したものである。図2(1),(2),(3)に示したように、GPSデータは周期TgでGPSセンサ102から出力され、ジャイロデータは周期Twでジャイロセンサ103から出力され、車速データは周期Tvで車速センサ104から出力される。ここで、ジャイロデータの周期Twおよび車速データの周期Tvは、GPSデータの周期Tgよりも短い。そのため、測位処理モジュール301は、GPSデータがこない間は、ジャイロデータおよび車速データを用いて移動体の位置および方位角を推定する。また、各センサからデータが出力されるタイミングは、一般に互いに同期していない。そこで、測位処理モジュール301は、各センサからの出力タイミングが互いに同期していない場合には、例えば、ジャイロセンサ103または車速センサ104の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定する。なお、本実施の形態では、ジャイロセンサ103の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定するものとする。
【0043】
図3は、移動体の位置および方位角が時々刻々と変化する様子を模式的に表したものである。なお、図3には、図2(1),(2),(3)の時刻t0,t1,t2における移動体の変位を高さ方向から眺めた様子が例示されている。図2(1),(2),(3)および図3から、測位処理モジュール301は、時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、GPSデータから直接得ることができる。他方、時刻t1における移動体の位置(xg(t1),yg(t1))および方位角(orig(t1))を、GPSデータから直接得ることができない。そこで、測位処理モジュール301は、車速データから得られた移動体の車速v(t0)にΔt(=時刻t1−時刻t0)を乗算することにより移動距離Δd(t0)を求めると共に、ジャイロデータから得られた移動体の角速度w(t0)にΔtを乗算することにより方位角の変位量Δori(t0)を求めたのち、移動距離Δd(t0)を(orig(t0)+Δori(t0))(=orig(t1))を用いてx成分とy成分とに分解することにより、時刻t1における移動体の位置(xg(t1),yg(t1))および方位角(orig(t1))を推定する。これらを一般化したものを以下の数1〜3に示す。なお、数1〜3中のiは、ゼロ以上の整数である。また、数1〜3中の(Cx,Cy,Cori)は、後述するように補償量であるが、このときは(0,0,0)とする。
【0044】
【数1】
【数2】
【数3】
【0045】
このように、測位処理モジュール301は、GPSデータから直接得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データを利用して推定する。
【0046】
次に、測位処理モジュール301は、図2に示したように、時刻taにおいて次のGPSデータが得られた場合には、まず、GPSの測位品質を評価することが好ましい。具体的には、補足衛星数や、DOP値、測位の次元(2次元または3次元)を評価することが考えられる。この場合には、例えば、補足衛星数が7個以下、DOP値が2.0以上、測位の次元が2次元となっているときには、GPSデータから得られる時刻taにおける移動体の位置(xg’(ta),yg’(ta))は大きな誤差を含んでおり、GPSの測位品質が悪いと判断する。また、GPSデータから時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を算出したのちに、時刻taにおける移動体の位置(xg’(ta),yg’(ta))と時刻t0における移動体の位置(xg(t0),yg(t0))との差分と、車速データを用いて算出された時刻t0から時刻taまでの移動距離との誤差を評価することが考えられる。この場合には、その誤差が所定の値を超えたときには、GPSの測位品質が悪いと判断する。以上のようにしてGPSの測位品質を評価した結果、GPSの測位品質が悪いと判断した場合には、その値を用いないで、引き続き、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データを利用して推定する。逆に、GPSデータから得られる時刻taにおける移動体の位置(xg’(ta),yg’(ta))が大きな誤差を含んでいないと判断した場合には、以下の手順を実行する。
【0047】
なお、時刻taに得られる予定のGPSデータが得られない場合には、引き続き、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データを利用して推定する。また、上の評価において、GPSデータから時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を算出する手順が含まれていない場合には、移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))をGPSデータから算出したのち、以下の手順を実行するものとする。また、以下の手順において、GPSデータから移動体の位置および方位角を算出する場合には、上記した評価方法を実行することが好ましい。
【0048】
次に、測位処理モジュール301は、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を導出する。
【数4】
【0049】
ここで、予想誤差(ΔDx,ΔDy,ΔDori)は、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含むものである。予想誤差(ΔDx,ΔDy,ΔDori)は、例えば、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分である。なお、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))から、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))に転換行列の各成分(Hx,Hy,Hori)を重み付けとして乗算したものを減算することにより、予想誤差(ΔDx,ΔDy,ΔDori)を求めた例を数5に示した。
【数5】
【0050】
また、補償係数(Kx,Ky,Kori)は、拡張されたカルマンフィルタであり、例えば、数6に示したように、1から、予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を減算することにより求められる。ここで、確率密度関数(Px,Py,Pori)は、例えば、数7に示したようなガウス分布として規定されている。
【数6】
【数7】
【0051】
次に、図4に示したように、時刻taにおける推定値((xg(ta),yg(ta),orig(ta))に対してマップマッチング処理を行うことにより、道路上の移動体の位置(Xg(ta),Xg(ta)) および方位角(ORIg(ta))を求める。なお、図4は、時刻taにおける移動体の位置((xg(ta),yg(ta))および方位角(orig(ta))がマップマッチング処理により所定の位置(Xg(ta),Xg(ta)) および方位角(ORIg(ta))に移動する様子を模式的に表したものである。
【0052】
マップマッチング処理は、例えば、生成された道路形状モデルと、移動体の種々の情報とを対比し、その対比の結果に基づいて、移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置を強制的に移動させる処理であり、例えば、以下の2つの方法により行うことが可能である。
【0053】
なお、上記の道路形状モデルは、例えば、移動体の位置および方位角をベースに、移動体位置の前方の道路に関するデータを検索し、検索により得られた道路に関するデータに基づいて、道路の水平形状を表す数学モデルであるクロソイド(clothoid)曲線モデルのパラメータを推定することにより生成することが可能である。
【0054】
まず、1つ目の方法では、(1)推定により得られた時刻taおよびその直近の時刻において移動体が走行した経路(走行経路)の形状と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))の近傍(所定の距離以下)に存在する1または複数の道路の形状とを対比し、移動体の走行経路の形状に最も近似した形状を有する道路を1つ抽出する。例えば、図5に示したように、移動体の走行経路Tの形状と、移動体の近傍にある道路R1,R2のそれぞれの形状とを対比し、走行経路Tの形状により近い道路R1を抽出する。(2)次に、抽出した道路の中央線上に移動体の位置(xg(ta),yg(ta))を移動して、中央線上に移動した後の移動体の位置(Xg(ta),Xg(ta))を求める。例えば、抽出した道路の中央線に対して移動体の位置(xg(ta),yg(ta))から下ろした垂線が、抽出した道路の中央線と交わる位置を算出し、その位置に移動体の位置(xg(ta),yg(ta))を移動する。
【0055】
次に、2つ目の方法では、(1)推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、推定により得られた移動体の位置(xg(ta),yg(ta))の近傍(所定の距離以下)に存在する1または複数の道路の中央線の位置および方位角とを対比し、移動体が最も実在しそうな位置および方位角の中央線を有する道路を1つ抽出する。例えば、図5に示したように、移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、移動体の近傍にある道路R1,R2のそれぞれの中央線の位置および方位角とを対比し、移動体が最も実在しそうな道路R1を抽出する。(2)次に、抽出した道路の中央線のうち、移動体が最も実在しそうな位置および方位角を有する中央線上に移動体の位置(xg(ta),yg(ta))を移動して、中央線上に移動した後の移動体の位置(Xg(ta),Xg(ta))を求める。例えば、移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))に最も近似した位置および方位角の中央線上に、移動体の位置(xg(ta),yg(ta))を移動する。
【0056】
なお、マップマッチング処理において、移動体の方位角(orig(ta))に対して、移動先の中央線の方位角と等しくする補正をすることにより道路の央線上に移動した後の移動体の方位角(ORIg(ta))を得てもよいし、全く補正しないで道路の央線上に移動した後の移動体の方位角(ORIg(ta))を得てもよい。
【0057】
次に、測位処理モジュール301は、図6に示したように、マップマッチング処理された移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データと、上で求めた補償量(Cx,Cy,Cori)とを利用して推定する。具体的には、上記した数1〜4を利用して推定する。
【0058】
次に、測位処理モジュール301は、時刻tbにおいて次のGPSデータが得られた場合には、GPSデータから時刻tbにおける最新の移動体の位置(xg’(tb),yg’(tb))および方位角(orig’(ta))を算出する。続いて、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を更新する。
【0059】
その後、時刻tbにおける推定値((xg(tb),yg(tb),orig(tb))に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置(Xg(tb),Xg(tb))および方位角(ORIg(tb))を求める。そして、時刻tb以降の移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))については、上記した方法を繰り返し行うことにより求めることができる。
【0060】
このように、測位処理モジュール301は、新たなGPSデータが得られた時刻(例えばtb)における移動体の位置(例えば((xg(tb),yg(tb)))および方位角(例えば(orig(tb))をマップマッチング処理し、それにより得られた移動体の位置(例えば((Xg(tb),Yg(tb)))および方位角(例えば(ORIg(tb))と、新たなGPSデータから得られた移動体の位置(例えば((xg’(tb),yg’(tb)))および方位角(例えば(orig’(tb))とを用いることにより補償量(Cx,Cy,Cori)を求めている。しかし、本実施の形態では、測位処理モジュール301は、上記した処理手順に限られるものではなく、例えば、以下に示したような処理手順を行うことにより補償量(Cx,Cy,Cori)を求めることが可能である。
【0061】
図7(1),(2),(3)は、各センサからデータが出力されるタイミングを模式的に表したものである。図8は、時刻taにおける移動体の位置((xg(ta),yg(ta))がマップマッチング処理により所定の位置(Xg(ta),Xg(ta))に移動する様子を模式的に表したものであり、図9は、時刻tdにおける移動体の位置((xg(td),yg(td))がマップマッチング処理により所定の位置(Xg(td),Xg(td))に移動する様子を模式的に表したものである。
【0062】
測位処理モジュール301は、GPSデータから時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を算出したのち、算出した移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、数1〜3を利用して推定し、新たなGPSデータが得られる時刻taよりも後の時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))も数1〜3を利用して推定する。なお、この場合には、数1〜3において、補償量(Cx,Cy,Cori)を(0,0,0)とする。
【0063】
また、測位処理モジュール301は、GPSデータから時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を算出したのち、算出した移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を始点として、時刻taよりも後の時刻tcにおける移動体の位置((xg’(tc),yg’(tc))および方位角(orig’(tc))を、数1〜3を利用して推定する。なお、この場合には、数1〜3において、補償量(Cx,Cy,Cori)を(0,0,0)とする。
【0064】
次に、測位処理モジュール301は、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を導出する。
【0065】
ここで、予想誤差(ΔDx,ΔDy,ΔDori)は、一方の推定により得られた時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含むものである。この予想誤差(ΔDx,ΔDy,ΔDori)は、例えば、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) と、一方の推定により得られた時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))との差分、または、位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置((xg(tc),yg(tc))および方位角(orig(tc))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) と、位置((xg(tc),yg(tc))および方位角(orig(tc))との差分である。
【0066】
なお、数5において、時刻taを時刻tcに置き換えることにより、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))から、一方の推定により得られた時刻taにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))に転換行列の各成分(Hx,Hy,Hori)を重み付けとして乗算したものを減算することにより、予想誤差(ΔDx,ΔDy,ΔDori)を求めた例を得ることができる。また、数7においても、時刻taを時刻tcに置き換えることにより、確率密度関数(Px,Py,Pori)を得ることができる。
【0067】
次に、測位処理モジュール301は、図8に示したように、新たなGPSデータが得られた時刻taよりも後の時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置((Xg(tc),Yg(tc))および方位角(ORIg(tc))を求める。
【0068】
次に、測位処理モジュール301は、図9に示したように、マップマッチング処理された移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データと、上で求めた補償量(Cx,Cy,Cori)とを利用して推定する。具体的には、上記した数1〜4を利用して推定する。
【0069】
その後、測位処理モジュール301は、新たなGPSデータが得られた時刻tbよりも後の時刻tdにおける移動体の位置((xg(td),yg(td))および方位角(orig(td))に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置((Xg(td),Yg(td))および方位角(ORIg(td))を求める。
【0070】
また、測位処理モジュール301は、GPSデータから時刻tbにおける最新の移動体の位置(xg’(tb),yg’(tb))および方位角(orig’(tb))を算出したのち、算出した移動体の位置(xg’(tb),yg’(tb))および方位角(orig’(tb))を始点として、時刻tbよりも後の時刻tdにおける移動体の位置((xg’(td),yg’(td))および方位角(orig’(td))を、数1〜3を利用して推定する。なお、この場合には、数1〜3において、補償量(Cx,Cy,Cori)を(0,0,0)とする。
【0071】
その後、測位処理モジュール301は、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を更新する。そして、時刻td以降の移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))については、上記した方法を繰り返し行うことにより求めることができる。
【0072】
このように、測位処理モジュール301は、新たなGPSデータが得られた時刻よりも後の時刻(例えばtb)における移動体の位置(例えば((xg(td),yg(td)))および方位角(例えば(orig(td))をマップマッチング処理し、そのマップマッチング処理により得られた移動体の位置(例えば((Xg(td),Yg(td)))および方位角(例えば(ORIg(td))と、新たなGPSデータから得られた移動体の位置(例えば((xg’(tb),yg’(tb)))および方位角(例えば(orig’(tb))を始点としてジャイロデータおよび車速データを用いて推定された移動体の位置(例えば((Xg’(td),Yg’(td)))および方位角(例えば(ORIg’(td))とを用いることにより補償量(Cx,Cy,Cori)を求めることも可能である。
【0073】
ところで、GPSデータから得られる移動体の位置および方位角は、電離層の影響などによるドリフトや、周期が長いことに起因するジャンプにより、ガウス分布となっていない。そのため、補償係数(Kx,Ky,Kori)を仮に一般的な線形のカルマンフィルタで規定した場合には、数1の解が不安定となり、精度良く移動体の位置を求めることが困難となる。一方、本実施の形態では、補償係数(Kx,Ky,Kori)を、確率密度関数を用いて規定しているので、数1の解が安定となり、精度良く移動体の位置を求めることが可能となる。
【0074】
図11は、予想誤差(ΔDx,ΔDy,ΔDori)の値(破線)の一例と、補償量(Cx,Cy,Cori)の値(実線)の一例とを示したものである。ここで、予想誤差(ΔDx,ΔDy,ΔDori)の値は、数5において転換行列(Hx,Hy,Hori)の各成分を1としたときに得られたものであり、補償量(Cx,Cy,Cori)の値は、数4において数6および数7を代入したときに得られたものである。
【0075】
従来のように、補償係数(Kx,Ky,Kori)を一般的な線形のカルマンフィルタで規定した場合には、補償係数(Kx,Ky,Kori)が一種のローパスフィルタとして振る舞うので、補償量(Cx,Cy,Cori)の値が急激かつ極端にジャンプするのが抑制されるだけでなく、さらに、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化することも抑制されてしまう。その結果、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができない。
【0076】
一方、本実施の形態では、図11から、補償量(Cx,Cy,Cori)の値が急激かつ極端にジャンプするのが抑制されているものの、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化しているのがわかる。つまり、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができる。
【0077】
次に、制御モジュール303について説明する。
【0078】
制御モジュール303は、数8に示したように、測位処理モジュール301において求められた移動体の位置((xg(ti),yg(ti))に、CCDカメラ101の画像データから得られる道路2の所定の位置からのオフセット(xv(ti),yv(ti))を加算すると共に、測位処理モジュール301において求められた移動体の方位角(oriv(ti))に、方位角(orig(ti))と道路2と平行な線(例えば道路2の中央線3)とのなす角(oriv(ti))を加算する。これは、主に、測位処理モジュール301のマップマッチング処理によって移動体の位置((xg(ti),yg(ti)))や方位角(orig(ti))が道路上の所定の場所(例えば道路の中央線)に移動させられたことによる誤差を取り除くために行われるものである。従って、測位処理モジュール301において求められた移動体の位置((xg(ti),yg(ti)))および方位角(orig(ti))を、数8を用いて補正することにより、実際の移動体の位置((x(ti),y(ti)))および方位角(ori(ti))に戻すことが可能となる。なお、このプロセスは、測位処理モジュール301においてマップマッチング処理により求められた移動体の位置((Xg(ti),Yg(ti)や方位角(ORIg(ti))に対してももちろん適用可能である。
【数8】
【0079】
ここで、オフセット(xv(ti),yv(ti))は、例えば、数9に示したように、地図管理部20から読み出された地図データから抽出された道路幅Wから、CCDカメラ101の画像データから得られた移動体の現在位置と道路2の路側線4との距離d(ti)を減算して得られた値を、π/2に、移動体の方位角(orig(ti))と、道路2と平行な線(例えば道路2の中央線3)と直交する直交線5と移動体の方位角(oriv(ti))とのなす角(θ(ti))とを加算して得られた角度を用いてx成分とy成分とに分解することにより得られる。
【数9】
【0080】
このように、制御モジュール303は、CCDカメラ101の画像データと、地図管理部20から読み出された地図データとを利用して、測位処理モジュール301において求められた移動体の位置((xg(ti),yg(ti)))および方位角(orig(ti))を、実際の移動体の位置((x(ti),y(ti)))および方位角(ori(ti))に戻したことにより、移動体をリアルタイムに制御することが可能となる。
【0081】
例えば、移動体の現在の位置((x(ti),y(ti)))の前方の道路形状と、移動体の現在の車速v(ti)および方位角(ori(ti))とに基づいて、移動体のブレーキや、ハンドルの操舵角などを制御する制御信号を生成する。また、移動体の走行ルートがあらかじめ設定されている場合には、その走行ルートに基づいて、移動体のアクセルや、ブレーキ、ハンドル、ウインカなどを制御する制御信号を生成する。その結果、制御部50は、その制御信号に応じた制御を制御対象の機器(例えば、移動体のアクセルや、ブレーキ、ハンドル、ウインカなど)に対して行うこととなる。
【0082】
次に、本実施の形態の移動体位置検出装置1の動作について説明する。
【0083】
この移動体位置検出装置1では、測位処理モジュール301において、例えば、一のGPSデータを始点として導出された時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、その次のGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) とを含む予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0084】
また、この移動体位置検出装置1では、測位処理モジュール301において、例えば、一のGPSデータを始点として導出された時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、その次のGPSデータを始点として導出された時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) とを含む予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0085】
これにより、本実施の形態では、リアルタイムで精度良く時刻ta以降の移動体の現在位置を検出することが可能となる。
【0086】
また、本実施の形態では、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正しているので、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。
【0087】
以上、実施の形態を挙げて本発明を説明したが、本発明はこれに限定されるものではなく、種々の変形が可能である。
【0088】
例えば、上記実施の形態では、測位処理モジュール301、画像処理モジュール302および制御モジュール303はデバイス(ハードウェア)として記述されていたが、中央処理部30にロードされたプログラムであってもよい。また、記録媒体2に記録されている地図データについても、あらかじめ中央処理部30にロードされていてもよい。
【図面の簡単な説明】
【0089】
【図1】本発明の一実施の形態に係る移動体位置検出装置の概要構成図である。
【図2】センサ入力部からの各入力の周期およびタイミングを表す関係図である。
【図3】移動体の変位の様子の一例を表す平面図である。
【図4】移動体の位置および方位角の補正の一例について説明するための平面図である。
【図5】マップマッチング処理について説明するための平面図である。
【図6】図4に続いて行われる補正について説明するための平面図である。
【図7】センサ入力部からの各入力の周期およびタイミングを表す関係図である。
【図8】移動体の位置および方位角の補正の他の例について説明するための平面図である。
【図9】図8に続いて行われる補正について説明するための平面図である。
【図10】カメラの画像データを利用して移動体の位置および方位角を補正する方法について説明するための平面図である。
【図11】実施例における移動体の位置(x,y)および方位角の変位の様子を表す変位図である。
【符号の説明】
【0090】
1…移動体位置検出装置、2…道路、3…道路中央線、4…路側線、5…直交線、10…センサ入力部、20…地図管理部、30…中央処理部、40…映像表示部、50…制御部、101…CCDカメラ、102…GPSセンサ、103…ジャイロセンサ、104…車速センサ、201…記録媒体、202…ドライバ、301…測位処理モジュール、302…画像処理モジュール、303…制御モジュール。
【技術分野】
【0001】
本発明は、移動体の現在位置を検出する移動体位置検出方法および移動体位置検出装置に関する。
【背景技術】
【0002】
自動車車両のような移動体の現在位置を検出し、検出された現在位置を周辺の道路地図と共に表示画面に表示する移動体位置検出装置が各種開発されている。近年の移動体位置検出装置では、GPS(Global Positioning System)と慣性センサとを組み合わせることにより移動体の現在位置を割り出す手法が一般的に用いられている。さらに、CD−ROMや、DVD、ハードディスクなどに記録された地図情報を必要に応じて読み出し、移動体の走行経路の情報と照合することにより、移動体の現在位置を正確に特定するマップマッチングという手法も採り入れられている。このようにして得られた移動体の現在位置の情報を、地図情報に重ね合わせることにより、現在位置とその付近の所定範囲内の道路地図とを表す画像を画像表示装置の画面に表示することが可能となる(特許文献1参照)。
【0003】
【特許文献1】WO2004/048895
【0004】
ここで、GPSとは、地球上の現在位置を調べるための衛星測位システムであり、宇宙空間に打ち上げられた多数のGPS衛星から得られたGPSデータに基づいて現在位置(絶対位置情報)を導出するものである。一般に、3個の衛星からの電波を捕えることができれば地球上の平面での位置がわかり(2次元測位)、4個以上の衛星からの電波を捕えることができればさらに高度の情報を得ることができる(3次元測位)。このGPSの精度は、例えば、補足衛星数や、DOP(Dilution of Precision)値、測位の次元(2次元または3次元)、マルチパス誤差、衛星自身の位置誤差、衛星に搭載された時計の誤差、衛星からの電波を受信するレシーバの雑音レベル、電離層による誤差などによって変化するが、廉価な商用GPSでは、位置(緯度および経度)の精度がおよそ20メートル、方位角の精度がおよそ10度となっており、GPS衛星から1秒周期でGPSデータが出力される。
【0005】
また、慣性センサとは、移動体に取り付けられるジャイロスコープや速度センサなどのことであり、移動体の角速度や走行速度などを計測するものである。この慣性センサでは、1/60秒または1/50秒周期で移動体の角速度や走行速度が計測されるので、この計測された角速度や走行速度に基づいて移動体の移動距離や方位角など(相対位置情報)を導出することができる。そのため、慣性センサは、GPSデータを受信できないとき(例えば、トンネルの中にいるとき)や、GPSデータに大きなノイズが含まれているとき(例えば、ビルとビルの谷間にいるとき)、GPSデータを補間する目的で用いられることが多い。
【発明の開示】
【発明が解決しようとする課題】
【0006】
ところで、従来の移動体位置検出装置では、GPSデータを取得してから移動体の現在位置を特定するまでの間に、上記した様々な誤差要素を除去して現在位置の計算を行ったり、慣性センサから得られた移動体の角速度や走行速度を用いて現在位置の補正を行ったり、さらに、マップマッチングを行うなど、種々の情報処理が必要である。そのため、移動体の現在位置を特定するまでの時間(遅延時間)が非常に長く、しかもばらつきを有している場合もあり、移動体の現在位置を特定した時には既にGPSデータを取得した時の位置とは全く別の場所に移動体が移動してしまっていることもある。従って、単に移動体の走行経路の履歴を画面に表示するだけでよいような、リアルタイム性のあまり重要でない用途であれば、遅延時間が長くなったとしてもあまり問題とはならない。しかし、移動体の現在位置などに基づいて、その先の道路特性に応じた制御を移動体に対して行ったり、移動体の運転に役立つ情報を提供するような、リアルタイム性を厳しく要求される用途では、遅延時間を短縮するか、または、別の方策を用いて移動体の現在位置などを即座に推定することが求められる。
【0007】
そこで、そのようなリアルタイム性を厳しく要求される用途においては、例えば、処理能力を上げて、遅延時間を短縮することが考えられる。しかし、処理能力を上げるために、高速に演算処理することの可能な部品を用いると、コストアップに繋がるので、このような方策を採ることは汎用機器においては現実的ではない。もっとも、仮に高速に演算処理することの可能な部品を用いて遅延時間を短縮した場合であっても、GPSデータは1秒周期にしか得ることができないので、GPSデータが来ない間はリアルタイムに現在位置を特定することができない。また、例えば、直近のGPSデータから既に得られている位置および方位角を、慣性センサからのデータを用いて補正することにより、最新のGPSデータから移動体の現在位置が特定されるまでの間、移動体の現在位置を推定することが考えられる。しかし、慣性センサでは推定時間が長くなるほど現在位置の誤差が大きくなる傾向があり、また、直近のGPSデータから既に得られた位置および方位角や、最新のGPSデータから得られた位置に大きな誤差が含まれている場合もある。そのため、慣性センサからのデータを用いて得られた現在位置と、最新のGPSデータから得られた現在位置とが大きく食い違ってしまう可能性がある。そのときに、例えば、慣性センサからのデータを用いて得られた現在位置を、最新のGPSデータから得られた現在位置で置き換えてしまうと、置き換えた瞬間に現在位置がジャンプしてしまう。このような現在位置のジャンプはリアルタイム性を厳しく要求される用途では許容することができない。
【0008】
このように、従来の技術では、リアルタイムで精度良く移動体の現在位置および方位角を検出することが容易ではないという問題があった。
【0009】
本発明はかかる問題点に鑑みてなされたもので、その目的は、リアルタイムで精度良く移動体の現在位置および方位角を検出することの可能な移動体位置検出方法および移動体位置検出装置を提供することにある。
【課題を解決するための手段】
【0010】
本発明による第1の移動体位置検出方法は、以下の(A1)〜(A4)の各ステップを含むものである。
(A1)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定するステップ
(A2)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出するステップ
(A3)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動するステップ
(A4)移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定するステップ
【0011】
本発明による第1の移動体位置検出装置は、以下の(B1)〜(B5)の各手段を備えたものである。
(B1)受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
(B2)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定する移動体位置推定手段
(B3)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段
(B4)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動する移動体位置移動手段
(B5)移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を特定する移動体位置特定手段
【0012】
本発明による第1の移動体位置検出方法および第1の移動体位置検出装置では、一のGPSデータを始点として推定された時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、その次のGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出された補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0013】
ここで、予想誤差を、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分とすることが可能である。
【0014】
移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。
【0015】
本発明による第2の移動体位置検出方法は、以下の(C1)〜(C5)の各ステップを含むものである。
(C1)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定するステップ
(C2)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定するステップ
(C3)一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出するステップ
(C4)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動するステップ
(C5)移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定するステップ
【0016】
本発明による第2の移動体位置検出装置は、以下の(D1)〜(D6)の各手段を備えたものである。
(D1)受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段
(D2)GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定する第1移動体位置推定手段
(D3)次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定する第2移動体位置推定手段
(D4)一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段
(D5)移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動する移動体位置移動手段
(D6)移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する移動体位置特定手段
【0017】
本発明による第2の移動体位置検出方法および第2の移動体位置検出装置では、一のGPSデータを始点として推定された時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、その次のGPSデータを始点として推定された時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0018】
ここで、予想誤差を、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、一方の推定により得られた時刻tcにおける位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分、または、移動体の位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))の少なくとも1つに対して重み付けがなされた上での、移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分であるとすることが可能である。
【0019】
また、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正することが可能である。
【発明の効果】
【0020】
本発明による第1の移動体位置検出方法および第1の移動体位置検出装置によれば、補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。これにより、リアルタイムで精度良く時刻ta以降の移動体の現在位置および方位角を検出することが可能となる。
【0021】
本発明による第2の移動体位置検出方法および第2の移動体位置検出装置によれば、補償量(Cx,Cy,Cori)を用いて、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。これにより、リアルタイムで精度良く時刻tc以降の移動体の現在位置および方位角を検出することが可能となる。
【0022】
ここで、本発明による第1および第2の移動体位置検出方法ならびに第1および第2の移動体位置検出装置において、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正した場合には、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。
【発明を実施するための最良の形態】
【0023】
以下、本発明を実施するための最良の形態について図面を参照して詳細に説明する。
【0024】
図1は、本発明の移動体位置検出装置1の概要構成を表したものである。なお、本発明の実施の形態に係る移動体位置検出方法は、この移動体位置検出装置1の動作によって具現化されるものであるから、以下、それらを併せて説明する。
【0025】
この移動体位置検出装置1は、運転者の運転する移動体(例えば自動車)に搭載されたものであり、例えば、センサ入力部10と、地図管理部20と、中央処理部30と、映像表示部40と、制御部50とを、その主要部として備えている。なお、センサ入力部10、地図管理部20、映像表示部40および制御部50が、移動体位置検出装置1の外部装置として設けられていてもよい。
【0026】
センサ入力部10は、CCD(固体撮像素子)カメラ101と、GPSセンサ102と、ジャイロセンサ103(慣性センサ)と、車速センサ104(速度センサ)とを備えている。
【0027】
CCDカメラ101は、例えば運転者がフロントガラス越しに見る視線とほぼ同様のカメラアングルで車両前方の景観を撮影(撮像)することができるように、移動体に固定されている。このCCDカメラ101は、例えば固定焦点距離の単眼方式のものであり、道路を含んだ前方の景観を例えば1/30秒ごとに撮像して、撮像により得られた画像データを中央処理部30に転送するようになっている。
【0028】
GPSセンサ102は、例えば、地球上の現在位置を調べるための衛星測位システム(GPS)において、宇宙空間に打ち上げられた多数のGPS衛星から地表に向けて送信されてくる電波を受信するレシーバであり、受信した電波に含まれる各種データ(GPSデータ)を中央処理部30に転送するようになっている。なお、中央処理部30は、GPSデータから、移動体の位置(緯度および経度)と、移動体の方位角とを算出することができる。
【0029】
GPS衛星からは、例えば1秒ごとにGPSデータを含む電波が出力されているが、その電波は、地表に到達するまでの間に電離層などの影響を受けて、1秒から2秒の不確定性を持ってレシーバに到達する。ここで、廉価な商用GPSでは、位置(緯度および経度)の精度がおよそ20メートル、方位角の精度がおよそ10度となっている。
【0030】
なお、GPS衛星から送信されてくる電波を受信する代わりに、DGPS(Differential GPS)において、基準局から送信されてくる中波やFM放送などの地上波を受信するようにしてもよい。このDGPSとは、位置の分かっている基準局でGPSによる測量を行い、実際の位置とGPSで算出された位置とのずれを中波やFM放送などの地上波で送信することにより、GPS衛星からの信号により計測した結果を補正することの可能なシステムであり、GPSよりも精度よく測位することが可能である。
【0031】
ジャイロセンサ103は、例えば、コリオリ力による振動を電気的に検出する3次元振動ジャイロからなり、検出した振動データを中央処理部30に転送するようになっている。なお、中央処理部30は、得られた振動データから、例えば1/60秒ごとに移動体の角速度を算出することが可能である。
【0032】
車速センサ104は、例えば、車速の大きさに応じたパルス信号を出力するパルスセンサであり、出力されたパルス信号を中央処理部30に転送するようになっている。なお、中央処理部30は、得られたパルス信号から、例えば1/50秒ごとに移動体の車速を算出することが可能である。
【0033】
地図管理部20は、所定の地理的領域の地図データが記録された記録媒体201と、この記録媒体201から地図データを読み出すドライバ202とを有している。この地図管理部20は、中央処理部30からの読み出し命令に従って、地図データ(例えば、所定の地域に含まれる道路に関するデータや、その道路に付随するデータなど)を記録媒体201から読み出して、中央処理部30に転送するようになっている。
【0034】
ここで、道路に関するデータには、例えば、ノードと呼ばれる交差点、分岐点および補間点などの位置(緯度、経度、海抜)や、ノード間を繋ぐ道路セグメントの等級(国道、県道、農道などにそれぞれ割り当てられている数値)、道路セグメントの形状(水平曲率、縦断曲率)、道路セグメントの車線数などが含まれている。また、道路に付随するデータには、例えば、ランドマーク、鉄道の駅、病院およびガソリンスタンドなどの位置、形状、大きさおよびアイコンなどが含まれている。
【0035】
中央処理部30は、例えば、測位処理モジュール301と、画像処理モジュール302と、制御モジュール303とをその主要部として有している。
【0036】
測位処理モジュール301は、センサ入力部10から入力された各種データおよびパルス信号から、移動体の位置、方位角、角速度および車速を算出し、算出した移動体の位置、方位角、角速度および車速と、地図管理部20から読み出された道路に関するデータとに基づいて、移動体の位置および方位角を特定する演算を行うデバイスである。この測位処理モジュール301は本実施の形態における要部の1つであることから、これについては後に詳述する。
【0037】
画像処理モジュール302は、例えば、地図管理部20から読み出された地図データ上に、測位処理モジュール301で特定された移動体の位置および方位角を重ね合わせた画像データを生成したり、CCDカメラ101で撮像された画像データ上に、道路に付随するデータをアイコンで表現したものを重ね合わせた画像データを生成するするデバイスである。
【0038】
制御モジュール303は、測位処理モジュール301で特定された移動体の位置および方位角と、CCDカメラ101から得られた画像データと、地図管理部20から読み出された地図データとに基づいて、移動体をリアルタイムに制御する制御信号を生成するデバイスである。この制御モジュール303も本実施の形態における要部の1つであることから、これについても後に詳述する。
【0039】
映像表示部40は、例えば液晶表示装置からなり、画像処理モジュール302によって生成された画像データを画面に表示するようになっている。
【0040】
制御部50は、制御モジュール303によって生成された制御信号に基づいて、制御対象の機器(例えばブレーキやハンドル)を制御するデバイスである。
【0041】
次に、測位処理モジュール301について説明する。
【0042】
図2(1),(2),(3)は、各センサからデータが出力されるタイミングを模式的に表したものである。図2(1),(2),(3)に示したように、GPSデータは周期TgでGPSセンサ102から出力され、ジャイロデータは周期Twでジャイロセンサ103から出力され、車速データは周期Tvで車速センサ104から出力される。ここで、ジャイロデータの周期Twおよび車速データの周期Tvは、GPSデータの周期Tgよりも短い。そのため、測位処理モジュール301は、GPSデータがこない間は、ジャイロデータおよび車速データを用いて移動体の位置および方位角を推定する。また、各センサからデータが出力されるタイミングは、一般に互いに同期していない。そこで、測位処理モジュール301は、各センサからの出力タイミングが互いに同期していない場合には、例えば、ジャイロセンサ103または車速センサ104の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定する。なお、本実施の形態では、ジャイロセンサ103の出力タイミングの時刻に合わせて、移動体の位置および方位角を推定するものとする。
【0043】
図3は、移動体の位置および方位角が時々刻々と変化する様子を模式的に表したものである。なお、図3には、図2(1),(2),(3)の時刻t0,t1,t2における移動体の変位を高さ方向から眺めた様子が例示されている。図2(1),(2),(3)および図3から、測位処理モジュール301は、時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、GPSデータから直接得ることができる。他方、時刻t1における移動体の位置(xg(t1),yg(t1))および方位角(orig(t1))を、GPSデータから直接得ることができない。そこで、測位処理モジュール301は、車速データから得られた移動体の車速v(t0)にΔt(=時刻t1−時刻t0)を乗算することにより移動距離Δd(t0)を求めると共に、ジャイロデータから得られた移動体の角速度w(t0)にΔtを乗算することにより方位角の変位量Δori(t0)を求めたのち、移動距離Δd(t0)を(orig(t0)+Δori(t0))(=orig(t1))を用いてx成分とy成分とに分解することにより、時刻t1における移動体の位置(xg(t1),yg(t1))および方位角(orig(t1))を推定する。これらを一般化したものを以下の数1〜3に示す。なお、数1〜3中のiは、ゼロ以上の整数である。また、数1〜3中の(Cx,Cy,Cori)は、後述するように補償量であるが、このときは(0,0,0)とする。
【0044】
【数1】
【数2】
【数3】
【0045】
このように、測位処理モジュール301は、GPSデータから直接得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データを利用して推定する。
【0046】
次に、測位処理モジュール301は、図2に示したように、時刻taにおいて次のGPSデータが得られた場合には、まず、GPSの測位品質を評価することが好ましい。具体的には、補足衛星数や、DOP値、測位の次元(2次元または3次元)を評価することが考えられる。この場合には、例えば、補足衛星数が7個以下、DOP値が2.0以上、測位の次元が2次元となっているときには、GPSデータから得られる時刻taにおける移動体の位置(xg’(ta),yg’(ta))は大きな誤差を含んでおり、GPSの測位品質が悪いと判断する。また、GPSデータから時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を算出したのちに、時刻taにおける移動体の位置(xg’(ta),yg’(ta))と時刻t0における移動体の位置(xg(t0),yg(t0))との差分と、車速データを用いて算出された時刻t0から時刻taまでの移動距離との誤差を評価することが考えられる。この場合には、その誤差が所定の値を超えたときには、GPSの測位品質が悪いと判断する。以上のようにしてGPSの測位品質を評価した結果、GPSの測位品質が悪いと判断した場合には、その値を用いないで、引き続き、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データを利用して推定する。逆に、GPSデータから得られる時刻taにおける移動体の位置(xg’(ta),yg’(ta))が大きな誤差を含んでいないと判断した場合には、以下の手順を実行する。
【0047】
なお、時刻taに得られる予定のGPSデータが得られない場合には、引き続き、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データを利用して推定する。また、上の評価において、GPSデータから時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を算出する手順が含まれていない場合には、移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))をGPSデータから算出したのち、以下の手順を実行するものとする。また、以下の手順において、GPSデータから移動体の位置および方位角を算出する場合には、上記した評価方法を実行することが好ましい。
【0048】
次に、測位処理モジュール301は、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を導出する。
【数4】
【0049】
ここで、予想誤差(ΔDx,ΔDy,ΔDori)は、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含むものである。予想誤差(ΔDx,ΔDy,ΔDori)は、例えば、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分である。なお、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))から、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))に転換行列の各成分(Hx,Hy,Hori)を重み付けとして乗算したものを減算することにより、予想誤差(ΔDx,ΔDy,ΔDori)を求めた例を数5に示した。
【数5】
【0050】
また、補償係数(Kx,Ky,Kori)は、拡張されたカルマンフィルタであり、例えば、数6に示したように、1から、予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を減算することにより求められる。ここで、確率密度関数(Px,Py,Pori)は、例えば、数7に示したようなガウス分布として規定されている。
【数6】
【数7】
【0051】
次に、図4に示したように、時刻taにおける推定値((xg(ta),yg(ta),orig(ta))に対してマップマッチング処理を行うことにより、道路上の移動体の位置(Xg(ta),Xg(ta)) および方位角(ORIg(ta))を求める。なお、図4は、時刻taにおける移動体の位置((xg(ta),yg(ta))および方位角(orig(ta))がマップマッチング処理により所定の位置(Xg(ta),Xg(ta)) および方位角(ORIg(ta))に移動する様子を模式的に表したものである。
【0052】
マップマッチング処理は、例えば、生成された道路形状モデルと、移動体の種々の情報とを対比し、その対比の結果に基づいて、移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置を強制的に移動させる処理であり、例えば、以下の2つの方法により行うことが可能である。
【0053】
なお、上記の道路形状モデルは、例えば、移動体の位置および方位角をベースに、移動体位置の前方の道路に関するデータを検索し、検索により得られた道路に関するデータに基づいて、道路の水平形状を表す数学モデルであるクロソイド(clothoid)曲線モデルのパラメータを推定することにより生成することが可能である。
【0054】
まず、1つ目の方法では、(1)推定により得られた時刻taおよびその直近の時刻において移動体が走行した経路(走行経路)の形状と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))の近傍(所定の距離以下)に存在する1または複数の道路の形状とを対比し、移動体の走行経路の形状に最も近似した形状を有する道路を1つ抽出する。例えば、図5に示したように、移動体の走行経路Tの形状と、移動体の近傍にある道路R1,R2のそれぞれの形状とを対比し、走行経路Tの形状により近い道路R1を抽出する。(2)次に、抽出した道路の中央線上に移動体の位置(xg(ta),yg(ta))を移動して、中央線上に移動した後の移動体の位置(Xg(ta),Xg(ta))を求める。例えば、抽出した道路の中央線に対して移動体の位置(xg(ta),yg(ta))から下ろした垂線が、抽出した道路の中央線と交わる位置を算出し、その位置に移動体の位置(xg(ta),yg(ta))を移動する。
【0055】
次に、2つ目の方法では、(1)推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、推定により得られた移動体の位置(xg(ta),yg(ta))の近傍(所定の距離以下)に存在する1または複数の道路の中央線の位置および方位角とを対比し、移動体が最も実在しそうな位置および方位角の中央線を有する道路を1つ抽出する。例えば、図5に示したように、移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、移動体の近傍にある道路R1,R2のそれぞれの中央線の位置および方位角とを対比し、移動体が最も実在しそうな道路R1を抽出する。(2)次に、抽出した道路の中央線のうち、移動体が最も実在しそうな位置および方位角を有する中央線上に移動体の位置(xg(ta),yg(ta))を移動して、中央線上に移動した後の移動体の位置(Xg(ta),Xg(ta))を求める。例えば、移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))に最も近似した位置および方位角の中央線上に、移動体の位置(xg(ta),yg(ta))を移動する。
【0056】
なお、マップマッチング処理において、移動体の方位角(orig(ta))に対して、移動先の中央線の方位角と等しくする補正をすることにより道路の央線上に移動した後の移動体の方位角(ORIg(ta))を得てもよいし、全く補正しないで道路の央線上に移動した後の移動体の方位角(ORIg(ta))を得てもよい。
【0057】
次に、測位処理モジュール301は、図6に示したように、マップマッチング処理された移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データと、上で求めた補償量(Cx,Cy,Cori)とを利用して推定する。具体的には、上記した数1〜4を利用して推定する。
【0058】
次に、測位処理モジュール301は、時刻tbにおいて次のGPSデータが得られた場合には、GPSデータから時刻tbにおける最新の移動体の位置(xg’(tb),yg’(tb))および方位角(orig’(ta))を算出する。続いて、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を更新する。
【0059】
その後、時刻tbにおける推定値((xg(tb),yg(tb),orig(tb))に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置(Xg(tb),Xg(tb))および方位角(ORIg(tb))を求める。そして、時刻tb以降の移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))については、上記した方法を繰り返し行うことにより求めることができる。
【0060】
このように、測位処理モジュール301は、新たなGPSデータが得られた時刻(例えばtb)における移動体の位置(例えば((xg(tb),yg(tb)))および方位角(例えば(orig(tb))をマップマッチング処理し、それにより得られた移動体の位置(例えば((Xg(tb),Yg(tb)))および方位角(例えば(ORIg(tb))と、新たなGPSデータから得られた移動体の位置(例えば((xg’(tb),yg’(tb)))および方位角(例えば(orig’(tb))とを用いることにより補償量(Cx,Cy,Cori)を求めている。しかし、本実施の形態では、測位処理モジュール301は、上記した処理手順に限られるものではなく、例えば、以下に示したような処理手順を行うことにより補償量(Cx,Cy,Cori)を求めることが可能である。
【0061】
図7(1),(2),(3)は、各センサからデータが出力されるタイミングを模式的に表したものである。図8は、時刻taにおける移動体の位置((xg(ta),yg(ta))がマップマッチング処理により所定の位置(Xg(ta),Xg(ta))に移動する様子を模式的に表したものであり、図9は、時刻tdにおける移動体の位置((xg(td),yg(td))がマップマッチング処理により所定の位置(Xg(td),Xg(td))に移動する様子を模式的に表したものである。
【0062】
測位処理モジュール301は、GPSデータから時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を算出したのち、算出した移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、数1〜3を利用して推定し、新たなGPSデータが得られる時刻taよりも後の時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))も数1〜3を利用して推定する。なお、この場合には、数1〜3において、補償量(Cx,Cy,Cori)を(0,0,0)とする。
【0063】
また、測位処理モジュール301は、GPSデータから時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を算出したのち、算出した移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta))を始点として、時刻taよりも後の時刻tcにおける移動体の位置((xg’(tc),yg’(tc))および方位角(orig’(tc))を、数1〜3を利用して推定する。なお、この場合には、数1〜3において、補償量(Cx,Cy,Cori)を(0,0,0)とする。
【0064】
次に、測位処理モジュール301は、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を導出する。
【0065】
ここで、予想誤差(ΔDx,ΔDy,ΔDori)は、一方の推定により得られた時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含むものである。この予想誤差(ΔDx,ΔDy,ΔDori)は、例えば、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) と、一方の推定により得られた時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))との差分、または、位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置((xg(tc),yg(tc))および方位角(orig(tc))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) と、位置((xg(tc),yg(tc))および方位角(orig(tc))との差分である。
【0066】
なお、数5において、時刻taを時刻tcに置き換えることにより、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))から、一方の推定により得られた時刻taにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))に転換行列の各成分(Hx,Hy,Hori)を重み付けとして乗算したものを減算することにより、予想誤差(ΔDx,ΔDy,ΔDori)を求めた例を得ることができる。また、数7においても、時刻taを時刻tcに置き換えることにより、確率密度関数(Px,Py,Pori)を得ることができる。
【0067】
次に、測位処理モジュール301は、図8に示したように、新たなGPSデータが得られた時刻taよりも後の時刻tcにおける移動体の位置((xg(tc),yg(tc))および方位角(orig(tc))に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置((Xg(tc),Yg(tc))および方位角(ORIg(tc))を求める。
【0068】
次に、測位処理モジュール301は、図9に示したように、マップマッチング処理された移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を始点として、時刻tiにおける移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))を、ジャイロデータおよび車速データと、上で求めた補償量(Cx,Cy,Cori)とを利用して推定する。具体的には、上記した数1〜4を利用して推定する。
【0069】
その後、測位処理モジュール301は、新たなGPSデータが得られた時刻tbよりも後の時刻tdにおける移動体の位置((xg(td),yg(td))および方位角(orig(td))に対して、上記と同様の方法を用いてマップマッチング処理を行うことにより、道路上の移動体の位置((Xg(td),Yg(td))および方位角(ORIg(td))を求める。
【0070】
また、測位処理モジュール301は、GPSデータから時刻tbにおける最新の移動体の位置(xg’(tb),yg’(tb))および方位角(orig’(tb))を算出したのち、算出した移動体の位置(xg’(tb),yg’(tb))および方位角(orig’(tb))を始点として、時刻tbよりも後の時刻tdにおける移動体の位置((xg’(td),yg’(td))および方位角(orig’(td))を、数1〜3を利用して推定する。なお、この場合には、数1〜3において、補償量(Cx,Cy,Cori)を(0,0,0)とする。
【0071】
その後、測位処理モジュール301は、数4に示したように、予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより補償量(Cx,Cy,Cori)を更新する。そして、時刻td以降の移動体の位置((xg(ti),yg(ti))および方位角(orig(ti))については、上記した方法を繰り返し行うことにより求めることができる。
【0072】
このように、測位処理モジュール301は、新たなGPSデータが得られた時刻よりも後の時刻(例えばtb)における移動体の位置(例えば((xg(td),yg(td)))および方位角(例えば(orig(td))をマップマッチング処理し、そのマップマッチング処理により得られた移動体の位置(例えば((Xg(td),Yg(td)))および方位角(例えば(ORIg(td))と、新たなGPSデータから得られた移動体の位置(例えば((xg’(tb),yg’(tb)))および方位角(例えば(orig’(tb))を始点としてジャイロデータおよび車速データを用いて推定された移動体の位置(例えば((Xg’(td),Yg’(td)))および方位角(例えば(ORIg’(td))とを用いることにより補償量(Cx,Cy,Cori)を求めることも可能である。
【0073】
ところで、GPSデータから得られる移動体の位置および方位角は、電離層の影響などによるドリフトや、周期が長いことに起因するジャンプにより、ガウス分布となっていない。そのため、補償係数(Kx,Ky,Kori)を仮に一般的な線形のカルマンフィルタで規定した場合には、数1の解が不安定となり、精度良く移動体の位置を求めることが困難となる。一方、本実施の形態では、補償係数(Kx,Ky,Kori)を、確率密度関数を用いて規定しているので、数1の解が安定となり、精度良く移動体の位置を求めることが可能となる。
【0074】
図11は、予想誤差(ΔDx,ΔDy,ΔDori)の値(破線)の一例と、補償量(Cx,Cy,Cori)の値(実線)の一例とを示したものである。ここで、予想誤差(ΔDx,ΔDy,ΔDori)の値は、数5において転換行列(Hx,Hy,Hori)の各成分を1としたときに得られたものであり、補償量(Cx,Cy,Cori)の値は、数4において数6および数7を代入したときに得られたものである。
【0075】
従来のように、補償係数(Kx,Ky,Kori)を一般的な線形のカルマンフィルタで規定した場合には、補償係数(Kx,Ky,Kori)が一種のローパスフィルタとして振る舞うので、補償量(Cx,Cy,Cori)の値が急激かつ極端にジャンプするのが抑制されるだけでなく、さらに、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化することも抑制されてしまう。その結果、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができない。
【0076】
一方、本実施の形態では、図11から、補償量(Cx,Cy,Cori)の値が急激かつ極端にジャンプするのが抑制されているものの、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化しているのがわかる。つまり、予想誤差(ΔDx,ΔDy,ΔDori)の値に応じて変化することが要求されるケースにおいて、充分な追従性を発揮することができる。
【0077】
次に、制御モジュール303について説明する。
【0078】
制御モジュール303は、数8に示したように、測位処理モジュール301において求められた移動体の位置((xg(ti),yg(ti))に、CCDカメラ101の画像データから得られる道路2の所定の位置からのオフセット(xv(ti),yv(ti))を加算すると共に、測位処理モジュール301において求められた移動体の方位角(oriv(ti))に、方位角(orig(ti))と道路2と平行な線(例えば道路2の中央線3)とのなす角(oriv(ti))を加算する。これは、主に、測位処理モジュール301のマップマッチング処理によって移動体の位置((xg(ti),yg(ti)))や方位角(orig(ti))が道路上の所定の場所(例えば道路の中央線)に移動させられたことによる誤差を取り除くために行われるものである。従って、測位処理モジュール301において求められた移動体の位置((xg(ti),yg(ti)))および方位角(orig(ti))を、数8を用いて補正することにより、実際の移動体の位置((x(ti),y(ti)))および方位角(ori(ti))に戻すことが可能となる。なお、このプロセスは、測位処理モジュール301においてマップマッチング処理により求められた移動体の位置((Xg(ti),Yg(ti)や方位角(ORIg(ti))に対してももちろん適用可能である。
【数8】
【0079】
ここで、オフセット(xv(ti),yv(ti))は、例えば、数9に示したように、地図管理部20から読み出された地図データから抽出された道路幅Wから、CCDカメラ101の画像データから得られた移動体の現在位置と道路2の路側線4との距離d(ti)を減算して得られた値を、π/2に、移動体の方位角(orig(ti))と、道路2と平行な線(例えば道路2の中央線3)と直交する直交線5と移動体の方位角(oriv(ti))とのなす角(θ(ti))とを加算して得られた角度を用いてx成分とy成分とに分解することにより得られる。
【数9】
【0080】
このように、制御モジュール303は、CCDカメラ101の画像データと、地図管理部20から読み出された地図データとを利用して、測位処理モジュール301において求められた移動体の位置((xg(ti),yg(ti)))および方位角(orig(ti))を、実際の移動体の位置((x(ti),y(ti)))および方位角(ori(ti))に戻したことにより、移動体をリアルタイムに制御することが可能となる。
【0081】
例えば、移動体の現在の位置((x(ti),y(ti)))の前方の道路形状と、移動体の現在の車速v(ti)および方位角(ori(ti))とに基づいて、移動体のブレーキや、ハンドルの操舵角などを制御する制御信号を生成する。また、移動体の走行ルートがあらかじめ設定されている場合には、その走行ルートに基づいて、移動体のアクセルや、ブレーキ、ハンドル、ウインカなどを制御する制御信号を生成する。その結果、制御部50は、その制御信号に応じた制御を制御対象の機器(例えば、移動体のアクセルや、ブレーキ、ハンドル、ウインカなど)に対して行うこととなる。
【0082】
次に、本実施の形態の移動体位置検出装置1の動作について説明する。
【0083】
この移動体位置検出装置1では、測位処理モジュール301において、例えば、一のGPSデータを始点として導出された時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))と、その次のGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) とを含む予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0084】
また、この移動体位置検出装置1では、測位処理モジュール301において、例えば、一のGPSデータを始点として導出された時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、その次のGPSデータを始点として導出された時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc)) とを含む予想誤差(ΔDx,ΔDy,ΔDori)に、その予想誤差(ΔDx,ΔDy,ΔDori)の確率密度関数(Px,Py,Pori)を含む補償係数(Kx,Ky,Kori)を乗算することにより導出される補償量(Cx,Cy,Cori)を用いて、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))が特定される。
【0085】
これにより、本実施の形態では、リアルタイムで精度良く時刻ta以降の移動体の現在位置を検出することが可能となる。
【0086】
また、本実施の形態では、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正しているので、より一層精度良く移動体の現在位置および方位角を検出することが可能となる。
【0087】
以上、実施の形態を挙げて本発明を説明したが、本発明はこれに限定されるものではなく、種々の変形が可能である。
【0088】
例えば、上記実施の形態では、測位処理モジュール301、画像処理モジュール302および制御モジュール303はデバイス(ハードウェア)として記述されていたが、中央処理部30にロードされたプログラムであってもよい。また、記録媒体2に記録されている地図データについても、あらかじめ中央処理部30にロードされていてもよい。
【図面の簡単な説明】
【0089】
【図1】本発明の一実施の形態に係る移動体位置検出装置の概要構成図である。
【図2】センサ入力部からの各入力の周期およびタイミングを表す関係図である。
【図3】移動体の変位の様子の一例を表す平面図である。
【図4】移動体の位置および方位角の補正の一例について説明するための平面図である。
【図5】マップマッチング処理について説明するための平面図である。
【図6】図4に続いて行われる補正について説明するための平面図である。
【図7】センサ入力部からの各入力の周期およびタイミングを表す関係図である。
【図8】移動体の位置および方位角の補正の他の例について説明するための平面図である。
【図9】図8に続いて行われる補正について説明するための平面図である。
【図10】カメラの画像データを利用して移動体の位置および方位角を補正する方法について説明するための平面図である。
【図11】実施例における移動体の位置(x,y)および方位角の変位の様子を表す変位図である。
【符号の説明】
【0090】
1…移動体位置検出装置、2…道路、3…道路中央線、4…路側線、5…直交線、10…センサ入力部、20…地図管理部、30…中央処理部、40…映像表示部、50…制御部、101…CCDカメラ、102…GPSセンサ、103…ジャイロセンサ、104…車速センサ、201…記録媒体、202…ドライバ、301…測位処理モジュール、302…画像処理モジュール、303…制御モジュール。
【特許請求の範囲】
【請求項1】
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定し、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出し、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動し、
移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する
ことを特徴とする移動体位置検出方法。
【請求項2】
前記予想誤差は、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分である
ことを特徴とする請求項1に記載の移動体位置検出方法。
【請求項3】
移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項1に記載の移動体位置検出方法。
【請求項4】
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定し、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定し、
一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出し、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動し、
移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する
ことを特徴とする移動体位置検出方法。
【請求項5】
前記予想誤差は、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、一方の推定により得られた時刻tcにおける位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分、または、移動体の位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))の少なくとも1つに対して重み付けがなされた上での、移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分である
ことを特徴とする請求項4に記載の移動体位置検出方法。
【請求項6】
移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項4に記載の移動体位置検出方法。
【請求項7】
受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定する移動体位置推定手段と、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段と、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動する移動体位置移動手段と、
移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を特定する移動体位置特定手段と
を備えたことを特徴とする移動体位置検出装置。
【請求項8】
前記予想誤差は、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分である
ことを特徴とする請求項7に記載の移動体位置検出装置。
【請求項9】
前記移動体位置特定手段は、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項7に記載の移動体位置検出装置。
【請求項10】
受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定する第1移動体位置推定手段と、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定する第2移動体位置推定手段と、
一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段と、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動する移動体位置移動手段と、
移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する移動体位置特定手段と
を備えたことを特徴とする移動体位置検出装置。
【請求項11】
前記予想誤差は、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、一方の推定により得られた時刻tcにおける位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分、または、移動体の位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))の少なくとも1つに対して重み付けがなされた上での、移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分である
ことを特徴とする請求項10に記載の移動体位置検出装置。
【請求項12】
前記移動体位置特定手段は、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項10に記載の移動体位置検出装置。
【請求項1】
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定し、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出し、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動し、
移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する
ことを特徴とする移動体位置検出方法。
【請求項2】
前記予想誤差は、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分である
ことを特徴とする請求項1に記載の移動体位置検出方法。
【請求項3】
移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項1に記載の移動体位置検出方法。
【請求項4】
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定し、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定し、
一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出し、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動し、
移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する
ことを特徴とする移動体位置検出方法。
【請求項5】
前記予想誤差は、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、一方の推定により得られた時刻tcにおける位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分、または、移動体の位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))の少なくとも1つに対して重み付けがなされた上での、移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分である
ことを特徴とする請求項4に記載の移動体位置検出方法。
【請求項6】
移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項4に記載の移動体位置検出方法。
【請求項7】
受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、次のGPSデータが得られる時刻taまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を推定する移動体位置推定手段と、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段と、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))を、その道路上の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))に移動する移動体位置移動手段と、
移動体の位置(Xg(ta),Yg(ta))および方位角(ORIg(ta))を、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻ta以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を特定する移動体位置特定手段と
を備えたことを特徴とする移動体位置検出装置。
【請求項8】
前記予想誤差は、GPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、推定により得られた時刻taにおける移動体の位置(xg(ta),yg(ta))および方位角(orig(ta))との差分、または、位置(xg’(ta),yg’(ta))、方位角(orig’(ta))、位置(xg(ta),yg(ta))および方位角(orig(ta))の少なくとも1つに対して重み付けがなされた上での、位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) と、位置(xg(ta),yg(ta))および方位角(orig(ta))との差分である
ことを特徴とする請求項7に記載の移動体位置検出装置。
【請求項9】
前記移動体位置特定手段は、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項7に記載の移動体位置検出装置。
【請求項10】
受信装置によって受信されたGPSデータから時刻ti(iは変数)における移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を取得すると共に、慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を取得する状態データ取得手段と、
GPSデータから得られた時刻t0における移動体の位置(xg(t0),yg(t0))および方位角(orig(t0))を、所定の時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量とを用いて補正することにより、時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を推定する第1移動体位置推定手段と、
次のGPSデータが得られたとき、そのGPSデータから得られた時刻taにおける移動体の位置(xg’(ta),yg’(ta))および方位角(orig’(ta)) を、時刻tcまでの間に慣性センサおよび速度センサから得られた移動体の移動距離および方位角の変位量を用いて補正することにより、時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))を推定する第2移動体位置推定手段と、
一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))と、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))とを含む予想誤差に、その予想誤差の確率密度関数を含む補償係数を乗算することにより補償量(Cx,Cy,Cori)を導出する補償量導出手段と、
移動体が実際に存在する可能性の最も高い道路上に移動体が存在するように移動体の位置をマップマッチング処理することにより、一方の推定により得られた時刻tcにおける移動体の位置(xg(tc),yg(tc))および方位角(orig(tc))を、その道路上の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))に移動する移動体位置移動手段と、
移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))を、慣性センサおよび速度センサから得られた移動体の移動距離と方位角の変位量と、補償量(Cx,Cy,Cori)とを用いて補正することにより、時刻tc以降の移動体の位置(xg(ti),yg(ti))および方位角(orig(ti)) (iは変数)を特定する移動体位置特定手段と
を備えたことを特徴とする移動体位置検出装置。
【請求項11】
前記予想誤差は、他方の推定により得られた時刻tcにおける移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、一方の推定により得られた時刻tcにおける位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分、または、移動体の位置(xg’(tc),yg’(tc))、方位角(orig’(tc))、位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))の少なくとも1つに対して重み付けがなされた上での、移動体の位置(xg’(tc),yg’(tc))および方位角(orig’(tc))と、移動体の位置(Xg(tc),Yg(tc))および方位角(ORIg(tc))との差分である
ことを特徴とする請求項10に記載の移動体位置検出装置。
【請求項12】
前記移動体位置特定手段は、移動体の位置(xg(ti),yg(ti))および方位角(orig(ti))を、時刻tiにおける移動体の位置(xg(ti),yg(ti)) の前方の映像データから得られた道路に関するデータを用いて補正する
ことを特徴とする請求項10に記載の移動体位置検出装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【公開番号】特開2008−175786(P2008−175786A)
【公開日】平成20年7月31日(2008.7.31)
【国際特許分類】
【出願番号】特願2007−11872(P2007−11872)
【出願日】平成19年1月22日(2007.1.22)
【出願人】(507080628)
【Fターム(参考)】
【公開日】平成20年7月31日(2008.7.31)
【国際特許分類】
【出願日】平成19年1月22日(2007.1.22)
【出願人】(507080628)
【Fターム(参考)】
[ Back to top ]