JP4964047B2 - 位置検出装置及び位置検出方法 - Google Patents

位置検出装置及び位置検出方法 Download PDF

Info

Publication number
JP4964047B2
JP4964047B2 JP2007182818A JP2007182818A JP4964047B2 JP 4964047 B2 JP4964047 B2 JP 4964047B2 JP 2007182818 A JP2007182818 A JP 2007182818A JP 2007182818 A JP2007182818 A JP 2007182818A JP 4964047 B2 JP4964047 B2 JP 4964047B2
Authority
JP
Japan
Prior art keywords
vehicle
component
gps
autonomous navigation
reliability
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2007182818A
Other languages
English (en)
Other versions
JP2009019992A (ja
Inventor
隆行 渡辺
高之 星崎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Alpine Electronics Inc
Original Assignee
Alpine Electronics Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Alpine Electronics Inc filed Critical Alpine Electronics Inc
Priority to JP2007182818A priority Critical patent/JP4964047B2/ja
Priority to US12/166,062 priority patent/US8510044B2/en
Publication of JP2009019992A publication Critical patent/JP2009019992A/ja
Application granted granted Critical
Publication of JP4964047B2 publication Critical patent/JP4964047B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Instructional Devices (AREA)
  • Traffic Control Systems (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Description

本発明は、車両の現在位置を検出する位置検出装置および位置検出方法に関わり、特に、自律航法により計算される位置データの精度を向上できる位置検出装置および位置検出方法に関する。
車載用ナビゲーション装置は、自律航法センサーを用いた自律航法（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を次式
Figure 0004964047
 により計算する(ステップ１０３)。ただし、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をそれぞれ計算する(ステップ５０７)。
Figure 0004964047
２）上下方向速度の誤差指標計算処理
図１４は上下方向速度の誤差指標計算処理フローである。
信頼度判定部１５ａの上下方向速度誤差指標計算部３６(図５)は、上下方向速度信頼度フラグがオンであるかチェックし(ステップ６０１)、オフであれば精度が悪いから上下方向速度の誤差指標δ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を計算する(ステップ１０４)。
Figure 0004964047
（Ｄ）位置検出装置の処理
図１８は図２の位置検出装置の全体の処理フローである。
最初、自律航法部１２に３次元車両位置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はセンサー座標系から車両固定座標系への座標変換行列要素であり、それぞれ次式により表現される。
Figure 0004964047
また、本発明のカルマンフィルタの観測式は
ΔＺ(k)＝Ｈ(k)δＸ(k)+ｖ(k) (18)
で表される。(18)式の観測行列Ｈは図２７に示す行列で表される。図２７において、観測行列Ｈの行列部分(1)は10Hz周期での速度誤差δVbxを計算する部分、(2)は10Hz周期での停車時における角速度オフセット誤差bwzを計算する部分、(3)は1Hz周期でのGPSの車両位置誤差δN,δE,δDおよび車両速度誤差δvnx,δvny,δvnzを計算する部分である。なお、観測行列Ｈは
Figure 0004964047
 と表現する。（1）は観測行列Ｈの第１行を、（2）は観測行列Ｈの第２行、（3）は観測行列Ｈの第3〜第8行を意味している。
カルマンフィルタは、Ｚ(t) (=δＺ(t)) が観測できるタイミングで(18)式によりＺ(t)(=δＺ(t))を計算し、計算した値と観測された値の差に基づいてＸ(t)(＝δＸ(t))を推定し、以後、次のZ（t)が観測されるまでＸ(t)を(17)式により更新する。そして、Ｚ（t)が観測されると上記差を再び演算し、該差に基づいてＸ(t)(＝δＸ(t))を推定し、以後、同様の処理を繰り返す。
観測行列Ｈの行列部分(1)は図１８の処理ステップ１１０７の第１補正処理において使用するカルマンフィルタの観測行列Ｈ1を構成し、
Figure 0004964047
 である。また、観測行列Ｈの行列部分(1)、（3）は図１８の処理ステップ１１０８の第２補正処理において使用するカルマンフィルタの観測行列Ｈ2を構成し、
