(57)【要約】【課題】比較的低価格のジャイロ及び速度（加速度）センサを使用しても、実際の移動軌跡との誤差が小さい軌跡を算出できる新しい算出方法を実現。【解決手段】移動物体の第１地点Pから第２地点Qへの間の移動軌跡を移動後に算出する移動軌跡算出装置であって、第１地点Pから第２地点Qへの移動中の移動方向及び移動速度を検出して移動データとして記憶する記憶装置21と、記憶した移動データを順方向に使用して、第１地点から第２地点への順方向移動軌跡を算出する順方向軌跡算出部31と、記憶した移動データを逆方向に使用して、第２地点から第１地点への逆方向移動軌跡を算出する逆方向軌跡算出部32と、順方向移動軌跡と逆方向移動軌跡を、第１地点と第２地点に対する位置に応じて変化する重み付け比で合成する合成処理部33と、を備える。【選択図】図３
2008014810.tif 000002
【特許請求の範囲】
【請求項１】
移動物体の第１地点から第２地点への間の移動軌跡を移動後に算出する移動軌跡算出方法であって、
前記第１地点から前記第２地点への移動中の移動方向及び移動速度を検出して移動データとして記憶し、
記憶した前記移動データを順方向に使用して、前記第１地点から前記第２地点への順方向移動軌跡を算出し、
記憶した前記移動データを逆方向に使用して、前記第２地点から前記第１地点への逆方向移動軌跡を算出し、
前記順方向移動軌跡と前記逆方向移動軌跡を、前記第１地点と前記第２地点に対する位置に応じて変化する重み付け比で合成する、ことを特徴とする移動軌跡算出方法。
【請求項２】
前記第１地点及び前記第２地点は、ＧＰＳ受信機で４機以上の可視衛星により位置検出した地点又はＤＯＰ値などにより精度が良いと判断される地点であり、前記第１地点から前記第２地点の間は、ＧＰＳ受信機による可視衛星が４機未満部分又はＤＯＰ値などにより精度が悪いと判断される部分である請求項１に記載の移動軌跡算出方法。
【請求項３】
前記移動方向の検出は、ジャイロで行われる請求項１又は２に記載の移動軌跡算出方法。
【請求項４】
前記移動物体は、車両であり、
前記移動速度の検出は、車速センサで行われる請求項１に記載の移動軌跡算出方法。
【請求項５】
前記移動速度及び前記移動方向は、前記ＧＰＳ受信機で検出した方向ごとの前記移動速度から算出される請求項２に記載の移動軌跡算出方法。
【請求項６】
前記順方向移動軌跡と前記逆方向移動軌跡は、カルマンフィルタにより算出される請求項１から５のいずれか１項に記載の移動軌跡算出方法。
【請求項７】
移動物体の第１地点から第２地点への間の移動軌跡を移動後に算出する移動軌跡算出装置であって、
前記第１地点から前記第２地点への移動中の移動方向及び移動速度を検出して移動データとして記憶する記憶装置と、
記憶した前記移動データを順方向に使用して、前記第１地点から前記第２地点への順方向移動軌跡を算出する順方向軌跡算出部と、
記憶した前記移動データを逆方向に使用して、前記第２地点から前記第１地点への逆方向移動軌跡を算出する逆方向軌跡算出部と、
前記順方向移動軌跡と前記逆方向移動軌跡を、前記第１地点と前記第２地点に対する位置に応じて変化する重み付け比で合成する合成処理部と、を備えることを特徴とする移動軌跡算出装置。
【請求項８】
ＧＰＳ受信機を備え、
前記第１地点及び前記第２地点は、前記ＧＰＳ受信機で４機以上の可視衛星により位置検出した地点又はＤＯＰ値などにより精度が良いと判断される地点であり、前記第１地点から前記第２地点の間は、ＧＰＳ受信機による可視衛星が４機未満であるか又はＤＯＰ値が大きく精度が悪いと判断される部分である請求項７に記載の移動軌跡算出装置。
【請求項９】
ジャイロを備え、
前記移動方向の検出は、前記ジャイロで行われる請求項７又は８に記載の移動軌跡算出装置。
【請求項１０】
前記移動物体は、車両であり、
前記車両の移動速度を検出する車速センサを備える請求項７に記載の移動軌跡算出装置。
【請求項１１】
前記ＧＰＳ受信機により検出された方向ごとの移動速度から前記移動速度及び前記移動方向を算出する移動速度算出部を備える請求項８に記載の移動軌跡算出装置。
【請求項１２】
前記軌跡算出部は、カルマンフィルタを備える請求項７から１１のいずれか１項に記載の移動軌跡算出装置。
【請求項１３】
地図データを生成する地図データ生成方法であって、
車両で道路を走行しながら、前記車両に搭載したＧＰＳ装置で走行位置を検出すると共に、移動中の移動方向及び移動速度を検出して移動データとして記憶し、
前記ＧＰＳ受信機で４機以上の可視衛星により位置検出した地点又はＤＯＰ値などにより精度が良いと判断される地点については前記ＧＰＳ装置で検出した走行位置を道路の地図データとし、
前記ＧＰＳ受信機による可視衛星が４機未満である部分又はＤＯＰ値などにより精度が悪いと判断される部分については、当該部分の両端の前記ＧＰＳ受信機で４機以上の可視衛星により位置検出した地点を第１地点及び第２地点として、記憶した前記移動データを順方向に使用して、前記第１地点から前記第２地点への順方向移動軌跡を算出し、
記憶した前記移動データを逆方向に使用して、前記第２地点から前記第１地点への逆方向移動軌跡を算出し、
前記順方向移動軌跡と前記逆方向移動軌跡を、前記第１地点と前記第２地点に対する位置に応じて変化する重み付け比で合成する、ことを特徴とする地図データ生成方法。
【請求項１４】
前記移動方向の検出は、ジャイロで行われる請求項１３に記載の地図データ生成方法。
【請求項１５】
前記移動速度の検出は、車速センサで行われる請求項１３に記載の地図データ生成方法。
【請求項１６】
前記移動速度及び前記移動方向は、前記ＧＰＳ受信機により検出された方向ごとの移動速度から算出される請求項１３に記載の地図データ生成方法。
【請求項１７】
前記順方向移動軌跡と前記逆方向移動軌跡は、カルマンフィルタにより算出される請求項１３から１６のいずれか１項に記載の地図データ生成方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ナビゲーションシステムや自動車シミュレータなどで使用される地図データを生成する地図データ生成方法に関し、特に車両で道路を走行しながら位置データを収集して地図データを生成する地図データ生成方法及びそこで使用する移動軌跡算出方法、移動軌跡算出装置に関する。
【背景技術】
【０００２】
ナビゲーションシステムでは、ＧＰＳ装置、ジャイロ、速度（加速度）センサなどにより走行中の位置を検出し、地図に走行位置を表示する。走行位置は、ＧＰＳ装置の受信状態が良好、即ち可視衛星数が４機以上の場合にはＧＰＳ装置の検出した走行位置を主とし、ＧＰＳ装置の受信状態が不十分、即ち可視衛星数が４機未満の場合又はＤＯＰの値などから検出精度が悪いと判断できる場合にはジャイロの検出した移動方向（方位）、速度（加速度）センサの検出した移動速度に基づいて算出した位置を主として、求めている。ここで、ＤＯＰの値は、ＧＰＳ衛星の配置から計算した指標で、測位方程式を最小二乗法を用いて解く時、逆行列の対角要素から計算される量で、標準偏差とも関連しており、ＤＯＰの値が小さいほど検出精度が良いことを示す。ＧＰＳ受信機はＤＯＰを出力する機能を標準で装備している。ただし、標準偏差を出力するＧＰＳ受信機は限られている。
また、ＤＯＰの値が小さくてもマルチパスを受けると検出誤差が増大するので、ＤＯＰだけでは精度判定はできないが、マルチパスのない天空視界が開けた場所では、ＤＯＰが検出精度に対応するといえる。
【０００３】
ナビゲーションシステムは、あらかじめ作成された地図データを有しており、検出した走行位置と地図データの道路上の位置を比較し、検出した走行位置が道路から外れた時には道路上の位置に補正することが行われる。
【０００４】
ナビゲーションシステムについては、例えば特許文献１などに記載されている。特許文献１は、各センサの出力に存在する誤差をカルマンフィルタで推定し、推定値に基づいて補正を行って位置を算出する構成を記載している。
【０００５】
上記のような地図データは、航空測量、航空写真、地上での測量などに基づいて作成される。道路は新たに建設されたり、補修などが行われるため、それに応じて地図データも随時修正する必要がある。しかし、上記のような航空測量、航空写真、地上での測量などを頻繁に行うことはコスト面からも難しい。そこで、もっとも簡易な方法として、走行位置を検出する装置を搭載した車両で、実際に道路を走行して、その移動軌跡を検出して地図データを作成することが考えられる。ここで使用する走行位置を検出する装置は、ナビゲーションシステムに類似しているが、ナビゲーションシステムがリアルタイムで走行位置を出力する必要があるのに対して、走行後に走行位置（移動軌跡）を求めればよいオフラインシステムである点である。
【０００６】
図１は、上記のようにして地図データを生成するシステムの構成を示す図である。図１に示すように、車両１は、ＧＰＳ装置２と、移動方向を検出するジャイロ３と、車速センサ４と、走行中にこれらの出力するデータを記憶するデータ記憶装置５と、を搭載している。走行後、データ記憶装置５に記憶された走行中のデータは地図データ生成装置６に送られ、地図データ生成装置６はこれらのデータを処理して走行位置、すなわち移動軌跡を算出して地図データとして出力する。ＧＰＳ装置２は、自動車などに搭載される通常のものよりは高精度でＧＰＳ衛星からの信号の受信及び処理が行えるものであり、車両の上部にアンテナと一体に搭載される。
【０００７】
図１のシステムにおける移動軌跡の算出は、基本的は従来のナビゲーションシステムで行われるのと同様の方法で行われる。ＧＰＳ装置２を使用することにより、可視衛星数が４機以上の時又はＤＯＰ値が小さい時には高精度の位置測定が行えるので、そのような場合にはＧＰＳ装置２の検出した位置を走行位置とする。谷間を走行する場合や建物の間を走行する場合には、天空が遮蔽物で遮られ、可視衛星数が４機未満になる場合又はＤＯＰ値が大きい場合が生じるが、その場合には、一般に高精度の測位解を得ることができない。そこで、ジャイロ３及び車速センサ４の検出データを用いて、カルマンフィルタにより位置推定を行う。
【０００８】
図２は、従来のシステムにおける移動軌跡の算出を説明する図である。図２の（Ａ）に示すように、矢印の方向に走行した場合の移動軌跡において、Ａ１及びＡ２は可視衛星数が４機以上の時又はＤＯＰ値が小さい時にＧＰＳ装置２で検出した走行位置から求めた移動軌跡を示す。このような場合には、上記のようにＧＰＳ装置２により高精度で走行位置を求めることができる。
【０００９】
移動軌跡Ａ１の端点ＰからＡ２の端点Ｑまでの間は、可視衛星数が４機未満又はＤＯＰ値が大の時であり、ジャイロ３及び車速センサ４の検出データを用いて、カルマンフィルタにより位置推定を行って移動軌跡を求める。従来は、図２の（Ａ）に示すように、点Ｐからの移動軌跡Ｂ１を求めるが、このようにして算出された軌跡は誤差が累積するため、図示のように、点Ｑを走行すべき時点で点Ｑ’が走行位置として算出されることになり、点Ｑ’を点Ｑに変更することになる。これは、上記のようにして算出された移動軌跡Ｂ１は実際の道路とはずれているということを意味する。例えば、移動軌跡Ｂ１は途中から道路外になり、建物内を通過したという事態になる。
【００１０】
図１のシステムは、上記のようにオフラインシステムであるので、記憶されたジャイロ３及び車速センサ４の検出データを用いて、図２の（Ｂ）に示すように、点Ｑから点Ｐに向かって逆方向移動軌跡Ｂ２を算出することも考えられるが、上記と同じように、点Ｐを走行すべき時点で点Ｐ’が走行位置として算出されることになり、同様の問題が生じる。
【００１１】
【特許文献１】特開平７−３０１５４１号公報
【発明の開示】
【発明が解決しようとする課題】
【００１２】
ジャイロは、長時間高精度の測定が行える高価なものから、安価ではあるが測定精度のあまり高くないものまである。同様に、速度（加速度）センサも高価なものを使用すれば、その分誤差を小さくできる。従って、高価なジャイロや速度（加速度）センサを使用すれば移動方向の誤差は小さく、算出した移動軌跡と実際の道路の差を小さくできるが、これではコストが高くなるという問題を生じる。
【００１３】
本発明は、比較的低価格のジャイロ及び速度（加速度）センサを使用しても、実際の移動軌跡との誤差が小さい軌跡を算出できる新しい算出方法を実現することを目的とする。
【課題を解決するための手段】
【００１４】
上記目的を実現するため、本発明の移動軌跡算出方法、移動軌跡算出装置及び地図データ生成方法は、移動物体の第１地点から第２地点への間の移動軌跡を移動後に算出する移動軌跡算出に関し、前記第１地点から前記第２地点への移動中の移動方向及び移動速度を検出して移動データとして記憶し、記憶した前記移動データを順方向に使用して、前記第１地点から前記第２地点への順方向移動軌跡を算出し、記憶した前記移動データを逆方向に使用して、前記第２地点から前記第１地点への逆方向移動軌跡を算出し、前記順方向移動軌跡と前記逆方向移動軌跡を、前記第１地点と前記第２地点に対する位置に応じて変化する重み付け比で合成する、ことを特徴とする。
【００１５】
図３は、本発明の原理を説明する図である。図３の（Ａ）において、例えば、移動軌跡Ａ１及びＡ２は車両を移動させた時の移動軌跡で、可視衛星数が４機以上の時又はＤＯＰ値が小さい時にＧＰＳ装置２で検出した走行位置から求めた高精度の移動軌跡を示す。従って、移動軌跡Ａ１及びＡ２のそれぞれの端点である第１地点Ｐ及び第２地点Ｑは、それぞれ高精度の位置精度である。検出して記憶した移動データに基づいて第１地点Ｐと第２地点Ｑの間の移動軌跡を算出する時に、記憶した移動データを順方向に使用して第１地点Ｐから第２地点Ｑへの順方向移動軌跡Ｂ１を算出し、記憶した移動データを逆方向に使用して第２地点Ｑから第１地点Ｐへの逆方向移動軌跡Ｂ２を算出し、順方向移動軌跡Ｂ１と逆方向移動軌跡Ｂ２を、第１地点Ｐと第２地点Ｑの位置に応じて変化する重み付け比で合成する。
【００１６】
図３の（Ｂ）は、合成の重み付け比の変化例を示す図であり、横軸が移動距離又は時間であり、縦軸が重み付け比を示す。
【００１７】
前述のように、ジャイロにより検出される移動方向は誤差が累積する上、更に移動位置は前に算出した移動位置に単位時間当たりの変位分を加算して算出するため、移動速度（加速度）に誤差があると、算出した移動軌跡も徐々に誤差が増加する。従って、順方向移動軌跡Ｂ１は地点Ｐに近い部分は比較的誤差は小さいが、地点Ｐから離れるに従って誤差が大きくなる。同様に、逆方向移動軌跡Ｂ２は地点Ｑに近い部分は比較的誤差は小さいが、地点Ｑから離れるに従って誤差が大きくなる。本発明によれば、地点Ｐに近い部分では順方向移動軌跡Ｂ１に対する重み付け比を大きくし、逆方向移動軌跡Ｂ２に対する重み付け比を小さくし、地点Ｑに近い部分では順方向移動軌跡Ｂ１に対する重み付け比を小さくし、逆方向移動軌跡Ｂ２に対する重み付け比を大きくするので、累積誤差の影響を低減して、実際の移動軌跡に近い軌跡が算出できる。
【００１８】
図３の（Ｂ）に示すように、地点Ｐ及びＱの外側の部分の移動データも使用して移動軌跡を算出する。これは、地点Ｐ及びＱにおいて滑らかな軌跡を得るためである。
【００１９】
なお、図３では軌跡Ａ１及びＡ２の部分は高精度であるとしたが、地点Ｐ及びＱの付近のみがそれぞれ高精度で、それ以外の部分は十分な精度でない場合に、本発明を適用することも可能である。言い換えれば、連続して高精度の移動位置を検出することができず、間の軌跡を移動データに基づいて算出する場合であれば適用可能である。
【００２０】
移動方向の検出は、ジャイロで行われるが、移動方向が検出できれば、ジャイロに限定されない。
【００２１】
移動速度の検出は、移動速度が検出できるものであればどのようなものでもよいが、例えば、移動物体が図１に示した車両の場合には、車速センサを使用できる。
【００２２】
また、ＧＰＳ受信機が各衛星からの受信する受信電波の周波数はドプラー効果により変化しているので、受信電波の周波数を検出すれば各方向の移動速度を検出することができるので、これを利用することも可能である。この場合、移動速度及び移動方向は、各方向の移動速度から算出でき、ジャイロ及び速度センサ（加速度センサ）は設ける必要がなく、ＧＰＳ受信機のみを設ければよい。
【００２３】
移動方向と移動速度による移動軌跡の算出にはカルマンフィルタを利用する。
【００２４】
以上のようにして算出された移動軌跡は、ナビゲーションシステムの地図データや、シミュレータ用のビジュアルデータベース作成や、自動車教習所などの教材の作成などに利用される。
【発明の効果】
【００２５】
本発明によれば、オフラインの移動軌跡算出において算出誤差を低減できる新しい算出アルゴリズムが実現され、低価格のジャイロ及び速度（加速度）センサを使用しても、実際の移動軌跡との誤差が小さい軌跡を算出できるようになる。
【発明を実施するための最良の形態】
【００２６】
図４は、本発明の第１実施例の地図データを生成するシステムの構成を示す図であり、図１のように車両に搭載して収集した移動データから地図データを生成するシステムである。
【００２７】
図４に示すように、第１実施例のシステムは、ＧＰＳ装置１１と、振動子ジャイロ１２と、車速センサ１３と、データ記憶部２１と、順方向軌跡算出部３１と、逆方向軌跡算出部３２と、統合処理部３３と、を有する。振動子ジャイロ１２は、平面ジャイロと垂直ジャイロで構成され、水平面（ｘｙ平面）内のへの移動方向の射影（方位）と、移動方向の水平面とのなす角度（経路角度）が検出される。データ記憶部２１は、ＧＰＳデータ記憶部２２と、移動方向データ記憶部２３と、移動速度データ記憶部２４と、で構成される。ＧＰＳ装置１１、振動子ジャイロ１２、車速センサ１３及びデータ記憶部２１は、図１と同様に、道路を走行する車両に搭載される。言い換えれば、実施例のシステムは、従来例と地図データ生成装置における処理が異なり、他の部分の構成は同じである。順方向軌跡算出部３１、逆方向軌跡算出部３２及び統合処理部３３は、コンピュータにより実現される。
【００２８】
統合処理部３３は、ＧＰＳ装置１１が電波を受信する可視衛星数が４機以上の部分又はＤＯＰ値が小さい部分については、ＧＰＳ装置１１で検出した走行位置から求めた移動軌跡をそのまま移動軌跡とし、可視衛星数が４機未満の部分又はＤＯＰ値が大きい部分については、順方向軌跡算出部３１及び逆方向軌跡算出部３２の出力する順方向軌跡と逆方向軌跡を合成して移動軌跡を算出する。可視衛星数が４機以上の時にＧＰＳ装置１１で検出した走行位置から求めた移動軌跡をそのまま移動軌跡とする点は、従来例も同じである。
【００２９】
順方向軌跡算出部３１は、ＧＰＳ装置１１の可視衛星数が４機未満の部分について、移動方向データ記憶部２３及び移動速度データ記憶部２４に記憶された移動方向データ及び移動速度データ（合わせて移動データ）を順方向に利用して、カルマンフィルタにより位置推定を行って順方向移動軌跡を算出する。これは、従来例も同じである。
【００３０】
逆方向軌跡算出部３２は、ＧＰＳ装置１１の可視衛星数が４機未満の部分について、移動方向データ記憶部２３及び移動速度データ記憶部２４に記憶された移動データを逆方向に利用して、逆方向移動軌跡を算出する。
【００３１】
なお、前述のように、ジャイロにより検出される移動方向は誤差が累積するので、図３における第２地点Ｑに相当する位置での移動方向データには大きな誤差が含まれている可能性がある。そこで、図３で、移動軌跡Ａ２から予測される第２地点Ｑの基準移動方向を算出して、移動方向データが基準移動方向に一致するような補正量を決定し、他の移動データをこの補正量だけ補正する。車速センサ１３の検出する移動速度データ自体は誤差が累積しないので、補正しない。後は、移動データを逆方向に利用して、順方向の場合と同様にカルマンフィルタにより位置推定を行って逆方向移動軌跡を算出する。
【００３２】
また、図３の（Ｂ）に示すように、地点ＰとＱの間の移動データだけでなく、地点ＰとＱの外側の移動データも、移動軌跡Ｂ１及びＢ２を算出するのに使用しているが、これは地点Ｐ及びＱの部分で滑らかな移動軌跡Ｂ１及びＢ２を算出するためである。しかし、地点Ｐ及びＱの外側の軌跡Ａ１及びＡ２の部分は高精度であるので、軌跡としてはＡ１及びＡ２が使用される。
【００３３】
統合処理部３３は、ＧＰＳ装置１１の可視衛星数が４機未満の部分について、順方向軌跡算出部３１の出力する順方向移動軌跡と逆方向軌跡算出部３２の出力する逆方向移動軌跡を図３の（Ｂ）のように変化する重み付け比で合成する。例えば、ある時点における順方向移動軌跡の位置をＪ、逆方向移動軌跡をＫとすると、合成位置Ｌは、Ｌ＝ＲＪ＋ＳＫで表される。ＲとＳは、例えば、図３の（Ｂ）のように変化するが、これに限定されるものではない。
【００３４】
上記のように、移動データを利用して、カルマンフィルタにより位置推定を行って逆方向移動軌跡を算出する手法は、従来例と同じであり、よく知られているが、ここで簡単に説明する。
【００３５】
図５は、この手法における座標系を説明する図である。ここでは３次元の直交座標ｘ、ｙ、ｚにおける車両の重心位置をｘ、ｙ、ｚで表し、ｘｙ平面における進行方向の方位角をψで、進行方向のｘｙ平面となす経路角をγで表す。微小時間Δｔごとに移動位置を算出するとして、時刻ｋにおける値をそれぞれｘk、ｙk、ｚk、ψk、γkで表す。時刻ｋ＋１における移動位置及び移動方向（方位角及び経路角）は、時刻ｋにおける値に対して式（１）で表される。
【００３６】
【数１】
2008014810.tif 000003
【００３７】
ただし、Ｖk、ωkは、時刻ｋでの車速、方位角速度である。ここで、位置ｘk、ｙk、方位角ψk、経路角γkはＧＰＳ装置１１の出力であり、速度Ｖk、方位角速度ωk、経路角速度αkはそれぞれ車速センサ、水平ジャイロ、垂直ジャイロの出力である。
【００３８】
ここで、式（１）を式（２）のように表すとすると、ｘk＋１は式（３）で表せる。
【００３９】
【数２】
2008014810.tif 000004
【００４０】
【数３】
2008014810.tif 000005
【００４１】
ここで、ｗkはｕkの駆動雑音で、正規分布Ｎ（０，Ｑk）に従う。
【００４２】
ＧＰＳ装置の観測方程式は、式（４）及び（５）で表される。
【００４３】
【数４】
2008014810.tif 000006
【００４４】
【数５】
2008014810.tif 000007
【００４５】
ここで、ｖkは観測雑音で、Ｎ（０，Ｒk）に従う。
【００４６】
ＲkはＧＰＳ測位結果を使用するとすれば、緯度、経度の標準偏差またはＤＯＰの関数として得られる。
【００４７】
式（３）で、Ｐkを時刻ｋまでの観測系列が与えられた時のｘkの推定値誤差の共分散行列Ｍk+1を時刻ｋまでの観測系列で与えられた時のｘk+1の１段予測誤差の共分散行列とすると、式（３）より近似的に次の式（６）が得られる。
【００４８】
【数６】
2008014810.tif 000008
【００４９】
ここで、更に式（７）及び（８）とした。
【００５０】
【数７】
2008014810.tif 000009
【００５１】
【数８】
2008014810.tif 000010
【００５２】
また、ｘkとｗkは無相関とした。
【００５３】
式（５）から次の式（９）が得られる。
【００５４】
【数９】
2008014810.tif 000011
【００５５】
従って、拡張カルマンフィルタによる推定アルゴリズムは次の式となり、１段予測値は次の式（１０）で表される。
【００５６】
【数１０】
2008014810.tif 000012
【００５７】
観測値の更新に伴う推定値は、式（１１）で表される。
【００５８】
【数１１】
2008014810.tif 000013
【００５９】
また、推定誤差共分散行列は、式（１２）で表される。
【００６０】
【数１２】
2008014810.tif 000014
【００６１】
カルマンゲインは、式（１３）で表される。
【００６２】
【数１３】
2008014810.tif 000015
【００６３】
また、式（４）から、ｚk（ｋ＝０，１，２，…，ｋ）が与えられた時の観測値ｚk+1の共分散行列は次の式（１４）となる。
【００６４】
【数１４】
2008014810.tif 000016
【００６５】
つまり、式（１５）で表される観測予測誤差は、正規分布Ｎ（０，Ｓk+1）に従う。
【００６６】
【数１５】
2008014810.tif 000017
【００６７】
移動データを利用し、カルマンフィルタにより位置推定を行って逆方向移動軌跡を算出する処理は、以上のように行われるが、ここでは、観測予測誤差のマハラビスの距離により、観測値の信頼度を計算し、低ければ、棄却する処理を行う。また、ＧＰＳ装置の位置と方位角は、擬似距離とドプラー速度より求められる量なので、独立と考えられる。従って、以下に説明するように、位置に関するマハラビスの距離と方位に関するマハラビスの距離を別々に計算する。
【００６８】
まず、位置に関するマハラビスの距離を説明する。
【００６９】
方位の信頼度が低いとして、観測誤差共分散Ｒの方位角分散が、式（１６）のように無限大であるとする。
【００７０】
【数１６】
2008014810.tif 000018
【００７１】
この場合の位置に関するマハラビスの距離は式（１７）ようになる。
【００７２】
【数１７】
2008014810.tif 000019
【００７３】
次に、方位と経路角に関するマハラビスの距離を説明する。
【００７４】
位置の信頼度が低いとして、位置（Ｘ，Ｙ，Ｚ）の分散が式（１８）のように無限大であるとする、
【００７５】
【数１８】
2008014810.tif 000020
【００７６】
この場合の方位と経路角に関するマハラビスの距離は式（１９）ようになる。
【００７７】
【数１９】
2008014810.tif 000021
【００７８】
式（１１）の推定値と式（１２）の共分散行列の更新は次の基準で行う。
【００７９】
式（１７）の左辺の値が式（１９）の左辺の値より大きい場合には、更新を行わず、前の値を使用する。
【００８０】
式（１７）の左辺の値がしきい値より大きい場合には、（ｘ、ｙ、ｚ）の信頼度が低いと考え、式（１８）を式（１２）に使用してＰkを求める。位置の推定値には前の値を使用し、移動方向についてのみ観測値を使用して更新する。
【００８１】
式（１９）の左辺の値がしきい値より大きい場合には、（ψk、γk）の信頼度が低いと考え、式（１６）を式（１２）に使用してＰkを求める。移動方向の推定値には前の値を使用し、位置についてのみ観測値を使用して更新する。
【００８２】
式（１７）の左辺の値がしきい値より大きく且つ式（１９）の左辺の値がしきい値より大きい場合には、通常のカルマンフィルタと同様に、観測値の更新を行う。
【００８３】
第１実施例では、移動方向を検出するために平面ジャイロと垂直ジャイロを有する振動子ジャイロ１２と、移動速度を検出するための車速センサ１３が使用した。ＧＰＳ装置１１は、各衛星に対する車両の移動により受信電波の周波数にドプラー効果が生じて受信周波数が変化する。この受信周波数の変化は車両がある程度以上の速度で移動していないと検出できないが、車両がある程度以上の速度で移動条件に限定すれば、ＧＰＳ装置１１により移動速度を検出することが可能である。第２実施例は、このような条件での実施例である。
【００８４】
図６は、本発明の第２実施例の地図データを生成するシステムにおいて使用されるＧＰＳ装置１１の構成を示す図である。第２実施例は、この構成により算出された移動方向及び移動速度を第１実施例の振動子ジャイロ１２の検出した移動方向及び車速センサ１３の検出した移動速度として使用する以外は、第１実施例と同じ構成である。従って、第２実施例では、振動子ジャイロ１２及び車速センサ１３を設ける必要はない。なお、ＧＰＳ装置１１が移動方向と移動速度を検出する必要があるのは、可視衛星が４機未満の時であるが、ＧＰＳ装置１１による移動方向及び移動速度の検出は常時行うことが望ましい。
【００８５】
図６に示すように、受信周波数解析部４１は、ＧＰＳ受信部４０で受信された複数の衛星からの受信電波の周波数（周期）を解析して、各衛星に対する周波数の基準周波数からの変位を算出する。この変位は各衛星に対するドプラー効果に起因するので、移動速度算出手段４２は、各衛星に対する変位から各衛星に対する速度を算出し、それらを合成して方向を含めた移動速度（移動ベクトル）を算出する。Ｖk算出部４３は、移動ベクトルの絶対値から移動速度Ｖkを算出する。ψk、γk算出部４４は、移動ベクトルの３軸方向の成分の比から方位角ψk、経路角γkを算出する。
【００８６】
上記のようにして算出した移動方向及び移動速度に基づいて、第１実施例と同様に移動軌跡を算出する。この場合、方位角速度、経路角速度は一定として、式（１）の代わりに式（２０）を状態変数として使用する。
【００８７】
【数２０】
2008014810.tif 000022
【００８８】
なお、方位角ψk、経路角γkは式（２１）に従って算出される。
【００８９】
【数２１】
2008014810.tif 000023
【００９０】
ここで、マルチパスにより、擬似距離に誤差が含まれる場合でも、速度を求めるのに使用するデルタレンジまたはドプラー速度は誤差を含まないので、正確な速度を求めることができる。
【００９１】
第２実施例では、第１実施例の式（７）及び（８）が式（２２）のようになる。この式に対してカルマンフィルタを構築する。
【００９２】
【数２２】
2008014810.tif 000024
【産業上の利用可能性】
【００９３】
本発明は、オフラインで軌跡算出を算出する場合に適用できる。
【図面の簡単な説明】
【００９４】
【図１】移動経路に関するデータを得るための車両を含むシステム構成を示す図である。
【図２】移動軌跡算出の従来例を説明する図である。
【図３】本発明の移動軌跡算出の原理を説明する図である。
【図４】本発明の第１実施例の地図データ生成システムの構成を示す図である。
【図５】第１実施例における座標系を説明する図である。
【図６】本発明の第１実施例の地図データ生成システムで使用するＧＰＳ装置の構成を示す図である。
【符号の説明】
【００９５】
１１    ＧＰＳ装置
１２    振動子ジャイロ
１３    車速センサ
２１    データ記憶部
３１    順方向軌跡算出部
３２    逆方向軌跡算出部
３３    統合処理部
【図１】
2008014810.tif 000025
【図２】
2008014810.tif 000026
【図３】
2008014810.tif 000027
【図４】
2008014810.tif 000028
【図５】
2008014810.tif 000029
【図６】
2008014810.tif 000030