位置検出装置及び位置検出方法
【目的】GPS出力データの各成分について信頼度の有無及び誤差指標に基づいて補正処理を行って位置決定精度を向上する「位置検出装置及び位置検出方法」を提供することである。
【構成】GPSの測位周期で該GPSにより測定される車両の各方向位置成分データ及び速度成分データを用いて自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、GPSにより測定される車両の各方向の位置成分データ及び速度成分データの信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、信頼度が無いと判定された測定成分の補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の補正処理における貢献度を誤差指標の大きさに応じて小さくする。
【構成】GPSの測位周期で該GPSにより測定される車両の各方向位置成分データ及び速度成分データを用いて自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、GPSにより測定される車両の各方向の位置成分データ及び速度成分データの信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、信頼度が無いと判定された測定成分の補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の補正処理における貢献度を誤差指標の大きさに応じて小さくする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の現在位置を検出する位置検出装置および位置検出方法に関わり、特に、自律航法により計算される位置データの精度を向上できる位置検出装置および位置検出方法に関する。
【背景技術】
【０００２】
車載用ナビゲーション装置は、自律航法センサーを用いた自律航法（Dead Reckoning）とGPS（Global Positioning System）レシーバを用いたGPS航法を併用している。
自律航法は、車両の加速度を検出する加速度センサーや車両の方位変化量を検出する相対方位センサー（ジャイロ等）、車両の速度（距離）を検出する距離センサー（車速センサー等）の出力を用いて、車両の位置・方位・車速等を検出する方法である。しかし、自律航法の出力（位置・方位・車速等）には、センサーの誤差が含まれるため、誤差が生じる。特に、位置、方位はセンサー出力を積算して算出するため、誤差が徐々に累積する。一方、GPSレシーバは、最大位置誤差が通常環境で３０m程度で絶対的な位置、方位、車速を求めることができる。このため、GPS受信時に自律航法の出力を該GPSレシーバの出力に整合させることにより、累積により誤差が大きくなったときの補正が可能である。例えば、自律航法で得られた位置を周知のマップマッチング法により道路地図上の道路位置に自車位置を修正した時の位置と、GPSレシーバで得られた位置との差が所定値より大きくなった時に、道路地図上の位置をGPSでレシーバ得られた位置に修正する。
【０００３】
ところで、自律航法は、上述のようにGPSレシーバの出力により補正することができるが、GPS非受信時においてセンサー出力の誤差、センサー取り付け誤差により自律航法の誤差が累積し出力精度が悪くなる問題が生じる。特に、立体駐車場や地下駐車場ではGPS信号が届かないため、１００m程の最大位置誤差が発生し、また、都心部では反射したGPS信号を受けることが多く、マルチパスが発生すると、３００ｍ程の最大位置誤差が発生する。
以上より、センサー出力の誤差を修正して現在位置を求める方法が提案されている。第１従来技術（特許文献１参照）は、自律推測航法から求められる車両の位置、方位、車速の情報とGPSレシーバから出力される車両の位置、方位、車速の情報により、カルマンフィルタにて、オフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を求めて、自律航法におけるそれぞれの補正を行う。
【０００４】
ところで、カルマンフィルタ処理にGPS出力（緯度、経度、高度の３方向位置データ、３方向速度データ）を適用するには、GPS出力の各成分データについて誤差指標を必要とする（特許文献1参照）。しかし、多くのGPSレシーバは水平位置に関する誤差指標しか出力せず、このためGPS出力データのうち誤差指標のないGPS出力データをカルマンフィルタ処理に利用できなかった。そこで、誤差指標を単純な計算により求めることが行われるが、適正な誤差指標を使う場合に比べて位置推定精度が大きく劣る。また、従来のGPS出力データは都心などではマルチパスの影響で精度が劣化し、その際誤差指標も不正確となる（誤差が大きいはずなのに、小さい数字を出す）。かかる場合、GPS出力データをカルマンフィルタ補正処理に使用すると位置推定精度が大きく落ちる。
上記問題を解決する方法は、全GPS出力成分について誤差指標を出力するGPSレシーバ（u-blox社製等）を使うことである。しかし、かかるGPSレシーバは高価であり、採用することができない。なお、誤差の少ないGPS出力データを選択的に用いることにより、位置決定精度を向上させるGPSナビゲーション装置があるが（特許文献２）、誤差の大小に基づいてカルマンフィルタ補正処理を制御するものではない。
【特許文献１】特開２００１−３３７１５０
【特許文献２】特開平８−３３４３３８
【発明の開示】
【発明が解決しようとする課題】
【０００５】
以上から、本発明の目的は、誤差指標を出力しないGPSレシーバの測定データ（緯度、経度、高度の３方向位置データ、緯度、経度、高度の３方向速度データ）を補正処理に適用できるようにすることである。
本発明の目的は、GPS出力データの各成分について信頼度の有無を判定し、信頼度有りの場合の誤差指標を計算し、これら信頼度の有無及び計算した誤差指標に基づいて補正処理を行って位置決定精度を向上することである。
本発明の目的は、GPS出力データの各成分について信頼度の有無を判定し、信頼度が無いと判定されたGPS出力データの補正処理における貢献度を零あるいは略零にして位置決定精度を向上することである。
本発明の目的は、GPS出力データの各成分について信頼度の有無を判定し、信頼度有りと判定されたGPS出力データの誤差指標を計算し、補正処理における貢献度を該誤差指標の大きさに応じて小さくして位置決定精度を向上することである。
本発明の目的は、測定成分が１つでも信頼度が無いと判定された場合には補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくして位置決定精度を向上することである。
本発明の目的は、速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくして位置決定精度を向上することである。
【課題を解決するための手段】
【０００６】
・位置検出方法
本発明の第１は、車両の現在位置を検出する位置検出方法である。
本発明の第１の位置検出方法は、所定の周期で自律航法により車両の位置を計算するステップ、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行うステップ、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するステップ、信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくするステップを備えている。
本発明の第２の位置検出方法は、所定の周期で自律航法により車両の位置を計算するステップ、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行うステップ、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するステップ、測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わず、全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくするステップを備えている。
本発明の第３の位置検出方法は、所定の周期で自律航法により車両の位置を計算するステップ、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行うステップ、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するステップ、GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくするステップ、を備えている。
上記第１、第２、第３の位置検出方法は、前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正するステップを有している。
【０００７】
・位置検出装置
本発明の第２は、車両の現在位置を検出する位置検出装置である。
本発明の第１の位置検出装置は、所定の周期で自律航法により車両の位置を計算する自律航法部、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて、前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくする手段を備えている。
本発明の第２の位置検出装置は、所定の周期で自律航法により車両の位置を計算する自律航法部、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御する手段、全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくする手段を備えている。
本発明の第３の位置検出装置は、所定の周期で自律航法により車両の位置を計算する自律航法部、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わないようにする手段、グループの全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする手段備えている。
上記第１、第２、第３の位置検出装置は、前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する第２の補正部を備えている。
【発明の効果】
【０００８】
本発明によれば、GPS出力データの各成分について信頼度の有無を判定し、信頼度が無いと判定されたGPS出力データの補正処理における貢献度を零あるいは略零にしたから、位置決定精度を向上することができる。
本発明によれば、GPS出力データの各成分について信頼度の有無を判定し、信頼度有りと判定されたGPS出力データの誤差指標を計算し、補正処理における貢献度を該誤差指標の大きさに応じて小さくしたから、位置決定精度を向上することができる。
本発明によれば、誤差指標を出力しないGPSレシーバであっても、GPS測定データ（緯度、経度、高度の３方向位置データ、緯度、経度、高度の３方向速度データ）の信頼度の有無を判定でき、しかも、信頼度有りの場合の誤差指標を計算できるため、高価なGPSレシーバを使用しなくてもカルマンフィルタを用いた補正処理を行うことができる。
本発明によれば、GPS測定成分が１つでも信頼度が無いと判定された場合には補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくしたから、位置決定精度を向上することができる。
本発明によれば、速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくしたから、グループ毎の測定精度に差がある場合、グループ毎に補正処理を制御して位置決定精度を向上することができる。
【発明を実施するための最良の形態】
【０００９】
(Ａ) 本発明の概要
図１は本発明の位置検出装置の説明図であり、自律航法用センサー１１、自律航法部１２、GPSレシーバ１４、GPSデータ判定部（GPSフィルタ）１５、補正部(カルマンフィルタ処理部)１６を備えている。自律航法用センサー１１は車両の移動距離を測定する移動距離検出部（車速センサー）、車両の方位変化量に応じた信号を出力する相対方位センサー（ジャイロ）、車両の加速度を検出する加速度センサーを備え、自律航法部１２は所定の周期で自立航法センサー出力信号を用いて自律航法により車両の位置を計算する。GPSレシーバ１４はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。GPSデータ判定部１５は、信頼度判定部１５aと誤差指標計算部１５bを備え、信頼度判定部１５aはGPSレシーバにより測定される車両の各方向の位置成分データ及び速度成分データのそれぞれの信頼度を判定し、誤差指標計算部１５bは信頼度無しの場合には、該成分データの誤差指標を補正処理に貢献しないように大きな値にし、信頼度ありと判定された場合には、該成分データの誤差指標を算出する。
補正部１６は、GPSレシーバ１４により測定される車両の各方向位置成分データ、速度成分データ及び各成分データの誤差指標を用いて自律航法部１２により計算された位置計算結果及び該自律航法部の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう。
この場合、補正部１６は、信頼度が無いと判定された測定成分の補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の補正処理における貢献度を誤差指標の大きさに応じて小さくする。
あるいは、補正部１６は、GPS測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御し、全ての測定成分が信頼度有りと判定された場合には補正処理を行うと共に、該補正処理における各GPS測定成分の貢献度を誤差指標に応じて小さくする。
あるいは、補正部１６は、GPS測定成分データを速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わない。しかし、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする。
【００１０】
（Ｂ）位置検出装置
図２は本発明の位置検出装置の詳細な構成図である。この位置検出装置には、自律航法用センサー１１として、車両の移動距離を測定する移動距離検出部、たとえば車両が所定距離走行する毎に１個のパルスを発生する車速センサー１１a、車両の方位変化量に応じた信号を出力する相対方位センサーであるジャイロ１１b、車両の加速度を検出する加速度センサー１１cが設けられている。車速センサー１１aは車輪に取り付けられ、ジャイロ１１bおよび加速度センサー１１cは一体にダッシュボードの所定位置に装着される。ジャイロ１１bおよび加速度センサー１１cは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図３（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１１bおよび加速度センサー１１cは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図３（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１１】
自律航法部１２は、各自律センサーからの出力信号を用いて高速度で、たとえば25Hzの周期で前後方向の車両速度Vsp(k)、車両の３次元位置（緯度方向位置N(k)、経度方向位置E(k)、高さD（ｋ））を計算して出力する。図４は加速度センサー１１cから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。車両CARには鉛直方向に重力加速度Gが加わっており、取り付けピッチ角Ａが０の場合、（Ａ）に示すように、その傾斜方向成分G0は
G0＝G×sinβ
である。したがって、加速度センサー１１cが測定する加速度Accは、車両の移動に伴う進行方向の加速度G1と重力の傾斜方向成分の和となり、次式
Acc=G×sinβ＋G1
で表現できる。なお、上式においてG×sinの符号は上り走行であれば負、下り走行であれば正である。取り付けピッチ角Ａが０でない場合、（Ｂ）に示すように、加速度センサー１１ｃはピッチ角θ（＝β＋Ａ）方向の加速度Accが測定される。したがって、（C）に示すようにピッチ角方向の重力加速度成分はG×sinθとなり、ピッチ角方向の車両移動に伴う加速度成分はG1×cosAとなり、次式
Acc= G×sinθ+ G1×cosA
が成立し、取り付けヨー角A2を考慮すると、次式
Acc= G×sinθ+ G1×cosA×cosA2
が成立する。この結果、傾斜方向の加速度G1は次式
G1=( Acc−G×sinθ)/ (cosA×cosA2） (1)
で表現できる。加速度測定周期をT1とすれば、変化速度ΔVは次式
ΔV=T1×( Acc−G×sinθ)/ (cosA×cosA2)
で与えられる。したがって、速度Vsp(k+1)は1つ前の離散時刻kにおける速度Vsp(k)とΔVより
Vsp(k+1)＝Vsp(k)＋T１×( Acc−G×sinθ)/ (cosA×cosA2) （2）
で与えられる。尚、加速度AccのオフセットをαOFとすれば加速度センサーの出力信号AccからαOFを差し引いた値をAccとして(２)式の演算を行う。すなわち、
Acc=Acc−αOF
とする。
【００１２】
また、自律航法部１２は、車両の３次元位置（緯度N(k)、経度E(k)、高さD（ｋ））を次式により計算して出力する。
N(k+1)= N(k) +S(cosθcosY cosAcosA2＋sinY sinA2＋sinθcosY sinAcosA2)
E(k+1)= E(k) +S(cosθsinY cosAcosA2−cosY sinA2＋sinθsinY sinAcosA2)
D(k+1)= D(k) +S(−sinθcosAcosA2＋cosθ sinAcosA2) (3)
ただし、S=（サンプル時間T1あたりの車速パルス数×パルス間距離）
=車がサンプル時間当たりに車両方向に進んだ距離
であり、４つの角度（θ、A、Y、A2）を使って、SをN-E-D座標系(North−East−Down座標系）に投影している。
速度計算部１３は、所定の周期T2(たとえば10Hzの周期)で車速センサー１１aから出力するパルス数Nと1パルスあたりの移動距離Lを用いて次式
Vx＝N×L／T2 (4)
により車速度を計算する。
GPSレシーバ１４はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。
【００１３】
GPSフィルタ１５は、図５に示すように信頼度判定部１５aと誤差指標計算部１５bを備え、信頼度判定部１５aはGPSレシーバにより測定される車両の各方向の位置成分データ（緯度、経度、高さ）及び速度成分データ（北方向速度、東方向速度、上下方向速度）のそれぞれの信頼度を判定し、誤差指標計算部１５bは信頼度無しの場合には、該成分データの誤差指標を補正処理に貢献しないように大きな値（例えば1000）にし、信頼度ありと判定された場合には、該成分データの誤差指標を算出する。信頼度判定部１５ａは、北・東方向速度信頼度判定部３１、上下方向速度信頼度判定部３２、GPS緯度・経度信頼度判定部３３、GPS高度信頼度判定部３４を備え、誤差指標計算部１５ｂは、北・東方向速度誤差指標計算部３５、上下方向速度誤差指標計算部３６、GPS緯度・経度誤差指標計算部３７、GPS高度誤差指標計算部３８を備えている。位置成分データ（緯度、経度、高さ）の誤差指標をδPx，δPy，δPzと表現し、速度成分データ（北方向速度、東方向速度、上下方向速度）の誤差指標をδVx，δVy，δVzと表現する。
GPSフィルタ１５は、GPSレシーバにより測定される車両の各方向の位置成分データ（緯度、経度、高さ）及び速度成分データ（北方向速度、東方向速度、上下方向速度）及び各成分データの誤差指標をカルマンフィルタ処理部１６に入力する。
信頼度判定部１５aによる信頼度判定処理及び誤差指標計算処理は図６乃至図１７にしたがって後述する。
【００１４】
カルマンフィルタ処理部１６は、ジャイロオフセット補正部２０と第１の補正部２１と第２の補正部２２を備えている。なお、カルマンフィルタ処理部１６は、第１の補正部２１を除外した構成とすることができる。
ジャイロオフセット補正部２０は、速度Vxが零（即ち、停車中）の時、停車中における角速度信号が[オフセット＋ノイズ]であることを利用し、該角速度信号出力ωOFと自律航法部１２で計算する角速度信号オフセットとの差をとり、後述のカルマンフィルタ処理により角速度信号オフセットωOFの補正を短時間で行う。
自律航法部１２はジャイロ１１bの出力信号を用いて計測した角速度信号ωから方位変化Δω(k）を次式
Δω（k）＝（ω−ωOF）×T1
により計算し、また周知の慣性航法システムの技術から導出される次式に基づいてピッチ角θとヨー角Yを求めて更新する。
ｃ00=cosθ(k+1) ×cosY(k+1) =−sinY(k)× Δω（ｋ）
ｃ10=cosθ(k+1) ×sinY(k+1) = cosY(k) ×Δω（ｋ） (5)
なお、自律航法部１２はセンサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFについては次式により、
A(k+1)=A(k)
A2(k+1)=A2(k)
ωOF(k+1)=ωOF(k)
αOF(k+1)=αOF(k） (6)
補正されるまで一定とする。
【００１５】
カルマンフィルタ処理部１６の第１の補正部２１は、第１の周期(たとえば１０Hz周期)で第1のカルマンフィルタ処理を行う。第1のカルマンフィルタ処理において第１の補正部２１は、速度計算部１３が計算した車両速度Vxと自律航法部１２が計算した車両速度Vspとの差に基づいて、該自律航法部で計算している車両速度Vsp、ピッチ角θ、センサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFを補正する。
カルマンフィルタ処理部の第２の補正部２２は、第１の周期より長い第２の周期(たとえば1Hz周期)でGPSレシーバ１４が出力する車両の各方向の位置成分データ（緯度、経度、高さ）、速度成分データ（北方向速度、東方向速度、上下方向速度）及び各成分データの誤差指標ならびに自律航法部１２が出力する三次元の車両位置と三次元の車両速度を用いて、該自律航法部で計算している緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOF（自律航法で計算している全てのパラメータ）を補正する。なお、第２の補正部２２は信頼度が無いと判定された測定成分データの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分データの補正処理における貢献度を誤差指標の大きさに応じて小さくする。
第１、第２の補正部２１，２２によるカルマンフィルタ処理の詳細は後述する。
自律航法部１２は第１の補正部２１により１０Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、センサー取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算し、また、第２の補正部２２により1Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算して出力する。
【００１６】
（Ｃ）信頼度判定及び誤差指標計算
図６乃至図１７は信頼度判定及び誤差指標計算処理フローである。
GPSフィルタ１５は図６に示す順序で信頼度測定、誤差指標計算処理を行う。すなわち、信頼度判定部１５aは、北・東方向速度の信頼度判定(ステップ１０)、上下方向速度の信頼度判定(ステップ２００)、GPS緯度・経度の信頼度判定(ステップ３００)、GPS高度の信頼度判定 (ステップ４００) を行い、誤差指標計算１５bは北・東方向速度誤差指標δVx，δVyの計算(ステップ５００)、上下方向速度誤差指標δVzの計算(ステップ６００)、GPS緯度・経度誤差指標δPx，δPyの計算(ステップ７００)、GPS高度誤差指標δPzの計算(ステップ８００)を行う。
（ａ）信頼度判定処理
１）北・東方向速度の信頼度判定処理
図７〜図９は北・東方向速度の信頼度判定処理フローである。
信頼度判定部１５ａの北・東方向速度信頼度判定部３１(図５)は、今回のGPS速度が予測速度範囲内であるかチェックする(ステップ１０１)。例えば図８に示すように、前回の速度と予測最大加速度とから今回の予測速度範囲ＶRNGを計算し(ステップ１０１ａ)、ついで、今回の実速度Ｖを測定し(ステップ１０１ｂ)、実速度Ｖと予測速度範囲ＶRNGを比較し(ステップ１０１ｃ)、Ｖ＜ＶRNGであればステップ１０１において「YES」と判定し(ステップ１０１d)、Ｖ≧ＶRNGであればステップ１０１において「NO」と判定する(３０１ｅ)。
ついで、北・東方向速度信頼度判定部３１は、GPS測定データから得られるGPS方位θGPSに信頼度があるか判定する(ステップ１０２)。図９はGPS方位θGPSの信頼度判定処理フローである。まず、GPS測定可能であるかチェックし(ステップ１０２a)、測定不可能であれば、GPS方位θGPSは信頼度が無く、ステップ１０２において「NO」と判定する(ステップ１０２ｅ)。測位可能であれば、以下の(１)〜（４）の測位条件を満たすか否か判定する（ステップ１０２ｂ）。
(1)GPS測位状態が３次元測位であること。これは、３次元測位は、２次元測位に比べて精度が高いためである。
(2)GPS速度が閾値以上であること（例えば、２０Ｋｍ／ｈ）。これは低速の場合には、GPS方位の精度が悪化するためである。
(3)走行距離比率αが一定の範囲以内であること（例えば、０．９≦走行距離比率≦１．１）。
走行距離比率は、GPS位置から得られる走行距離と、車速パルスから得られる走行距離の比である。この比が１に近いほど、GPS測位データの精度が高いことを示している。
(4)進行角誤差βが閾値以下（例えば、進行角誤差≦３０度）であること。
進行角誤差は、GPS位置の２点間の角度とGPS方位との差である。進行角誤差が０に近いほど、GPS測位データの精度が高いことを示している。
以上の測位条件(1)ないし(4)のすべてを満足するとき、GPS方位θGPSは信頼度が有るとして、ステップ１０２において「YES」と判定する(３０２f)。
ステップ１０２bのいずれかの測位条件を満足しなければ、北・東方向速度信頼度判定部３１は、GPS方位θGPSと自律航法により取得した航法方位θgyroが類似性があるか判定する(ステップ１０２c)。すなわち、北・東方向速度信頼度判定部３１はGPS方位とθGPSと航法方位θgyroの類似性を次式に
Δθ＝｜(θGPS1−θGPS2)−θgyro｜≦１０（degree） （7）
より判定する。上式において、θGPS1は今回のGPS方位、θGPS2は前回のGPS方位、θgyroは今回の航法方位である。Δθが１０度より大きい場合には、両者の方位に類似性がなく、GPS方位の信頼度はないと判定する(ステップ１０２e)。
Δθが１０度より小さければ、今回のGPS方位θGPS1が予測範囲内であるかチェックし(ステップ１０２d)、予測範囲外であればGPS方位の信頼度はないと判定し(ステップ１０２e)、予測範囲内であればGPS方位θGPSは信頼度が有ると判定する(ステップ１０２f)。予測範囲は、車両の直線性に依存するため、まず、移動が直線性か否かを判定する。直線性の移動であれば次式
Δθ′＝｜θGPS1−（θt-1+θgyro）｜≦１０（degree） (8)
により予測範囲を設定し、GPS方位θGPS1が予測範囲内に存在するか否かを判定する。なお、θt-1に前回の進行角である。直線性の移動でなければ、Δθ′≦１４（degree）により予測範囲を設定し、GPS方位θGPS1が予測範囲内に存在するか否かを判定する。
ステップ１０２において、GPS方位θGPSに信頼度があるか判定すれば、GPS傾斜角θGPS-SLを次式
【数１】
により計算する(ステップ１０３)。ただし、VNGPSは北方向速度、VEGPS は東方向速度、VDGPSは上下方向速度である。ついで、GPS傾斜角θGPS-SLの絶対値が１０（degree）以下であるか判定する(ステップ１０４)。これは道路交通法により１０（degree）以上傾斜する道路は存在しないからである。GPS傾斜角θGPS-SLの絶対値が１０（degree）より大きければ、北方向速度信頼度フラグ及び東方向速度信頼度フラグを共にオフ(信頼度なし)と設定し(ステップ１０５)、GPS傾斜角θGPS-SLの絶対値が１０（degree）より小さければ、北方向速度信頼度フラグ及び東方向速度信頼度フラグを共にオン(信頼度あり)と設定する(ステップ１０６)。
【００１７】
２)上下方向速度の信頼度判定処理
図１０は上下方向速度の信頼度判定処理フローである。
信頼度判定部１５ａの上下方向速度信頼度判定部３２(図５)は、GPS傾斜角の信頼性が高いかチェックし(ステップ２０１)、GPS傾斜角の信頼性が高ければ、上下方向速度の信頼度は高いと判断し、上下方向速度信頼度フラグをオンし(ステップ２０２)、GPS傾斜角の信頼性が低ければ、上下方向速度の信頼度は低いと判断し、上下方向速度信頼度フラグをオフする(ステップ２０３)。
ステップ２０１において、上下方向速度信頼度判定部３２は、まずGPSレシーバが３次元測位状態であるか判断し(ステップ２１１)、３次元測位状態でなければ、GPS傾斜角度の信頼度は低いと判定し(ステップ２１２)、上下方向速度信頼度フラグをオフする(ステップ２０３)。
上下方向速度信頼度判定部３２は、３次元測位状態であれば、(9)式によりGPS傾斜角θGPS-SLを計算する(ステップ２１３)。ついで、信頼度判定部３２は、算出したGPS傾斜角θGPS-SLの履歴がＮ個（例えば、Ｎ＝５）たまったかどうかを判定する（ステップ２１４）。この判定は、GPS傾斜角θGPS-SLの標準偏差を算出するのに必要なサンプル数がたまったかどうかの判定である。ここで、GPS傾斜角θGPSの履歴がＮ個たまったと判断した場合、続いて上下方向速度信頼度判定部３２は、次の３つの条件
(1) GPS速度が１０ｋｍ／ｈ以上であること、
(2)算出したGPS傾斜角θGPS-SLの絶対値が１０度以下であること、
(3)算出したGPS傾斜角θGPS-SLの標準偏差が３度以下であること
を全て満たすかどうか判定する（ステップ２１５）。
上下方向速度信頼度判定部３２は、上述の３つの条件を全て満たしていれば、GPS傾斜角θGPS-SLに信頼性があると判定し(ステップ２１６)、上下方向速度信頼度フラグをオンする(ステップ２０２)。一方、上述の３つの条件を１つでも満たしていなければ、GPS傾斜角θGPS-SLに信頼性がないと判定し(ステップ２１２)、上下方向速度信頼度フラグをオフする(ステップ２０３)。
【００１８】
３)GPS緯度・経度の信頼度判定処理
図１１はGPS緯度・経度の信頼度判定処理フローである。
信頼度判定部１５ａのGPS緯度・経度信頼度判定部３３(図５)は以下の測位条件(1)〜(4)を満たすか否か判断する（ステップ３０１）。
(1)GPS測位状態が３次元測位であること。
(2)GPS速度が閾値（例えば、１０Ｋｍ／ｈ）以上であること。
(3)走行距離比率αが一定の範囲以内（例えば、０．９≦走行距離比率≦１．１）であること。
(4)進行角誤差βが閾値以下（例えば、進行角誤差≦３０度）であること。
以上の測位条件(1)ないし(4)のすべてを満足するとき、GPS緯度・経度の信頼度が高いと判定し(ステップ３０２)、GPS緯度信頼度フラグ、GPS経度信頼度フラグを共にオンする(ステップ３０３)。一方、上記測位条件の１つでも満足しなければ、GPS緯度・経度の信頼度が低いと判定し(ステップ３０４)、GPS緯度信頼度フラグ、GPS経度信頼度フラグを共にオフする(ステップ３０５)。
【００１９】
４)GPS高度の信頼度判定処理
図１２はGPS高度の信頼度判定処理フローである。
信頼度判定部１５ａのGPS高度信頼度判定部３４(図５)は、GPS緯度・経度の信頼度がオンであるか判断し(ステップ４０１)、GPS緯度・経度の信頼度がオフであれば、GPS高度信頼度フラグをオフする(ステップ４０２)。一方、ステップ４０１において、GPS緯度・経度の信頼度がオンであれば、上下方向速度信頼度フラグがオンであるかチェックし(ステップ４０３)、上下方向速度信頼度フラグがオフであれば、GPS高度信頼度フラグをオフする(ステップ４０２)。しかし、ステップ４０３において、上下方向速度信頼度フラグがオンであれば、GPS高度信頼度フラグをオンする(ステップ４０５)。すなわち、GPS緯度・経度の信頼度および上下方向速度信頼度フラグが共にオンであれば、GPS高度の信頼度は高いとしてGPS高度信頼度フラグをオンし、それ以外はオフする。
【００２０】
（ｂ）誤差指標計算処理
誤差指標（標準偏差σ）は、リファレンス用の膨大な蓄積誤差データとGPSレシーバから得られるパラメータとの関連付けから得られる近似式を用いて計算する。この近似式は、まず誤差と関連のあるパラメータ（誤差因子）を選び、次にそれぞれの誤差因子を用いて蓄積誤差データの誤差指標（標準偏差σ）をあらわす近似式を決定する。最後に品質工学の手法を用いてそれぞれの近似式へのウェイトを決定して重み付けを行った和をとり、平方根をとる。
１）北・東方向速度の誤差指標計算処理
図１３は北・東方向速度の誤差指標計算処理フローである。
信頼度判定部１５ａの北・東方向速度誤差指標計算３５(図５)は、北・東方向速度信頼度フラグがオフであるか判断し(ステップ５０１)、オフであれば精度が悪いから北方向速度の誤差指標δVx，東方向速度の誤差指標δVyを共に1000m/s とする(ステップ５０２)。
ステップ５０１において、北・東方向速度誤差指標計算３５は、北・東方向速度信頼度フラグがオンであれば、３次元測位状態であるかチェックし(ステップ５０３)、３次元測位状態でなければ、２次元測位状態であるかチェックし(ステップ５０４)、２次元測位状態でなければ、北方向速度の誤差指標δVx，東方向速度の誤差指標δVyを共に1000m/s とする(ステップ５０２)。２次元測位状態であれば、誤差に関連する因子がないため、北方向速度の誤差指標δVx，東方向速度の誤差指標δVyを共に0.41m/s とする(ステップ５０５)。これは統計による結果である。
一方、ステップ５０３において、３次元測位状態であれば、北・東方向速度誤差指標計算３５は、水平方向の速度誤差に関連する誤差因子PDOP、GPS 速度（Vgps）をそれぞれ用いて(10)、(11)式の近似式により誤差指標を計算する(ステップ５０６)。PDOP（Position Dilution of Precision）は水平と垂直方向を踏まえた衛星配置指標であり、小さいほど精度が良く、次式
北・東方向速度誤差指標δV1＝0.0543×PDOP＋0.3138 (10)
によりPDOPに起因する北・東方向速度誤差指標δV1を計算する。また、GPS 速度（Vgps）は速いほど精度が良く、次式
北・東方向速度誤差指標δV2＝1.2609×exp( -0.0149×Vgps ) (11)
によりGPS 速度（Vgps）に起因する北・東方向速度誤差指標δV2を計算する。
ついで、北・東方向速度誤差指標計算３５は、次式によりδV1とδV2を重み付け合成して北方向速度誤差指標δVx、東方向速度誤差指標δVyをそれぞれ計算する(ステップ５０７)。
【数２】
【００２１】
２）上下方向速度の誤差指標計算処理
図１４は上下方向速度の誤差指標計算処理フローである。
信頼度判定部１５ａの上下方向速度誤差指標計算部３６(図５)は、上下方向速度信頼度フラグがオンであるかチェックし(ステップ６０１)、オフであれば精度が悪いから上下方向速度の誤差指標δVzを1000m/s とする(ステップ６０２)。しかし、上下方向速度信頼度フラグがオンの場合には、誤差に関連する因子がないため、上下方向速度誤差指標δVz＝0.52m/s とする(ステップ６０３)。この値は統計値によるものである。
【００２２】
３）GPS緯度・経度の誤差指標計算処理
図１５はGPS緯度・経度の誤差指標計算処理フローである。
信頼度判定部１５ａのGPS緯度・経度誤差指標計算部３７(図５)は、GPS緯度・経度信頼度フラグがオンであるか調べ(ステップ７０1)、オフであれば精度が悪いからGPS緯度誤差指標δPx、GPS経度誤差指標δPyを共に1000m/s とする(ステップ７０２)。しかし、GPS緯度・経度信頼度フラグがオンであれば、水平方向の位置誤差に関連するPDOPを誤差因子として利用して誤差指標δPx、δPyを計算する。PDOPは小さいほど精度が良いから、PDOP≦4であるか調べ(ステップ７０３)、PDOP≦4であれば、GPS緯度・経度誤差指標δPx、δPy＝4.6mとし(ステップ７０４)、PDOP＞4であれば、GPS緯度・経度誤差指標δPx、δPy＝4.9mとする(ステップ７０５)。
【００２３】
４）GPS高度誤差指標計算処理
図１６はGPS高度誤差指標計算処理フローである。
信頼度判定部１５ａのGPS高度誤差指標計算部３８(図５)は、GPS高度信頼度フラグがオンであるか調べ(ステップ８０１)、オフであれば精度が悪いからGPS高度誤差指標δPzを1000m/s とする(ステップ８０２)。しかし、GPS高度信頼度フラグがオンであれば、高度誤差指標計算部３８は、高度誤差に関連する誤差因子VDOP、過去4 秒間の高度標準偏差（Alt1σ）、GPS 速度（Vgps）をそれぞれ用いて(13)、(14)、(15)式の近似式により誤差指標を計算する(ステップ８０３)。すなわち、VDOPは垂直方向の衛星配置指標であり、小さいほど精度が良く、次式
GPS高度誤差指標δD1＝5.1077×exp（ 0.2043×VDOP ） (13)
によりVDOPに起因するGPS高度誤差指標δD1を計算する。また、過去4 秒間の高度標準偏差（Alt1σ）のバラツキが小さいほど精度が良いから、次式
GPS高度誤差指標δD2＝1.5362×Alt1σ＋5.4304 (14)
により高度標準偏差（Alt1σ）に起因するGPS高度誤差指標δD2を計算する。また、GPS 速度（Vgps）が速いほど精度が良く、次式
GPS高度誤差指標δD3＝19.61×exp（−0.0094×Vgps ） (15)
によりGPS 速度（Vgps）に起因するGPS高度誤差指標δD３を計算する。
図７（Ａ）は蓄誤差積データに基づいて高度誤差とVDOPの対応をプロットし、これより高度誤差指標を近似する(13)式を決定する例、（Ｂ）は高度誤差と過去4 秒間の高度標準偏差（Alt1σ）の対応をプロットし、これより高度誤差指標を近似する(14)式を決定する例、（Ｃ）は高度誤差とGPS 速度（Vgps）の対応をプロットし、これより高度誤差指標を近似する(15)式を決定する例である。
ステップ１０３において、GPS高度誤差指標δD1〜δD3が求まれば、高度誤差指標計算部３８は、次式によりδD1〜δD3を重み付け合成して高度誤差指標δPzを計算する(ステップ１０４)。
【数３】
【００２４】
（Ｄ）位置検出装置の処理
図１８は図２の位置検出装置の全体の処理フローである。
最初、自律航法部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロ１１ｂのオフセットωOF、加速度センサーのオフセットαOFの初期値を設定する（ステップ１１０１）。以後、自律航法部１２は車速センサー１１ａ、ジャイロ１１ｂ、加速度センサー１１ｃの出力を取り込み（ステップ１１０２）、第１周期（２５Hz周期）で（2）、（3）、(5)式の演算を行って車両速度Vsp（k+1）及び車両の３次元位置（緯度N(k+1)、経度E(k+1)、高さD(k+1)、ピッチ角θとヨー角Yに関する2つの値：
cosθ(k+1) ×cosY(k+1)、
cosθ(k+1) ×sinY(k+1)
を演算して出力する(ステップ１１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１１０４)、第２周期になっていなければ、ステップ１１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１１０６）、第３周期でなければカルマンフィルタ処理部１５の第１補正部２１は速度計算部１３が(4)式により計算した車両速度Vxと自律航法部１２が(2)式より計算した車両速度Vsp（ｋ）を用いてカルマンフィルタ処理により車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFを補正する（ステップ１１０７）。ステップ１１０７では後述するカルマンフィルタの観測行列Ｈ1を用いた第１補正処理が行われる。
ステップ１０６において、第３周期であればカルマンフィルタ処理部１５の第２補正部２２はGPSレシーバ１４が出力する３次元の車両位置（NGPS、EGPS、DGPS)、３次元の車両速度（VNGPS、VEGPS、VDGPS）、誤差指標を用いて、車両位置、車両速度、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFを補正する(ステップ１１０８)。ステップ１１０８では後述するカルマンフィルタの観測行列Ｈ2を用いた第２補正処理が行われる。
このステップ１１０８において、第２補正部２２は図１９のステップ１２０１に示すように、信頼度が無いと判定された測定成分データの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分データの補正処理における貢献度を誤差指標の大きさに応じて小さくするよう補正処理を行う。
あるいは、第２補正部２２はステップ１１０８において図２０に示すように、GPS測定成分データが１つでも信頼度が無いと判定された場合には（ステップ１２１１で「ＮＯ」）、補正処理を行わないよう制御し（ステップ１２１２）、全てのGPS測定成分データが信頼度有りと判定された場合には補正処理を行うと共に、該補正処理における各GPS測定成分の貢献度を誤差指標に応じて小さくする（ステップ１２１３）。かかる制御により、システムの精度を向上することができる。なお、図２０の制御を可能にするために、GPSフィルタ１５は、GPS測定成分データの１つでも信頼度が無いと判定した場合、全GPS測定成分データの誤差指標を1000にする必要がある。
あるいは、第２補正部２２はステップ１１０８において、図２１に示すようにGPS測定成分データを速度成分グループと位置成分グループに分け(ステップ１２２１)、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には(ステップ１２２２で「ＮＯ」)、該グループの測定成分を用いた補正処理を行わない(ステップ１２２３)。しかし、グループ全ての測定成分が信頼度有りと判定された場合には（ステップ１２２２で「ＹＥＳ」)、該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする(ステップ１２２４)。GPSレシーバでは、GPS速度データの方がGPS位置データより測定精度が良いから、上記の制御をすることによりシステムの精度を向上することができる。なお、図２１の制御を可能にするために、GPSフィルタ１５は、グループのGPS測定成分データの１つでも信頼度が無いと判定した場合、該グループの全GPS測定成分データの誤差指標を1000にする必要がある。
図１８に戻って、ステップ１１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１１０９）、第３周期でなければカルマンフィルタ処理部１５の第１補正部２１は前記ステップ１１０７の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１０）。このステップ１１１０では後述するカルマンフィルタの観測行列Ｈ3を用いた第３補正処理が行われる。
ステップ１１０９において、第３周期であれば、カルマンフィルタ１５の第２補正部２２は前記ステップ１１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１１）。このステップ１１１１では後述するカルマンフィルタの観測行列Ｈ4を用いた第４補正処理が行われる。
【００２５】
以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、第１補正部２１が誤差累積の補正を行うため精度の高い位置検出ができる。
図２２はGPS受信時とGPS非受信時における位置検出誤差の説明図であり、（Ａ）に示すようにGPS受信時に第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行い、第２の補正部２２は１Hz周期(GPS測位周期)で補正を行うため、誤差の累積を小さくできる。また、（Ｂ）に示すようにGPS非受信時でも第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行うため、誤差の累積度合を小さくできる。
【００２６】
図２３はＡ矢印方向から車両が移動してきて立体駐車場でグルグル回ってＢ矢印方向から去って行ったときの本発明の車両走行軌跡データの説明図であり、ほぼ実際の走行軌跡を示している。ただし、図２３はGPS測定データの誤差指標が正確に計算され、GPSレシーバとカルマンフィルタ処理部が最大に機能を発揮した例であり、マップマッチングMMはしていない。図２４（Ａ）はカルマンフィルタ処理部１６を使用せず、主にマップマッチングで補正したときの従来の走行軌跡データ例であり、図２４（Ｂ）はカルマンフィルタ処理部１６を使用するが、誤差指標が不正確、あるいは誤差指標がない場合の走行軌跡データ例である。図２４（Ｂ）より明らかなように誤差指標が不正確、あるいは誤差指標がない場合、正確な走行軌跡を示さなくなる。
【００２７】
（Ｅ）カルマンフィルタ処理
カルマンフィルタ処理は各時刻における予測値と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を予測するための算出式を設定し、この算出式を用いて観測値が得られる時刻ｎまで予測を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの推定値について確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図２５はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図２５に示すように、信号生成過程４０と観測過程４１に分けられる。図において、線形システムＦがあり、そのシステムの状態をX(t)とするとき、観測行列Ｈを介してＸ(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力をＺ(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値Ｘ(t)を求める。
【００２８】
本発明のカルマンフィルタ処理におけるシステムモデルの状態式は次式
ΔＸ（k+１）＝Ｆ（k）δＸ（k）＋ｗ(k） (17)
で表され、システム状態変数δＸは
ΔＸ＝[δN、δE、δD、δVbx、δc00、δc10、δc20、δp00、δp10、δp20、bwz、bax]
である。但し、Vbx=Vsp((２)式参照)、bwz=ωOF、bax=αOFとしている。また、c00〜p20のパラメータは座標変換行列の要素で、
ｃ00＝cosθcosY
ｃ10＝cosθsinY
ｃ20=−sinθ
ｐ00＝cosAcosA2
ｐ10＝cosAsinA2
p20＝−sinA
である。(17)式における線形システムＦは(２)、(３)、(５)式のシステムモデルを表す式より図２６に示す行列で表現でき、太枠内が行列要素である。なお、cijはセンサー座標系からN−E−D座標系への座標変換行列要素、pijはセンサー座標系から車両固定座標系への座標変換行列要素であり、それぞれ次式により表現される。
【数４】
【００２９】
また、本発明のカルマンフィルタの観測式は
ΔＺ(k)＝Ｈ(k)δＸ(k)+ｖ(k) (18)
で表される。(18)式の観測行列Ｈは図２７に示す行列で表される。図２７において、観測行列Ｈの行列部分(1)は10Hz周期での速度誤差δVbxを計算する部分、(2)は10Hz周期での停車時における角速度オフセット誤差bwzを計算する部分、(3)は1Hz周期でのGPSの車両位置誤差δN,δE,δDおよび車両速度誤差δvnx,δvny,δvnzを計算する部分である。なお、観測行列Ｈは
【数５】
と表現する。（1）は観測行列Ｈの第１行を、（2）は観測行列Ｈの第２行、（3）は観測行列Ｈの第3〜第8行を意味している。
カルマンフィルタは、Ｚ(t) (=δＺ(t)) が観測できるタイミングで(18)式によりＺ(t)(=δＺ(t))を計算し、計算した値と観測された値の差に基づいてＸ(t)(＝δＸ(t))を推定し、以後、次のZ（t)が観測されるまでＸ(t)を(17)式により更新する。そして、Ｚ（t)が観測されると上記差を再び演算し、該差に基づいてＸ(t)(＝δＸ(t))を推定し、以後、同様の処理を繰り返す。
【００３０】
観測行列Ｈの行列部分(1)は図１８の処理ステップ１１０７の第１補正処理において使用するカルマンフィルタの観測行列Ｈ1を構成し、
【数６】
である。また、観測行列Ｈの行列部分(1)、（3）は図１８の処理ステップ１１０８の第２補正処理において使用するカルマンフィルタの観測行列Ｈ2を構成し、
【数７】
である。また、観測行列Ｈの行列部分(1)、（2）は図１８の処理ステップ１１１０の第３補正処理において使用するカルマンフィルタの観測行列Ｈ3を構成し、
【数８】
である。また、観測行列Ｈの行列部分(1)、（2）、(3)は図１８の処理ステップ１１１１の第４補正処理において使用するカルマンフィルタの観測行列Ｈ4を構成し、
【数９】
である。
【００３１】
カルマンフィルタは、入力をＺ(t)として以下の(19)式を所定周期（Ｚ(t)の入力周期）で繰り返し実行することにより、最適推定値Ｘ(t｜t)(=δＸ(t｜t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのＡの推定値をＡ(i｜j)と表記する。
Ｘ(t｜t)＝Ｘ(t｜t−1)＋Ｋ(t)[Ｚ(t)−ＨＸ(t｜t−1)] (19)
ここでＸ(t｜t−1)は事前推定値、Ｋ(t)はカルマンゲインであり、それぞれ
Ｘ(t｜t−1)＝ＦＸ(t−1｜t−1) (20)
Ｋ(t)＝Ｐ(t｜t−1)ＨT (ＨＰ(t｜t−1)ＨT＋Ｖ)-1 (21)
と表現でき、事前推定値Ｘ(t｜t−1)は、Z(t)の入力周期より短い周期で(20)式により更新される。
また、Ｐは状態量Xの誤差共分散行列であり、Ｐ(t｜t−1)は誤差共分散の予測値、Ｐ(t−1｜t−1)は誤差共分散であり、それぞれ
Ｐ(t｜t−1)＝ＦＰ(t−1｜t−1)ＦT ＋Ｗ
Ｐ(t−1｜t−1)＝(I−Ｋ(t−1)Ｈ)Ｐ(t−1｜t−2)
である。Ｖは観測過程で発生する雑音ｖの分散、ずなわち、測定に関する誤差(誤差指標)の共分散行列、Ｗは信号過程で発生する雑音ｗの分散である。添字の（・）T は転置行列を意味し、(・)-1は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Ｐの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(17)式の計算を繰り返し行うことにより、状態量Ｘの精度を向上することができる。
【００３２】
(Ｆ)誤差指標のカルマンフィルタの補正処理に対する貢献度
誤差指標の共分散行列Ｖは誤差指標δPx，δPy，δPz，δVx，δVy，δVzを用いて次式
【数１０】
で表現される。
誤差指標のカルマンフィルタ処理に対する貢献度は、カルマンゲインＫ(t)を算出するための(19)式の右辺(ＨＰ(t｜t−1)ＨT＋Ｖ)-1より明らかなように、誤差指標の逆数に依存する。すなわち、誤差指標が大きいと貢献度は小さくなり、誤差指標が小さいと貢献度は大きくなる。例えば、誤差指標の共分散行列Ｖにおいて、全てのGPS測定成分の信頼度が無く、誤差指標δPx，δPy，δPz，δVx，δVy，δVzが全て1000であるとすれば、(ＨＰ(t｜t−1)ＨT＋Ｖ)-1は零あるいは略零となる。この結果、カルマンゲインＫ(t)は零となり、何らの補正は行われない。
【００３３】
本発明によれば、GPSフィルタを備えているため、どのGPSレシーバでもカルマンフィルタ複合システムに利用することができる。即ち、数社に限られるGPSレシーバだけでなく、多くのGPSレシーバを採用候補に挙げることができ、その中からコストパーフォーマンスが最適なGPSレシーバを採用することができる。また、GPSレシーバを用いる際、該GPSレシーバが出力する誤差指標よりも確からしい、あるいはGPSレシーバが出力しない誤差指標をGPSフィルタから出力できるので、システム精度を向上することができる。また、信頼度の無いGPS測定データのカルマンフィルタ補正処理における貢献度を零にできるため、システム精度を向上することができる。
以上本発明を実施例に従って説明したが、本発明は実施例に限定されるものではない。たとえば、以上では、カルマンフィルタを使用して各パラメータを補正する場合であるが、カルマンフィルタに限らず、Hインフィニティフィルタ、パーティクルフィルタなど、確率論に基づくフィルタリングシステムを利用して補正することが可能である。
また、以上では、カルマンフィルタ処理部において第１、第２補正処理を行う場合について説明したが、本発明はGPS測定データを用いた第２補正処理のみを行う場合（第１補正処理を行わない）にも適用できることは当然である。
【図面の簡単な説明】
【００３４】
【図１】本発明の位置検出装置の概要説明図である
【図２】本発明の位置検出装置の構成図である
【図３】姿勢パラメータ(ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2)説明図である。
【図４】加速度センサーから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。
【図５】GPSフィルタの構成図である。
【図６】GPSフィルタの信頼度測定、誤差指標計算処理フローである。
【図７】北・東方向速度の信頼度判定処理フローである。
【図８】GPS速度が予測速度範囲内にあるかの判定処理フローのである。
【図９】GPS方位θGPSに信頼度があるかの判定処理フローである。
【図１０】上下方向速度の信頼度判定処理フローである。
【図１１】GPS緯度・経度の信頼度判定処理フローである。
【図１２】GPS高度の信頼度判定処理フローである。
【図１３】北・東方向速度の誤差指標計算処理フローである。
【図１４】上下方向速度の誤差指標計算処理フローである。
【図１５】GPS緯度・経度の誤差指標計算処理フローである。
【図１６】GPS高度誤差指標計算処理フローである。
【図１７】GPS高度誤差指標を計算するための近似式決定説明図である。
【図１８】位置検出部の全体の処理フローである。
【図１９】信頼度及び誤差指標の補正処理における貢献度説明図である。
【図２０】１つでもGPS測定データに信頼度が無い場合、全GPS測定データの誤差指標を大きな値にして補正処理を行わないようにし、全てのGPS測定データに信頼度がある場合において補正処理を行う場合の処理フローである。
【図２１】GPS測定データを速度グループと位置グループに分け、グループのGPS測定データに１つでも信頼度が無い場合、該グループの測定データを用いた補正処理を行わないようにした処理フローである。
【図２２】第１、第２補正処理を行う場合のGPS受信時とGPS非受信時における位置検出誤差の説明図である。
【図２３】Ａ矢印方向から車両が移動してきて立体駐車場でグルグル回ってＢ矢印方向から去って行ったときの本願発明の車両走行軌跡データの説明図である。
【図２４】カルマンフィルタ処理部を使用せず、主にマップマッチングで補正したときの従来の走行軌跡データ例、およびカルマンフィルタ処理部を使用するが、誤差指標が不正確、あるいは誤差指標がない場合の走行軌跡データ例である。
【図２５】カルマンフィルタ処理の概要説明図である。
【図２６】カルマンフィルタの線形システムＦを示す行列例である。
【図２７】カルマンフィルタの観測行列Ｈを示す行列例である。
【符号の説明】
【００３５】
１１ 自律センサー
１２ 自律航法部
１４ GPSレシーバ
１５ GPSフィルタ
１６ カルマンフィルタ処理部
２１ 第１の補正部
２２ 第２の補正部
【技術分野】
【０００１】
本発明は、車両の現在位置を検出する位置検出装置および位置検出方法に関わり、特に、自律航法により計算される位置データの精度を向上できる位置検出装置および位置検出方法に関する。
【背景技術】
【０００２】
車載用ナビゲーション装置は、自律航法センサーを用いた自律航法（Dead Reckoning）とGPS（Global Positioning System）レシーバを用いたGPS航法を併用している。
自律航法は、車両の加速度を検出する加速度センサーや車両の方位変化量を検出する相対方位センサー（ジャイロ等）、車両の速度（距離）を検出する距離センサー（車速センサー等）の出力を用いて、車両の位置・方位・車速等を検出する方法である。しかし、自律航法の出力（位置・方位・車速等）には、センサーの誤差が含まれるため、誤差が生じる。特に、位置、方位はセンサー出力を積算して算出するため、誤差が徐々に累積する。一方、GPSレシーバは、最大位置誤差が通常環境で３０m程度で絶対的な位置、方位、車速を求めることができる。このため、GPS受信時に自律航法の出力を該GPSレシーバの出力に整合させることにより、累積により誤差が大きくなったときの補正が可能である。例えば、自律航法で得られた位置を周知のマップマッチング法により道路地図上の道路位置に自車位置を修正した時の位置と、GPSレシーバで得られた位置との差が所定値より大きくなった時に、道路地図上の位置をGPSでレシーバ得られた位置に修正する。
【０００３】
ところで、自律航法は、上述のようにGPSレシーバの出力により補正することができるが、GPS非受信時においてセンサー出力の誤差、センサー取り付け誤差により自律航法の誤差が累積し出力精度が悪くなる問題が生じる。特に、立体駐車場や地下駐車場ではGPS信号が届かないため、１００m程の最大位置誤差が発生し、また、都心部では反射したGPS信号を受けることが多く、マルチパスが発生すると、３００ｍ程の最大位置誤差が発生する。
以上より、センサー出力の誤差を修正して現在位置を求める方法が提案されている。第１従来技術（特許文献１参照）は、自律推測航法から求められる車両の位置、方位、車速の情報とGPSレシーバから出力される車両の位置、方位、車速の情報により、カルマンフィルタにて、オフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を求めて、自律航法におけるそれぞれの補正を行う。
【０００４】
ところで、カルマンフィルタ処理にGPS出力（緯度、経度、高度の３方向位置データ、３方向速度データ）を適用するには、GPS出力の各成分データについて誤差指標を必要とする（特許文献1参照）。しかし、多くのGPSレシーバは水平位置に関する誤差指標しか出力せず、このためGPS出力データのうち誤差指標のないGPS出力データをカルマンフィルタ処理に利用できなかった。そこで、誤差指標を単純な計算により求めることが行われるが、適正な誤差指標を使う場合に比べて位置推定精度が大きく劣る。また、従来のGPS出力データは都心などではマルチパスの影響で精度が劣化し、その際誤差指標も不正確となる（誤差が大きいはずなのに、小さい数字を出す）。かかる場合、GPS出力データをカルマンフィルタ補正処理に使用すると位置推定精度が大きく落ちる。
上記問題を解決する方法は、全GPS出力成分について誤差指標を出力するGPSレシーバ（u-blox社製等）を使うことである。しかし、かかるGPSレシーバは高価であり、採用することができない。なお、誤差の少ないGPS出力データを選択的に用いることにより、位置決定精度を向上させるGPSナビゲーション装置があるが（特許文献２）、誤差の大小に基づいてカルマンフィルタ補正処理を制御するものではない。
【特許文献１】特開２００１−３３７１５０
【特許文献２】特開平８−３３４３３８
【発明の開示】
【発明が解決しようとする課題】
【０００５】
以上から、本発明の目的は、誤差指標を出力しないGPSレシーバの測定データ（緯度、経度、高度の３方向位置データ、緯度、経度、高度の３方向速度データ）を補正処理に適用できるようにすることである。
本発明の目的は、GPS出力データの各成分について信頼度の有無を判定し、信頼度有りの場合の誤差指標を計算し、これら信頼度の有無及び計算した誤差指標に基づいて補正処理を行って位置決定精度を向上することである。
本発明の目的は、GPS出力データの各成分について信頼度の有無を判定し、信頼度が無いと判定されたGPS出力データの補正処理における貢献度を零あるいは略零にして位置決定精度を向上することである。
本発明の目的は、GPS出力データの各成分について信頼度の有無を判定し、信頼度有りと判定されたGPS出力データの誤差指標を計算し、補正処理における貢献度を該誤差指標の大きさに応じて小さくして位置決定精度を向上することである。
本発明の目的は、測定成分が１つでも信頼度が無いと判定された場合には補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくして位置決定精度を向上することである。
本発明の目的は、速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくして位置決定精度を向上することである。
【課題を解決するための手段】
【０００６】
・位置検出方法
本発明の第１は、車両の現在位置を検出する位置検出方法である。
本発明の第１の位置検出方法は、所定の周期で自律航法により車両の位置を計算するステップ、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行うステップ、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するステップ、信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくするステップを備えている。
本発明の第２の位置検出方法は、所定の周期で自律航法により車両の位置を計算するステップ、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行うステップ、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するステップ、測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わず、全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくするステップを備えている。
本発明の第３の位置検出方法は、所定の周期で自律航法により車両の位置を計算するステップ、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行うステップ、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するステップ、GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくするステップ、を備えている。
上記第１、第２、第３の位置検出方法は、前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正するステップを有している。
【０００７】
・位置検出装置
本発明の第２は、車両の現在位置を検出する位置検出装置である。
本発明の第１の位置検出装置は、所定の周期で自律航法により車両の位置を計算する自律航法部、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて、前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくする手段を備えている。
本発明の第２の位置検出装置は、所定の周期で自律航法により車両の位置を計算する自律航法部、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御する手段、全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくする手段を備えている。
本発明の第３の位置検出装置は、所定の周期で自律航法により車両の位置を計算する自律航法部、前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わないようにする手段、グループの全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする手段備えている。
上記第１、第２、第３の位置検出装置は、前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する第２の補正部を備えている。
【発明の効果】
【０００８】
本発明によれば、GPS出力データの各成分について信頼度の有無を判定し、信頼度が無いと判定されたGPS出力データの補正処理における貢献度を零あるいは略零にしたから、位置決定精度を向上することができる。
本発明によれば、GPS出力データの各成分について信頼度の有無を判定し、信頼度有りと判定されたGPS出力データの誤差指標を計算し、補正処理における貢献度を該誤差指標の大きさに応じて小さくしたから、位置決定精度を向上することができる。
本発明によれば、誤差指標を出力しないGPSレシーバであっても、GPS測定データ（緯度、経度、高度の３方向位置データ、緯度、経度、高度の３方向速度データ）の信頼度の有無を判定でき、しかも、信頼度有りの場合の誤差指標を計算できるため、高価なGPSレシーバを使用しなくてもカルマンフィルタを用いた補正処理を行うことができる。
本発明によれば、GPS測定成分が１つでも信頼度が無いと判定された場合には補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくしたから、位置決定精度を向上することができる。
本発明によれば、速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、全ての測定成分が信頼度有りと判定された場合に補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくしたから、グループ毎の測定精度に差がある場合、グループ毎に補正処理を制御して位置決定精度を向上することができる。
【発明を実施するための最良の形態】
【０００９】
(Ａ) 本発明の概要
図１は本発明の位置検出装置の説明図であり、自律航法用センサー１１、自律航法部１２、GPSレシーバ１４、GPSデータ判定部（GPSフィルタ）１５、補正部(カルマンフィルタ処理部)１６を備えている。自律航法用センサー１１は車両の移動距離を測定する移動距離検出部（車速センサー）、車両の方位変化量に応じた信号を出力する相対方位センサー（ジャイロ）、車両の加速度を検出する加速度センサーを備え、自律航法部１２は所定の周期で自立航法センサー出力信号を用いて自律航法により車両の位置を計算する。GPSレシーバ１４はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。GPSデータ判定部１５は、信頼度判定部１５aと誤差指標計算部１５bを備え、信頼度判定部１５aはGPSレシーバにより測定される車両の各方向の位置成分データ及び速度成分データのそれぞれの信頼度を判定し、誤差指標計算部１５bは信頼度無しの場合には、該成分データの誤差指標を補正処理に貢献しないように大きな値にし、信頼度ありと判定された場合には、該成分データの誤差指標を算出する。
補正部１６は、GPSレシーバ１４により測定される車両の各方向位置成分データ、速度成分データ及び各成分データの誤差指標を用いて自律航法部１２により計算された位置計算結果及び該自律航法部の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう。
この場合、補正部１６は、信頼度が無いと判定された測定成分の補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の補正処理における貢献度を誤差指標の大きさに応じて小さくする。
あるいは、補正部１６は、GPS測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御し、全ての測定成分が信頼度有りと判定された場合には補正処理を行うと共に、該補正処理における各GPS測定成分の貢献度を誤差指標に応じて小さくする。
あるいは、補正部１６は、GPS測定成分データを速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わない。しかし、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする。
【００１０】
（Ｂ）位置検出装置
図２は本発明の位置検出装置の詳細な構成図である。この位置検出装置には、自律航法用センサー１１として、車両の移動距離を測定する移動距離検出部、たとえば車両が所定距離走行する毎に１個のパルスを発生する車速センサー１１a、車両の方位変化量に応じた信号を出力する相対方位センサーであるジャイロ１１b、車両の加速度を検出する加速度センサー１１cが設けられている。車速センサー１１aは車輪に取り付けられ、ジャイロ１１bおよび加速度センサー１１cは一体にダッシュボードの所定位置に装着される。ジャイロ１１bおよび加速度センサー１１cは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図３（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１１bおよび加速度センサー１１cは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図３（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１１】
自律航法部１２は、各自律センサーからの出力信号を用いて高速度で、たとえば25Hzの周期で前後方向の車両速度Vsp(k)、車両の３次元位置（緯度方向位置N(k)、経度方向位置E(k)、高さD（ｋ））を計算して出力する。図４は加速度センサー１１cから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。車両CARには鉛直方向に重力加速度Gが加わっており、取り付けピッチ角Ａが０の場合、（Ａ）に示すように、その傾斜方向成分G0は
G0＝G×sinβ
である。したがって、加速度センサー１１cが測定する加速度Accは、車両の移動に伴う進行方向の加速度G1と重力の傾斜方向成分の和となり、次式
Acc=G×sinβ＋G1
で表現できる。なお、上式においてG×sinの符号は上り走行であれば負、下り走行であれば正である。取り付けピッチ角Ａが０でない場合、（Ｂ）に示すように、加速度センサー１１ｃはピッチ角θ（＝β＋Ａ）方向の加速度Accが測定される。したがって、（C）に示すようにピッチ角方向の重力加速度成分はG×sinθとなり、ピッチ角方向の車両移動に伴う加速度成分はG1×cosAとなり、次式
Acc= G×sinθ+ G1×cosA
が成立し、取り付けヨー角A2を考慮すると、次式
Acc= G×sinθ+ G1×cosA×cosA2
が成立する。この結果、傾斜方向の加速度G1は次式
G1=( Acc−G×sinθ)/ (cosA×cosA2） (1)
で表現できる。加速度測定周期をT1とすれば、変化速度ΔVは次式
ΔV=T1×( Acc−G×sinθ)/ (cosA×cosA2)
で与えられる。したがって、速度Vsp(k+1)は1つ前の離散時刻kにおける速度Vsp(k)とΔVより
Vsp(k+1)＝Vsp(k)＋T１×( Acc−G×sinθ)/ (cosA×cosA2) （2）
で与えられる。尚、加速度AccのオフセットをαOFとすれば加速度センサーの出力信号AccからαOFを差し引いた値をAccとして(２)式の演算を行う。すなわち、
Acc=Acc−αOF
とする。
【００１２】
また、自律航法部１２は、車両の３次元位置（緯度N(k)、経度E(k)、高さD（ｋ））を次式により計算して出力する。
N(k+1)= N(k) +S(cosθcosY cosAcosA2＋sinY sinA2＋sinθcosY sinAcosA2)
E(k+1)= E(k) +S(cosθsinY cosAcosA2−cosY sinA2＋sinθsinY sinAcosA2)
D(k+1)= D(k) +S(−sinθcosAcosA2＋cosθ sinAcosA2) (3)
ただし、S=（サンプル時間T1あたりの車速パルス数×パルス間距離）
=車がサンプル時間当たりに車両方向に進んだ距離
であり、４つの角度（θ、A、Y、A2）を使って、SをN-E-D座標系(North−East−Down座標系）に投影している。
速度計算部１３は、所定の周期T2(たとえば10Hzの周期)で車速センサー１１aから出力するパルス数Nと1パルスあたりの移動距離Lを用いて次式
Vx＝N×L／T2 (4)
により車速度を計算する。
GPSレシーバ１４はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。
【００１３】
GPSフィルタ１５は、図５に示すように信頼度判定部１５aと誤差指標計算部１５bを備え、信頼度判定部１５aはGPSレシーバにより測定される車両の各方向の位置成分データ（緯度、経度、高さ）及び速度成分データ（北方向速度、東方向速度、上下方向速度）のそれぞれの信頼度を判定し、誤差指標計算部１５bは信頼度無しの場合には、該成分データの誤差指標を補正処理に貢献しないように大きな値（例えば1000）にし、信頼度ありと判定された場合には、該成分データの誤差指標を算出する。信頼度判定部１５ａは、北・東方向速度信頼度判定部３１、上下方向速度信頼度判定部３２、GPS緯度・経度信頼度判定部３３、GPS高度信頼度判定部３４を備え、誤差指標計算部１５ｂは、北・東方向速度誤差指標計算部３５、上下方向速度誤差指標計算部３６、GPS緯度・経度誤差指標計算部３７、GPS高度誤差指標計算部３８を備えている。位置成分データ（緯度、経度、高さ）の誤差指標をδPx，δPy，δPzと表現し、速度成分データ（北方向速度、東方向速度、上下方向速度）の誤差指標をδVx，δVy，δVzと表現する。
GPSフィルタ１５は、GPSレシーバにより測定される車両の各方向の位置成分データ（緯度、経度、高さ）及び速度成分データ（北方向速度、東方向速度、上下方向速度）及び各成分データの誤差指標をカルマンフィルタ処理部１６に入力する。
信頼度判定部１５aによる信頼度判定処理及び誤差指標計算処理は図６乃至図１７にしたがって後述する。
【００１４】
カルマンフィルタ処理部１６は、ジャイロオフセット補正部２０と第１の補正部２１と第２の補正部２２を備えている。なお、カルマンフィルタ処理部１６は、第１の補正部２１を除外した構成とすることができる。
ジャイロオフセット補正部２０は、速度Vxが零（即ち、停車中）の時、停車中における角速度信号が[オフセット＋ノイズ]であることを利用し、該角速度信号出力ωOFと自律航法部１２で計算する角速度信号オフセットとの差をとり、後述のカルマンフィルタ処理により角速度信号オフセットωOFの補正を短時間で行う。
自律航法部１２はジャイロ１１bの出力信号を用いて計測した角速度信号ωから方位変化Δω(k）を次式
Δω（k）＝（ω−ωOF）×T1
により計算し、また周知の慣性航法システムの技術から導出される次式に基づいてピッチ角θとヨー角Yを求めて更新する。
ｃ00=cosθ(k+1) ×cosY(k+1) =−sinY(k)× Δω（ｋ）
ｃ10=cosθ(k+1) ×sinY(k+1) = cosY(k) ×Δω（ｋ） (5)
なお、自律航法部１２はセンサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFについては次式により、
A(k+1)=A(k)
A2(k+1)=A2(k)
ωOF(k+1)=ωOF(k)
αOF(k+1)=αOF(k） (6)
補正されるまで一定とする。
【００１５】
カルマンフィルタ処理部１６の第１の補正部２１は、第１の周期(たとえば１０Hz周期)で第1のカルマンフィルタ処理を行う。第1のカルマンフィルタ処理において第１の補正部２１は、速度計算部１３が計算した車両速度Vxと自律航法部１２が計算した車両速度Vspとの差に基づいて、該自律航法部で計算している車両速度Vsp、ピッチ角θ、センサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFを補正する。
カルマンフィルタ処理部の第２の補正部２２は、第１の周期より長い第２の周期(たとえば1Hz周期)でGPSレシーバ１４が出力する車両の各方向の位置成分データ（緯度、経度、高さ）、速度成分データ（北方向速度、東方向速度、上下方向速度）及び各成分データの誤差指標ならびに自律航法部１２が出力する三次元の車両位置と三次元の車両速度を用いて、該自律航法部で計算している緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOF（自律航法で計算している全てのパラメータ）を補正する。なお、第２の補正部２２は信頼度が無いと判定された測定成分データの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分データの補正処理における貢献度を誤差指標の大きさに応じて小さくする。
第１、第２の補正部２１，２２によるカルマンフィルタ処理の詳細は後述する。
自律航法部１２は第１の補正部２１により１０Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、センサー取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算し、また、第２の補正部２２により1Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算して出力する。
【００１６】
（Ｃ）信頼度判定及び誤差指標計算
図６乃至図１７は信頼度判定及び誤差指標計算処理フローである。
GPSフィルタ１５は図６に示す順序で信頼度測定、誤差指標計算処理を行う。すなわち、信頼度判定部１５aは、北・東方向速度の信頼度判定(ステップ１０)、上下方向速度の信頼度判定(ステップ２００)、GPS緯度・経度の信頼度判定(ステップ３００)、GPS高度の信頼度判定 (ステップ４００) を行い、誤差指標計算１５bは北・東方向速度誤差指標δVx，δVyの計算(ステップ５００)、上下方向速度誤差指標δVzの計算(ステップ６００)、GPS緯度・経度誤差指標δPx，δPyの計算(ステップ７００)、GPS高度誤差指標δPzの計算(ステップ８００)を行う。
（ａ）信頼度判定処理
１）北・東方向速度の信頼度判定処理
図７〜図９は北・東方向速度の信頼度判定処理フローである。
信頼度判定部１５ａの北・東方向速度信頼度判定部３１(図５)は、今回のGPS速度が予測速度範囲内であるかチェックする(ステップ１０１)。例えば図８に示すように、前回の速度と予測最大加速度とから今回の予測速度範囲ＶRNGを計算し(ステップ１０１ａ)、ついで、今回の実速度Ｖを測定し(ステップ１０１ｂ)、実速度Ｖと予測速度範囲ＶRNGを比較し(ステップ１０１ｃ)、Ｖ＜ＶRNGであればステップ１０１において「YES」と判定し(ステップ１０１d)、Ｖ≧ＶRNGであればステップ１０１において「NO」と判定する(３０１ｅ)。
ついで、北・東方向速度信頼度判定部３１は、GPS測定データから得られるGPS方位θGPSに信頼度があるか判定する(ステップ１０２)。図９はGPS方位θGPSの信頼度判定処理フローである。まず、GPS測定可能であるかチェックし(ステップ１０２a)、測定不可能であれば、GPS方位θGPSは信頼度が無く、ステップ１０２において「NO」と判定する(ステップ１０２ｅ)。測位可能であれば、以下の(１)〜（４）の測位条件を満たすか否か判定する（ステップ１０２ｂ）。
(1)GPS測位状態が３次元測位であること。これは、３次元測位は、２次元測位に比べて精度が高いためである。
(2)GPS速度が閾値以上であること（例えば、２０Ｋｍ／ｈ）。これは低速の場合には、GPS方位の精度が悪化するためである。
(3)走行距離比率αが一定の範囲以内であること（例えば、０．９≦走行距離比率≦１．１）。
走行距離比率は、GPS位置から得られる走行距離と、車速パルスから得られる走行距離の比である。この比が１に近いほど、GPS測位データの精度が高いことを示している。
(4)進行角誤差βが閾値以下（例えば、進行角誤差≦３０度）であること。
進行角誤差は、GPS位置の２点間の角度とGPS方位との差である。進行角誤差が０に近いほど、GPS測位データの精度が高いことを示している。
以上の測位条件(1)ないし(4)のすべてを満足するとき、GPS方位θGPSは信頼度が有るとして、ステップ１０２において「YES」と判定する(３０２f)。
ステップ１０２bのいずれかの測位条件を満足しなければ、北・東方向速度信頼度判定部３１は、GPS方位θGPSと自律航法により取得した航法方位θgyroが類似性があるか判定する(ステップ１０２c)。すなわち、北・東方向速度信頼度判定部３１はGPS方位とθGPSと航法方位θgyroの類似性を次式に
Δθ＝｜(θGPS1−θGPS2)−θgyro｜≦１０（degree） （7）
より判定する。上式において、θGPS1は今回のGPS方位、θGPS2は前回のGPS方位、θgyroは今回の航法方位である。Δθが１０度より大きい場合には、両者の方位に類似性がなく、GPS方位の信頼度はないと判定する(ステップ１０２e)。
Δθが１０度より小さければ、今回のGPS方位θGPS1が予測範囲内であるかチェックし(ステップ１０２d)、予測範囲外であればGPS方位の信頼度はないと判定し(ステップ１０２e)、予測範囲内であればGPS方位θGPSは信頼度が有ると判定する(ステップ１０２f)。予測範囲は、車両の直線性に依存するため、まず、移動が直線性か否かを判定する。直線性の移動であれば次式
Δθ′＝｜θGPS1−（θt-1+θgyro）｜≦１０（degree） (8)
により予測範囲を設定し、GPS方位θGPS1が予測範囲内に存在するか否かを判定する。なお、θt-1に前回の進行角である。直線性の移動でなければ、Δθ′≦１４（degree）により予測範囲を設定し、GPS方位θGPS1が予測範囲内に存在するか否かを判定する。
ステップ１０２において、GPS方位θGPSに信頼度があるか判定すれば、GPS傾斜角θGPS-SLを次式
【数１】
により計算する(ステップ１０３)。ただし、VNGPSは北方向速度、VEGPS は東方向速度、VDGPSは上下方向速度である。ついで、GPS傾斜角θGPS-SLの絶対値が１０（degree）以下であるか判定する(ステップ１０４)。これは道路交通法により１０（degree）以上傾斜する道路は存在しないからである。GPS傾斜角θGPS-SLの絶対値が１０（degree）より大きければ、北方向速度信頼度フラグ及び東方向速度信頼度フラグを共にオフ(信頼度なし)と設定し(ステップ１０５)、GPS傾斜角θGPS-SLの絶対値が１０（degree）より小さければ、北方向速度信頼度フラグ及び東方向速度信頼度フラグを共にオン(信頼度あり)と設定する(ステップ１０６)。
【００１７】
２)上下方向速度の信頼度判定処理
図１０は上下方向速度の信頼度判定処理フローである。
信頼度判定部１５ａの上下方向速度信頼度判定部３２(図５)は、GPS傾斜角の信頼性が高いかチェックし(ステップ２０１)、GPS傾斜角の信頼性が高ければ、上下方向速度の信頼度は高いと判断し、上下方向速度信頼度フラグをオンし(ステップ２０２)、GPS傾斜角の信頼性が低ければ、上下方向速度の信頼度は低いと判断し、上下方向速度信頼度フラグをオフする(ステップ２０３)。
ステップ２０１において、上下方向速度信頼度判定部３２は、まずGPSレシーバが３次元測位状態であるか判断し(ステップ２１１)、３次元測位状態でなければ、GPS傾斜角度の信頼度は低いと判定し(ステップ２１２)、上下方向速度信頼度フラグをオフする(ステップ２０３)。
上下方向速度信頼度判定部３２は、３次元測位状態であれば、(9)式によりGPS傾斜角θGPS-SLを計算する(ステップ２１３)。ついで、信頼度判定部３２は、算出したGPS傾斜角θGPS-SLの履歴がＮ個（例えば、Ｎ＝５）たまったかどうかを判定する（ステップ２１４）。この判定は、GPS傾斜角θGPS-SLの標準偏差を算出するのに必要なサンプル数がたまったかどうかの判定である。ここで、GPS傾斜角θGPSの履歴がＮ個たまったと判断した場合、続いて上下方向速度信頼度判定部３２は、次の３つの条件
(1) GPS速度が１０ｋｍ／ｈ以上であること、
(2)算出したGPS傾斜角θGPS-SLの絶対値が１０度以下であること、
(3)算出したGPS傾斜角θGPS-SLの標準偏差が３度以下であること
を全て満たすかどうか判定する（ステップ２１５）。
上下方向速度信頼度判定部３２は、上述の３つの条件を全て満たしていれば、GPS傾斜角θGPS-SLに信頼性があると判定し(ステップ２１６)、上下方向速度信頼度フラグをオンする(ステップ２０２)。一方、上述の３つの条件を１つでも満たしていなければ、GPS傾斜角θGPS-SLに信頼性がないと判定し(ステップ２１２)、上下方向速度信頼度フラグをオフする(ステップ２０３)。
【００１８】
３)GPS緯度・経度の信頼度判定処理
図１１はGPS緯度・経度の信頼度判定処理フローである。
信頼度判定部１５ａのGPS緯度・経度信頼度判定部３３(図５)は以下の測位条件(1)〜(4)を満たすか否か判断する（ステップ３０１）。
(1)GPS測位状態が３次元測位であること。
(2)GPS速度が閾値（例えば、１０Ｋｍ／ｈ）以上であること。
(3)走行距離比率αが一定の範囲以内（例えば、０．９≦走行距離比率≦１．１）であること。
(4)進行角誤差βが閾値以下（例えば、進行角誤差≦３０度）であること。
以上の測位条件(1)ないし(4)のすべてを満足するとき、GPS緯度・経度の信頼度が高いと判定し(ステップ３０２)、GPS緯度信頼度フラグ、GPS経度信頼度フラグを共にオンする(ステップ３０３)。一方、上記測位条件の１つでも満足しなければ、GPS緯度・経度の信頼度が低いと判定し(ステップ３０４)、GPS緯度信頼度フラグ、GPS経度信頼度フラグを共にオフする(ステップ３０５)。
【００１９】
４)GPS高度の信頼度判定処理
図１２はGPS高度の信頼度判定処理フローである。
信頼度判定部１５ａのGPS高度信頼度判定部３４(図５)は、GPS緯度・経度の信頼度がオンであるか判断し(ステップ４０１)、GPS緯度・経度の信頼度がオフであれば、GPS高度信頼度フラグをオフする(ステップ４０２)。一方、ステップ４０１において、GPS緯度・経度の信頼度がオンであれば、上下方向速度信頼度フラグがオンであるかチェックし(ステップ４０３)、上下方向速度信頼度フラグがオフであれば、GPS高度信頼度フラグをオフする(ステップ４０２)。しかし、ステップ４０３において、上下方向速度信頼度フラグがオンであれば、GPS高度信頼度フラグをオンする(ステップ４０５)。すなわち、GPS緯度・経度の信頼度および上下方向速度信頼度フラグが共にオンであれば、GPS高度の信頼度は高いとしてGPS高度信頼度フラグをオンし、それ以外はオフする。
【００２０】
（ｂ）誤差指標計算処理
誤差指標（標準偏差σ）は、リファレンス用の膨大な蓄積誤差データとGPSレシーバから得られるパラメータとの関連付けから得られる近似式を用いて計算する。この近似式は、まず誤差と関連のあるパラメータ（誤差因子）を選び、次にそれぞれの誤差因子を用いて蓄積誤差データの誤差指標（標準偏差σ）をあらわす近似式を決定する。最後に品質工学の手法を用いてそれぞれの近似式へのウェイトを決定して重み付けを行った和をとり、平方根をとる。
１）北・東方向速度の誤差指標計算処理
図１３は北・東方向速度の誤差指標計算処理フローである。
信頼度判定部１５ａの北・東方向速度誤差指標計算３５(図５)は、北・東方向速度信頼度フラグがオフであるか判断し(ステップ５０１)、オフであれば精度が悪いから北方向速度の誤差指標δVx，東方向速度の誤差指標δVyを共に1000m/s とする(ステップ５０２)。
ステップ５０１において、北・東方向速度誤差指標計算３５は、北・東方向速度信頼度フラグがオンであれば、３次元測位状態であるかチェックし(ステップ５０３)、３次元測位状態でなければ、２次元測位状態であるかチェックし(ステップ５０４)、２次元測位状態でなければ、北方向速度の誤差指標δVx，東方向速度の誤差指標δVyを共に1000m/s とする(ステップ５０２)。２次元測位状態であれば、誤差に関連する因子がないため、北方向速度の誤差指標δVx，東方向速度の誤差指標δVyを共に0.41m/s とする(ステップ５０５)。これは統計による結果である。
一方、ステップ５０３において、３次元測位状態であれば、北・東方向速度誤差指標計算３５は、水平方向の速度誤差に関連する誤差因子PDOP、GPS 速度（Vgps）をそれぞれ用いて(10)、(11)式の近似式により誤差指標を計算する(ステップ５０６)。PDOP（Position Dilution of Precision）は水平と垂直方向を踏まえた衛星配置指標であり、小さいほど精度が良く、次式
北・東方向速度誤差指標δV1＝0.0543×PDOP＋0.3138 (10)
によりPDOPに起因する北・東方向速度誤差指標δV1を計算する。また、GPS 速度（Vgps）は速いほど精度が良く、次式
北・東方向速度誤差指標δV2＝1.2609×exp( -0.0149×Vgps ) (11)
によりGPS 速度（Vgps）に起因する北・東方向速度誤差指標δV2を計算する。
ついで、北・東方向速度誤差指標計算３５は、次式によりδV1とδV2を重み付け合成して北方向速度誤差指標δVx、東方向速度誤差指標δVyをそれぞれ計算する(ステップ５０７)。
【数２】
【００２１】
２）上下方向速度の誤差指標計算処理
図１４は上下方向速度の誤差指標計算処理フローである。
信頼度判定部１５ａの上下方向速度誤差指標計算部３６(図５)は、上下方向速度信頼度フラグがオンであるかチェックし(ステップ６０１)、オフであれば精度が悪いから上下方向速度の誤差指標δVzを1000m/s とする(ステップ６０２)。しかし、上下方向速度信頼度フラグがオンの場合には、誤差に関連する因子がないため、上下方向速度誤差指標δVz＝0.52m/s とする(ステップ６０３)。この値は統計値によるものである。
【００２２】
３）GPS緯度・経度の誤差指標計算処理
図１５はGPS緯度・経度の誤差指標計算処理フローである。
信頼度判定部１５ａのGPS緯度・経度誤差指標計算部３７(図５)は、GPS緯度・経度信頼度フラグがオンであるか調べ(ステップ７０1)、オフであれば精度が悪いからGPS緯度誤差指標δPx、GPS経度誤差指標δPyを共に1000m/s とする(ステップ７０２)。しかし、GPS緯度・経度信頼度フラグがオンであれば、水平方向の位置誤差に関連するPDOPを誤差因子として利用して誤差指標δPx、δPyを計算する。PDOPは小さいほど精度が良いから、PDOP≦4であるか調べ(ステップ７０３)、PDOP≦4であれば、GPS緯度・経度誤差指標δPx、δPy＝4.6mとし(ステップ７０４)、PDOP＞4であれば、GPS緯度・経度誤差指標δPx、δPy＝4.9mとする(ステップ７０５)。
【００２３】
４）GPS高度誤差指標計算処理
図１６はGPS高度誤差指標計算処理フローである。
信頼度判定部１５ａのGPS高度誤差指標計算部３８(図５)は、GPS高度信頼度フラグがオンであるか調べ(ステップ８０１)、オフであれば精度が悪いからGPS高度誤差指標δPzを1000m/s とする(ステップ８０２)。しかし、GPS高度信頼度フラグがオンであれば、高度誤差指標計算部３８は、高度誤差に関連する誤差因子VDOP、過去4 秒間の高度標準偏差（Alt1σ）、GPS 速度（Vgps）をそれぞれ用いて(13)、(14)、(15)式の近似式により誤差指標を計算する(ステップ８０３)。すなわち、VDOPは垂直方向の衛星配置指標であり、小さいほど精度が良く、次式
GPS高度誤差指標δD1＝5.1077×exp（ 0.2043×VDOP ） (13)
によりVDOPに起因するGPS高度誤差指標δD1を計算する。また、過去4 秒間の高度標準偏差（Alt1σ）のバラツキが小さいほど精度が良いから、次式
GPS高度誤差指標δD2＝1.5362×Alt1σ＋5.4304 (14)
により高度標準偏差（Alt1σ）に起因するGPS高度誤差指標δD2を計算する。また、GPS 速度（Vgps）が速いほど精度が良く、次式
GPS高度誤差指標δD3＝19.61×exp（−0.0094×Vgps ） (15)
によりGPS 速度（Vgps）に起因するGPS高度誤差指標δD３を計算する。
図７（Ａ）は蓄誤差積データに基づいて高度誤差とVDOPの対応をプロットし、これより高度誤差指標を近似する(13)式を決定する例、（Ｂ）は高度誤差と過去4 秒間の高度標準偏差（Alt1σ）の対応をプロットし、これより高度誤差指標を近似する(14)式を決定する例、（Ｃ）は高度誤差とGPS 速度（Vgps）の対応をプロットし、これより高度誤差指標を近似する(15)式を決定する例である。
ステップ１０３において、GPS高度誤差指標δD1〜δD3が求まれば、高度誤差指標計算部３８は、次式によりδD1〜δD3を重み付け合成して高度誤差指標δPzを計算する(ステップ１０４)。
【数３】
【００２４】
（Ｄ）位置検出装置の処理
図１８は図２の位置検出装置の全体の処理フローである。
最初、自律航法部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロ１１ｂのオフセットωOF、加速度センサーのオフセットαOFの初期値を設定する（ステップ１１０１）。以後、自律航法部１２は車速センサー１１ａ、ジャイロ１１ｂ、加速度センサー１１ｃの出力を取り込み（ステップ１１０２）、第１周期（２５Hz周期）で（2）、（3）、(5)式の演算を行って車両速度Vsp（k+1）及び車両の３次元位置（緯度N(k+1)、経度E(k+1)、高さD(k+1)、ピッチ角θとヨー角Yに関する2つの値：
cosθ(k+1) ×cosY(k+1)、
cosθ(k+1) ×sinY(k+1)
を演算して出力する(ステップ１１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１１０４)、第２周期になっていなければ、ステップ１１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１１０６）、第３周期でなければカルマンフィルタ処理部１５の第１補正部２１は速度計算部１３が(4)式により計算した車両速度Vxと自律航法部１２が(2)式より計算した車両速度Vsp（ｋ）を用いてカルマンフィルタ処理により車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFを補正する（ステップ１１０７）。ステップ１１０７では後述するカルマンフィルタの観測行列Ｈ1を用いた第１補正処理が行われる。
ステップ１０６において、第３周期であればカルマンフィルタ処理部１５の第２補正部２２はGPSレシーバ１４が出力する３次元の車両位置（NGPS、EGPS、DGPS)、３次元の車両速度（VNGPS、VEGPS、VDGPS）、誤差指標を用いて、車両位置、車両速度、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、角速度信号オフセットωOF、加速度信号オフセットαOFを補正する(ステップ１１０８)。ステップ１１０８では後述するカルマンフィルタの観測行列Ｈ2を用いた第２補正処理が行われる。
このステップ１１０８において、第２補正部２２は図１９のステップ１２０１に示すように、信頼度が無いと判定された測定成分データの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分データの補正処理における貢献度を誤差指標の大きさに応じて小さくするよう補正処理を行う。
あるいは、第２補正部２２はステップ１１０８において図２０に示すように、GPS測定成分データが１つでも信頼度が無いと判定された場合には（ステップ１２１１で「ＮＯ」）、補正処理を行わないよう制御し（ステップ１２１２）、全てのGPS測定成分データが信頼度有りと判定された場合には補正処理を行うと共に、該補正処理における各GPS測定成分の貢献度を誤差指標に応じて小さくする（ステップ１２１３）。かかる制御により、システムの精度を向上することができる。なお、図２０の制御を可能にするために、GPSフィルタ１５は、GPS測定成分データの１つでも信頼度が無いと判定した場合、全GPS測定成分データの誤差指標を1000にする必要がある。
あるいは、第２補正部２２はステップ１１０８において、図２１に示すようにGPS測定成分データを速度成分グループと位置成分グループに分け(ステップ１２２１)、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には(ステップ１２２２で「ＮＯ」)、該グループの測定成分を用いた補正処理を行わない(ステップ１２２３)。しかし、グループ全ての測定成分が信頼度有りと判定された場合には（ステップ１２２２で「ＹＥＳ」)、該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする(ステップ１２２４)。GPSレシーバでは、GPS速度データの方がGPS位置データより測定精度が良いから、上記の制御をすることによりシステムの精度を向上することができる。なお、図２１の制御を可能にするために、GPSフィルタ１５は、グループのGPS測定成分データの１つでも信頼度が無いと判定した場合、該グループの全GPS測定成分データの誤差指標を1000にする必要がある。
図１８に戻って、ステップ１１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１１０９）、第３周期でなければカルマンフィルタ処理部１５の第１補正部２１は前記ステップ１１０７の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１０）。このステップ１１１０では後述するカルマンフィルタの観測行列Ｈ3を用いた第３補正処理が行われる。
ステップ１１０９において、第３周期であれば、カルマンフィルタ１５の第２補正部２２は前記ステップ１１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１１）。このステップ１１１１では後述するカルマンフィルタの観測行列Ｈ4を用いた第４補正処理が行われる。
【００２５】
以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、第１補正部２１が誤差累積の補正を行うため精度の高い位置検出ができる。
図２２はGPS受信時とGPS非受信時における位置検出誤差の説明図であり、（Ａ）に示すようにGPS受信時に第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行い、第２の補正部２２は１Hz周期(GPS測位周期)で補正を行うため、誤差の累積を小さくできる。また、（Ｂ）に示すようにGPS非受信時でも第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行うため、誤差の累積度合を小さくできる。
【００２６】
図２３はＡ矢印方向から車両が移動してきて立体駐車場でグルグル回ってＢ矢印方向から去って行ったときの本発明の車両走行軌跡データの説明図であり、ほぼ実際の走行軌跡を示している。ただし、図２３はGPS測定データの誤差指標が正確に計算され、GPSレシーバとカルマンフィルタ処理部が最大に機能を発揮した例であり、マップマッチングMMはしていない。図２４（Ａ）はカルマンフィルタ処理部１６を使用せず、主にマップマッチングで補正したときの従来の走行軌跡データ例であり、図２４（Ｂ）はカルマンフィルタ処理部１６を使用するが、誤差指標が不正確、あるいは誤差指標がない場合の走行軌跡データ例である。図２４（Ｂ）より明らかなように誤差指標が不正確、あるいは誤差指標がない場合、正確な走行軌跡を示さなくなる。
【００２７】
（Ｅ）カルマンフィルタ処理
カルマンフィルタ処理は各時刻における予測値と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を予測するための算出式を設定し、この算出式を用いて観測値が得られる時刻ｎまで予測を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの推定値について確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図２５はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図２５に示すように、信号生成過程４０と観測過程４１に分けられる。図において、線形システムＦがあり、そのシステムの状態をX(t)とするとき、観測行列Ｈを介してＸ(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力をＺ(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値Ｘ(t)を求める。
【００２８】
本発明のカルマンフィルタ処理におけるシステムモデルの状態式は次式
ΔＸ（k+１）＝Ｆ（k）δＸ（k）＋ｗ(k） (17)
で表され、システム状態変数δＸは
ΔＸ＝[δN、δE、δD、δVbx、δc00、δc10、δc20、δp00、δp10、δp20、bwz、bax]
である。但し、Vbx=Vsp((２)式参照)、bwz=ωOF、bax=αOFとしている。また、c00〜p20のパラメータは座標変換行列の要素で、
ｃ00＝cosθcosY
ｃ10＝cosθsinY
ｃ20=−sinθ
ｐ00＝cosAcosA2
ｐ10＝cosAsinA2
p20＝−sinA
である。(17)式における線形システムＦは(２)、(３)、(５)式のシステムモデルを表す式より図２６に示す行列で表現でき、太枠内が行列要素である。なお、cijはセンサー座標系からN−E−D座標系への座標変換行列要素、pijはセンサー座標系から車両固定座標系への座標変換行列要素であり、それぞれ次式により表現される。
【数４】
【００２９】
また、本発明のカルマンフィルタの観測式は
ΔＺ(k)＝Ｈ(k)δＸ(k)+ｖ(k) (18)
で表される。(18)式の観測行列Ｈは図２７に示す行列で表される。図２７において、観測行列Ｈの行列部分(1)は10Hz周期での速度誤差δVbxを計算する部分、(2)は10Hz周期での停車時における角速度オフセット誤差bwzを計算する部分、(3)は1Hz周期でのGPSの車両位置誤差δN,δE,δDおよび車両速度誤差δvnx,δvny,δvnzを計算する部分である。なお、観測行列Ｈは
【数５】
と表現する。（1）は観測行列Ｈの第１行を、（2）は観測行列Ｈの第２行、（3）は観測行列Ｈの第3〜第8行を意味している。
カルマンフィルタは、Ｚ(t) (=δＺ(t)) が観測できるタイミングで(18)式によりＺ(t)(=δＺ(t))を計算し、計算した値と観測された値の差に基づいてＸ(t)(＝δＸ(t))を推定し、以後、次のZ（t)が観測されるまでＸ(t)を(17)式により更新する。そして、Ｚ（t)が観測されると上記差を再び演算し、該差に基づいてＸ(t)(＝δＸ(t))を推定し、以後、同様の処理を繰り返す。
【００３０】
観測行列Ｈの行列部分(1)は図１８の処理ステップ１１０７の第１補正処理において使用するカルマンフィルタの観測行列Ｈ1を構成し、
【数６】
である。また、観測行列Ｈの行列部分(1)、（3）は図１８の処理ステップ１１０８の第２補正処理において使用するカルマンフィルタの観測行列Ｈ2を構成し、
【数７】
である。また、観測行列Ｈの行列部分(1)、（2）は図１８の処理ステップ１１１０の第３補正処理において使用するカルマンフィルタの観測行列Ｈ3を構成し、
【数８】
である。また、観測行列Ｈの行列部分(1)、（2）、(3)は図１８の処理ステップ１１１１の第４補正処理において使用するカルマンフィルタの観測行列Ｈ4を構成し、
【数９】
である。
【００３１】
カルマンフィルタは、入力をＺ(t)として以下の(19)式を所定周期（Ｚ(t)の入力周期）で繰り返し実行することにより、最適推定値Ｘ(t｜t)(=δＸ(t｜t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのＡの推定値をＡ(i｜j)と表記する。
Ｘ(t｜t)＝Ｘ(t｜t−1)＋Ｋ(t)[Ｚ(t)−ＨＸ(t｜t−1)] (19)
ここでＸ(t｜t−1)は事前推定値、Ｋ(t)はカルマンゲインであり、それぞれ
Ｘ(t｜t−1)＝ＦＸ(t−1｜t−1) (20)
Ｋ(t)＝Ｐ(t｜t−1)ＨT (ＨＰ(t｜t−1)ＨT＋Ｖ)-1 (21)
と表現でき、事前推定値Ｘ(t｜t−1)は、Z(t)の入力周期より短い周期で(20)式により更新される。
また、Ｐは状態量Xの誤差共分散行列であり、Ｐ(t｜t−1)は誤差共分散の予測値、Ｐ(t−1｜t−1)は誤差共分散であり、それぞれ
Ｐ(t｜t−1)＝ＦＰ(t−1｜t−1)ＦT ＋Ｗ
Ｐ(t−1｜t−1)＝(I−Ｋ(t−1)Ｈ)Ｐ(t−1｜t−2)
である。Ｖは観測過程で発生する雑音ｖの分散、ずなわち、測定に関する誤差(誤差指標)の共分散行列、Ｗは信号過程で発生する雑音ｗの分散である。添字の（・）T は転置行列を意味し、(・)-1は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Ｐの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(17)式の計算を繰り返し行うことにより、状態量Ｘの精度を向上することができる。
【００３２】
(Ｆ)誤差指標のカルマンフィルタの補正処理に対する貢献度
誤差指標の共分散行列Ｖは誤差指標δPx，δPy，δPz，δVx，δVy，δVzを用いて次式
【数１０】
で表現される。
誤差指標のカルマンフィルタ処理に対する貢献度は、カルマンゲインＫ(t)を算出するための(19)式の右辺(ＨＰ(t｜t−1)ＨT＋Ｖ)-1より明らかなように、誤差指標の逆数に依存する。すなわち、誤差指標が大きいと貢献度は小さくなり、誤差指標が小さいと貢献度は大きくなる。例えば、誤差指標の共分散行列Ｖにおいて、全てのGPS測定成分の信頼度が無く、誤差指標δPx，δPy，δPz，δVx，δVy，δVzが全て1000であるとすれば、(ＨＰ(t｜t−1)ＨT＋Ｖ)-1は零あるいは略零となる。この結果、カルマンゲインＫ(t)は零となり、何らの補正は行われない。
【００３３】
本発明によれば、GPSフィルタを備えているため、どのGPSレシーバでもカルマンフィルタ複合システムに利用することができる。即ち、数社に限られるGPSレシーバだけでなく、多くのGPSレシーバを採用候補に挙げることができ、その中からコストパーフォーマンスが最適なGPSレシーバを採用することができる。また、GPSレシーバを用いる際、該GPSレシーバが出力する誤差指標よりも確からしい、あるいはGPSレシーバが出力しない誤差指標をGPSフィルタから出力できるので、システム精度を向上することができる。また、信頼度の無いGPS測定データのカルマンフィルタ補正処理における貢献度を零にできるため、システム精度を向上することができる。
以上本発明を実施例に従って説明したが、本発明は実施例に限定されるものではない。たとえば、以上では、カルマンフィルタを使用して各パラメータを補正する場合であるが、カルマンフィルタに限らず、Hインフィニティフィルタ、パーティクルフィルタなど、確率論に基づくフィルタリングシステムを利用して補正することが可能である。
また、以上では、カルマンフィルタ処理部において第１、第２補正処理を行う場合について説明したが、本発明はGPS測定データを用いた第２補正処理のみを行う場合（第１補正処理を行わない）にも適用できることは当然である。
【図面の簡単な説明】
【００３４】
【図１】本発明の位置検出装置の概要説明図である
【図２】本発明の位置検出装置の構成図である
【図３】姿勢パラメータ(ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2)説明図である。
【図４】加速度センサーから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。
【図５】GPSフィルタの構成図である。
【図６】GPSフィルタの信頼度測定、誤差指標計算処理フローである。
【図７】北・東方向速度の信頼度判定処理フローである。
【図８】GPS速度が予測速度範囲内にあるかの判定処理フローのである。
【図９】GPS方位θGPSに信頼度があるかの判定処理フローである。
【図１０】上下方向速度の信頼度判定処理フローである。
【図１１】GPS緯度・経度の信頼度判定処理フローである。
【図１２】GPS高度の信頼度判定処理フローである。
【図１３】北・東方向速度の誤差指標計算処理フローである。
【図１４】上下方向速度の誤差指標計算処理フローである。
【図１５】GPS緯度・経度の誤差指標計算処理フローである。
【図１６】GPS高度誤差指標計算処理フローである。
【図１７】GPS高度誤差指標を計算するための近似式決定説明図である。
【図１８】位置検出部の全体の処理フローである。
【図１９】信頼度及び誤差指標の補正処理における貢献度説明図である。
【図２０】１つでもGPS測定データに信頼度が無い場合、全GPS測定データの誤差指標を大きな値にして補正処理を行わないようにし、全てのGPS測定データに信頼度がある場合において補正処理を行う場合の処理フローである。
【図２１】GPS測定データを速度グループと位置グループに分け、グループのGPS測定データに１つでも信頼度が無い場合、該グループの測定データを用いた補正処理を行わないようにした処理フローである。
【図２２】第１、第２補正処理を行う場合のGPS受信時とGPS非受信時における位置検出誤差の説明図である。
【図２３】Ａ矢印方向から車両が移動してきて立体駐車場でグルグル回ってＢ矢印方向から去って行ったときの本願発明の車両走行軌跡データの説明図である。
【図２４】カルマンフィルタ処理部を使用せず、主にマップマッチングで補正したときの従来の走行軌跡データ例、およびカルマンフィルタ処理部を使用するが、誤差指標が不正確、あるいは誤差指標がない場合の走行軌跡データ例である。
【図２５】カルマンフィルタ処理の概要説明図である。
【図２６】カルマンフィルタの線形システムＦを示す行列例である。
【図２７】カルマンフィルタの観測行列Ｈを示す行列例である。
【符号の説明】
【００３５】
１１ 自律センサー
１２ 自律航法部
１４ GPSレシーバ
１５ GPSフィルタ
１６ カルマンフィルタ処理部
２１ 第１の補正部
２２ 第２の補正部
【特許請求の範囲】
【請求項１】
車両の現在位置を検出する位置検出方法において、
所定の周期で自律航法により車両の位置を計算し、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくする、
ことを特徴とする位置検出方法。
【請求項２】
車両の現在位置を検出する位置検出方法において、
所定の周期で自律航法により車両の位置を計算し、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わず、
全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくする、
ことを特徴とする位置検出方法。
【請求項３】
車両の現在位置を検出する位置検出方法において、
所定の周期で自律航法により車両の位置を計算し、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする、
ことを特徴とする位置検出方法。
【請求項４】
前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する、
ことを特徴とする請求項１記載の位置検出方法。
【請求項５】
前記信頼度が無いと判定された前記成分の誤差指標を、該成分が前記補正処理に貢献しないように大きな値にする、
ことを特徴とする請求項１乃至３記載の位置検出方法。
【請求項６】
カルマンフィルタ処理に基づいて前記補正処理を行い、
前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該共分散行列により各測定成分の前記補正処理における貢献度を該誤差指標に応じて小さくする、
ことを特徴とする請求項５記載の位置検出方法。
【請求項７】
車両の現在位置を検出する位置検出方法において、
車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、
前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第２ステップ、
GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第３ステップ、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出する第４ステップ、
信頼度が無いと判定された測定成分の前記第３ステップの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を前記誤差指標の大きさに応じて小さくする第５ステップ、
を備えたことを特徴とする位置検出方法。
【請求項８】
前記第４ステップにおいて、前記信頼度が無いと判定された前記成分の誤差指標を該成分が前記補正処理に貢献しないように大きな値にし、
前記第３ステップにおいて、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、各成分の前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該演算により各測定成分の前記補正処理における貢献度を該誤差指標に応じて小さくする、
ことを特徴とする請求項７記載の位置検出方法。
【請求項９】
車両の現在位置を検出する位置検出装置において、
所定の周期で自律航法により車両の位置を計算する自律航法部、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて、前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、
前記補正部は、信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくする手段、
を備えることを特徴とする位置検出装置。
【請求項１０】
車両の現在位置を検出する位置検出装置において、
所定の周期で自律航法により車両の位置を計算する自律航法部、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、
測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御する手段、
全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくする手段、
を備えたことを特徴とする位置検出装置。
【請求項１１】
車両の現在位置を検出する位置検出装置において、
所定の周期で自律航法により車両の位置を計算する自律航法部、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部、
を備え、前記補正部は、
GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わないようにする手段、
グループの全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする手段、
を備えたことを特徴とする位置検出装置。
【請求項１２】
前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する第２の補正部、
を備えることを特徴とする請求項９乃至１１記載の位置検出装置。
【請求項１３】
前記GPSデータ判定部は、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定する信頼度判定部、
信頼度ありと判定された成分の誤差指標を算出する誤差指標計算部、
を備えることを特徴とする請求項９乃至１１記載の位置検出装置。
【請求項１４】
前記誤差指標計算部は、前記信頼度が無いと判定された前記成分の誤差指標を、該成分が前記補正処理に貢献しないように大きな値にする、
ことを特徴とする請求項１４記載の位置検出装置。
【請求項１５】
前記補正部は、カルマンフィルタ処理に基づいて前記補正処理を行い、前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該共分散行列により各測定成分の前記補正処理における貢献度を該誤差指標に応じて小さくするカルマンフィルタ処理、
を備えたことを特徴とする請求項９乃至１１記載の位置検出装置。
【請求項１６】
車両の現在位置を検出する位置検出装置において、
車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、
前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第１の補正部、
GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第２の補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記第２補正部は、
信頼度が無いと判定された測定成分の前記第３ステップの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を前記誤差指標の大きさに応じて小さくする手段、
を備えたことを特徴とする位置検出装置。
【請求項１７】
前記GPSデータ判定部は、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定する信頼度判定部、
信頼度ありと判定された成分の誤差指標を算出する誤差指標計算部、
を備えることを特徴とする請求項１６記載の位置検出装置。
【請求項１８】
前記誤差指標計算部は、前記信頼度が無いと判定された前記成分の誤差指標を、該成分が前記補正処理に貢献しないように大きな値にする、
ことを特徴とする請求項１７記載の位置検出装置。
【請求項１】
車両の現在位置を検出する位置検出方法において、
所定の周期で自律航法により車両の位置を計算し、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくする、
ことを特徴とする位置検出方法。
【請求項２】
車両の現在位置を検出する位置検出方法において、
所定の周期で自律航法により車両の位置を計算し、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わず、
全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくする、
ことを特徴とする位置検出方法。
【請求項３】
車両の現在位置を検出する位置検出方法において、
所定の周期で自律航法により車両の位置を計算し、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする、
ことを特徴とする位置検出方法。
【請求項４】
前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する、
ことを特徴とする請求項１記載の位置検出方法。
【請求項５】
前記信頼度が無いと判定された前記成分の誤差指標を、該成分が前記補正処理に貢献しないように大きな値にする、
ことを特徴とする請求項１乃至３記載の位置検出方法。
【請求項６】
カルマンフィルタ処理に基づいて前記補正処理を行い、
前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該共分散行列により各測定成分の前記補正処理における貢献度を該誤差指標に応じて小さくする、
ことを特徴とする請求項５記載の位置検出方法。
【請求項７】
車両の現在位置を検出する位置検出方法において、
車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、
前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第２ステップ、
GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第３ステップ、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出する第４ステップ、
信頼度が無いと判定された測定成分の前記第３ステップの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を前記誤差指標の大きさに応じて小さくする第５ステップ、
を備えたことを特徴とする位置検出方法。
【請求項８】
前記第４ステップにおいて、前記信頼度が無いと判定された前記成分の誤差指標を該成分が前記補正処理に貢献しないように大きな値にし、
前記第３ステップにおいて、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、各成分の前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該演算により各測定成分の前記補正処理における貢献度を該誤差指標に応じて小さくする、
ことを特徴とする請求項７記載の位置検出方法。
【請求項９】
車両の現在位置を検出する位置検出装置において、
所定の周期で自律航法により車両の位置を計算する自律航法部、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて、前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、
前記補正部は、信頼度が無いと判定された測定成分の前記補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の前記補正処理における貢献度を前記誤差指標の大きさに応じて小さくする手段、
を備えることを特徴とする位置検出装置。
【請求項１０】
車両の現在位置を検出する位置検出装置において、
所定の周期で自律航法により車両の位置を計算する自律航法部、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記補正部は、
測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御する手段、
全ての測定成分が信頼度有りと判定された場合には前記補正処理を行うと共に、該補正処理における各測定成分の貢献度を前記誤差指標に応じて小さくする手段、
を備えたことを特徴とする位置検出装置。
【請求項１１】
車両の現在位置を検出する位置検出装置において、
所定の周期で自律航法により車両の位置を計算する自律航法部、
前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部、
を備え、前記補正部は、
GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わないようにする手段、
グループの全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする手段、
を備えたことを特徴とする位置検出装置。
【請求項１２】
前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する第２の補正部、
を備えることを特徴とする請求項９乃至１１記載の位置検出装置。
【請求項１３】
前記GPSデータ判定部は、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定する信頼度判定部、
信頼度ありと判定された成分の誤差指標を算出する誤差指標計算部、
を備えることを特徴とする請求項９乃至１１記載の位置検出装置。
【請求項１４】
前記誤差指標計算部は、前記信頼度が無いと判定された前記成分の誤差指標を、該成分が前記補正処理に貢献しないように大きな値にする、
ことを特徴とする請求項１４記載の位置検出装置。
【請求項１５】
前記補正部は、カルマンフィルタ処理に基づいて前記補正処理を行い、前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該共分散行列により各測定成分の前記補正処理における貢献度を該誤差指標に応じて小さくするカルマンフィルタ処理、
を備えたことを特徴とする請求項９乃至１１記載の位置検出装置。
【請求項１６】
車両の現在位置を検出する位置検出装置において、
車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、
前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第１の補正部、
GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第２の補正部、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部を備え、前記第２補正部は、
信頼度が無いと判定された測定成分の前記第３ステップの補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を前記誤差指標の大きさに応じて小さくする手段、
を備えたことを特徴とする位置検出装置。
【請求項１７】
前記GPSデータ判定部は、
前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定する信頼度判定部、
信頼度ありと判定された成分の誤差指標を算出する誤差指標計算部、
を備えることを特徴とする請求項１６記載の位置検出装置。
【請求項１８】
前記誤差指標計算部は、前記信頼度が無いと判定された前記成分の誤差指標を、該成分が前記補正処理に貢献しないように大きな値にする、
ことを特徴とする請求項１７記載の位置検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【公開番号】特開２００９−１９９９２（Ｐ２００９−１９９９２Ａ）
【公開日】平成２１年１月２９日（２００９．１．２９）
【国際特許分類】
【出願番号】特願２００７−１８２８１８（Ｐ２００７−１８２８１８）
【出願日】平成１９年７月１２日（２００７．７．１２）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
【公開日】平成２１年１月２９日（２００９．１．２９）
【国際特許分類】
【出願日】平成１９年７月１２日（２００７．７．１２）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
[ Back to top ]