位置補正装置
【課題】測位結果からバイアス誤差を適切に除去できるようにする。
【解決手段】本装置は、ＧＰＳで計測された位置座標を、ＧＰＳ速度ベクトルと自律航法センサ出力とを用いて補正し、ｔ＝Ｋ・Ｔ時間前までに得た補正後の位置座標Ｐｃ（ｎ），…，Ｐｃ（ｎ−Ｋ）から、軌跡データ｛Ｑ（０），Ｑ（１），…，Ｑ（Ｍ）｝を生成する。Ｑ（ｍ）は、現在地点Ｑ（０）＝Ｐｃ（ｎ）から、長さｍ・ｄＬ遡った地点の位置座標であり、Ｑ（Ｍ）＝Ｐｃ（ｎ−Ｋ）である。この他、本装置は、地点Ｑ（０）を前回のバイアス誤差推定値Ｙで補正して座標Ｘを求め、座標Ｘから最短の道路上の座標Ｒ（０）を算出する。また、座標Ｒ（０）を基点に、長さｄＬで等間隔に配置される道路上の地点の位置座標Ｒ（１），…，Ｒ（Ｍ）を求め、道路区間データ｛Ｒ（０），…，Ｒ（Ｍ）｝を生成し、各地点の誤差Δ（ｍ）＝Ｑ（ｍ）−Ｒ（ｍ）の重み付け平均εを、バイアス誤差として推定する。
【解決手段】本装置は、ＧＰＳで計測された位置座標を、ＧＰＳ速度ベクトルと自律航法センサ出力とを用いて補正し、ｔ＝Ｋ・Ｔ時間前までに得た補正後の位置座標Ｐｃ（ｎ），…，Ｐｃ（ｎ−Ｋ）から、軌跡データ｛Ｑ（０），Ｑ（１），…，Ｑ（Ｍ）｝を生成する。Ｑ（ｍ）は、現在地点Ｑ（０）＝Ｐｃ（ｎ）から、長さｍ・ｄＬ遡った地点の位置座標であり、Ｑ（Ｍ）＝Ｐｃ（ｎ−Ｋ）である。この他、本装置は、地点Ｑ（０）を前回のバイアス誤差推定値Ｙで補正して座標Ｘを求め、座標Ｘから最短の道路上の座標Ｒ（０）を算出する。また、座標Ｒ（０）を基点に、長さｄＬで等間隔に配置される道路上の地点の位置座標Ｒ（１），…，Ｒ（Ｍ）を求め、道路区間データ｛Ｒ（０），…，Ｒ（Ｍ）｝を生成し、各地点の誤差Δ（ｍ）＝Ｑ（ｍ）−Ｒ（ｍ）の重み付け平均εを、バイアス誤差として推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、位置補正装置に関する。
【背景技術】
【０００２】
従来、ＧＰＳ（Global Positioning System）測位では、測位結果に、伝播遅延によるバイアス誤差、マルチパスによる測位誤差、その他、種々の要因によるランダム誤差が、生じることが知られている。
【０００３】
バイアス誤差は、ＧＰＳ衛星と地上との間に介在する電離層をＧＰＳ信号が通過する際に、伝播遅延が生じることを主な原因として発生するものである。このバイアス誤差は、電離層を主な原因として生じるものであるため、常時発生するが、誤差の程度は、電離層の状態によって時々刻々と変化する。
【０００４】
このため、従来装置（カーナビゲーション装置等）では、バイアス誤差の情報を、ＦＭ放送局（所謂ＤＧＰＳ局）から、ＦＭ放送波の形態で受信し、受信したバイアス誤差情報に従って、ＧＰＳ受信機により計測された位置座標を、補正することが行われている（例えば、特許文献１参照）。
【特許文献１】特開平１０−１８５６００号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、ＦＭ放送局からバイアス誤差情報を受信して、ＧＰＳ受信機により計測された位置座標を補正する方法では、装置にＦＭ受信機を搭載する必要があるため、製品コストが高くなるといった問題があった。
【０００６】
また、この方法では、測位地点がＦＭ放送局から離れた地点にある場合、周辺環境の相違から、測位地点でのバイアス誤差がＦＭ放送局でのバイアス誤差と異なるため、ＦＭ放送局からバイアス誤差情報を受信しても、受信先装置（ナビゲーション装置）では、バイアス誤差を、ＧＰＳ受信機の測位結果から精度よく取り除くことができず、精度よく現在地点の位置座標を検出することができないといった問題があった。また、従来方法では、ＦＭ放送局から受信した確度の低いバイアス誤差情報に基づいて、ＧＰＳ受信機の測位結果を補正することで、むしろ位置精度が悪化する可能性があった。
【０００７】
本発明は、こうした問題に鑑みなされたものであり、ＧＰＳ受信機の測位結果からバイアス誤差を従来よりも適切に取り除くことが可能な位置補正装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
かかる目的を達成するためになされた本発明（請求項１記載）の位置補正装置は、バイアス誤差推定手段を備え、次のようにして、ＧＰＳ受信機が計測する位置座標のバイアス誤差を推定し、推定したバイアス誤差に基づき、ＧＰＳ受信機か計測した位置座標を補正するものである。
【０００９】
本発明の位置補正装置は、位置情報取得手段によって、ＧＰＳ受信機が計測した位置座標を取得し、位置情報取得手段が取得した位置座標を基に、過去所定期間におけるＧＰＳ受信機の位置座標を複数地点表した軌跡データを、軌跡データ生成手段によって生成する。
【００１０】
また、位置補正装置は、敷設された道路の位置座標を表す道路データを記憶する道路データ記憶手段を備え、この道路データと、位置情報取得手段が取得した位置座標と、に基づき、道路推定手段にて、過去所定期間にＧＰＳ受信機が移動した道路区間を推定する。
【００１１】
そして、バイアス誤差推定手段は、上記道路推定手段により推定された道路区間における複数地点の位置座標と、上記軌跡データが示すＧＰＳ受信機の複数地点の位置座標と、に基づき、バイアス誤差を推定する。
【００１２】
本発明の位置補正装置は、このようにして推定したバイアス誤差に基づき、ＧＰＳ受信機により計測された位置座標を補正し、補正後の位置座標を出力する（補正出力手段）。
この位置補正装置は、道路を移動する移動体（車両等）に搭載されたＧＰＳ受信機の測位結果を補正するために用いられるものであり、移動体が道路に沿って移動しているという仮定の下で、バイアス誤差を推定するものである。そして、本発明の特徴は、計測された複数地点の位置座標と、道路の複数地点の位置座標と、に基づいて、バイアス誤差を推定する点にある。
【００１３】
即ち、本発明では、計測された複数地点の位置座標と、道路の複数地点の位置座標と、に基づいて、バイアス誤差を推定することにより、ＧＰＳ受信機の移動軌跡と道路形状とを複数地点に渡って比較できるようにし、正確にバイアス誤差を推定することができるようにした。
【００１４】
従来技術では、ＦＭ放送局から受信したバイアス誤差情報に基づき、測位結果を補正しているので、受信情報が示すバイアス誤差が、測位地点でのバイアス誤差の真値から大きくずれている場合、測位結果からバイアス誤差を適切に除去して、測位結果を精度よく補正できないといった問題があったが、本発明によれば、上述した方法で、測位地点でのバイアス誤差を精度よく推定することができるので、従来よりも、計測された位置座標から適切にバイアス誤差を除去でき、測位結果を正確に補正することができる。また、この位置補正装置によれば、ＦＭ受信機を装置内に搭載する必要がないので、製品を安価に製造することができる。
【００１５】
尚、道路データに基づき測位結果を補正する技術としては、マップマッチング技術が知られているが、周知のマップマッチング技術は、測位結果と道路位置とを比較して、測位結果を、道路上の位置座標に強制的に補正するものである。従って、この技術では、必ず道路を示す線上に移動体の位置座標が乗ってしまい、道に対する相対的な移動体の軌跡、つまり道に対する移動体の運動を正確に表現することができない。
【００１６】
これに対し、本発明によれば、上述の方法により、バイアス誤差を適切に求めることができ、求めたバイアス誤差を使って測位結果を適切に補正することができるので、移動体の運動を正確に表現したまま位置座標を補正することができる。従って、この技術を、車両制御に応用すれば、道路に対する車両運動を検出することができ、危険な運転の判定や、それに応じてドライバ操作をアシストする制御を正確に実行することができる。
【００１７】
例えば、従来技術では、ＧＰＳ受信機の測位結果を補正して、カーブを通過する車両の正確な移動軌跡を求めようとしても、測位結果と道路位置とを比較して、測位結果を、道路上の位置座標に補正するため、図９上段に示すように、適切に補正を行うことができず、補正により道路に対する相対的な運動情報が失われて、車両の運動を正確に表現することができないといった問題があった。このため、ＧＰＳ受信機による測位結果と道路形状から危険な運転を判定することや、それに応じてドライバ操作をアシストする制御を実行しようとしても、当該制御を、適切に実行することができないといった問題があった。
【００１８】
これに対し、本発明の位置補正装置を用いれば、図９下段に示すように、正確にバイアス誤差を推定して測位結果を補正することができるので、補正により位置座標の連続的な変化に関する情報が失われることがなく、正確な車両運動を表現することができ、ドライバ操作をアシストする制御を正確に実行することができる。
【００１９】
ところで、道路推定手段は、軌跡データが示す各地点間の位置関係に従って、軌跡データが示す各地点の位置座標に対応する道路区間上の位置座標を、推定する構成にすることができる。また、バイアス誤差推定手段は、軌跡データが示す各地点の位置座標毎に、軌跡データが示す位置座標と、当該位置座標に対応する上記推定された道路区間上の位置座標との誤差を算出し、算出した各地点の誤差の平均を、バイアス誤差として推定する構成にすることができる（請求項２）。
【００２０】
バイアス誤差は、ランダム誤差よりも時間的に緩やかに変化するものであるので、短い期間において、ＧＰＳ受信機の位置座標は、バイアス誤差の影響により、真の位置座標よりも一定方向に一定距離ずれて、計測される。従って、軌跡データが示す各地点の位置関係とパターンが一致するようにして道路区間上の地点を設定し、各地点での軌跡データが示す位置座標と、道路上の位置座標との誤差の平均を、バイアス誤差として推定するようにすれば、正確にバイアス誤差を推定することができる。
【００２１】
また、バイアス誤差推定手段は、各地点の誤差の単純平均を、バイアス誤差の推定値として算出する構成にされてもよいが、各地点の誤差の重み付け平均を、バイアス誤差の推定値として算出する構成にされてもよい。
【００２２】
具体的に、バイアス誤差の推定値として、重み付け平均を求める場合には、ＧＰＳ受信機が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、重み付け平均を算出するように、バイアス誤差推定手段を構成するとよい（請求項３）。このように、バイアス誤差を推定すれば、電離層の変化（バイアス誤差の時間変化）にも適切に対応して、正確にバイアス誤差を推定することができる。
【００２３】
また、道路推定手段は、バイアス誤差推定手段が過去に推定したバイアス誤差を加味して、過去所定期間にＧＰＳ受信機が移動した道路区間を推定する構成にされると好ましい（請求項４）。ＧＰＳ受信機から得られる位置座標には、当然のことながら、バイアス誤差が含まれる。従って、バイアス誤差推定手段により推定されたバイアス誤差で、ＧＰＳ受信機から得られた位置座標を補正して、ＧＰＳ受信機が移動した道路を推定するようにすれば、一層正確に、ＧＰＳ受信機が移動した道路区間を推定することができ、結果として、バイアス誤差を、精度よく検出することができる。
【００２４】
尚、道路推定手段は、バイアス誤差推定手段が過去に推定したバイアス誤差に基づいて、軌跡データが示す位置座標を補正し、当該補正後の位置座標に基づき、軌跡データが示す地点の分布と相関が高い道路を探索し、探索により得られた道路上で、過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にすることができる（請求項５）。
【００２５】
この他、道路推定手段は、軌跡データが示す各地点の位置座標の内、特定地点の位置座標を、バイアス誤差推定手段が過去に推定したバイアス誤差に基づき補正し、補正後の位置座標から最も近い道路を探索して、探索により得られた最も近い道路を、ＧＰＳ受信機が移動した道路に推定し、この道路上で、過去所定期間にＧＰＳ受信機が移動した道路区間を推定する構成にすることができる（請求項６）。
【００２６】
このような手法で、道路区間を推定すれば、簡単な手順で正確に、ＧＰＳ受信機が移動した道路区間を推定することができる。
また、道路区間の推定方法としては、軌跡データが示す各地点の位置座標の内、特定地点の位置座標を、バイアス誤差推定手段が過去に推定したバイアス誤差に基づき補正し、補正後の位置座標から最も近い道路上の地点を基点にして、基点からの道路に沿う距離（道路長）が、軌跡データが示す特定地点からの移動軌跡に沿う距離（経路長）に一致するように、上記軌跡データが示す各地点の位置座標に対応する道路上の位置座標を推定することで、軌跡データが示す位置座標に対応する道路上の位置座標を、各地点間の位置関係に従って推定し、推定した道路上の位置座標の両端を結ぶ区間を、道路区間に推定する方法を挙げることができる。
【００２７】
また、位置補正装置は、道路推定手段により上記探索された道路と上記補正後の特定地点の位置座標との距離が、所定の上限値を超えている場合に、バイアス誤差推定手段での軌跡データに基づいたバイアス誤差の推定動作を停止可能な構成にされるとよい。そして、補正出力手段は、バイアス誤差推定手段によりバイアス誤差が推定されない期間、その期間前にバイアス誤差推定手段により推定されたバイアス誤差に基づき、ＧＰＳ受信機により計測された位置座標を補正し、当該補正後の位置座標を出力する構成にされるとよい（請求項７）。
【００２８】
このように位置補正装置を構成すれば、ＧＰＳ受信機が移動した道路として、誤った道路が推定され、結果として、誤ったバイアス誤差が推定されてしまうのを防止することができる。
【００２９】
また、車両に搭載されたＧＰＳ受信機を対象に位置補正装置を構成する場合には、当該位置補正装置に、車両に搭載された自律航法センサの出力に基づき、車両の変位量を算出して、当該変位量に基づき、位置情報取得手段が取得した位置座標を補正する前処理手段を設けて、軌跡データ生成手段は、前処理手段による補正後の位置座標に基づき、過去所定期間におけるＧＰＳ受信機の位置座標を複数地点表した軌跡データを生成する構成にされるとよい（請求項８）。
【００３０】
このように位置補正装置を構成すれば、より一層高精度にバイアス誤差を求めることができ、ＧＰＳ受信機により計測された位置座標を適切に補正することができる。
【発明を実施するための最良の形態】
【００３１】
以下に本発明の実施例について、図面と共に説明する。図１は、車両に搭載された位置検出装置１の構成を表す説明図である。
図１に示すように、本実施例の位置検出装置１は、ＧＰＳ受信機１０と、自律航法センサ群２０と、位置補正部３０と、道路データベース４０と、インタフェース部５０と、を備える。
【００３２】
ＧＰＳ受信機１０は、周知のＧＰＳ受信機と同様の構成にされており、ＧＰＳ衛星から送信されてくるＧＰＳ信号を、アンテナ１１を通じて受信して、ＧＰＳ信号からＧＰＳメッセージを抽出し、複数（４以上）のＧＰＳ衛星に関して得られた擬似距離及び衛星軌道情報に基づいて、幾何学的な演算により自装置の位置座標Ｐを算出する。位置座標Ｐは、例えば、ＥＮＵ座標系やＥＣＥＦ座標系などの３次元座標系で算出される。
【００３３】
また、本実施例のＧＰＳ受信機１０は、ＧＰＳ信号のドップラーシフト量に基づいて、自装置の速度ベクトルＶを算出する機能を有する。ＧＰＳ信号のドップラーシフト量に基づいた速度ベクトルＶの算出方法は、周知であるので、ここでは、速度ベクトルＶの算出方法について、簡単に述べるが、速度ベクトルＶの算出に当たっては、複数（４以上）のＧＰＳ衛星について、受信したメッセージからＧＰＳ衛星の速度ベクトルを算出し、更に、ＧＰＳ受信機からＧＰＳ衛星への単位方向ベクトルを算出する。そして、これらのベクトル情報と、各ＧＰＳ衛星から受信したＧＰＳ信号のキャリア周波数と、各ＧＰＳ衛星から送出されるＧＰＳ信号のキャリア周波数と、に基づいて、自装置の速度ベクトルＶを算出する。
【００３４】
ＧＰＳ受信機１０は、このようにして算出した速度ベクトルＶを、自装置の位置座標Ｐと共に、位置補正部３０に向けて出力する。また、ＧＰＳ受信機１０は、位置座標Ｐ及び速度ベクトルＶと同時に、ＤＯＰ値に基づいて求めた位置座標Ｐの測定精度εｐ及び速度ベクトルＶの測定精度εｖを表す測定精度情報を出力する。
【００３５】
尚、本実施例では、測定精度εｐ，εｖについての高精度な情報は、必要ないので、測定精度の算出方法については、ここでは問わない。例えば、位置座標Ｐの測定精度εｐは、各ＧＰＳ衛星からＧＰＳ受信機までの距離の測定値と推定値との差の平均値と、ＤＯＰ値との積で求めることができる。同様に、速度ベクトルＶの測定精度εｖも、ＧＰＳ受信機の速度の測定値と推定値との差と、ＤＯＰ値との積で求めることができる。
【００３６】
また、自律航法センサ群２０は、車輪速センサ、加速度センサ、ヨーレートセンサ等の車両の変位量を計測するのに必要な複数の自律航法センサで構成される。この自律航法センサ群２０を構成する各センサの出力は、位置補正部３０に入力される。
【００３７】
この他、道路データベース４０は、所定地域（例えば、日本全域）に敷設された各道路の位置座標を表す道路データを記憶するものである。尚、道路データとしては、周知のカーナビゲーション装置が地図データベースに備える、道路位置及び道路の接続関係を表すリンクデータ及びノードデータを流用することができる。従って、位置検出装置１には、経路案内機能を付加し、位置検出装置１は、カーナビゲーション装置として構成されてもよい。
【００３８】
また、インタフェース部５０は、エンジン制御を行うエンジンＥＣＵやブレーキ制御を行うブレーキＥＣＵ、ステアリング制御を行うステアリングＥＣＵなどの車両に搭載された各種の制御系ＥＣＵと通信して、当該位置検出装置１にて検出された車両の位置座標Ｐｏを、制御系ＥＣＵに入力するためのものである。このインタフェース部５０は、シリアルインタフェース又はＬＡＮ（ＣＡＮ通信）インタフェースとして構成され、車内の制御系ＥＣＵに対し、検出された位置座標Ｐｏを入力する。
【００３９】
また、位置補正部３０は、ＧＰＳ受信機１０により計測されたＧＰＳ受信機１０の位置座標Ｐからバイアス誤差を取り除き、補正後の位置座標Ｐｏを、インタフェース部５０を通じて外部に出力するものである。具体的に、位置補正部３０は、図２に示す構成にされている。尚、図２は、位置補正部３０の詳細構成を表すブロック図である。
【００４０】
図２に示すように、位置補正部３０は、自律速度ベクトル算出部３１と、前処理部３３と、バイアス誤差推定部３５と、バイアス誤差除去部３７と、を備える。
自律速度ベクトル算出部３１は、自律航法センサ群２０を構成する各センサに接続され、各センサの出力に基づき、単位時間当たりの車両の変位量、即ち、車両の速度ベクトルＶｓを算出するものである。自律速度ベクトル算出部３１は、算出した車両の速度ベクトルＶｓを、前処理部３３に出力する。
【００４１】
一方、前処理部３３は、ＧＰＳ受信機１０及び自律速度ベクトル算出部３１に接続され、ＧＰＳ受信機１０から取得した位置座標Ｐ、速度ベクトルＶ、測定精度εｐ，εｖの情報、及び、自律速度ベクトル算出部３１から取得した速度ベクトルＶｓの情報に基づいて、位置座標Ｐを補正し、補正後の位置座標Ｐｃを、バイアス誤差推定部３５及びバイアス誤差除去部３７に出力するものである。
【００４２】
具体的に、前処理部３３は、図３に示す位置精度向上処理を実行して、位置座標Ｐを補正する。図３は、前処理部３３が繰返し実行する位置精度向上処理を表すフローチャートである。具体的に、前処理部３３は、ＧＰＳ受信機１０の計測周期Ｔ及び自律速度ベクトル算出部３１のベクトル算出周期Ｔに合わせて、周期Ｔ毎に繰返し、この位置精度向上処理を実行する。
【００４３】
位置精度向上処理を開始すると、前処理部３３は、ＧＰＳ受信機１０から現時点で計測された最新の位置座標Ｐ（ｎ）、速度ベクトルＶ（ｎ）、及び、測定精度εｐ（ｎ），εｖ（ｎ）の情報を取得し（Ｓ１１０）、更に、自律速度ベクトル算出部３１から現時点で計測された最新の速度ベクトルＶｓ（ｎ）の情報を取得する（Ｓ１２０）。
【００４４】
尚、パラメータＰ，Ｖ，εｐ，εｖ，Ｖｓに付すサフィックス（ｎ）は、パラメータＰ，Ｖ，εｐ，εｖ，Ｖｓの値が、最新サイクルで、取得された値であることを示し、サフィックス（ｎ−１）は、パラメータＰ，Ｖ，εｐ，εｖ，Ｖｓの値が、最新サイクルより１つ前のサイクルで、取得された値であることを示す。
【００４５】
Ｓ１２０での処理を終えると、前処理部３３は、ＧＰＳ受信機１０から取得した位置座標Ｐ（ｎ）及び速度ベクトルＶ（ｎ）について、位置座標Ｐ（ｎ）の前回取得値Ｐ（ｎ−１）及び速度ベクトルＶ（ｎ）の前回取得値Ｖ（ｎ−１）からの変化量δｐ，δｖを算出する（Ｓ１３０）。
【００４６】
δｐ＝｜Ｐ（ｎ）−Ｐ（ｎ−１）｜
δｖ＝｜Ｖ（ｎ）−Ｖ（ｎ−１）｜
このようにして、変化量δｐ，δｖを算出すると、前処理部３３は、Ｓ１４０に移行し、変化量δｐが、上限値Ｍｐよりも大きいか否かを判断する。尚、上限値Ｍｐは、時間Ｔが経過するまでの期間に、車両が理論上移動しえる距離の最大値に、設計段階で予め定められる。
【００４７】
そして、変化量δｐが上限値Ｍｐより大きいと判断すると（Ｓ１４０でＹｅｓ）、ＧＰＳ受信機１０から取得した位置座標Ｐ（ｎ）を、δｐ＝Ｍｐとなるように補正する（Ｓ１４５）。具体的には、位置座標Ｐ（ｎ）を、前回値Ｐ（ｎ−１）にベクトルＭｐ・（Ｐ（ｎ）−Ｐ（ｎ−１））／δｐを加算した値に、更新する。その後、Ｓ１５０に移行する。
【００４８】
Ｐ（ｎ）←Ｐ（ｎ−１）＋Ｍｐ・（Ｐ（ｎ）−Ｐ（ｎ−１））／δｐ
一方、変化量δｐが、上限値Ｍｐ以下であると判断すると（Ｓ１４０でＮｏ）、前処理部３３は、Ｓ１４５の処理を実行せずに、Ｓ１５０に移行する。そして、変化量δｖが、上限値Ｍｖよりも大きいか否かを判断する（Ｓ１５０）。尚、上限値Ｍｖは、時間Ｔが経過するまでの期間に、理論上ありうる車両速度の変化量の最大値に、設計段階で予め定められる。
【００４９】
そして、変化量δｖが上限値Ｍｖより大きいと判断すると（Ｓ１５０でＹｅｓ）、ＧＰＳ受信機１０から取得した速度ベクトルＶ（ｎ）を、δｖ＝Ｍｖとなるように補正する（Ｓ１５５）。具体的には、速度ベクトルＶ（ｎ）を、前回値Ｖ（ｎ−１）にベクトルＭｖ・（Ｖ（ｎ）−Ｖ（ｎ−１））／δｖを加算した値に、更新する。その後、Ｓ１６０に移行する。
【００５０】
Ｖ（ｎ）←Ｖ（ｎ−１）＋Ｍｖ・（Ｖ（ｎ）−Ｖ（ｎ−１））／δｖ
これに対し、変化量δｖが、上限値Ｍｖ以下であると判断すると（Ｓ１５０でＮｏ）、前処理部３３は、Ｓ１５５の処理を実行せずに、Ｓ１６０に移行する。
【００５１】
また、Ｓ１６０に移行すると、前処理部３３は、内蔵の係数設定マップに従い、係数Ａ１，Ａ２，Ａ３の値を、測定精度εｐ（ｎ），εｖ（ｎ）に対応した値に設定する。その後、位置座標Ｐｃ（ｎ）を、前回算出した位置座標Ｐｃ（ｎ−１）に基づき、次式に従って算出する（Ｓ１７０）。
【００５２】
Ｐｃ（ｎ）＝Ａ１・Ｐ（ｎ）＋Ａ２・（Ｐｃ（ｎ−１）＋Ｖ（ｎ）・Ｔ）
＋Ａ３・（Ｐｃ（ｎ−１）＋Ｖｓ（ｎ）・Ｔ）
但し、パラメータＴは、位置精度向上処理の実行周期Ｔである。また、パラメータＰｃ（ｎ）は、最新サイクルの位置座標Ｐｃを表し、パラメータＰｃ（ｎ−ｋ）は、最新サイクルよりｋ回前のサイクルで算出された位置座標Ｐｃを表す。
【００５３】
即ち、Ｓ１７０において、前処理部３３は、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）と、速度ベクトルＶ（ｎ）に基づいて求められる位置座標Ｐ（ｎ−１）＋Ｖ（ｎ）・Ｔと、自律航法センサ群２０の出力に基づいて求められる位置座標Ｐ（ｎ−１）＋Ｖｓ（ｎ）・Ｔとの重み付け平均を、係数Ａ１，Ａ２，Ａ３に従って算出する。
【００５４】
そして、位置座標Ｐｃ（ｎ）を算出し終えると、前処理部３３は、この位置座標Ｐｃ（ｎ）をバイアス誤差推定部３５及びバイアス誤差除去部３７に出力する（Ｓ１８０）。その後、位置精度向上処理を終了する。
【００５５】
尚、本実施例において、係数Ａ１，Ａ２，Ａ３の値は、Ｓ１６０で、係数設定マップから動的に設定されるが、係数Ａ１，Ａ２，Ａ３の値は、予め決められた算出式から動的に設定されてもよい。但し、具体的な係数の算出式については、測定精度εｐ，εｖの算出方法に依存するため、ここでの説明は割愛し、本実施例では、係数設定マップを用いて、係数Ａ１，Ａ２，Ａ３を設定するものとして、その具体例を、図４を用いて説明する。図４（ａ）は、係数設定マップの構成を表す説明図であり、図４（ｂ）は、係数Ａ１，Ａ２，Ａ３の大小関係を示した係数Ａ１，Ａ２，Ａ３の設定方法に関する説明図である。
【００５６】
図４（ａ）に示すように、係数設定マップは、予め定められた測定精度εｐ，εｖの領域毎に、Ａ１＋Ａ２＋Ａ３＝１，Ａ１≧０，Ａ２≧０，Ａ３≧０を満足する係数Ａ１，Ａ２，Ａ３の値が記述された構成にされている。前処理部３３は、この係数設定マップに従って、ＧＰＳ受信機１０から取得した測定精度情報εｐ（ｎ），εｖ（ｎ）が属する領域を判定し、判定した領域に関連付けられて係数設定マップに記述された係数Ａ１，Ａ２，Ａ３の値の組合せに従って、係数Ａ１，Ａ２，Ａ３の値を設定する。
【００５７】
具体的に、本実施例では、測定精度εｐ及び測定精度εｖに、境界値Ｔｈ１，Ｔｈ２を定め、測定精度εｐ及び測定精度εｖの座標空間に、４つの領域を定めている。即ち、εｐ＜Ｔｈ１且つεｖ＜Ｔｈ２の領域を、第一領域とし、εｐ≧Ｔｈ１且つεｖ＜Ｔｈ２の領域を、第二領域とし、εｐ＜Ｔｈ１且つεｖ≧Ｔｈ２の領域を、第三領域とし、εｐ≧Ｔｈ１且つεｖ≧Ｔｈ２の領域を、第四領域として定めている。尚、本実施例において、測定精度εｐ，εｖは、値が小さいほど高精度であるものとする。つまり、測定精度εｐ，εｖは、値が大きいほど、ＧＰＳ受信機１０で測定した位置，速度の誤差が大きい可能性があることを示す。
【００５８】
そして、第一領域に対しては、係数Ａ３が、係数Ａ１，Ａ２よりも小さくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。即ち、本実施例では、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）及び速度ベクトルＶ（ｎ）の測定精度が高い場合、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）、及び、速度ベクトルＶ（ｎ）に基づいて求められる位置座標Ｐｃ（ｎ−１）＋Ｖ（ｎ）・Ｔの重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００５９】
また、第二領域に対しては、係数Ａ２が、係数Ａ１，Ａ３よりも大きくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。このようにして、本実施例では、位置座標Ｐ（ｎ）の測定精度が低く速度ベクトルＶ（ｎ）の測定精度が高い場合、速度ベクトルＶ（ｎ）に基づいて求められる位置座標Ｐｃ（ｎ−１）＋Ｖ（ｎ）・Ｔの重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００６０】
また、第三領域に対しては、係数Ａ１が、係数Ａ２，Ａ３よりも大きくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。即ち、本実施例では、位置座標Ｐ（ｎ）の測定精度が高く速度ベクトルＶ（ｎ）の測定精度が低い場合、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）の重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００６１】
この他、第四領域に対しては、係数Ａ３が、係数Ａ１，Ａ２よりも大きくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。即ち、本実施例では、ＧＰＳ受信機１０から得られる位置座標Ｐ（ｎ）及び速度ベクトルＶ（ｎ）の測定精度が低い場合、自律航法センサ群２０の出力から得られる位置座標Ｐｃ（ｎ−１）＋Ｖｓ（ｎ）・Ｔの重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００６２】
このようにして、前処理部３３は、ＧＰＳ受信機１０から取得した位置座標Ｐ（ｎ）を補正し、位置精度の高い位置座標Ｐｃ（ｎ）を得る。そして、この位置座標Ｐｃ（ｎ）を、バイアス誤差推定部３５及びバイアス誤差除去部３７に出力する。
【００６３】
続いては、バイアス誤差推定部３５の動作について説明する。バイアス誤差推定部３５は、前処理部３３から出力される位置座標Ｐｃを取得し、この取得情報と、道路データベース４０が記憶する道路データとから、ＧＰＳ受信機１０が移動した道路区間を推定し、バイアス誤差を推定する。
【００６４】
図５は、バイアス誤差推定部３５が実行するバイアス誤差推定処理を表すフローチャートである。バイアス誤差推定部３５は、前処理部３３と連動して、周期Ｔで繰返し図５に示すバイアス誤差推定処理を実行し、バイアス誤差推定値Ｙを求める。
【００６５】
バイアス誤差推定処理を開始すると、バイアス誤差推定部３５は、前処理部３３から、位置座標Ｐｃ（ｎ）を取得し、これを内蔵のバッファに記録する（Ｓ２１０）。
また、この処理を終えると、バイアス誤差推定部３５は、現在からｔ＝Ｋ・Ｔ時間前までの期間に取得した複数の位置座標｛Ｐｃ（ｎ−ｋ）｜ｋ＝０，１，２，…，Ｋ｝に基づき、現在地点Ｐｃ（ｎ）からｔ＝ｋ・Ｔ時間前に測位された地点Ｐｃ（ｎ−ｋ）までの経路長Ｌ（ｋ）を、線形補間の手法を用いて、ｋ＝０，１，…，Ｋの各地点Ｐｃ（ｎ−ｋ）毎に、算出する（Ｓ２２０）。
【００６６】
【数１】
そして、経路長Ｌ（ｋ）と位置座標Ｐｃ（ｎ−ｋ）との関係に基づき、ＧＰＳ受信機１０の移動軌跡を表す軌跡データＤ１を生成する。
【００６７】
具体的に、バイアス誤差推定部３５は、線形補間の手法を用いて、地点Ｐｃ（ｎ）から地点Ｐｃ（ｎ−Ｋ）までの経路を、等しい経路長ｄＬ間隔でＭ個に分割してできる、分割地点の位置座標Ｑ（１），Ｑ（２），…，Ｑ（Ｍ−１）を求め、これらの位置座標Ｑ（１），Ｑ（２），…，Ｑ（Ｍ−１）と、両端地点の位置座標Ｑ（０）＝Ｐｃ（ｎ）、Ｑ（Ｍ）＝Ｐｃ（ｎ−Ｋ）と、を記述して、軌跡データＤ１＝｛Ｑ（０），Ｑ（１），Ｑ（２），…，Ｑ（Ｍ−１），Ｑ（Ｍ）｝を生成する。尚、位置座標Ｑ（ｍ）（但し、ｍ＝０，１，２，…，Ｍ）は、現在地点Ｑ（０）＝Ｐｃ（ｎ）から経路（ＧＰＳ受信機１０の移動軌跡）に沿って長さｍ・ｄＬ遡った地点の位置座標を表す。
【００６８】
また、このようにして軌跡データＤ１を生成し終えると、バイアス誤差推定部３５は、Ｓ２４０に移行し、位置座標Ｑ（０）と、道路データベース４０が記憶する道路データとに基づき、車両の走行道路を推定する。即ち、ＧＰＳ受信機１０が移動した道路を推定する。具体的に、バイアス誤差推定部３５は、図６に示す道路推定処理を実行することにより、車両の走行道路を推定する。図６は、バイアス誤差推定部３５が実行する道路推定処理を表すフローチャートである。
【００６９】
道路推定処理を開始すると、バイアス誤差推定部３５は、最後に推定したバイアス誤差推定値Ｙを用いて、位置座標Ｑ（０）を補正し、推定現在位置座標Ｘを算出する（Ｓ３１０）。具体的には、位置座標Ｘを、次式に従って算出する。
【００７０】
Ｘ＝Ｑ（０）−Ｙ
例えば、位置座標系として、ＥＮＵ座標系が採用されているものとし、位置座標Ｑ（０）がＥＮＵ座標系のベクトルＱ（０）＝（ＱＥ（０），ＱＮ（０），ＱＵ（０））で表され、バイアス誤差推定値ＹがＹ＝（ＹＥ，ＹＮ，ＹＵ）で表されるものとすると、位置座標Ｘ＝（ＸＥ，ＸＮ，ＸＵ）として、Ｘ＝（ＱＥ（０）−ＹＥ，ＱＮ（０）−ＹＮ，ＱＵ（０）−ＹＵ）を算出する。
【００７１】
但し、バイアス誤差推定値Ｙは、Ｓ２９０にて、バイアス誤差推定部３５により更新されるものである。従って、初回のＳ２４０実行時、バイアス誤差推定部３５は、現在のバイアス誤差推定値Ｙとして、予め定められた初期値Ｙ０を用いる。
【００７２】
また、この処理を終えると、バイアス誤差推定部３５は、道路データベース４０が有する道路データに基づき、位置座標Ｘから最も近い道路（位置座標Ｘからの距離（直線距離）が最も短い道路）を探索し、位置座標Ｘから最も近い道路上の地点の位置座標Ｒ（０）＝（ＲＥ（０），ＲＮ（０），ＲＵ（０））を求める（Ｓ３２０）。但し、位置座標Ｘが既に道路上の座標である場合には、座標Ｒ（０）＝Ｘに設定する。
【００７３】
このようにして、位置座標Ｒ（０）を求めると、バイアス誤差推定部３５は、座標Ｘから座標Ｒ（０）までの距離δ＝｜Ｘ−Ｒ（０）｜を算出し、距離δが、予め定められた上限値以下であるか否かを判断する（Ｓ３３０）。そして、距離δが上限値以下であると判断すると（Ｓ３３０でＹｅｓ）、Ｓ３４０に移行し、距離δが上限値を超えていると判断すると（Ｓ３３０でＮｏ）、Ｓ３６０に移行する。
【００７４】
また、Ｓ３４０に移行すると、バイアス誤差推定部３５は、上記探索の結果、特定された位置座標Ｘから最も近い道路が、形状的に、軌跡データＤ１が示すＧＰＳ受信機１０の移動軌跡と相関のある道路であるか否かを判断する。
【００７５】
Ｓ３４０では、具体的に、Ｓ３２０で特定された位置座標Ｘから最も近い道路上の地点であって、座標Ｒ（０）から道路に沿って、軌跡データＤ１と同一の距離ｄＬ離れた地点の位置座標Ｒ（１）を求める。
【００７６】
尚、道路は、行き止まりとなっていない限り、座標Ｒ（０）から２方向に延びているので、特定された道路が座標Ｒ（０）から２方向に延びている場合には、座標Ｒ（０）から道路に沿って各方向に距離ｄＬ離れた地点の位置座標Ｒａ（１），Ｒｂ（１）を、位置座標Ｒ（１）として、求める。ちなみに、図７（ａ）は、位置座標Ｒ（１）と位置座標Ｒ（０）との関係を表した説明図である。
【００７７】
また、この処理を終えると、バイアス誤差推定部３５は、ベクトルＺ１＝Ｑ（１）−Ｑ（０）を算出すると共に、ベクトルＺ２＝Ｒ（１）−Ｒ（０）を算出する。尚、上記特定された道路が座標Ｒ（０）から２方向に延びている場合には、ベクトルＺ２として、ベクトルＺａ＝Ｒａ（１）−Ｒ（０）及びベクトルＺｂ＝Ｒｂ（１）−Ｒ（０）を算出する。
【００７８】
また、ベクトルＺ１，Ｚ２を算出すると、バイアス誤差推定部３５は、ベクトルＺ１とベクトルＺ２とのなす角度θを算出する。
θ＝ａｒｃｃｏｓ（Ｚ１・Ｚ２／（｜Ｚ１｜・｜Ｚ２｜））
尚、図７（ｂ）は、ベクトルＺ１及びベクトルＺ２の設定方法、及び、ベクトルＺ１とベクトルＺ２とのなす角度θを示した説明図である。
【００７９】
そして、角度θが、予め定められた閾値θｔｈ以下である場合（θ≦θｔｈである場合）には、上記特定された道路の形状がＧＰＳ受信機１０の移動軌跡と類似しているとして、この道路が、ＧＰＳ受信機１０の移動軌跡と相関のある道路であると判断する（Ｓ３４０でＹｅｓ）。
【００８０】
一方、角度θが、予め定められた閾値θｔｈを超える場合（θ＞θｔｈである場合）には、上記特定された道路の形状がＧＰＳ受信機１０の移動軌跡と類似していないとして、この道路が、ＧＰＳ受信機１０の移動軌跡と相関のない道路であると判断する（Ｓ３４０でＮｏ）。
【００８１】
但し、ベクトルＺ２として、ベクトルＺａ及びベクトルＺｂを算出した場合には、ベクトルＺ１とベクトルＺａとのなす角度θａ、ベクトルＺ１とベクトルＺｂとのなす角度θｂを算出し、角度θａ，θｂのいずれか一方が、閾値θｔｈ以下である場合、Ｓ３４０でＹｅｓと判断し、角度θａ，θｂの両者が閾値θｔｈを超える場合に限って、Ｓ３４０でＮｏと判断する。また、角度θａ，θｂのいずれか一方が、閾値θｔｈ以下である場合には、座標Ｒａ（１）及び座標Ｒｂ（１）の内、角度θとして大きい角度が求められた側の位置座標を破棄し、角度θとして小さい角度が求められた側の位置座標を、位置座標Ｒ（１）に決定する。
【００８２】
このようにして、上記特定された道路が、ＧＰＳ受信機１０の移動軌跡と相関のある道路であるか否かを判断し（Ｓ３４０）、上記特定された道路が、ＧＰＳ受信機１０の移動軌跡と相関のある道路であると判断すると（Ｓ３４０でＹｅｓ）、バイアス誤差推定部３５は、Ｓ３５０に移行して、上記特定された道路を、車両の走行道路に推定する。その後、当該道路推定処理を終了する。
【００８３】
一方、上記特定された道路が、ＧＰＳ受信機１０の移動軌跡と相関のない道路であると判断すると（Ｓ３４０でＮｏ）、バイアス誤差推定部３５は、Ｓ３６０に移行し、車両の走行道路として正確な走行道路が推定できないとして、エラー判定を行う。その後、当該道路推定処理を終了する。
【００８４】
尚、本実施例のバイアス誤差推定処理では、ＧＰＳ受信機１０が車両と共に道路上を移動しているものと仮定して、車両の走行道路を推定し、車両の走行道路と、ＧＰＳ受信機１０の移動軌跡との比較により、バイアス誤差を推定する。従って、Ｓ２４０では、車両の走行道路を正しく推定する必要がある。
【００８５】
このため、上記閾値θｔｈとしては、例えば、π／４よりも十分小さい値を設定する。このように、閾値θｔｈを設定すると、交差点付近で、真に車両が走行した道路とは直交する道路を、車両の走行道路と推定しまうのを防止することができ、道路推定処理にて、車両の走行道路（ＧＰＳ受信機１０が移動した道路）を正しく推定することができる。
【００８６】
また、本実施例では、距離δが上限値を超える場合、上記特定された道路が、車両の走行道路である可能性が低いので、エラー判定を行うようにした。従って、本実施例の道路推定処理によれば、上記道路を車両の走行道路に推定してしまうことによる不都合を解消することができる。
【００８７】
このような内容の道路推定処理をＳ２４０で実行し終えると、バイアス誤差推定部３５は、Ｓ２５０に移行し、道路推定処理で車両の走行道路を推定できたか否かを判断する。具体的に、道路推定処理でエラー判定がなされた場合、バイアス誤差推定部３５は、車両の走行道路を推定することができなかったとして、Ｓ２５０でＮｏと判断する。一方、道路推定処理にてエラー判定がなされず車両の走行道路が推定された場合、Ｓ２５０でＹｅｓと判断する。
【００８８】
そして、車両の走行道路を推定できなかったと判断すると（Ｓ２５０でＮｏ）、バイアス誤差推定部３５は、Ｓ２９５に移行し、最後に更新したバイアス誤差推定値Ｙを出力する。その後、当該バイアス誤差推定処理を終了する。尚、Ｓ２９０の処理を一度も実行したことがない場合には、ここで、バイアス誤差推定値Ｙとして、予め定められた初期値Ｙ０を出力する。
【００８９】
これに対し、車両の走行道路を推定できたと判断すると（Ｓ２５０でＹｅｓ）、バイアス誤差推定部３５は、Ｓ２６０に移行して、車両が走行したと推定される道路区間の複数地点の位置座標を表す道路区間データＤ２＝｛Ｒ（０），Ｒ（１），…，Ｒ（Ｍ）｝を生成する。
【００９０】
尚、道路区間データＤ２を構成する位置座標Ｒ（０），Ｒ（１）は、先の道路推定処理（Ｓ２４０）で求められた位置座標Ｒ（０），Ｒ（１）である。また、位置座標Ｒ（ｍ）（ｍ＝０，１，２，…，Ｍ）は、地点Ｒ（０）を基点として、地点Ｒ（０）から地点Ｒ（１）方向に、道路に沿って長さｍ・ｄＬ遡った地点の位置座標を表すものである。
【００９１】
即ち、Ｓ２６０では、軌跡データＤ１と同じように、地点Ｒ（０）を基点として、距離ｄＬ毎に配置される道路上の各地点に関し、既に求めた位置座標Ｒ（０），Ｒ（１）を除く、各地点の位置座標Ｒ（２），…，Ｒ（Ｍ）を求め、これら各地点の位置座標Ｒ（０），Ｒ（１），…，Ｒ（Ｍ）を記して、道路区間データを生成する。
【００９２】
また、この処理を終えると、バイアス誤差推定部３５は、Ｓ２７０に移行し、軌跡データＤ１と道路区間データＤ２とに基づいて、図８に示すように、軌跡データＤ１が示す各地点の位置座標Ｑ（ｍ）と、位置座標Ｑ（ｍ）に対応する道路上の位置座標Ｒ（ｍ）との誤差Δ（ｍ）＝Ｑ（ｍ）−Ｒ（ｍ）を算出し、更に、その重み付け平均εを算出する。
【００９３】
【数２】
尚、図８は、重み付け平均εの算出方法に関する説明図である。この他、上式に示すパラメータα＝｛α（０），α（１），…，α（Ｍ）｝は、次の関係を満足する重み付け係数である。
【００９４】
【数３】
即ち、本実施例では、各地点の誤差Δ（ｍ）＝Ｑ（ｍ）−Ｒ（ｍ）（ｍ＝０，１，…，Ｍ）の内、ＧＰＳ受信機１０が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、重み付け平均εを算出する。尚、誤差Δ（ｍ）は、図８に示すように、各方向Ｅ，Ｎ，Ｕでの、位置座標Ｑ（ｍ）＝（ＱＥ（ｍ），ＱＮ（ｍ），ＱＵ（ｍ））と、位置座標Ｑ（ｍ）に対応する道路上の位置座標Ｒ（ｍ）＝（ＲＥ（ｍ），ＲＮ（ｍ），ＲＵ（ｍ））との誤差を表すものであり（Δ（ｍ）＝（ＱＥ（ｍ）−ＲＥ（ｍ），ＱＮ（ｍ）−ＲＮ（ｍ），ＱＵ（ｍ）−ＲＵ（ｍ）））、重み付け平均εは、各方向成分の誤差の重み付け平均を、ベクトル表現してなるものである。
【００９５】
このようにして、重み付け平均εを算出すると、バイアス誤差推定部３５は、Ｓ２８０に移行して、算出した重み付け平均εが、予め定められた上限値以下であるか否かを判断し、重み付け平均εが、上限値以下であると判断すると（Ｓ２８０でＹｅｓ）、Ｓ２９０に移行して、バイアス誤差推定値Ｙを、Ｓ２７０で算出した重み付け平均εに更新する（Ｙ←ε）。そして、更新後のバイアス誤差推定値Ｙを、バイアス誤差除去部３７に出力する（Ｓ２９５）。その後、当該バイアス誤差推定処理を終了する。
【００９６】
一方、重み付け平均εが上限値を超えていると判断すると（Ｓ２８０でＮｏ）、バイアス誤差推定部３５は、Ｓ２９０の処理を実行することなく、Ｓ２９５に移行して、バイアス誤差推定値Ｙをバイアス誤差除去部３７に出力し、その後、バイアス誤差推定処理を終了する。このようにして、バイアス誤差推定部３５は、重み付け平均εが大きく、推定した車両の走行道路に誤りがある可能性が高い場合には、バイアス誤差推定値Ｙを更新せずに、バイアス誤差推定値Ｙとして、最後に更新した値を出力する。
【００９７】
この他、本実施例では、前処理部３３及びバイアス誤差推定部３５と連動して、バイアス誤差除去部３７が動作し、バイアス誤差除去部３７は、バイアス誤差推定部３５から出力されたバイアス誤差推定値Ｙに基づき、前処理部３３から入力された測位結果Ｐｃ（ｎ）からバイアス誤差を除去して、補正後の位置座標Ｐｏ＝Ｐｃ（ｎ）−Ｙを、インタフェース部５０を通じて外部に出力する。
【００９８】
以上、本実施例の位置検出装置１について説明したが、この位置検出装置１では、計測した複数地点の位置座標Ｑ（０），Ｑ（１），…，Ｑ（Ｍ）と、道路の複数地点の位置座標Ｒ（０），Ｒ（１），…，Ｒ（Ｍ）とから、バイアス誤差推定値Ｙを算出することで、ＧＰＳ受信機１０の移動軌跡と道路形状とを複数地点に渡って比較して、バイアス誤差を推定するようにした。
【００９９】
具体的に、本実施例では、位置座標Ｒ（０）から等距離間隔ｄＬに地点を設定して、設定した地点の位置座標Ｒ（１），…，Ｒ（Ｍ）を求めることで、軌跡データＤ１が示す各地点の位置関係に従い、軌跡データＤ１が示す各地点の位置座標Ｑ（ｍ）に対応する道路上の位置座標Ｒ（ｍ）を求め、各地点の誤差Δ（ｍ）の平均εを、バイアス誤差推定値Ｙとして算出するようにした。
【０１００】
従って、この位置検出装置１によれば、精度良くバイアス誤差を推定することができ、精度よく、ＧＰＳ受信機１０の測位結果からバイアス誤差成分を除去して位置を補正することができる。
【０１０１】
従来技術では、ＦＭ放送局から受信したバイアス誤差情報に基づき、測位結果を補正するので、ＦＭ放送局と測位地点とが遠く離れている場合など、受信情報が示すバイアス誤差が、測位地点でのバイアス誤差の真値から大きくずれている場合、測位結果からバイアス誤差を適切に除去して、測位結果を精度よく補正できないといった問題があったが、本実施例によれば、上述した方法で、測位地点でのバイアス誤差を精度よく推定することができるので、従来よりも、ＧＰＳ受信機１０の測位結果を適切に補正することができる。また、この位置検出装置１によれば、ＦＭ受信機を装置内に搭載する必要がないので、製品を安価に製造することができる。
【０１０２】
また、従来のマップマッチング技術では、測位結果と道路位置とを比較して、測位結果を、道路上の位置座標に強制的に補正するので、図９上段に示すように、道に対する相対的な車両の軌跡、つまり道に対する車両の運動を正確に表現することができなかったが、本実施例によれば、上述した方法で、適切に位置座標を補正することができるので、このような問題を解消することができ、図９下段に示すように、車両の運動を正確に表現することができる。
【０１０３】
この他、本実施例では、重み付け平均εを求める際、ＧＰＳ受信機１０が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、重み付け平均εを算出するようにした。従って、本実施例の位置検出装置１によれば、電離層の変化にも適切に対応して、正確にバイアス誤差を推定することができる。
【０１０４】
また、本実施例では、車両の走行道路を推定する際、ＧＰＳ受信機１０により計測された前処理後の位置座標Ｑ（０）＝Ｐｃ（ｎ）を、前回推定したバイアス誤差に基づいて補正し、補正後の値Ｘ＝Ｑ（０）−Ｙを用いて、この地点Ｘから最も近い道路を車両の走行道路と推定するようにした。この他、上記最も近い道路の形状が、計測された位置座標の軌跡と相関があるか否かを、ベクトルＺ１及びベクトルＺ２のなす角度θに基づいて判断し、相関がない場合には、車両の走行道路の推定を止めるようにした。従って、本実施例の位置検出装置１によれば、簡単な手順で正確に、車両の走行道路（ＧＰＳ受信機１０が移動した道路）を推定することができる。
【０１０５】
また、本実施例では、位置座標Ｘから最も近い道路までの距離が、離れている場合、対応する軌跡データに基づいたバイアス誤差の推定動作を、実行しないようにした（Ｓ２５０でＮｏ）。そして、このとき計測された位置座標Ｐｃ（ｎ）を、それまでに算出されたバイアス誤差推定値Ｙを用いて、補正して、補正後の位置座標Ｐｏを出力するようにした。従って、本実施例によれば、車両の走行道路として誤った道路を推定してしまうことにより、誤った位置座標Ｐｏが算出・出力されてしまうのを防止することができる。
【０１０６】
この他、本実施例では、車両に搭載された自律航法センサに出力に基づき、車両の変位量Ｖｓを算出して、当該変位量Ｖｓに基づき、ＧＰＳ受信機１０により計測された位置座標Ｐを補正するようにした。そして、補正後の位置座標Ｐｃに基づき、軌跡データを生成するようにした。従って、本実施例によれば、ランダム誤差の影響を抑え、適切にバイアス誤差を推定することができる。
【０１０７】
尚、本発明の道路データ記憶手段は、本実施例の道路データベース４０に相当する。また、本発明の位置座標取得手段は、前処理部３３が実行するＳ１１０の処理にて実現され、軌跡データ生成手段は、バイアス誤差推定部３５が実行するＳ２１０〜Ｓ２３０の処理にて実現されている。
【０１０８】
この他、道路推定手段は、バイアス誤差推定部３５が実行するＳ２４０，Ｓ２６０の処理にて実現され、バイアス誤差推定手段は、バイアス誤差推定部３５が実行するＳ２７０，Ｓ２９０の処理にて実現されている。また、補正出力手段は、バイアス誤差除去部３７が位置座標Ｐｏ＝Ｐｃ（ｎ）−Ｙを算出し、出力する動作にて実現され、前処理手段は、自律速度ベクトル算出部３１及び前処理部３３が実行するＳ１２０〜Ｓ１８０の処理により実現されている。
【０１０９】
また、本発明は、上記実施例に限定されるものではなく、種々の態様を採ることができる。
例えば、上記実施例では、誤差Δ（ｍ）の重み付け平均εを、バイアス誤差推定値Ｙとして算出するようにしたが、バイアス誤差推定値Ｙについては、誤差Δ（ｍ）の単純平均で、算出されてもよい。即ち、全てのｍについてα（ｍ）＝１／（Ｍ＋１）に設定して、バイアス誤差推定値Ｙを求めるようにしてもよい。
【０１１０】
その他、本実施例では、ＧＰＳ受信機１０にて計測された位置座標Ｐ（ｎ）を、位置座標Ｐｃ（ｎ）に補正して、位置精度を向上させた補正後の位置座標Ｐｃ（ｎ）を、バイアス誤差推定部３５及びバイアス誤差除去部３７に入力するようにしたが、位置座標Ｐｃ（ｎ）に代えて、ＧＰＳ受信機１０にて計測された位置座標Ｐ（ｎ）を、そのまま、バイアス誤差推定部３５及びバイアス誤差除去部３７に入力するように、位置検出装置１を構成してもよい。
【０１１１】
また、上記実施例では、ＧＰＳ受信機１０から前処理部３３を介して得られる位置座標Ｐｃに基づき、線形補間の手法により、車両の走行軌跡を等距離間隔で表した軌跡データを生成し、軌跡データが示す各地点の位置座標Ｑ（ｍ）に対応する道路上の地点の位置座標として、等距離間隔に道路の位置座標Ｒ（ｍ）を表した道路区間データを生成し、これらのデータに基づいて、バイアス誤差を求めるようにしたが、線形補間せずに、車両の走行軌跡を、前処理部３３から得られる位置座標Ｐｃ（ｎ），…，Ｐｃ（ｎ−Ｋ）で表して軌跡データを生成し、軌跡データが示す各地点の位置座標Ｐｃに基づき、各地点の位置座標Ｐｃに対応する道路上の位置座標を、地点間の距離が、軌跡データが示す地点間の距離と等しくなるように求めて、道路区間データを生成し、これらのデータに基づいて、バイアス誤差を求めるようにしてもよい。このようにすれば、演算処理負荷を軽減することができ、データ量を削減することが可能である。
【図面の簡単な説明】
【０１１２】
【図１】位置検出装置１の構成を表すブロック図である。
【図２】位置補正部３０の構成を表すブロック図である。
【図３】前処理部３３が実行する位置精度向上処理を表すフローチャートである。
【図４】係数設定マップの構成を表す説明図（ａ）、及び、係数Ａ１，Ａ２，Ａ３の大小関係を示した係数Ａ１，Ａ２，Ａ３の設定方法に関する説明図（ｂ）である。
【図５】バイアス誤差推定部３５が実行するバイアス誤差推定処理を表すフローチャートである。
【図６】バイアス誤差推定部３５が実行する道路推定処理を表すフローチャートである。
【図７】座標Ｒ（１），Ｒ（０）の位置関係を表す説明図（ａ）及びベクトルＺ１，Ｚ２のなす角度θを示した説明図（ｂ）である。
【図８】重み付け平均εの算出方法に関する説明図である。
【図９】測位結果の従来型補正方法及び本発明の補正方法を記した説明図である。
【符号の説明】
【０１１３】
１…位置検出装置、１０…ＧＰＳ受信機、１１…アンテナ、２０…自律航法センサ群、３０…位置補正部、３１…自律速度ベクトル算出部、３３…前処理部、３５…バイアス誤差推定部、３７…バイアス誤差除去部、４０…道路データベース、５０…インタフェース部
【技術分野】
【０００１】
本発明は、位置補正装置に関する。
【背景技術】
【０００２】
従来、ＧＰＳ（Global Positioning System）測位では、測位結果に、伝播遅延によるバイアス誤差、マルチパスによる測位誤差、その他、種々の要因によるランダム誤差が、生じることが知られている。
【０００３】
バイアス誤差は、ＧＰＳ衛星と地上との間に介在する電離層をＧＰＳ信号が通過する際に、伝播遅延が生じることを主な原因として発生するものである。このバイアス誤差は、電離層を主な原因として生じるものであるため、常時発生するが、誤差の程度は、電離層の状態によって時々刻々と変化する。
【０００４】
このため、従来装置（カーナビゲーション装置等）では、バイアス誤差の情報を、ＦＭ放送局（所謂ＤＧＰＳ局）から、ＦＭ放送波の形態で受信し、受信したバイアス誤差情報に従って、ＧＰＳ受信機により計測された位置座標を、補正することが行われている（例えば、特許文献１参照）。
【特許文献１】特開平１０−１８５６００号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、ＦＭ放送局からバイアス誤差情報を受信して、ＧＰＳ受信機により計測された位置座標を補正する方法では、装置にＦＭ受信機を搭載する必要があるため、製品コストが高くなるといった問題があった。
【０００６】
また、この方法では、測位地点がＦＭ放送局から離れた地点にある場合、周辺環境の相違から、測位地点でのバイアス誤差がＦＭ放送局でのバイアス誤差と異なるため、ＦＭ放送局からバイアス誤差情報を受信しても、受信先装置（ナビゲーション装置）では、バイアス誤差を、ＧＰＳ受信機の測位結果から精度よく取り除くことができず、精度よく現在地点の位置座標を検出することができないといった問題があった。また、従来方法では、ＦＭ放送局から受信した確度の低いバイアス誤差情報に基づいて、ＧＰＳ受信機の測位結果を補正することで、むしろ位置精度が悪化する可能性があった。
【０００７】
本発明は、こうした問題に鑑みなされたものであり、ＧＰＳ受信機の測位結果からバイアス誤差を従来よりも適切に取り除くことが可能な位置補正装置を提供することを目的とする。
【課題を解決するための手段】
【０００８】
かかる目的を達成するためになされた本発明（請求項１記載）の位置補正装置は、バイアス誤差推定手段を備え、次のようにして、ＧＰＳ受信機が計測する位置座標のバイアス誤差を推定し、推定したバイアス誤差に基づき、ＧＰＳ受信機か計測した位置座標を補正するものである。
【０００９】
本発明の位置補正装置は、位置情報取得手段によって、ＧＰＳ受信機が計測した位置座標を取得し、位置情報取得手段が取得した位置座標を基に、過去所定期間におけるＧＰＳ受信機の位置座標を複数地点表した軌跡データを、軌跡データ生成手段によって生成する。
【００１０】
また、位置補正装置は、敷設された道路の位置座標を表す道路データを記憶する道路データ記憶手段を備え、この道路データと、位置情報取得手段が取得した位置座標と、に基づき、道路推定手段にて、過去所定期間にＧＰＳ受信機が移動した道路区間を推定する。
【００１１】
そして、バイアス誤差推定手段は、上記道路推定手段により推定された道路区間における複数地点の位置座標と、上記軌跡データが示すＧＰＳ受信機の複数地点の位置座標と、に基づき、バイアス誤差を推定する。
【００１２】
本発明の位置補正装置は、このようにして推定したバイアス誤差に基づき、ＧＰＳ受信機により計測された位置座標を補正し、補正後の位置座標を出力する（補正出力手段）。
この位置補正装置は、道路を移動する移動体（車両等）に搭載されたＧＰＳ受信機の測位結果を補正するために用いられるものであり、移動体が道路に沿って移動しているという仮定の下で、バイアス誤差を推定するものである。そして、本発明の特徴は、計測された複数地点の位置座標と、道路の複数地点の位置座標と、に基づいて、バイアス誤差を推定する点にある。
【００１３】
即ち、本発明では、計測された複数地点の位置座標と、道路の複数地点の位置座標と、に基づいて、バイアス誤差を推定することにより、ＧＰＳ受信機の移動軌跡と道路形状とを複数地点に渡って比較できるようにし、正確にバイアス誤差を推定することができるようにした。
【００１４】
従来技術では、ＦＭ放送局から受信したバイアス誤差情報に基づき、測位結果を補正しているので、受信情報が示すバイアス誤差が、測位地点でのバイアス誤差の真値から大きくずれている場合、測位結果からバイアス誤差を適切に除去して、測位結果を精度よく補正できないといった問題があったが、本発明によれば、上述した方法で、測位地点でのバイアス誤差を精度よく推定することができるので、従来よりも、計測された位置座標から適切にバイアス誤差を除去でき、測位結果を正確に補正することができる。また、この位置補正装置によれば、ＦＭ受信機を装置内に搭載する必要がないので、製品を安価に製造することができる。
【００１５】
尚、道路データに基づき測位結果を補正する技術としては、マップマッチング技術が知られているが、周知のマップマッチング技術は、測位結果と道路位置とを比較して、測位結果を、道路上の位置座標に強制的に補正するものである。従って、この技術では、必ず道路を示す線上に移動体の位置座標が乗ってしまい、道に対する相対的な移動体の軌跡、つまり道に対する移動体の運動を正確に表現することができない。
【００１６】
これに対し、本発明によれば、上述の方法により、バイアス誤差を適切に求めることができ、求めたバイアス誤差を使って測位結果を適切に補正することができるので、移動体の運動を正確に表現したまま位置座標を補正することができる。従って、この技術を、車両制御に応用すれば、道路に対する車両運動を検出することができ、危険な運転の判定や、それに応じてドライバ操作をアシストする制御を正確に実行することができる。
【００１７】
例えば、従来技術では、ＧＰＳ受信機の測位結果を補正して、カーブを通過する車両の正確な移動軌跡を求めようとしても、測位結果と道路位置とを比較して、測位結果を、道路上の位置座標に補正するため、図９上段に示すように、適切に補正を行うことができず、補正により道路に対する相対的な運動情報が失われて、車両の運動を正確に表現することができないといった問題があった。このため、ＧＰＳ受信機による測位結果と道路形状から危険な運転を判定することや、それに応じてドライバ操作をアシストする制御を実行しようとしても、当該制御を、適切に実行することができないといった問題があった。
【００１８】
これに対し、本発明の位置補正装置を用いれば、図９下段に示すように、正確にバイアス誤差を推定して測位結果を補正することができるので、補正により位置座標の連続的な変化に関する情報が失われることがなく、正確な車両運動を表現することができ、ドライバ操作をアシストする制御を正確に実行することができる。
【００１９】
ところで、道路推定手段は、軌跡データが示す各地点間の位置関係に従って、軌跡データが示す各地点の位置座標に対応する道路区間上の位置座標を、推定する構成にすることができる。また、バイアス誤差推定手段は、軌跡データが示す各地点の位置座標毎に、軌跡データが示す位置座標と、当該位置座標に対応する上記推定された道路区間上の位置座標との誤差を算出し、算出した各地点の誤差の平均を、バイアス誤差として推定する構成にすることができる（請求項２）。
【００２０】
バイアス誤差は、ランダム誤差よりも時間的に緩やかに変化するものであるので、短い期間において、ＧＰＳ受信機の位置座標は、バイアス誤差の影響により、真の位置座標よりも一定方向に一定距離ずれて、計測される。従って、軌跡データが示す各地点の位置関係とパターンが一致するようにして道路区間上の地点を設定し、各地点での軌跡データが示す位置座標と、道路上の位置座標との誤差の平均を、バイアス誤差として推定するようにすれば、正確にバイアス誤差を推定することができる。
【００２１】
また、バイアス誤差推定手段は、各地点の誤差の単純平均を、バイアス誤差の推定値として算出する構成にされてもよいが、各地点の誤差の重み付け平均を、バイアス誤差の推定値として算出する構成にされてもよい。
【００２２】
具体的に、バイアス誤差の推定値として、重み付け平均を求める場合には、ＧＰＳ受信機が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、重み付け平均を算出するように、バイアス誤差推定手段を構成するとよい（請求項３）。このように、バイアス誤差を推定すれば、電離層の変化（バイアス誤差の時間変化）にも適切に対応して、正確にバイアス誤差を推定することができる。
【００２３】
また、道路推定手段は、バイアス誤差推定手段が過去に推定したバイアス誤差を加味して、過去所定期間にＧＰＳ受信機が移動した道路区間を推定する構成にされると好ましい（請求項４）。ＧＰＳ受信機から得られる位置座標には、当然のことながら、バイアス誤差が含まれる。従って、バイアス誤差推定手段により推定されたバイアス誤差で、ＧＰＳ受信機から得られた位置座標を補正して、ＧＰＳ受信機が移動した道路を推定するようにすれば、一層正確に、ＧＰＳ受信機が移動した道路区間を推定することができ、結果として、バイアス誤差を、精度よく検出することができる。
【００２４】
尚、道路推定手段は、バイアス誤差推定手段が過去に推定したバイアス誤差に基づいて、軌跡データが示す位置座標を補正し、当該補正後の位置座標に基づき、軌跡データが示す地点の分布と相関が高い道路を探索し、探索により得られた道路上で、過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にすることができる（請求項５）。
【００２５】
この他、道路推定手段は、軌跡データが示す各地点の位置座標の内、特定地点の位置座標を、バイアス誤差推定手段が過去に推定したバイアス誤差に基づき補正し、補正後の位置座標から最も近い道路を探索して、探索により得られた最も近い道路を、ＧＰＳ受信機が移動した道路に推定し、この道路上で、過去所定期間にＧＰＳ受信機が移動した道路区間を推定する構成にすることができる（請求項６）。
【００２６】
このような手法で、道路区間を推定すれば、簡単な手順で正確に、ＧＰＳ受信機が移動した道路区間を推定することができる。
また、道路区間の推定方法としては、軌跡データが示す各地点の位置座標の内、特定地点の位置座標を、バイアス誤差推定手段が過去に推定したバイアス誤差に基づき補正し、補正後の位置座標から最も近い道路上の地点を基点にして、基点からの道路に沿う距離（道路長）が、軌跡データが示す特定地点からの移動軌跡に沿う距離（経路長）に一致するように、上記軌跡データが示す各地点の位置座標に対応する道路上の位置座標を推定することで、軌跡データが示す位置座標に対応する道路上の位置座標を、各地点間の位置関係に従って推定し、推定した道路上の位置座標の両端を結ぶ区間を、道路区間に推定する方法を挙げることができる。
【００２７】
また、位置補正装置は、道路推定手段により上記探索された道路と上記補正後の特定地点の位置座標との距離が、所定の上限値を超えている場合に、バイアス誤差推定手段での軌跡データに基づいたバイアス誤差の推定動作を停止可能な構成にされるとよい。そして、補正出力手段は、バイアス誤差推定手段によりバイアス誤差が推定されない期間、その期間前にバイアス誤差推定手段により推定されたバイアス誤差に基づき、ＧＰＳ受信機により計測された位置座標を補正し、当該補正後の位置座標を出力する構成にされるとよい（請求項７）。
【００２８】
このように位置補正装置を構成すれば、ＧＰＳ受信機が移動した道路として、誤った道路が推定され、結果として、誤ったバイアス誤差が推定されてしまうのを防止することができる。
【００２９】
また、車両に搭載されたＧＰＳ受信機を対象に位置補正装置を構成する場合には、当該位置補正装置に、車両に搭載された自律航法センサの出力に基づき、車両の変位量を算出して、当該変位量に基づき、位置情報取得手段が取得した位置座標を補正する前処理手段を設けて、軌跡データ生成手段は、前処理手段による補正後の位置座標に基づき、過去所定期間におけるＧＰＳ受信機の位置座標を複数地点表した軌跡データを生成する構成にされるとよい（請求項８）。
【００３０】
このように位置補正装置を構成すれば、より一層高精度にバイアス誤差を求めることができ、ＧＰＳ受信機により計測された位置座標を適切に補正することができる。
【発明を実施するための最良の形態】
【００３１】
以下に本発明の実施例について、図面と共に説明する。図１は、車両に搭載された位置検出装置１の構成を表す説明図である。
図１に示すように、本実施例の位置検出装置１は、ＧＰＳ受信機１０と、自律航法センサ群２０と、位置補正部３０と、道路データベース４０と、インタフェース部５０と、を備える。
【００３２】
ＧＰＳ受信機１０は、周知のＧＰＳ受信機と同様の構成にされており、ＧＰＳ衛星から送信されてくるＧＰＳ信号を、アンテナ１１を通じて受信して、ＧＰＳ信号からＧＰＳメッセージを抽出し、複数（４以上）のＧＰＳ衛星に関して得られた擬似距離及び衛星軌道情報に基づいて、幾何学的な演算により自装置の位置座標Ｐを算出する。位置座標Ｐは、例えば、ＥＮＵ座標系やＥＣＥＦ座標系などの３次元座標系で算出される。
【００３３】
また、本実施例のＧＰＳ受信機１０は、ＧＰＳ信号のドップラーシフト量に基づいて、自装置の速度ベクトルＶを算出する機能を有する。ＧＰＳ信号のドップラーシフト量に基づいた速度ベクトルＶの算出方法は、周知であるので、ここでは、速度ベクトルＶの算出方法について、簡単に述べるが、速度ベクトルＶの算出に当たっては、複数（４以上）のＧＰＳ衛星について、受信したメッセージからＧＰＳ衛星の速度ベクトルを算出し、更に、ＧＰＳ受信機からＧＰＳ衛星への単位方向ベクトルを算出する。そして、これらのベクトル情報と、各ＧＰＳ衛星から受信したＧＰＳ信号のキャリア周波数と、各ＧＰＳ衛星から送出されるＧＰＳ信号のキャリア周波数と、に基づいて、自装置の速度ベクトルＶを算出する。
【００３４】
ＧＰＳ受信機１０は、このようにして算出した速度ベクトルＶを、自装置の位置座標Ｐと共に、位置補正部３０に向けて出力する。また、ＧＰＳ受信機１０は、位置座標Ｐ及び速度ベクトルＶと同時に、ＤＯＰ値に基づいて求めた位置座標Ｐの測定精度εｐ及び速度ベクトルＶの測定精度εｖを表す測定精度情報を出力する。
【００３５】
尚、本実施例では、測定精度εｐ，εｖについての高精度な情報は、必要ないので、測定精度の算出方法については、ここでは問わない。例えば、位置座標Ｐの測定精度εｐは、各ＧＰＳ衛星からＧＰＳ受信機までの距離の測定値と推定値との差の平均値と、ＤＯＰ値との積で求めることができる。同様に、速度ベクトルＶの測定精度εｖも、ＧＰＳ受信機の速度の測定値と推定値との差と、ＤＯＰ値との積で求めることができる。
【００３６】
また、自律航法センサ群２０は、車輪速センサ、加速度センサ、ヨーレートセンサ等の車両の変位量を計測するのに必要な複数の自律航法センサで構成される。この自律航法センサ群２０を構成する各センサの出力は、位置補正部３０に入力される。
【００３７】
この他、道路データベース４０は、所定地域（例えば、日本全域）に敷設された各道路の位置座標を表す道路データを記憶するものである。尚、道路データとしては、周知のカーナビゲーション装置が地図データベースに備える、道路位置及び道路の接続関係を表すリンクデータ及びノードデータを流用することができる。従って、位置検出装置１には、経路案内機能を付加し、位置検出装置１は、カーナビゲーション装置として構成されてもよい。
【００３８】
また、インタフェース部５０は、エンジン制御を行うエンジンＥＣＵやブレーキ制御を行うブレーキＥＣＵ、ステアリング制御を行うステアリングＥＣＵなどの車両に搭載された各種の制御系ＥＣＵと通信して、当該位置検出装置１にて検出された車両の位置座標Ｐｏを、制御系ＥＣＵに入力するためのものである。このインタフェース部５０は、シリアルインタフェース又はＬＡＮ（ＣＡＮ通信）インタフェースとして構成され、車内の制御系ＥＣＵに対し、検出された位置座標Ｐｏを入力する。
【００３９】
また、位置補正部３０は、ＧＰＳ受信機１０により計測されたＧＰＳ受信機１０の位置座標Ｐからバイアス誤差を取り除き、補正後の位置座標Ｐｏを、インタフェース部５０を通じて外部に出力するものである。具体的に、位置補正部３０は、図２に示す構成にされている。尚、図２は、位置補正部３０の詳細構成を表すブロック図である。
【００４０】
図２に示すように、位置補正部３０は、自律速度ベクトル算出部３１と、前処理部３３と、バイアス誤差推定部３５と、バイアス誤差除去部３７と、を備える。
自律速度ベクトル算出部３１は、自律航法センサ群２０を構成する各センサに接続され、各センサの出力に基づき、単位時間当たりの車両の変位量、即ち、車両の速度ベクトルＶｓを算出するものである。自律速度ベクトル算出部３１は、算出した車両の速度ベクトルＶｓを、前処理部３３に出力する。
【００４１】
一方、前処理部３３は、ＧＰＳ受信機１０及び自律速度ベクトル算出部３１に接続され、ＧＰＳ受信機１０から取得した位置座標Ｐ、速度ベクトルＶ、測定精度εｐ，εｖの情報、及び、自律速度ベクトル算出部３１から取得した速度ベクトルＶｓの情報に基づいて、位置座標Ｐを補正し、補正後の位置座標Ｐｃを、バイアス誤差推定部３５及びバイアス誤差除去部３７に出力するものである。
【００４２】
具体的に、前処理部３３は、図３に示す位置精度向上処理を実行して、位置座標Ｐを補正する。図３は、前処理部３３が繰返し実行する位置精度向上処理を表すフローチャートである。具体的に、前処理部３３は、ＧＰＳ受信機１０の計測周期Ｔ及び自律速度ベクトル算出部３１のベクトル算出周期Ｔに合わせて、周期Ｔ毎に繰返し、この位置精度向上処理を実行する。
【００４３】
位置精度向上処理を開始すると、前処理部３３は、ＧＰＳ受信機１０から現時点で計測された最新の位置座標Ｐ（ｎ）、速度ベクトルＶ（ｎ）、及び、測定精度εｐ（ｎ），εｖ（ｎ）の情報を取得し（Ｓ１１０）、更に、自律速度ベクトル算出部３１から現時点で計測された最新の速度ベクトルＶｓ（ｎ）の情報を取得する（Ｓ１２０）。
【００４４】
尚、パラメータＰ，Ｖ，εｐ，εｖ，Ｖｓに付すサフィックス（ｎ）は、パラメータＰ，Ｖ，εｐ，εｖ，Ｖｓの値が、最新サイクルで、取得された値であることを示し、サフィックス（ｎ−１）は、パラメータＰ，Ｖ，εｐ，εｖ，Ｖｓの値が、最新サイクルより１つ前のサイクルで、取得された値であることを示す。
【００４５】
Ｓ１２０での処理を終えると、前処理部３３は、ＧＰＳ受信機１０から取得した位置座標Ｐ（ｎ）及び速度ベクトルＶ（ｎ）について、位置座標Ｐ（ｎ）の前回取得値Ｐ（ｎ−１）及び速度ベクトルＶ（ｎ）の前回取得値Ｖ（ｎ−１）からの変化量δｐ，δｖを算出する（Ｓ１３０）。
【００４６】
δｐ＝｜Ｐ（ｎ）−Ｐ（ｎ−１）｜
δｖ＝｜Ｖ（ｎ）−Ｖ（ｎ−１）｜
このようにして、変化量δｐ，δｖを算出すると、前処理部３３は、Ｓ１４０に移行し、変化量δｐが、上限値Ｍｐよりも大きいか否かを判断する。尚、上限値Ｍｐは、時間Ｔが経過するまでの期間に、車両が理論上移動しえる距離の最大値に、設計段階で予め定められる。
【００４７】
そして、変化量δｐが上限値Ｍｐより大きいと判断すると（Ｓ１４０でＹｅｓ）、ＧＰＳ受信機１０から取得した位置座標Ｐ（ｎ）を、δｐ＝Ｍｐとなるように補正する（Ｓ１４５）。具体的には、位置座標Ｐ（ｎ）を、前回値Ｐ（ｎ−１）にベクトルＭｐ・（Ｐ（ｎ）−Ｐ（ｎ−１））／δｐを加算した値に、更新する。その後、Ｓ１５０に移行する。
【００４８】
Ｐ（ｎ）←Ｐ（ｎ−１）＋Ｍｐ・（Ｐ（ｎ）−Ｐ（ｎ−１））／δｐ
一方、変化量δｐが、上限値Ｍｐ以下であると判断すると（Ｓ１４０でＮｏ）、前処理部３３は、Ｓ１４５の処理を実行せずに、Ｓ１５０に移行する。そして、変化量δｖが、上限値Ｍｖよりも大きいか否かを判断する（Ｓ１５０）。尚、上限値Ｍｖは、時間Ｔが経過するまでの期間に、理論上ありうる車両速度の変化量の最大値に、設計段階で予め定められる。
【００４９】
そして、変化量δｖが上限値Ｍｖより大きいと判断すると（Ｓ１５０でＹｅｓ）、ＧＰＳ受信機１０から取得した速度ベクトルＶ（ｎ）を、δｖ＝Ｍｖとなるように補正する（Ｓ１５５）。具体的には、速度ベクトルＶ（ｎ）を、前回値Ｖ（ｎ−１）にベクトルＭｖ・（Ｖ（ｎ）−Ｖ（ｎ−１））／δｖを加算した値に、更新する。その後、Ｓ１６０に移行する。
【００５０】
Ｖ（ｎ）←Ｖ（ｎ−１）＋Ｍｖ・（Ｖ（ｎ）−Ｖ（ｎ−１））／δｖ
これに対し、変化量δｖが、上限値Ｍｖ以下であると判断すると（Ｓ１５０でＮｏ）、前処理部３３は、Ｓ１５５の処理を実行せずに、Ｓ１６０に移行する。
【００５１】
また、Ｓ１６０に移行すると、前処理部３３は、内蔵の係数設定マップに従い、係数Ａ１，Ａ２，Ａ３の値を、測定精度εｐ（ｎ），εｖ（ｎ）に対応した値に設定する。その後、位置座標Ｐｃ（ｎ）を、前回算出した位置座標Ｐｃ（ｎ−１）に基づき、次式に従って算出する（Ｓ１７０）。
【００５２】
Ｐｃ（ｎ）＝Ａ１・Ｐ（ｎ）＋Ａ２・（Ｐｃ（ｎ−１）＋Ｖ（ｎ）・Ｔ）
＋Ａ３・（Ｐｃ（ｎ−１）＋Ｖｓ（ｎ）・Ｔ）
但し、パラメータＴは、位置精度向上処理の実行周期Ｔである。また、パラメータＰｃ（ｎ）は、最新サイクルの位置座標Ｐｃを表し、パラメータＰｃ（ｎ−ｋ）は、最新サイクルよりｋ回前のサイクルで算出された位置座標Ｐｃを表す。
【００５３】
即ち、Ｓ１７０において、前処理部３３は、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）と、速度ベクトルＶ（ｎ）に基づいて求められる位置座標Ｐ（ｎ−１）＋Ｖ（ｎ）・Ｔと、自律航法センサ群２０の出力に基づいて求められる位置座標Ｐ（ｎ−１）＋Ｖｓ（ｎ）・Ｔとの重み付け平均を、係数Ａ１，Ａ２，Ａ３に従って算出する。
【００５４】
そして、位置座標Ｐｃ（ｎ）を算出し終えると、前処理部３３は、この位置座標Ｐｃ（ｎ）をバイアス誤差推定部３５及びバイアス誤差除去部３７に出力する（Ｓ１８０）。その後、位置精度向上処理を終了する。
【００５５】
尚、本実施例において、係数Ａ１，Ａ２，Ａ３の値は、Ｓ１６０で、係数設定マップから動的に設定されるが、係数Ａ１，Ａ２，Ａ３の値は、予め決められた算出式から動的に設定されてもよい。但し、具体的な係数の算出式については、測定精度εｐ，εｖの算出方法に依存するため、ここでの説明は割愛し、本実施例では、係数設定マップを用いて、係数Ａ１，Ａ２，Ａ３を設定するものとして、その具体例を、図４を用いて説明する。図４（ａ）は、係数設定マップの構成を表す説明図であり、図４（ｂ）は、係数Ａ１，Ａ２，Ａ３の大小関係を示した係数Ａ１，Ａ２，Ａ３の設定方法に関する説明図である。
【００５６】
図４（ａ）に示すように、係数設定マップは、予め定められた測定精度εｐ，εｖの領域毎に、Ａ１＋Ａ２＋Ａ３＝１，Ａ１≧０，Ａ２≧０，Ａ３≧０を満足する係数Ａ１，Ａ２，Ａ３の値が記述された構成にされている。前処理部３３は、この係数設定マップに従って、ＧＰＳ受信機１０から取得した測定精度情報εｐ（ｎ），εｖ（ｎ）が属する領域を判定し、判定した領域に関連付けられて係数設定マップに記述された係数Ａ１，Ａ２，Ａ３の値の組合せに従って、係数Ａ１，Ａ２，Ａ３の値を設定する。
【００５７】
具体的に、本実施例では、測定精度εｐ及び測定精度εｖに、境界値Ｔｈ１，Ｔｈ２を定め、測定精度εｐ及び測定精度εｖの座標空間に、４つの領域を定めている。即ち、εｐ＜Ｔｈ１且つεｖ＜Ｔｈ２の領域を、第一領域とし、εｐ≧Ｔｈ１且つεｖ＜Ｔｈ２の領域を、第二領域とし、εｐ＜Ｔｈ１且つεｖ≧Ｔｈ２の領域を、第三領域とし、εｐ≧Ｔｈ１且つεｖ≧Ｔｈ２の領域を、第四領域として定めている。尚、本実施例において、測定精度εｐ，εｖは、値が小さいほど高精度であるものとする。つまり、測定精度εｐ，εｖは、値が大きいほど、ＧＰＳ受信機１０で測定した位置，速度の誤差が大きい可能性があることを示す。
【００５８】
そして、第一領域に対しては、係数Ａ３が、係数Ａ１，Ａ２よりも小さくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。即ち、本実施例では、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）及び速度ベクトルＶ（ｎ）の測定精度が高い場合、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）、及び、速度ベクトルＶ（ｎ）に基づいて求められる位置座標Ｐｃ（ｎ−１）＋Ｖ（ｎ）・Ｔの重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００５９】
また、第二領域に対しては、係数Ａ２が、係数Ａ１，Ａ３よりも大きくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。このようにして、本実施例では、位置座標Ｐ（ｎ）の測定精度が低く速度ベクトルＶ（ｎ）の測定精度が高い場合、速度ベクトルＶ（ｎ）に基づいて求められる位置座標Ｐｃ（ｎ−１）＋Ｖ（ｎ）・Ｔの重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００６０】
また、第三領域に対しては、係数Ａ１が、係数Ａ２，Ａ３よりも大きくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。即ち、本実施例では、位置座標Ｐ（ｎ）の測定精度が高く速度ベクトルＶ（ｎ）の測定精度が低い場合、ＧＰＳ受信機１０から得られた位置座標Ｐ（ｎ）の重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００６１】
この他、第四領域に対しては、係数Ａ３が、係数Ａ１，Ａ２よりも大きくなるように、係数Ａ１，Ａ２，Ａ３の値を定めている。即ち、本実施例では、ＧＰＳ受信機１０から得られる位置座標Ｐ（ｎ）及び速度ベクトルＶ（ｎ）の測定精度が低い場合、自律航法センサ群２０の出力から得られる位置座標Ｐｃ（ｎ−１）＋Ｖｓ（ｎ）・Ｔの重み付けが大きくなるように、係数Ａ１，Ａ２，Ａ３の値を設定して、位置座標Ｐ（ｎ）を補正し、位置座標Ｐｃ（ｎ）を求める。
【００６２】
このようにして、前処理部３３は、ＧＰＳ受信機１０から取得した位置座標Ｐ（ｎ）を補正し、位置精度の高い位置座標Ｐｃ（ｎ）を得る。そして、この位置座標Ｐｃ（ｎ）を、バイアス誤差推定部３５及びバイアス誤差除去部３７に出力する。
【００６３】
続いては、バイアス誤差推定部３５の動作について説明する。バイアス誤差推定部３５は、前処理部３３から出力される位置座標Ｐｃを取得し、この取得情報と、道路データベース４０が記憶する道路データとから、ＧＰＳ受信機１０が移動した道路区間を推定し、バイアス誤差を推定する。
【００６４】
図５は、バイアス誤差推定部３５が実行するバイアス誤差推定処理を表すフローチャートである。バイアス誤差推定部３５は、前処理部３３と連動して、周期Ｔで繰返し図５に示すバイアス誤差推定処理を実行し、バイアス誤差推定値Ｙを求める。
【００６５】
バイアス誤差推定処理を開始すると、バイアス誤差推定部３５は、前処理部３３から、位置座標Ｐｃ（ｎ）を取得し、これを内蔵のバッファに記録する（Ｓ２１０）。
また、この処理を終えると、バイアス誤差推定部３５は、現在からｔ＝Ｋ・Ｔ時間前までの期間に取得した複数の位置座標｛Ｐｃ（ｎ−ｋ）｜ｋ＝０，１，２，…，Ｋ｝に基づき、現在地点Ｐｃ（ｎ）からｔ＝ｋ・Ｔ時間前に測位された地点Ｐｃ（ｎ−ｋ）までの経路長Ｌ（ｋ）を、線形補間の手法を用いて、ｋ＝０，１，…，Ｋの各地点Ｐｃ（ｎ−ｋ）毎に、算出する（Ｓ２２０）。
【００６６】
【数１】
そして、経路長Ｌ（ｋ）と位置座標Ｐｃ（ｎ−ｋ）との関係に基づき、ＧＰＳ受信機１０の移動軌跡を表す軌跡データＤ１を生成する。
【００６７】
具体的に、バイアス誤差推定部３５は、線形補間の手法を用いて、地点Ｐｃ（ｎ）から地点Ｐｃ（ｎ−Ｋ）までの経路を、等しい経路長ｄＬ間隔でＭ個に分割してできる、分割地点の位置座標Ｑ（１），Ｑ（２），…，Ｑ（Ｍ−１）を求め、これらの位置座標Ｑ（１），Ｑ（２），…，Ｑ（Ｍ−１）と、両端地点の位置座標Ｑ（０）＝Ｐｃ（ｎ）、Ｑ（Ｍ）＝Ｐｃ（ｎ−Ｋ）と、を記述して、軌跡データＤ１＝｛Ｑ（０），Ｑ（１），Ｑ（２），…，Ｑ（Ｍ−１），Ｑ（Ｍ）｝を生成する。尚、位置座標Ｑ（ｍ）（但し、ｍ＝０，１，２，…，Ｍ）は、現在地点Ｑ（０）＝Ｐｃ（ｎ）から経路（ＧＰＳ受信機１０の移動軌跡）に沿って長さｍ・ｄＬ遡った地点の位置座標を表す。
【００６８】
また、このようにして軌跡データＤ１を生成し終えると、バイアス誤差推定部３５は、Ｓ２４０に移行し、位置座標Ｑ（０）と、道路データベース４０が記憶する道路データとに基づき、車両の走行道路を推定する。即ち、ＧＰＳ受信機１０が移動した道路を推定する。具体的に、バイアス誤差推定部３５は、図６に示す道路推定処理を実行することにより、車両の走行道路を推定する。図６は、バイアス誤差推定部３５が実行する道路推定処理を表すフローチャートである。
【００６９】
道路推定処理を開始すると、バイアス誤差推定部３５は、最後に推定したバイアス誤差推定値Ｙを用いて、位置座標Ｑ（０）を補正し、推定現在位置座標Ｘを算出する（Ｓ３１０）。具体的には、位置座標Ｘを、次式に従って算出する。
【００７０】
Ｘ＝Ｑ（０）−Ｙ
例えば、位置座標系として、ＥＮＵ座標系が採用されているものとし、位置座標Ｑ（０）がＥＮＵ座標系のベクトルＱ（０）＝（ＱＥ（０），ＱＮ（０），ＱＵ（０））で表され、バイアス誤差推定値ＹがＹ＝（ＹＥ，ＹＮ，ＹＵ）で表されるものとすると、位置座標Ｘ＝（ＸＥ，ＸＮ，ＸＵ）として、Ｘ＝（ＱＥ（０）−ＹＥ，ＱＮ（０）−ＹＮ，ＱＵ（０）−ＹＵ）を算出する。
【００７１】
但し、バイアス誤差推定値Ｙは、Ｓ２９０にて、バイアス誤差推定部３５により更新されるものである。従って、初回のＳ２４０実行時、バイアス誤差推定部３５は、現在のバイアス誤差推定値Ｙとして、予め定められた初期値Ｙ０を用いる。
【００７２】
また、この処理を終えると、バイアス誤差推定部３５は、道路データベース４０が有する道路データに基づき、位置座標Ｘから最も近い道路（位置座標Ｘからの距離（直線距離）が最も短い道路）を探索し、位置座標Ｘから最も近い道路上の地点の位置座標Ｒ（０）＝（ＲＥ（０），ＲＮ（０），ＲＵ（０））を求める（Ｓ３２０）。但し、位置座標Ｘが既に道路上の座標である場合には、座標Ｒ（０）＝Ｘに設定する。
【００７３】
このようにして、位置座標Ｒ（０）を求めると、バイアス誤差推定部３５は、座標Ｘから座標Ｒ（０）までの距離δ＝｜Ｘ−Ｒ（０）｜を算出し、距離δが、予め定められた上限値以下であるか否かを判断する（Ｓ３３０）。そして、距離δが上限値以下であると判断すると（Ｓ３３０でＹｅｓ）、Ｓ３４０に移行し、距離δが上限値を超えていると判断すると（Ｓ３３０でＮｏ）、Ｓ３６０に移行する。
【００７４】
また、Ｓ３４０に移行すると、バイアス誤差推定部３５は、上記探索の結果、特定された位置座標Ｘから最も近い道路が、形状的に、軌跡データＤ１が示すＧＰＳ受信機１０の移動軌跡と相関のある道路であるか否かを判断する。
【００７５】
Ｓ３４０では、具体的に、Ｓ３２０で特定された位置座標Ｘから最も近い道路上の地点であって、座標Ｒ（０）から道路に沿って、軌跡データＤ１と同一の距離ｄＬ離れた地点の位置座標Ｒ（１）を求める。
【００７６】
尚、道路は、行き止まりとなっていない限り、座標Ｒ（０）から２方向に延びているので、特定された道路が座標Ｒ（０）から２方向に延びている場合には、座標Ｒ（０）から道路に沿って各方向に距離ｄＬ離れた地点の位置座標Ｒａ（１），Ｒｂ（１）を、位置座標Ｒ（１）として、求める。ちなみに、図７（ａ）は、位置座標Ｒ（１）と位置座標Ｒ（０）との関係を表した説明図である。
【００７７】
また、この処理を終えると、バイアス誤差推定部３５は、ベクトルＺ１＝Ｑ（１）−Ｑ（０）を算出すると共に、ベクトルＺ２＝Ｒ（１）−Ｒ（０）を算出する。尚、上記特定された道路が座標Ｒ（０）から２方向に延びている場合には、ベクトルＺ２として、ベクトルＺａ＝Ｒａ（１）−Ｒ（０）及びベクトルＺｂ＝Ｒｂ（１）−Ｒ（０）を算出する。
【００７８】
また、ベクトルＺ１，Ｚ２を算出すると、バイアス誤差推定部３５は、ベクトルＺ１とベクトルＺ２とのなす角度θを算出する。
θ＝ａｒｃｃｏｓ（Ｚ１・Ｚ２／（｜Ｚ１｜・｜Ｚ２｜））
尚、図７（ｂ）は、ベクトルＺ１及びベクトルＺ２の設定方法、及び、ベクトルＺ１とベクトルＺ２とのなす角度θを示した説明図である。
【００７９】
そして、角度θが、予め定められた閾値θｔｈ以下である場合（θ≦θｔｈである場合）には、上記特定された道路の形状がＧＰＳ受信機１０の移動軌跡と類似しているとして、この道路が、ＧＰＳ受信機１０の移動軌跡と相関のある道路であると判断する（Ｓ３４０でＹｅｓ）。
【００８０】
一方、角度θが、予め定められた閾値θｔｈを超える場合（θ＞θｔｈである場合）には、上記特定された道路の形状がＧＰＳ受信機１０の移動軌跡と類似していないとして、この道路が、ＧＰＳ受信機１０の移動軌跡と相関のない道路であると判断する（Ｓ３４０でＮｏ）。
【００８１】
但し、ベクトルＺ２として、ベクトルＺａ及びベクトルＺｂを算出した場合には、ベクトルＺ１とベクトルＺａとのなす角度θａ、ベクトルＺ１とベクトルＺｂとのなす角度θｂを算出し、角度θａ，θｂのいずれか一方が、閾値θｔｈ以下である場合、Ｓ３４０でＹｅｓと判断し、角度θａ，θｂの両者が閾値θｔｈを超える場合に限って、Ｓ３４０でＮｏと判断する。また、角度θａ，θｂのいずれか一方が、閾値θｔｈ以下である場合には、座標Ｒａ（１）及び座標Ｒｂ（１）の内、角度θとして大きい角度が求められた側の位置座標を破棄し、角度θとして小さい角度が求められた側の位置座標を、位置座標Ｒ（１）に決定する。
【００８２】
このようにして、上記特定された道路が、ＧＰＳ受信機１０の移動軌跡と相関のある道路であるか否かを判断し（Ｓ３４０）、上記特定された道路が、ＧＰＳ受信機１０の移動軌跡と相関のある道路であると判断すると（Ｓ３４０でＹｅｓ）、バイアス誤差推定部３５は、Ｓ３５０に移行して、上記特定された道路を、車両の走行道路に推定する。その後、当該道路推定処理を終了する。
【００８３】
一方、上記特定された道路が、ＧＰＳ受信機１０の移動軌跡と相関のない道路であると判断すると（Ｓ３４０でＮｏ）、バイアス誤差推定部３５は、Ｓ３６０に移行し、車両の走行道路として正確な走行道路が推定できないとして、エラー判定を行う。その後、当該道路推定処理を終了する。
【００８４】
尚、本実施例のバイアス誤差推定処理では、ＧＰＳ受信機１０が車両と共に道路上を移動しているものと仮定して、車両の走行道路を推定し、車両の走行道路と、ＧＰＳ受信機１０の移動軌跡との比較により、バイアス誤差を推定する。従って、Ｓ２４０では、車両の走行道路を正しく推定する必要がある。
【００８５】
このため、上記閾値θｔｈとしては、例えば、π／４よりも十分小さい値を設定する。このように、閾値θｔｈを設定すると、交差点付近で、真に車両が走行した道路とは直交する道路を、車両の走行道路と推定しまうのを防止することができ、道路推定処理にて、車両の走行道路（ＧＰＳ受信機１０が移動した道路）を正しく推定することができる。
【００８６】
また、本実施例では、距離δが上限値を超える場合、上記特定された道路が、車両の走行道路である可能性が低いので、エラー判定を行うようにした。従って、本実施例の道路推定処理によれば、上記道路を車両の走行道路に推定してしまうことによる不都合を解消することができる。
【００８７】
このような内容の道路推定処理をＳ２４０で実行し終えると、バイアス誤差推定部３５は、Ｓ２５０に移行し、道路推定処理で車両の走行道路を推定できたか否かを判断する。具体的に、道路推定処理でエラー判定がなされた場合、バイアス誤差推定部３５は、車両の走行道路を推定することができなかったとして、Ｓ２５０でＮｏと判断する。一方、道路推定処理にてエラー判定がなされず車両の走行道路が推定された場合、Ｓ２５０でＹｅｓと判断する。
【００８８】
そして、車両の走行道路を推定できなかったと判断すると（Ｓ２５０でＮｏ）、バイアス誤差推定部３５は、Ｓ２９５に移行し、最後に更新したバイアス誤差推定値Ｙを出力する。その後、当該バイアス誤差推定処理を終了する。尚、Ｓ２９０の処理を一度も実行したことがない場合には、ここで、バイアス誤差推定値Ｙとして、予め定められた初期値Ｙ０を出力する。
【００８９】
これに対し、車両の走行道路を推定できたと判断すると（Ｓ２５０でＹｅｓ）、バイアス誤差推定部３５は、Ｓ２６０に移行して、車両が走行したと推定される道路区間の複数地点の位置座標を表す道路区間データＤ２＝｛Ｒ（０），Ｒ（１），…，Ｒ（Ｍ）｝を生成する。
【００９０】
尚、道路区間データＤ２を構成する位置座標Ｒ（０），Ｒ（１）は、先の道路推定処理（Ｓ２４０）で求められた位置座標Ｒ（０），Ｒ（１）である。また、位置座標Ｒ（ｍ）（ｍ＝０，１，２，…，Ｍ）は、地点Ｒ（０）を基点として、地点Ｒ（０）から地点Ｒ（１）方向に、道路に沿って長さｍ・ｄＬ遡った地点の位置座標を表すものである。
【００９１】
即ち、Ｓ２６０では、軌跡データＤ１と同じように、地点Ｒ（０）を基点として、距離ｄＬ毎に配置される道路上の各地点に関し、既に求めた位置座標Ｒ（０），Ｒ（１）を除く、各地点の位置座標Ｒ（２），…，Ｒ（Ｍ）を求め、これら各地点の位置座標Ｒ（０），Ｒ（１），…，Ｒ（Ｍ）を記して、道路区間データを生成する。
【００９２】
また、この処理を終えると、バイアス誤差推定部３５は、Ｓ２７０に移行し、軌跡データＤ１と道路区間データＤ２とに基づいて、図８に示すように、軌跡データＤ１が示す各地点の位置座標Ｑ（ｍ）と、位置座標Ｑ（ｍ）に対応する道路上の位置座標Ｒ（ｍ）との誤差Δ（ｍ）＝Ｑ（ｍ）−Ｒ（ｍ）を算出し、更に、その重み付け平均εを算出する。
【００９３】
【数２】
尚、図８は、重み付け平均εの算出方法に関する説明図である。この他、上式に示すパラメータα＝｛α（０），α（１），…，α（Ｍ）｝は、次の関係を満足する重み付け係数である。
【００９４】
【数３】
即ち、本実施例では、各地点の誤差Δ（ｍ）＝Ｑ（ｍ）−Ｒ（ｍ）（ｍ＝０，１，…，Ｍ）の内、ＧＰＳ受信機１０が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、重み付け平均εを算出する。尚、誤差Δ（ｍ）は、図８に示すように、各方向Ｅ，Ｎ，Ｕでの、位置座標Ｑ（ｍ）＝（ＱＥ（ｍ），ＱＮ（ｍ），ＱＵ（ｍ））と、位置座標Ｑ（ｍ）に対応する道路上の位置座標Ｒ（ｍ）＝（ＲＥ（ｍ），ＲＮ（ｍ），ＲＵ（ｍ））との誤差を表すものであり（Δ（ｍ）＝（ＱＥ（ｍ）−ＲＥ（ｍ），ＱＮ（ｍ）−ＲＮ（ｍ），ＱＵ（ｍ）−ＲＵ（ｍ）））、重み付け平均εは、各方向成分の誤差の重み付け平均を、ベクトル表現してなるものである。
【００９５】
このようにして、重み付け平均εを算出すると、バイアス誤差推定部３５は、Ｓ２８０に移行して、算出した重み付け平均εが、予め定められた上限値以下であるか否かを判断し、重み付け平均εが、上限値以下であると判断すると（Ｓ２８０でＹｅｓ）、Ｓ２９０に移行して、バイアス誤差推定値Ｙを、Ｓ２７０で算出した重み付け平均εに更新する（Ｙ←ε）。そして、更新後のバイアス誤差推定値Ｙを、バイアス誤差除去部３７に出力する（Ｓ２９５）。その後、当該バイアス誤差推定処理を終了する。
【００９６】
一方、重み付け平均εが上限値を超えていると判断すると（Ｓ２８０でＮｏ）、バイアス誤差推定部３５は、Ｓ２９０の処理を実行することなく、Ｓ２９５に移行して、バイアス誤差推定値Ｙをバイアス誤差除去部３７に出力し、その後、バイアス誤差推定処理を終了する。このようにして、バイアス誤差推定部３５は、重み付け平均εが大きく、推定した車両の走行道路に誤りがある可能性が高い場合には、バイアス誤差推定値Ｙを更新せずに、バイアス誤差推定値Ｙとして、最後に更新した値を出力する。
【００９７】
この他、本実施例では、前処理部３３及びバイアス誤差推定部３５と連動して、バイアス誤差除去部３７が動作し、バイアス誤差除去部３７は、バイアス誤差推定部３５から出力されたバイアス誤差推定値Ｙに基づき、前処理部３３から入力された測位結果Ｐｃ（ｎ）からバイアス誤差を除去して、補正後の位置座標Ｐｏ＝Ｐｃ（ｎ）−Ｙを、インタフェース部５０を通じて外部に出力する。
【００９８】
以上、本実施例の位置検出装置１について説明したが、この位置検出装置１では、計測した複数地点の位置座標Ｑ（０），Ｑ（１），…，Ｑ（Ｍ）と、道路の複数地点の位置座標Ｒ（０），Ｒ（１），…，Ｒ（Ｍ）とから、バイアス誤差推定値Ｙを算出することで、ＧＰＳ受信機１０の移動軌跡と道路形状とを複数地点に渡って比較して、バイアス誤差を推定するようにした。
【００９９】
具体的に、本実施例では、位置座標Ｒ（０）から等距離間隔ｄＬに地点を設定して、設定した地点の位置座標Ｒ（１），…，Ｒ（Ｍ）を求めることで、軌跡データＤ１が示す各地点の位置関係に従い、軌跡データＤ１が示す各地点の位置座標Ｑ（ｍ）に対応する道路上の位置座標Ｒ（ｍ）を求め、各地点の誤差Δ（ｍ）の平均εを、バイアス誤差推定値Ｙとして算出するようにした。
【０１００】
従って、この位置検出装置１によれば、精度良くバイアス誤差を推定することができ、精度よく、ＧＰＳ受信機１０の測位結果からバイアス誤差成分を除去して位置を補正することができる。
【０１０１】
従来技術では、ＦＭ放送局から受信したバイアス誤差情報に基づき、測位結果を補正するので、ＦＭ放送局と測位地点とが遠く離れている場合など、受信情報が示すバイアス誤差が、測位地点でのバイアス誤差の真値から大きくずれている場合、測位結果からバイアス誤差を適切に除去して、測位結果を精度よく補正できないといった問題があったが、本実施例によれば、上述した方法で、測位地点でのバイアス誤差を精度よく推定することができるので、従来よりも、ＧＰＳ受信機１０の測位結果を適切に補正することができる。また、この位置検出装置１によれば、ＦＭ受信機を装置内に搭載する必要がないので、製品を安価に製造することができる。
【０１０２】
また、従来のマップマッチング技術では、測位結果と道路位置とを比較して、測位結果を、道路上の位置座標に強制的に補正するので、図９上段に示すように、道に対する相対的な車両の軌跡、つまり道に対する車両の運動を正確に表現することができなかったが、本実施例によれば、上述した方法で、適切に位置座標を補正することができるので、このような問題を解消することができ、図９下段に示すように、車両の運動を正確に表現することができる。
【０１０３】
この他、本実施例では、重み付け平均εを求める際、ＧＰＳ受信機１０が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、重み付け平均εを算出するようにした。従って、本実施例の位置検出装置１によれば、電離層の変化にも適切に対応して、正確にバイアス誤差を推定することができる。
【０１０４】
また、本実施例では、車両の走行道路を推定する際、ＧＰＳ受信機１０により計測された前処理後の位置座標Ｑ（０）＝Ｐｃ（ｎ）を、前回推定したバイアス誤差に基づいて補正し、補正後の値Ｘ＝Ｑ（０）−Ｙを用いて、この地点Ｘから最も近い道路を車両の走行道路と推定するようにした。この他、上記最も近い道路の形状が、計測された位置座標の軌跡と相関があるか否かを、ベクトルＺ１及びベクトルＺ２のなす角度θに基づいて判断し、相関がない場合には、車両の走行道路の推定を止めるようにした。従って、本実施例の位置検出装置１によれば、簡単な手順で正確に、車両の走行道路（ＧＰＳ受信機１０が移動した道路）を推定することができる。
【０１０５】
また、本実施例では、位置座標Ｘから最も近い道路までの距離が、離れている場合、対応する軌跡データに基づいたバイアス誤差の推定動作を、実行しないようにした（Ｓ２５０でＮｏ）。そして、このとき計測された位置座標Ｐｃ（ｎ）を、それまでに算出されたバイアス誤差推定値Ｙを用いて、補正して、補正後の位置座標Ｐｏを出力するようにした。従って、本実施例によれば、車両の走行道路として誤った道路を推定してしまうことにより、誤った位置座標Ｐｏが算出・出力されてしまうのを防止することができる。
【０１０６】
この他、本実施例では、車両に搭載された自律航法センサに出力に基づき、車両の変位量Ｖｓを算出して、当該変位量Ｖｓに基づき、ＧＰＳ受信機１０により計測された位置座標Ｐを補正するようにした。そして、補正後の位置座標Ｐｃに基づき、軌跡データを生成するようにした。従って、本実施例によれば、ランダム誤差の影響を抑え、適切にバイアス誤差を推定することができる。
【０１０７】
尚、本発明の道路データ記憶手段は、本実施例の道路データベース４０に相当する。また、本発明の位置座標取得手段は、前処理部３３が実行するＳ１１０の処理にて実現され、軌跡データ生成手段は、バイアス誤差推定部３５が実行するＳ２１０〜Ｓ２３０の処理にて実現されている。
【０１０８】
この他、道路推定手段は、バイアス誤差推定部３５が実行するＳ２４０，Ｓ２６０の処理にて実現され、バイアス誤差推定手段は、バイアス誤差推定部３５が実行するＳ２７０，Ｓ２９０の処理にて実現されている。また、補正出力手段は、バイアス誤差除去部３７が位置座標Ｐｏ＝Ｐｃ（ｎ）−Ｙを算出し、出力する動作にて実現され、前処理手段は、自律速度ベクトル算出部３１及び前処理部３３が実行するＳ１２０〜Ｓ１８０の処理により実現されている。
【０１０９】
また、本発明は、上記実施例に限定されるものではなく、種々の態様を採ることができる。
例えば、上記実施例では、誤差Δ（ｍ）の重み付け平均εを、バイアス誤差推定値Ｙとして算出するようにしたが、バイアス誤差推定値Ｙについては、誤差Δ（ｍ）の単純平均で、算出されてもよい。即ち、全てのｍについてα（ｍ）＝１／（Ｍ＋１）に設定して、バイアス誤差推定値Ｙを求めるようにしてもよい。
【０１１０】
その他、本実施例では、ＧＰＳ受信機１０にて計測された位置座標Ｐ（ｎ）を、位置座標Ｐｃ（ｎ）に補正して、位置精度を向上させた補正後の位置座標Ｐｃ（ｎ）を、バイアス誤差推定部３５及びバイアス誤差除去部３７に入力するようにしたが、位置座標Ｐｃ（ｎ）に代えて、ＧＰＳ受信機１０にて計測された位置座標Ｐ（ｎ）を、そのまま、バイアス誤差推定部３５及びバイアス誤差除去部３７に入力するように、位置検出装置１を構成してもよい。
【０１１１】
また、上記実施例では、ＧＰＳ受信機１０から前処理部３３を介して得られる位置座標Ｐｃに基づき、線形補間の手法により、車両の走行軌跡を等距離間隔で表した軌跡データを生成し、軌跡データが示す各地点の位置座標Ｑ（ｍ）に対応する道路上の地点の位置座標として、等距離間隔に道路の位置座標Ｒ（ｍ）を表した道路区間データを生成し、これらのデータに基づいて、バイアス誤差を求めるようにしたが、線形補間せずに、車両の走行軌跡を、前処理部３３から得られる位置座標Ｐｃ（ｎ），…，Ｐｃ（ｎ−Ｋ）で表して軌跡データを生成し、軌跡データが示す各地点の位置座標Ｐｃに基づき、各地点の位置座標Ｐｃに対応する道路上の位置座標を、地点間の距離が、軌跡データが示す地点間の距離と等しくなるように求めて、道路区間データを生成し、これらのデータに基づいて、バイアス誤差を求めるようにしてもよい。このようにすれば、演算処理負荷を軽減することができ、データ量を削減することが可能である。
【図面の簡単な説明】
【０１１２】
【図１】位置検出装置１の構成を表すブロック図である。
【図２】位置補正部３０の構成を表すブロック図である。
【図３】前処理部３３が実行する位置精度向上処理を表すフローチャートである。
【図４】係数設定マップの構成を表す説明図（ａ）、及び、係数Ａ１，Ａ２，Ａ３の大小関係を示した係数Ａ１，Ａ２，Ａ３の設定方法に関する説明図（ｂ）である。
【図５】バイアス誤差推定部３５が実行するバイアス誤差推定処理を表すフローチャートである。
【図６】バイアス誤差推定部３５が実行する道路推定処理を表すフローチャートである。
【図７】座標Ｒ（１），Ｒ（０）の位置関係を表す説明図（ａ）及びベクトルＺ１，Ｚ２のなす角度θを示した説明図（ｂ）である。
【図８】重み付け平均εの算出方法に関する説明図である。
【図９】測位結果の従来型補正方法及び本発明の補正方法を記した説明図である。
【符号の説明】
【０１１３】
１…位置検出装置、１０…ＧＰＳ受信機、１１…アンテナ、２０…自律航法センサ群、３０…位置補正部、３１…自律速度ベクトル算出部、３３…前処理部、３５…バイアス誤差推定部、３７…バイアス誤差除去部、４０…道路データベース、５０…インタフェース部
【特許請求の範囲】
【請求項１】
ＧＰＳ受信機により計測された位置座標を取得する位置情報取得手段と、
敷設された道路の位置座標を表す道路データを記憶する道路データ記憶手段と、
前記位置情報取得手段が取得した位置座標を基に、過去所定期間における前記ＧＰＳ受信機の位置座標を複数地点表した軌跡データを生成する軌跡データ生成手段と、
前記道路データ記憶手段が記憶する道路データと、前記位置情報取得手段が取得した位置座標と、に基づき、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する道路推定手段と、
前記道路推定手段により推定された道路区間における複数地点の位置座標と、前記軌跡データが示す前記ＧＰＳ受信機の複数地点の位置座標と、に基づき、前記ＧＰＳ受信機が計測する位置座標のバイアス誤差を推定するバイアス誤差推定手段と、
前記バイアス誤差推定手段により推定されたバイアス誤差に基づき、前記ＧＰＳ受信機により計測された位置座標を補正し、当該補正後の位置座標を出力する補正出力手段と、
を備えることを特徴とする位置補正装置。
【請求項２】
前記道路推定手段は、前記軌跡データが示す各地点間の位置関係を加味して、前記軌跡データが示す各地点の位置座標毎に、この位置座標に対応する前記道路区間上の位置座標を、推定する構成にされ、
前記バイアス誤差推定手段は、前記軌跡データが示す各地点の位置座標毎に、前記軌跡データが示す位置座標と、当該位置座標に対応する前記推定された道路区間上の位置座標と、の誤差を算出し、算出した前記各地点の誤差の平均を、前記バイアス誤差として推定する構成にされていること
を特徴とする請求項１記載の位置補正装置。
【請求項３】
前記バイアス誤差推定手段は、前記各地点の誤差の内、前記ＧＰＳ受信機が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、前記各地点の誤差の重み付け平均を算出し、当該重み付け平均を、前記バイアス誤差として推定可能な構成にされていること
を特徴とする請求項２記載の位置補正装置。
【請求項４】
前記道路推定手段は、前記バイアス誤差推定手段が過去に推定したバイアス誤差を加味して、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にされていることを特徴とする請求項１〜請求項３のいずれかに記載の位置補正装置。
【請求項５】
前記道路推定手段は、前記バイアス誤差推定手段が過去に推定したバイアス誤差に基づいて、前記軌跡データが示す位置座標を補正し、当該補正後の位置座標に基づき、前記軌跡データが示す地点の分布と相関が高い道路を探索し、前記探索により得られた道路上で、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にされていること
を特徴とする請求項１〜請求項４のいずれかに記載の位置補正装置。
【請求項６】
前記道路推定手段は、前記軌跡データが示す各地点の位置座標の内、特定地点の位置座標を、前記バイアス誤差推定手段が過去に推定したバイアス誤差に基づき補正し、補正後の位置座標に基づき、当該補正後の位置座標から最も近い道路を探索して、前記探索により得られた前記最も近い道路上で、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にされていること
を特徴とする請求項１〜請求項５のいずれかに記載の位置補正装置。
【請求項７】
前記バイアス誤差推定手段は、前記道路推定手段により探索された前記最も近い道路と補正後の前記特定地点の位置座標との距離が、所定の上限値を超える場合に、前記軌跡データに基づいたバイアス誤差の推定動作を停止可能な構成にされ、
前記補正出力手段は、前記バイアス誤差推定手段によりバイアス誤差が推定されない期間、その期間前に前記バイアス誤差推定手段により推定されたバイアス誤差に基づき、前記ＧＰＳ受信機により計測された位置座標を補正し、当該補正後の位置座標を出力する構成にされていること
を特徴とする請求項６記載の位置補正装置。
【請求項８】
前記ＧＰＳ受信機は、車両に搭載され、
前記位置補正装置は、
前記車両に搭載された自律航法センサの出力に基づき、前記車両の変位量を算出して、当該変位量に基づき、前記位置情報取得手段が取得した位置座標を補正する前処理手段
を備え、
前記軌跡データ生成手段は、前記前処理手段による補正後の位置座標に基づき、前記過去所定期間における前記ＧＰＳ受信機の位置座標を複数地点表した前記軌跡データを生成する構成にされていること
を特徴とする請求項１〜請求項７のいずれかに記載の位置補正装置。
【請求項１】
ＧＰＳ受信機により計測された位置座標を取得する位置情報取得手段と、
敷設された道路の位置座標を表す道路データを記憶する道路データ記憶手段と、
前記位置情報取得手段が取得した位置座標を基に、過去所定期間における前記ＧＰＳ受信機の位置座標を複数地点表した軌跡データを生成する軌跡データ生成手段と、
前記道路データ記憶手段が記憶する道路データと、前記位置情報取得手段が取得した位置座標と、に基づき、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する道路推定手段と、
前記道路推定手段により推定された道路区間における複数地点の位置座標と、前記軌跡データが示す前記ＧＰＳ受信機の複数地点の位置座標と、に基づき、前記ＧＰＳ受信機が計測する位置座標のバイアス誤差を推定するバイアス誤差推定手段と、
前記バイアス誤差推定手段により推定されたバイアス誤差に基づき、前記ＧＰＳ受信機により計測された位置座標を補正し、当該補正後の位置座標を出力する補正出力手段と、
を備えることを特徴とする位置補正装置。
【請求項２】
前記道路推定手段は、前記軌跡データが示す各地点間の位置関係を加味して、前記軌跡データが示す各地点の位置座標毎に、この位置座標に対応する前記道路区間上の位置座標を、推定する構成にされ、
前記バイアス誤差推定手段は、前記軌跡データが示す各地点の位置座標毎に、前記軌跡データが示す位置座標と、当該位置座標に対応する前記推定された道路区間上の位置座標と、の誤差を算出し、算出した前記各地点の誤差の平均を、前記バイアス誤差として推定する構成にされていること
を特徴とする請求項１記載の位置補正装置。
【請求項３】
前記バイアス誤差推定手段は、前記各地点の誤差の内、前記ＧＰＳ受信機が測位した時期が新しい地域の誤差ほど、重み付けを大きくして、前記各地点の誤差の重み付け平均を算出し、当該重み付け平均を、前記バイアス誤差として推定可能な構成にされていること
を特徴とする請求項２記載の位置補正装置。
【請求項４】
前記道路推定手段は、前記バイアス誤差推定手段が過去に推定したバイアス誤差を加味して、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にされていることを特徴とする請求項１〜請求項３のいずれかに記載の位置補正装置。
【請求項５】
前記道路推定手段は、前記バイアス誤差推定手段が過去に推定したバイアス誤差に基づいて、前記軌跡データが示す位置座標を補正し、当該補正後の位置座標に基づき、前記軌跡データが示す地点の分布と相関が高い道路を探索し、前記探索により得られた道路上で、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にされていること
を特徴とする請求項１〜請求項４のいずれかに記載の位置補正装置。
【請求項６】
前記道路推定手段は、前記軌跡データが示す各地点の位置座標の内、特定地点の位置座標を、前記バイアス誤差推定手段が過去に推定したバイアス誤差に基づき補正し、補正後の位置座標に基づき、当該補正後の位置座標から最も近い道路を探索して、前記探索により得られた前記最も近い道路上で、前記過去所定期間に前記ＧＰＳ受信機が移動した道路区間を推定する構成にされていること
を特徴とする請求項１〜請求項５のいずれかに記載の位置補正装置。
【請求項７】
前記バイアス誤差推定手段は、前記道路推定手段により探索された前記最も近い道路と補正後の前記特定地点の位置座標との距離が、所定の上限値を超える場合に、前記軌跡データに基づいたバイアス誤差の推定動作を停止可能な構成にされ、
前記補正出力手段は、前記バイアス誤差推定手段によりバイアス誤差が推定されない期間、その期間前に前記バイアス誤差推定手段により推定されたバイアス誤差に基づき、前記ＧＰＳ受信機により計測された位置座標を補正し、当該補正後の位置座標を出力する構成にされていること
を特徴とする請求項６記載の位置補正装置。
【請求項８】
前記ＧＰＳ受信機は、車両に搭載され、
前記位置補正装置は、
前記車両に搭載された自律航法センサの出力に基づき、前記車両の変位量を算出して、当該変位量に基づき、前記位置情報取得手段が取得した位置座標を補正する前処理手段
を備え、
前記軌跡データ生成手段は、前記前処理手段による補正後の位置座標に基づき、前記過去所定期間における前記ＧＰＳ受信機の位置座標を複数地点表した前記軌跡データを生成する構成にされていること
を特徴とする請求項１〜請求項７のいずれかに記載の位置補正装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２００９−４１９８８（Ｐ２００９−４１９８８Ａ）
【公開日】平成２１年２月２６日（２００９．２．２６）
【国際特許分類】
【出願番号】特願２００７−２０５４８７（Ｐ２００７−２０５４８７）
【出願日】平成１９年８月７日（２００７．８．７）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
【公開日】平成２１年２月２６日（２００９．２．２６）
【国際特許分類】
【出願日】平成１９年８月７日（２００７．８．７）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
[ Back to top ]