Figure 0004964047
 である。また、観測行列Ｈの行列部分(1)、（2）は図１８の処理ステップ１１１０の第３補正処理において使用するカルマンフィルタの観測行列Ｈ3を構成し、
Figure 0004964047
 である。また、観測行列Ｈの行列部分(1)、（2）、(3)は図１８の処理ステップ１１１１の第４補正処理において使用するカルマンフィルタの観測行列Ｈ4を構成し、
Figure 0004964047
 である。
カルマンフィルタは、入力をＺ(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を用いて次式
Figure 0004964047
 で表現される。
誤差指標のカルマンフィルタ処理に対する貢献度は、カルマンゲインＫ(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フィルタ
１６ カルマンフィルタ処理部
２１ 第１の補正部
２２ 第２の補正部

Claims (13)

  1. 車両の現在位置を検出する位置検出方法において、
    所定の周期で自律航法により車両の位置を計算し、
    前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定し、
    信頼度ありと判定された成分の誤差指標を算出すると共に、信頼度なしと判定された成分の誤差指標を最大の一定値とし、
    前記誤差指標が大きいほど前記補正処理における貢献度が低くなるように補正処理を行い、信頼度が無いと判定された測定成分の該補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を誤差指標が大きいほど小さくする、
    ことを特徴とする位置検出方法。
  2. 車両の現在位置を検出する位置検出方法において、
    所定の周期で自律航法により車両の位置を計算し、
    前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
    測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わず、
    全ての測定成分が信頼度有りと判定された場合には、前記誤差指標が大きい測定成分ほど前記補正処理における貢献度が低くなるように該補正処理を行う、
    ことを特徴とする位置検出方法。
  3. 車両の現在位置を検出する位置検出方法において、
    所定の周期で自律航法により車両の位置を計算し、
    前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行い、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出し、
    GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わず、グループ全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする、
    ことを特徴とする位置検出方法。
  4. 前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する、
    ことを特徴とする請求項１乃至３記載の位置検出方法。
  5. カルマンフィルタ処理に基づいて前記補正処理を行い、
    前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該共分散行列により各測定成分の前記補正処理における貢献度を該誤差指標が大きいほど小さくする、
    ことを特徴とする請求項１乃至３記載の位置検出方法。
  6. 車両の現在位置を検出する位置検出方法において、
    車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、
    前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第２ステップ、
    GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第３ステップ、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定する第４ステップ、
    信頼度ありと判定された成分の誤差指標を算出すると共に、信頼度なしと判定された成分の誤差指標を最大の一定値とする第５ステップ、
    前記誤差指標が大きいほど前記補正処理における貢献度が低くなるように前記第３ステップの補正処理を行い、信頼度が無いと判定された測定成分の該補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を誤差指標が大きいほど小さくする第６ステップ、
    を備えたことを特徴とする位置検出方法。
  7. 前記第３ステップにおいて、カルマンフィルタ処理に基づいて前記補正処理を行い、
    各測定成分の前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該演算により各測定成分の前記補正処理における貢献度を該誤差指標が大きいほど小さくする、
    ことを特徴とする請求項６記載の位置検出方法。
  8. 車両の現在位置を検出する位置検出装置において、
    所定の周期で自律航法により車両の位置を計算する自律航法部、
    前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて、前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定するGPSデータ判定部、
    信頼度ありと判定された成分の誤差指標を算出すると共に、信頼度なしと判定された成分の誤差指標を最大の一定値とする誤差指標計算部、を備え、
    前記補正部は、前記誤差指標が大きいほど前記補正処理における貢献度が低くなるように補正処理を行い、信頼度が無いと判定された測定成分の該補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を誤差指標が大きいほど小さくする手段、
    を備えることを特徴とする位置検出装置。
  9. 車両の現在位置を検出する位置検出装置において、
    所定の周期で自律航法により車両の位置を計算する自律航法部、
    前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定するGPSデータ判定部、
    信頼度ありと判定された成分の誤差指標を算出すると共に、信頼度なしと判定された成分の誤差指標を最大の一定値とする誤差指標計算部、を備え、
    前記補正部は、
    測定成分が１つでも信頼度が無いと判定された場合には前記補正処理を行わないよう制御する手段、
    全ての測定成分が信頼度有りと判定された場合には、前記誤差指標が大きい測定成分ほど前記補正処理における貢献度が低くなるように該補正処理を行う手段、
    を備えたことを特徴とする位置検出装置。
  10. 車両の現在位置を検出する位置検出装置において、
    所定の周期で自律航法により車両の位置を計算する自律航法部、
    前記周期より長いGPSレシーバの測位周期で該GPSレシーバにより測定される車両の各方向位置成分及び速度成分を用いて前記自律航法による位置計算結果及び該自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角を補正する補正処理を行なう補正部、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定すると共に、信頼度ありと判定された成分の誤差指標を算出するGPSデータ判定部、
    を備え、前記補正部は、
    GPS測定成分を速度成分グループと位置成分グループに分け、グループ毎に、測定成分が１つでも信頼度が無いと判定された場合には該グループの測定成分を用いた補正処理を行わないようにする手段、
    グループの全ての測定成分が信頼度有りと判定された場合に該グループの測定成分を用いた補正処理を行うと共に、該補正処理における各測定成分の貢献度を誤差指標に応じて小さくする手段、
    を備えたことを特徴とする位置検出装置。
  11. 前記自律航法による位置計算周期より長く、かつ、前記GPS周期以下の周期で、移動距離検出センサーの出力信号を用いて車両速度を測定し、該車両速度と前記自律航法で計算した前記車両速度との速度差に基づいて、自律航法で用いる前記車両速度、前記ピッチ角、前記センサーの取り付け姿勢角を補正する第２の補正部、
    を備えることを特徴とする請求項８乃至１０記載の位置検出装置。
  12. 前記補正部は、カルマンフィルタ処理に基づいて前記補正処理を行い、前記誤差指標を用いて該カルマンフィルタ処理に用いる測定誤差の共分散行列を演算し、該共分散行列により各測定成分の前記補正処理における貢献度を該誤差指標が大きいほど小さくする、
    ことを特徴とする請求項８乃至１０記載の位置検出装置。
  13. 車両の現在位置を検出する位置検出装置において、
    車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、
    前記移動距離検出部の出力信号を用いて前記第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第１の補正部、
    GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第２の補正部、
    前記GPSレシーバにより測定される車両の各方向の位置成分及び速度成分の信頼度を判定するGPSデータ判定部、
    信頼度ありと判定された成分の誤差指標を算出すると共に、信頼度なしと判定された成分の誤差指標を最大の一定値とする誤差指標計算部、
    を備え、前記第２補正部は、前記誤差指標が大きいほど前記補正処理における貢献度が低くなるように補正処理を行い、信頼度が無いと判定された測定成分の該補正処理における貢献度を零あるいは略零にし、信頼度有りと判定された測定成分の該補正処理における貢献度を誤差指標が大きいほど小さくする手段、
    を備えたことを特徴とする位置検出装置。
JP2007182818A 2007-07-12 2007-07-12 位置検出装置及び位置検出方法 Active JP4964047B2 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2007182818A JP4964047B2 (ja) 2007-07-12 2007-07-12 位置検出装置及び位置検出方法
US12/166,062 US8510044B2 (en) 2007-07-12 2008-07-01 Position sensing device and method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2007182818A JP4964047B2 (ja) 2007-07-12 2007-07-12 位置検出装置及び位置検出方法

Publications (2)

Publication Number Publication Date
JP2009019992A JP2009019992A (ja) 2009-01-29
JP4964047B2 true JP4964047B2 (ja) 2012-06-27

Family

ID=40253846

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2007182818A Active JP4964047B2 (ja) 2007-07-12 2007-07-12 位置検出装置及び位置検出方法

Country Status (2)

Country Link
US (1) US8510044B2 (ja)
JP (1) JP4964047B2 (ja)

Families Citing this family (62)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7856336B2 (en) 2008-06-11 2010-12-21 Trimble Navigation Limited Forward-looking altitude detector
JP5328252B2 (ja) * 2008-07-30 2013-10-30 アルパイン株式会社 ナビゲーションシステムの位置検出装置および位置検出方法
JP5142047B2 (ja) * 2009-02-26 2013-02-13 アイシン・エィ・ダブリュ株式会社 ナビゲーション装置及びナビゲーション用プログラム
JP5051550B2 (ja) * 2009-02-26 2012-10-17 アイシン・エィ・ダブリュ株式会社 ナビゲーション装置及びナビゲーション用プログラム
JP5736106B2 (ja) * 2009-05-19 2015-06-17 古野電気株式会社 移動状態検出装置
JP5691145B2 (ja) * 2009-08-10 2015-04-01 ソニー株式会社 車両経路判定方法およびナビゲーション装置
TWI484207B (zh) * 2009-11-24 2015-05-11 Inst Information Industry 用於一移動設備之定位裝置、定位方法及其電腦程式產品
EP2336721A1 (en) * 2009-12-21 2011-06-22 Converteam Technology Ltd Fault detection methods
US8849474B2 (en) * 2009-12-25 2014-09-30 Yamaha Hatsudoki Kabushiki Kaisha Rider characteristic determining apparatus, and a saddle riding type vehicle having the same
US9234760B2 (en) * 2010-01-29 2016-01-12 Blackberry Limited Portable mobile transceiver for GPS navigation and vehicle data input for dead reckoning mode
US8700324B2 (en) * 2010-08-25 2014-04-15 Caterpillar Inc. Machine navigation system having integrity checking
US9199616B2 (en) 2010-12-20 2015-12-01 Caterpillar Inc. System and method for determining a ground speed of a machine
JP6094026B2 (ja) * 2011-03-02 2017-03-15 セイコーエプソン株式会社 姿勢判定方法、位置算出方法及び姿勢判定装置
JP5811558B2 (ja) * 2011-03-22 2015-11-11 富士通株式会社 移動端末及び移動経路算出プログラム
JP2012215491A (ja) * 2011-04-01 2012-11-08 Seiko Epson Corp 位置算出方法及び位置算出装置
JP5742450B2 (ja) * 2011-05-10 2015-07-01 セイコーエプソン株式会社 位置算出方法及び位置算出装置
JP5724676B2 (ja) * 2011-06-27 2015-05-27 富士通株式会社 携帯端末装置、速度算出方法及び速度算出プログラム
US9151613B2 (en) * 2011-08-12 2015-10-06 Qualcomm Incorporated Methods and apparatus for detecting, measuring, and mitigating effects of moving an inertial navigation device's cradle
US10151843B2 (en) 2011-11-22 2018-12-11 Radio Systems Corporation Systems and methods of tracking position and speed in GNSS applications
US20130127658A1 (en) 2011-11-22 2013-05-23 Radio Systems Corporation Method and Apparatus to Determine Actionable Position and Speed in GNSS Applications
JP5810898B2 (ja) * 2011-12-26 2015-11-11 株式会社デンソー 車両姿勢推定装置、及び、盗難通知装置
JP2013145168A (ja) * 2012-01-13 2013-07-25 Denso Corp 車載用ジャイロの角速度誤差補正装置
US9057615B2 (en) * 2012-10-26 2015-06-16 Texas Instruments Incorporated Systems and methods for navigating using corrected yaw bias values
JP6020163B2 (ja) * 2012-12-28 2016-11-02 株式会社Ｊｖｃケンウッド 導出装置、導出方法、プログラム
JP6083741B2 (ja) * 2013-03-27 2017-02-22 アルパイン株式会社 Ｇｐｓ信頼度判定装置、位置検出装置及びナビゲーション装置
US9541420B2 (en) * 2013-04-03 2017-01-10 Caterpillar Inc. System for determining error in a sensed machine position
JP2015055534A (ja) * 2013-09-11 2015-03-23 株式会社リコー 情報処理装置、情報処理装置の制御プログラム及び情報処理装置の制御方法
JP2016033474A (ja) * 2014-07-31 2016-03-10 セイコーエプソン株式会社 位置算出方法及び位置算出装置
JP2016033473A (ja) * 2014-07-31 2016-03-10 セイコーエプソン株式会社 位置算出方法及び位置算出装置
JP6386839B2 (ja) * 2014-09-04 2018-09-05 日立建機株式会社 位置算出装置及び運搬車両
JP6191580B2 (ja) * 2014-10-28 2017-09-06 トヨタ自動車株式会社 移動体のセンサ校正方法
CN106324633B (zh) * 2015-06-26 2022-07-05 无线电通信系统公司 Gnss应用中跟踪位置及速度的系统及方法
US10018732B2 (en) * 2015-12-21 2018-07-10 Clarion Co., Ltd. Information processing apparatus and information processing system
JP6593879B2 (ja) * 2016-03-24 2019-10-23 日本電気株式会社 衛星測位システム、測位端末、測位方法、及びプログラム
US10339068B2 (en) * 2017-04-24 2019-07-02 Advanced Micro Devices, Inc. Fully virtualized TLBs
DE102017212506A1 (de) * 2017-07-20 2019-01-24 Bayerische Motoren Werke Aktiengesellschaft Verfahren zur Ermittlung von Höhen-Information für ein Fahrzeug
US10473790B2 (en) 2017-11-17 2019-11-12 Swift Navigation, Inc. Systems and methods for distributed dense network processing of satellite positioning data
KR102005380B1 (ko) * 2017-11-23 2019-07-30 동국대학교 경주캠퍼스 산학협력단 도보 네비게이션 방법
US10578747B2 (en) 2017-12-14 2020-03-03 Swift Navigation, Inc. Systems and methods for reduced-outlier satellite positioning
JP7143722B2 (ja) * 2018-10-31 2022-09-29 トヨタ自動車株式会社 自車位置推定装置
KR101996623B1 (ko) * 2018-12-11 2019-07-04 주식회사 맵퍼스 Gps 궤적 품질 지표를 이용한 고정밀 지도 데이터 구축 방법 및 시스템
DE102018222663A1 (de) * 2018-12-20 2020-06-25 Robert Bosch Gmbh Verfahren zum adaptiven Ermitteln eines Integritätsbereichs einer Parameterschätzung
US11747142B2 (en) * 2019-04-30 2023-09-05 Stmicroelectronics, Inc. Inertial navigation system capable of dead reckoning in vehicles
KR20210152549A (ko) 2019-05-01 2021-12-15 스위프트 내비게이션, 인크. 고-무결성 위성 포지셔닝을 위한 시스템 및 방법
CN114502987A (zh) * 2019-08-01 2022-05-13 斯威夫特导航股份有限公司 用于高斯过程增强的gnss校正生成的系统和方法
CN110501013B (zh) * 2019-08-07 2023-09-05 腾讯科技（深圳）有限公司 位置补偿方法、装置及电子设备
WO2021202004A2 (en) 2020-02-14 2021-10-07 Swift Navigation, Inc. System and method for reconverging gnss position estimates
FR3107589B1 (fr) * 2020-02-21 2022-03-18 Commissariat Energie Atomique Procédé de détermination de la position et de l’orientation d’un véhicule.
WO2021252638A1 (en) 2020-06-09 2021-12-16 Swift Navigation, Inc. System and method for satellite positioning
CN116261676A (zh) 2020-07-13 2023-06-13 斯威夫特导航股份有限公司 用于确定gnss定位校正的系统和方法
WO2022046317A2 (en) 2020-07-17 2022-03-03 Swift Navigation, Inc. System and method for providing gnss corrections
KR20220078831A (ko) * 2020-12-04 2022-06-13 주식회사 에이치엘클레무브 운전자 보조 시스템 및 운전자 보조 방법
US11550067B2 (en) 2020-12-17 2023-01-10 Swift Navigation, Inc. System and method for fusing dead reckoning and GNSS data streams
JP7177862B2 (ja) * 2021-01-04 2022-11-24 本田技研工業株式会社 位置決定装置
US20230035856A1 (en) * 2021-07-20 2023-02-02 Cambridge Mobile Telematics Inc. Identifying unreliable global navigation satellite system (gnss) data
US11733397B2 (en) 2021-07-24 2023-08-22 Swift Navigation, Inc. System and method for computing positioning protection levels
US11693120B2 (en) 2021-08-09 2023-07-04 Swift Navigation, Inc. System and method for providing GNSS corrections
WO2023167899A1 (en) 2022-03-01 2023-09-07 Swift Navigation, Inc. System and method for fusing sensor and satellite measurements for positioning determination
WO2023167916A1 (en) 2022-03-01 2023-09-07 Swift Navigation, Inc. System and method for detecting outliers in gnss observations
DE102022205872A1 (de) 2022-06-09 2023-12-14 Continental Automotive Technologies GmbH Verfahren und Vorrichtung zum Ermitteln eines Fahrzeugzustands eines Fahrzeugs
WO2024050094A1 (en) 2022-09-01 2024-03-07 Swift Navigation, Inc. System and method for determining gnss corrections
WO2024058999A1 (en) 2022-09-12 2024-03-21 Swift Navigation, Inc. System and method for gnss correction transmission

Family Cites Families (30)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH01316607A (ja) * 1988-03-30 1989-12-21 Furuno Electric Co Ltd 航法装置
DE69206073T2 (de) * 1991-07-09 1996-06-27 Pioneer Electronic Corp GPS-Navigationssystem mit lokaler Geschwindigkeits- und Richtungserfassung und mit PDOP-Genauigkeitsbewertung.
JP2981025B2 (ja) * 1991-07-09 1999-11-22 パイオニア株式会社 Ｇｐｓデータのフィルタリング処理装置
US5394333A (en) * 1991-12-23 1995-02-28 Zexel Usa Corp. Correcting GPS position in a hybrid naviation system
JPH08297033A (ja) * 1995-04-27 1996-11-12 Omron Corp ナビゲーション装置
JPH08334338A (ja) 1995-06-09 1996-12-17 Zanavy Informatics:Kk Ｇｐｓナビゲーション装置
US5774829A (en) * 1995-12-12 1998-06-30 Pinterra Corporation Navigation and positioning system and method using uncoordinated beacon signals in conjunction with an absolute positioning system
US5745868A (en) * 1995-12-26 1998-04-28 Motorola, Inc. Method for rapid recovery from dead reckoning system heading loss
JPH09196691A (ja) * 1996-01-19 1997-07-31 Matsushita Electric Ind Co Ltd ナビゲーション装置
JPH1194573A (ja) * 1997-09-16 1999-04-09 Bio Oriented Technol Res Advancement Inst 移動体用位置姿勢測定装置
JPH11149326A (ja) * 1997-11-17 1999-06-02 Mitsubishi Electric Corp データ処理装置
JP3449240B2 (ja) * 1998-09-24 2003-09-22 株式会社デンソー 車両用現在位置検出装置、車両用現在位置表示装置、ナビゲーション装置および記録媒体
JP3571305B2 (ja) 2000-03-24 2004-09-29 クラリオン株式会社 カルマンフィルタの誤差推定値を加味した２ｄｒｍｓを出力するｇｐｓレシーバ
EP1136838B1 (en) * 2000-03-24 2005-09-28 CLARION Co., Ltd. GPS receiver capable of calculating accurate 2DRMS
JP2001280974A (ja) * 2000-03-28 2001-10-10 Clarion Co Ltd Ｄｒセンササンプリング機能付きｇｐｓレシーバ
US6735523B1 (en) * 2000-06-19 2004-05-11 American Gnc Corp. Process and system of coupled real-time GPS/IMU simulation with differential GPS
JP3570372B2 (ja) * 2000-11-08 2004-09-29 株式会社デンソー 車両用現在位置検出装置、車両用現在位置表示装置、ナビゲーション装置および記録媒体
US20020158796A1 (en) * 2001-04-25 2002-10-31 Ian Humphrey Integrated GPS and IGS system and method
JP2002333332A (ja) * 2001-05-08 2002-11-22 Pioneer Electronic Corp ハイブリッド処理方法及び装置、ナビゲーションシステム並びにコンピュータプログラム
US6801855B1 (en) * 2002-06-28 2004-10-05 Garmin Ltd. Systems and methods with integrated GPS and dead reckoning capabilities
JP4191499B2 (ja) * 2003-02-03 2008-12-03 パイオニア株式会社 車載装置
US7305303B2 (en) * 2004-03-02 2007-12-04 Honeywell International Inc. Personal navigation using terrain-correlation and/or signal-of-opportunity information
US7831354B2 (en) * 2004-03-23 2010-11-09 Continental Teves, Inc. Body state estimation of a vehicle
US7031816B2 (en) * 2004-03-23 2006-04-18 Continental Teves, Inc. Active rollover protection
US7245215B2 (en) * 2005-02-10 2007-07-17 Pinc Solutions Position-tracking device for position-tracking system
JP2007064853A (ja) * 2005-08-31 2007-03-15 Hitachi Ltd 複合測位による移動体の測位制御装置、システムおよびプログラム
US20080071476A1 (en) * 2006-09-19 2008-03-20 Takayuki Hoshizaki Vehicle dynamics conditioning method on MEMS based integrated INS/GPS vehicle navigation system
US7702459B2 (en) * 2006-10-17 2010-04-20 Alpine Electronics, Inc. GPS accuracy adjustment to mitigate multipath problems for MEMS based integrated INS/GPS navigation systems
JP4925909B2 (ja) * 2007-05-02 2012-05-09 アルパイン株式会社 位置検出装置及び位置検出方法
US8433514B1 (en) * 2012-05-17 2013-04-30 Trimble Navigation, Limited Navigation sensor mounting-angle calibration

Also Published As

Publication number Publication date
US8510044B2 (en) 2013-08-13
US20090018772A1 (en) 2009-01-15
JP2009019992A (ja) 2009-01-29

Similar Documents

Publication Publication Date Title
JP4964047B2 (ja) 位置検出装置及び位置検出方法
JP4781300B2 (ja) 位置検出装置および位置検出方法
CN106289275B (zh) 用于改进定位精度的单元和方法
US8775063B2 (en) System and method of lane path estimation using sensor fusion
JP5328252B2 (ja) ナビゲーションシステムの位置検出装置および位置検出方法
JP5419665B2 (ja) 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム
JP4925909B2 (ja) 位置検出装置及び位置検出方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
US11079494B2 (en) Positioning device
US11747142B2 (en) Inertial navigation system capable of dead reckoning in vehicles
JP5602070B2 (ja) 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム
TWI522258B (zh) Based on electronic map, global navigation satellite system and vehicle motion detection technology Lane identification method
KR102360465B1 (ko) 항법용 관성센서 캘리브레이션 방법
KR102371985B1 (ko) 관성센서 캘리브레이션 방법
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
JP2016218015A (ja) 車載センサ補正装置、自己位置推定装置、プログラム
CN113074757A (zh) 车载惯导安装误差角的标定方法
CN113063441B (zh) 里程计累计推算误差的数据源纠正方法及装置
JP2018112520A (ja) センサ誤差補正装置及び方法
JP2021018112A (ja) 自己位置推定装置
JP4884109B2 (ja) 移動軌跡算出方法、移動軌跡算出装置及び地図データ生成方法
JP7407947B2 (ja) 車両制御装置
WO2024075498A1 (ja) 自己位置推定方法及び自己位置推定装置
US20240321096A1 (en) Localization using position coordination of road signs

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20100427

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20110704

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20111116

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20111122

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20120105

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20120327

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20120327

R150 Certificate of patent or registration of utility model

Ref document number: 4964047

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20150406

Year of fee payment: 3