JP3727489B2 - ロケータ装置 - Google Patents
ロケータ装置 Download PDFInfo
- Publication number
- JP3727489B2 JP3727489B2 JP15439699A JP15439699A JP3727489B2 JP 3727489 B2 JP3727489 B2 JP 3727489B2 JP 15439699 A JP15439699 A JP 15439699A JP 15439699 A JP15439699 A JP 15439699A JP 3727489 B2 JP3727489 B2 JP 3727489B2
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- speed
- error
- scale factor
- azimuth
- 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.)
- Expired - Lifetime
Links
Images
Description
【０００１】
【発明の属する技術分野】
この発明は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測情報と、自律航法用複合センサの計測情報を用いて、車両の現在位置と進行方位および速度等を計算する複合航法を備えたロケータ装置に関するものである。
【０００２】
【従来の技術】
図２０は例えば、特開平８−６８６５５号公報に示された従来のロケータ装置の構成の一例を示すブロック図である。図において、１は距離センサ、２は相対方位センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、５は信号処理器である。また、信号処理器５内において、５２０は距離センサ１と相対方位センサ２の出力信号を基に相対軌跡を演算する相対軌跡演算、５２１はそれらの絶対軌跡を演算する絶対軌跡演算であり、５２２は距離センサ１の距離係数、相対方位センサ２のオフセット、絶対位置および絶対方位の各補正を行うカルマンフィルタである。
【０００３】
次に動作について説明する。
なお、相対軌跡演算５２０、絶対軌跡演算５２１、およびカルマンフィルタ５２２はマイクロコンピュータによる演算処理によって実現されるものであり、以下、これらについて説明する。ここで、図２１は自律航法のメインルーチンの演算処理を示す流れ図、図２２は進行方位変化と移動距離の演算処理を示す流れ図、図２３は相対軌跡演算処理を示す流れ図、図２４は絶対方位、絶対位置の演算処理を示す流れ図であり、図２５は自律航法とＧＰＳ航法を複合化するためのカルマンフィルタの演算処理を示す流れ図である。
【０００４】
まず、図２１のステップＳＴ２１０１において、車両の進行方位変化と移動距離を計算する。この車両の進行方位変化と移動距離の演算の処理内容の詳細を図２２に示す。この処理が開始されると、ステップＳＴ２２０１において相対方位センサ２から車両の進行方位変化をまず求める。次にステップＳＴ２２０２でオフセット補正量に前回からの移動距離Ｌを掛けたものを進行方位変化から差し引き、進行方位変化のオフセット補正を行う。その後、ステップＳＴ２２０３にて距離センサ１の信号に距離係数を掛けて移動距離を算出し、当該車両の進行方位変化と移動距離の演算処理を終了する。
【０００５】
ステップＳＴ２１０１による車両の進行方位変化と移動距離の演算処理が終了すると、次にステップＳＴ２１０２において相対軌跡演算処理を行う。この相対軌跡演算の処理内容の詳細を図２３に示す。この処理が開始されると、まずステップＳＴ２３０１にて進行方位変化を積算して相対方位を更新する。次にステップＳＴ２３０２に進み、この更新した相対方位およびステップＳＴ２２０３で求めた移動距離に基づく移動ベクトルのＸ，Ｙ成分をそれまでの相対位置座標に加算して新たな相対位置座標を求め、この相対軌跡演算処理を終了する。
【０００６】
ステップＳＴ２１０２による相対軌跡の演算処理が終了すると、次にステップＳＴ２１０３において絶対方位・絶対位置の演算処理を行う。この絶対方位・絶対位置演算の処理内容の詳細を図２４に示す。この処理が開始されると、まずステップＳＴ２４０１にて進行方位変化を基に絶対方位を更新する。次にステップＳＴ２４０２に進み、この更新した絶対方位およびステップＳＴ２２０３にて求めた移動距離によって絶対位置座標の更新を行い、この絶対方位・絶対位置の演算処理を終了する。このステップＳＴ２１０３の処理にて更新された絶対方位Ａと絶対位置は、次のステップＳＴ２１０４にけるＧＰＳ受信機３の観測情報との複合化処理に利用される。
【０００７】
そのステップＳＴ２１０４によるＧＰＳ受信機３の観測情報との複合化の処理内容の詳細を図２５に示す。図２５はカルマンフィルタ５２２の演算手順を示した流れ図であるが、自律航法とＧＰＳ航法とを複合化するためのカルマンフィルタ５２２について先に説明する。カルマンフィルタ５２２は信号生成過程と観測過程に分けられる。線形システムで表されるシステムの状態Ｘ（ｔ）に対して、観測行列Ｈで関係付けられるシステムの状態Ｘ（ｔ）の一部が観測できる場合に、フィルタはＸ（ｔ）の最適な推定値を与える。ここで、ωは信号生成過程で発生するシステム誤差であり、ｖは観測過程にて発生する観測誤差である。このフィルタの入力は観測値Ｙ（ｔ）であり、出力はＸ（ｔ）の最適推定値である。時刻ｔにおいて時刻ｔまでの情報を用いた状態ベクトルの推定量Ｘ（ｔ｜ｔ）は、次に示す式（１）により求められる。
【０００８】
【０００９】
ここで、上記式（１）において、Ｘ（ｔ｜ｔ−１）は時刻ｔ−１における時刻ｔでの状態ベクトルの予測量、Ｋ（ｔ）はカルマンゲインであり、それぞれ次の式（２）および式（３）により表される。
【００１０】
【００１１】
なお、上記式（３）において、Σ（ｔ｜ｔ−１）は時刻ｔ−１における時刻ｔでの誤差共分散の予測量である。そして、Σv は観測過程で発生する観測誤差ｖの分散であり、それぞれ次に示す式（４）および式（５）によって表される。
【００１２】
【００１３】
ここで、上記式（４）および式（５）において、Σw は信号過程で発生するシステム誤差ωの分散であり、Ｉは単位行列である。なお、添字のT は転置行列を意味し、-1は逆行列を意味する。さらに、システム誤差ωと観測誤差ｖは平均値０の白色ガウス雑音であり、互いに無相関である。
【００１４】
このようなカルマンフィルタ５２２は、まずシステムの状態Ｘと誤差共分散Σの初期値に適当な誤差を与えてやり、新しい観測が行われる度に、以上の計算を繰り返し行うものである。そこで、上記信号生成過程の定義についてまず説明する。自律航法でのカルマンフィルタ５２２は自律航法の誤差の補正を目的とするので、状態ベクトルの推定量Ｘの要素として、オフセット誤差εＧ、絶対方位誤差εＡ、距離係数誤差εＫ、絶対位置北方向誤差εＹ、および絶対位置東方向誤差εＸの５つの誤差値を定義する。そして、この誤差値の時間的な変化を与えるものが状態遷移行列Ｆである。
【００１５】
オフセット誤差εＧは、確定的な変化はなく、前回の誤差にノイズが付加されたものとして、次に示す式（６）により表される。
【００１６】
εＧt ＝εＧt-1 ＋ω0 ・・・（６）
【００１７】
絶対方位誤差εＡは、前回のオフセット誤差εＧに前回からの経過時間を乗算して求める方位誤差とノイズが付加されるものとして、次の式（７）によって表される。
【００１８】
εＡt ＝ＴεＧt-1 ＋εＡt-1 ＋ω1 ・・・（７）
【００１９】
距離係数誤差εＫは、確定的な変化はなく、前回の距離係数誤差εＫにノイズが付加されたものとして、次の式（８）によって表される。
【００２０】
εＫt ＝εＫt-1 ＋ω2 ・・・（８）
【００２１】
絶対位置北方向誤差εＹ、および絶対位置東方向誤差εＸは、前回の誤差εＹ，εＸに、方位誤差、距離誤差によって生じる誤差が付加されるものとして、次に示す式（９）および式（１０）により表される。なお、これら両式中のＡt は真の絶対方位、Ｌは前回からの移動距離、ｔは前回からの経過時間である。
【００２２】
εＹt ＝ｓｉｎ（Ａt ＋εＡt-1 ＋εＧt-1 ・ｔ／２）・Ｌ・（１＋εＫt-1 ）−ｓｉｎ（Ａt ）・Ｌ＋εＹt-1 ・・・（９）
εＸt ＝ｃｏｓ（Ａt ＋εＡt-1 ＋εＧt-1 ・ｔ／２）・Ｌ・（１＋εＫt-1 ）−ｃｏｓ（Ａt ）・Ｌ＋εＸt-1 ・・・（１０）
【００２３】
また、上記式（９）および式（１０）を状態量で偏微分し線形化すると、信号生成過程は、次に示す式（１１）〜式（１５）により表される。
【００２４】
【数１】
【００２５】
なお、上記式（１１）〜式（１５）において、Ａは真の絶対方位Ａt にセンサ誤差が加わったものであり、進行方位変化から求められる。また、ω0 はオフセット誤差（温度ドリフト等によるオフセット変動分）、ω1 は絶対方位誤差（ジャイロのスケールファクタ誤差）、ω2 は距離係数誤差（経年変化）、ω3 ，ω4 は絶対位置誤差を意味する。
【００２６】
次に観測過程について説明する。
観測値は自律航法とＧＰＳ航法の両出力の差により求める。それぞれの出力には誤差が含まれるため、自律航法の誤差とＧＰＳ観測情報の誤差の和が観測値として得られる。この観測値Ｙとシステムの状態Ｘを関連付け、次に示した式（１６）〜式（１９）のように定義する。
【００２７】
【数２】
【００２８】
上記式（１６）〜式（１９）において、行列の要素における添字DRt は、距離センサ１、相対方位センサ２からの出力信号に基づいて、時刻ｔに自律航法にて求められた値を意味し、同様にして、添字GPStは時刻ｔにＧＰＳ受信機３から出力される値を意味する。観測値Ｙt の要素（εＡDRt −εＡGPSt）は、自律航法によって求められた絶対方位と、ＧＰＳ受信機３から出力される方位の差、すなわち自律航法により求められた絶対方位には真の絶対方位とその誤差εＡDRt が含まれており、またＧＰＳ受信機３から出力される方位も真の絶対方位とその誤差εＡGPStが含まれていることから、それらの差を取ることによって求められるものである。
【００２９】
観測値Ｙt の要素（εＫDRt −εＫGPSt）は、自律航法により求められる速度とＧＰＳ受信機３から出力される速度の差から求まる距離係数誤差であり、（自律航法による速度−ＧＰＳ受信機による速度）／（自律航法による速度）により求められる。また、要素（εＹDRt −εＹGPSt）は自律航法により求められる絶対位置のＹ成分とＧＰＳ受信機３から出力される位置のＹ成分の誤差の差であり、要素（εＸDRt −εＸGPSt）は位置のＸ成分の誤差の差である。
【００３０】
また、観測誤差ｖはＧＰＳ受信機３の誤差であり、観測誤差ｖの各要素は、以下のように求められている。まず、ドップラ周波数の計測誤差とＨＤＯＰ（Ｈｏｒｉｚｏｎｔａｌ Ｄｉｌｕｔｉｏｎ ｏｆ Ｐｒｅｓｉｓｉｏｎ）の関係により、速度精度がドップラ周波数の計測誤差×ＨＤＯＰで求められ、速度精度と車両の速度から方位精度がｔａｎ-1（速度精度／車両の速度）で求められる。これら方位精度と速度精度をそれぞれ２乗することによって、要素（−εＡGPSt）および要素（−εＫGPSt）が求められる。また、ＧＰＳ受信機３における擬似距離の計算誤差ＵＥＲＥ（Ｕｓｅｒ Ｅｑｕｉｖａｌｅｎｔ Ｒａｎｇｉｎｇ Ｅｒｒｏｒ）と上記ＨＤＯＰとの関係から、ＵＥＲＥ×ＨＤＯＰにより測位精度が求められる。要素（−εＹGPSt）と要素（−εＸGPSt）は、この測位精度を２乗することによりそれぞれ求められる。
【００３１】
次に、カルマンフィルタ５２２の処理手順を図２５の流れ図に従って説明する。まずステップＳＴ２５０１において、前回の測位または予測計算からＴ１秒経過したか否かを判定する。この判定処理はＧＰＳ受信機３で２次元あるいは３次元測位が行われる度にステップＳＴ２５０３からステップＳＴ２５０９にて自律航法の誤差を補正する処理を行うが、ＧＰＳ受信機３で２次元あるいは３次元測位できない場合には誤差が大きくなるため、それに対応した誤差の予測計算をステップＳＴ２５１０，ＳＴ２５１１にて定期的に行うために設けられたものである。
【００３２】
前回の測位または予測計算からＴ１秒経過しておらず、ステップＳＴ２５０１による判定の結果がＮｏになると、ステップＳＴ２５０２に分岐して、ＧＰＳ受信機３からの観測情報があるか否かを判定する。ＧＰＳ受信機３からの観測情報があり、ステップＳＴ２５０２の判定結果がＹｅｓになると、ステップＳＴ２５０３以降のカルマンフィルタの演算処理に進む。ステップＳＴ２５０３ではまず、観測値Ｙの計算を行う。これは、ＧＰＳ受信機３から出力される速度、位置、および方位に関する観測情報と、自律航法におけるステップＳＴ２１０３の処理にて求めた絶対方位、絶対位置と、図示していない速度演算処理により距離センサ１の信号から求めた車両の速度とから、上記式（１６）〜式（１９）で示した観測値Ｙを計算するものであり、あわせて観測過程で発生する観測誤差ｖをＧＰＳ受信機３の観測情報等をもとに計算する。
【００３３】
次にステップＳＴ２５０４において、状態遷移行列Ｆの計算を行う。これは前回の状態遷移行列Ｆの計算時点からの移動距離Ｌ、および経過時間ｔと、ステップＳＴ２４０１で求めた絶対方位Ａとから、上記式（１１）〜式（１５）に示す状態遷移行列Ｆを求めるものである。このようにして計算した観測値Ｙおよび状態遷移行列Ｆを基に、上記式（１）〜式（５）による計算を行ってシステムの状態Ｘを求める。すなわち、ステップＳＴ２５０５で誤差共分散の予測計算を行い、ステップＳＴ２５０６ではカルマンゲインの計算を行う。次にステップＳＴ２５０７にて誤差共分散を計算を行い、その後、ステップＳＴ２５０８において、カルマンゲインおよび観測値に基づいて状態ベクトルの推定量を求める。この状態ベクトルの推定量は、オフセット誤差、絶対方位誤差、距離係数誤差、絶対位置北方向誤差および絶対位置東方向誤差を表している。
【００３４】
ステップＳＴ２５０８までの処理が終了するとステップＳＴ２５０９に進み、その計算結果に基づいて自律航法の誤差の修正を行う。すなわち、相対方位センサ３のオフセット補正量は、前回のオフセット補正量よりオフセット誤差εＧを減算することによって、距離センサ１の距離係数は、前回の距離係数に１から距離係数誤差εＫを引いたものを乗算することによって、絶対方位は前回の絶対方位より絶対方位誤差εＡを減算することによって、北方向絶対位置は前回の北方向絶対位置より絶対位置北方向誤差εＹを、東方向絶対位置は前回の東方向絶対位置より絶対位置東方向誤差εＸをそれぞれ減算することによって、自律航法の誤差補正が行われる。これらの処理は、ＧＰＳ受信機３からの観測情報を受信する度に繰り返し実行して、上記誤差修正を行うものである。
【００３５】
また、信号生成過程で発生するシステム誤差ωの行列の各要素、すなわち、オフセット誤差ω0 、絶対方位誤差ω1 、距離係数誤差ω2 、絶対位置誤差ω3 ，ω4 は、それぞれ次のように求められる。まず、オフセット誤差ω0 と絶対方位誤差ω1 には規定値を設定する。その後、車両の挙動状態に応じて発生する所定の誤差発生要因毎の誤差分散の総和として絶対方位誤差ω1 を再計算する。また、絶対位置誤差ω3 ，ω4 については、誤差共分散行列の要素（σAA 2 ）が所定値以下の場合のみに、絶対方位誤差による位置誤差の発生量の大きさをＸ，Ｙ軸に分割した値をもとに計算する。
【００３６】
また、従来のロケータ装置において、装置に内蔵した加速度センサから移動距離を計測するものが提案されている。加速度センサは、加速度ａACC が０の時でも、出力信号ＳＧACC が所定の電圧値（［ｍＶ］、以下、オフセットＯＦACC という）を示すように調整されたもので、加速時にはオフセットＯＦACCより大きな電圧値が、また減速時にはオフセットＯＦACC より小さな電圧値が出力される。この加速度センサの出力信号ＳＧACC から加速度ａACC を求めるには、次の式（２０）に示すように、出力信号ＳＧACC からオフセットＯＦACC を減算した後に、電圧値を加速度の単位に変換するスケールファクタＳＦACC （［ｍ／Ｓ2 ／ｍＶ］）を乗算すればよい。なお、式（２０）の添字ACC は加速度センサ関連のデータを指すものであり、ｉは時間を示すものである。また、ΔＴは加速度センサのサンプリング周期である。
【００３７】
ａACCi＝（ＳＧACCi−ＯＦACCi）×ＳＦACCi×ΔＴ ・・・（２０）
【００３８】
しかしながら、現実には温度変化等の要因によりオフセットＯＦACC とスケールファクタＳＦACC が変動するので、オフセットＯＦACC とスケールファクタＳＦACC を適宜適正な値に補正することが必要とされる。加速度センサの補正については、例えば特開平１０−３０７０３２号公報で提案されているので、ここではその内容を簡単に説明する。
【００３９】
図２６はそのような従来のロケータ装置の構成を示すブロック図であり、図において、１は距離センサ、２は相対方位センサ、３はＧＰＳ受信機、４は車両の前後方向の加速度を計測する上記加速度センサであり、５は信号処理器である。また、信号処理器５内において、５０は距離センサ１、相対方位センサ２、ＧＰＳ受信機３、加速度センサ４の各出力信号をインタフェースするためのインタフェース回路であり、５１はあらかじめメモリ領域に格納されている制御プログラムに従って、車両の挙動情報を推定するとともに、上記各センサの補正を行うマイクロコンピュータである。
【００４０】
次に動作について説明する。
ここで、図２７は上記マイクロコンピュータ５１の全体の処理動作の概要を示す流れ図であり、以下、この図２７に基づいて説明する。ロケータ装置に電源が投入されると、ステップＳＴ２７０１において、まず初期値設定が行われる。次にステップＳＴ２７０２で、加速度センサ４の出力信号に基づく前後方向の加速度の算出と車両の速度の推定を行う。次にステップＳＴ２７０３で、ＧＰＳ受信機３の観測した車両の現在位置、進行方位、速度（以下、ＧＰＳ位置、ＧＰＳ方位、ＧＰＳ速度という）等の観測情報を受信し、さらにステップＳＴ２７０４において、相対方位センサ２の出力信号から車両の進行方位変化を算出する。次にステップＳＴ２７０５に進み、これらの算出値に基づいて車両の走行距離を算出し、ＧＰＳ受信機３および各種センサの出力信号に基づいて車両の現在位置等を推定する。
【００４１】
その後、ステップＳＴ２７０６にて相対方位センサ２の出力信号に基づいて停車状態判定処理と発車状態判定処理を行うか、あるいは、ステップＳＴ２７０７にて相対方位センサ２の出力信号に基づいて停車時または走行中におけるオフセットの更新処理を行う。さらに、ステップＳＴ２７０８にて加速度センサ４の出力信号に基づく停車状態判定処理と発車状態判定処理を行うか、あるいはステップＳＴ２７０９にて加速度センサ４の出力信号に基づいて停車時または走行中のオフセットの更新処理を行う。次にステップＳＴ２７１０に進み、ＧＰＳ受信機３の観測データを基準にして、加速度センサ４の出力信号から推定した速度のリセット処理を行う。次にステップＳＴ２７１１において、ＧＰＳ受信機３の観測情報を基準にして加速度センサ４のスケールファクタ補正を行う。以後、ステップＳＴ２７０２からステップＳＴ２７１１の処理を繰り返し実行する。
【００４２】
次に、加速度センサ４に関する動作の説明を補足する。まずステップＳＴ２７０２において、加速度センサ４の出力信号ＳＧACCに基づく前後方向の加速度ａACCと車両の速度ＶｅｌACC を、上記式（２０）と次の式（２１）を用いてそれぞれ演算し、また、ステップＳＴ２７０５における走行距離Δｄ（［ｍ］）を、次に示す式（２２）にて演算する。
【００４３】
ＶｅｌACCi＝ＶｅｌACCI-1＋ａACCi×ΔＴ ・・・（２１）
Δｄ＝ＶｅｌACCi×ΔＴ＋１／２×ａACCi×ΔＴ2 ・・・（２２）
【００４４】
次に、ステップＳＴ２７０６あるいはステップＳＴ２７０８で行う停車状態判定処理は、加速度センサ４あるいは相対方位センサ２の出力信号の一定時間における標準偏差がそれぞれ所定値以下になった場合、あるいはＧＰＳ測位時にＧＰＳ速度が０ｋｍ／ｈであった場合に停車状態であると判定する。さらに、所定時間における加速度センサ４の出力信号の平均値が負となる減速状態の後で、加速度センサ４の出力信号が所定値以上変化した場合についても停車状態と判定する。一方、発車状態判定処理は、停車状態であると判定している場合に、加速度センサ４あるいは相対方位センサ２の一定時間における出力信号の変化量が所定値より大きくなった時、あるいはＧＰＳ測位中にＧＰＳ速度が０ｋｍ／ｈでなくなった時には発車状態であると判定する。
【００４５】
また、ステップＳＴ２７０９のオフセット値の更新は、前後方向の加速度が発生していない停車状態と等速度走行時に実施するもので、加速度センサ４の出力信号の平均値をオフセットとして設定する。ここで、等速度走行状態とは、加速度センサ４の出力信号により推定した速度が３０ｋｍ／ｈ以上かつ、推定した速度と進行方位変化の変化量が少ない状態であり、ＧＰＳ測位中にはさらにＧＰＳ速度の変化量も少ないことも判定条件に加えている。
【００４６】
ステップＳＴ２７１０では、次の式（２３）および式（２４）に示すように、加速度センサ４による推定速度ＶｅｌACC をＧＰＳ速度ＶｅｌGPS に基づいて修正する。ただし、ｋの最大値は１．０であるものとする。
【００４７】
ＶｅｌACCi＝ＶｅｌACCi＋（ＶｅｌGPSi−ＶｅｌACCi）×ｋ ・・・（２３）
ｋ＝ＶｅｌGPSi／３０ ・・・（２４）
【００４８】
ステップＳＴ２７１１のスケールファクタ補正処理は、等速度走行状態、かつ加速度センサ４の出力信号から算出された加速度の変化量が少ない状態（等加速度走行状態）であって、相対方位センサ２の出力信号から算出された角速度の変化量が少ない状態（等角加速度走行状態）、かつＧＰＳ速度の変化量である加速度の変化量が少ない状態（等加速度走行状態）である場合において実行するものであり、走行状態における加速度センサ４の推定速度ＶｅｌACCiの平均値とＧＰＳ速度ＶｅｌGPSiの平均値を比較して、次の式（２５）に示すように、ＧＰＳ速度の平均値ＶｅｌGPSavei に加速度センサ４の推定速度の平均値ＶｅｌACCavei が合うように加速度センサ４のスケールファクタＳＦACCiを修正する。
【００４９】
ＳＦACCi＝ＶｅｌGPSavei ／ＶｅｌACCavei ×ＳＦACCi-1 ・・・（２５）
【００５０】
【発明が解決しようとする課題】
従来のロケータ装置（特開平８−６８６５５号公報に示されたロケータ装置）は以上のように構成されているので、カルマンフィルタ５２２にてオフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を求めて自律航法におけるそれぞれの補正が行われ、カルマンフィルタ５２２におけるシステム誤差と観測誤差の精度に応じて、カルマンフィルタ５２２で求めるオフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差の精度が決まるものであり、観測誤差の算出に用いるＨＤＯＰとＵＥＲＥは、利用者が位置精度を推定するための指標であるので、その時々の誤差を直接表すことができず、観測誤差を高い精度で設定することがきないという課題があった。
【００５１】
また、システム誤差に関しては、オフセット誤差ω0 と絶対方位誤差ω1 には規定値を設定し、車両の挙動状態に応じて発生する所定の誤差発生要因毎の誤差分散の総和として、絶対方位誤差ω1 を再計算しているため、絶対方位誤差ω1 を高精度で設定できないという課題があり、また絶対位置誤差ω3 ，ω4 についても、誤差共分散行列の方位誤差に関する要素が所定値以下の場合のみに、精度が高くない絶対方位誤差によって位置誤差の発生量の大きさをＸ，Ｙ軸に分割した値として計算しているため、その精度も高く設定することができないという課題があった。
【００５２】
さらに、オフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を状態ベクトルとして同時に計算しているため、システム誤差と観測誤差のどれか一つの要素が不正確だった場合には、状態ベクトルの他の行列要素もカルマンゲインで決まる量だけ値に反映され誤差を生じるという問題があり、従って、このカルマンフィルタ５２２では、各誤差の特徴に合わせた検出および設定ができないという課題があった。
【００５３】
さらに、ＧＰＳ速度は低速走行時に精度が低下することがあるが、これに関する対策が距離係数誤差の計算では示されておらず、またＧＰＳ速度の分散は、距離センサ１の出力信号から計算した速度の分散より大きいので、計算される距離係数誤差の分散が大きくなると考えられ、一方、相対方位センサ２に関しては、オフセット誤差のみが補正対象となっており、相対方位センサ２としてジャイロを使用する場合には、標準値に対して最大で約±２０％も変動し得るスケールファクタに関して考慮していないため、スケールファクタ誤差が原因で生じた方位誤差もオフセット誤差として誤って補正してしまうといった課題もあった。
【００５４】
さらに、特開平１０−３０７０３２号公報に示されたロケータ装置は、加速度センサ４の出力信号から車両の速度を推定し、また加速度センサ４のオフセットに生じたドリフトや、当該ロケータ装置取付け時の傾きに起因する加速度センサ４のスケールファクタの重力影響を補正するために、ＧＰＳ速度を基準にして加速度センサ４のオフセットとスケールファクタを補正するもので、この補正を行う車両の状態としては、停車状態、等速度走行状態、等加速度状態、あるいは等角速度状態の車両が安定した走行状態であることが求められているが、信号機の多い市街地等では、車両が安定した走行状態になることが少ないことから、加速度センサ４のオフセットとスケールファクタの補正が不十分になるばかりか、車両の走行中にドリフトが大きく生じた時にオフセットの補正が不十分だった場合には、たとえ前後方向の加速度が一定であってもオフセットが徐々に変化して、出力信号からオフセットを減算した値が一定でなくなってしまい、車両が加速あるいは減速されている状態にあると誤認識して、さらに補正ができなくなるという課題があった。
【００５５】
また、車両が安定な走行状態になったときに、ＧＰＳ速度が３０ｋｍ／ｈ以上であれば、加速度センサ４による推定速度とスケールファクタの補正量が大きくなっていたため、ＧＰＳ速度の誤差の影響を受けて推定速度とスケールファクタの精度も低下するという課題もあった。
【００５６】
この発明は上記のような課題を解決するためになされたもので、距離センサの出力信号から車両の速度または移動距離を計測する際に、車両により大きく異なる距離センサのスケールファクタを、走行開始後に迅速に補正できるロケータ装置を得ることを目的とする。
【００５７】
また、この発明は、距離センサの出力から車両の速度または移動距離を計測する際に、車両によって大きく異なる距離センサのスケールファクタを、走行開始後に安定して補正できるロケータ装置を得ることを目的とする。
【００５８】
さらに、この発明は、ＧＰＳ受信機が非測位状態となり易い場所を走行していても、走行開始後に直ちにスケールファクタを補正できるロケータ装置を得ることを目的とする。
【００５９】
さらに、この発明は、車両への着脱を容易に行えるロケータ装置を得ることを目的とする。
【００６０】
さらに、この発明は、ＧＰＳ航法と自律航法の複合化に際して、一つの計算方法でＧＰＳ受信機の観測情報と自律航法用のセンサの計測情報から、車両の現在位置と進行方位を簡単、かつ高精度で推定できるロケータ装置を得ることを目的とする。
【００６１】
さらに、この発明は、ジャイロ等の角速度センサ、または相対方位センサのオフセットとスケールファクタの補正を適確に行えるロケータ装置を得ることを目的とする。
【００６２】
さらに、この発明は、スケールファクタの誤補正を防止できるロケータ装置を得ることを目的とする。
【００６３】
さらに、この発明は、ロケータ装置が複数の車両へ載せ換えられて使用される際に、他の車両へ載せ換えられたことを判定できるとともに、載せ換えられたと判定された場合には、現在の車両に係る補正を直ちに行えるロケータ装置を得ることを目的とする。
【００６４】
さらに、この発明は、他の車両へ載せ換えられたときに、車両に係る補正を毎回やり直すことが不要なロケータ装置を得ることを目的とする。
【００６５】
【課題を解決するための手段】
この発明に係るロケータ装置は、絶対位置観測手段と、距離センサを備えるとともに、距離センサの出力信号にスケールファクタを乗算して車両の速度を計算するシステムモデルと、絶対位置観測手段で観測した車両の速度とそのシステムモデルにおける速度との関係を表した観測モデルに基づいて、状態ベクトルの行列要素にシステムモデルにおける速度とスケールファクタを、観測値に絶対位置観測手段で観測した速度をそれぞれ設定したカルマンフィルタによって、状態ベクトルの行列要素である速度とスケールファクタを計算する速度計算手段を備え、速度計算手段におけるシステムモデルと観測モデルでは、距離センサの出力信号と絶対位置観測手段で観測した速度の各平均値を扱い、またその平均値の時間的変化に伴う勾配の差異を絶対位置観測手段で観測した速度の平均値に乗算した値を観測誤差として設定するようにしたものである。
【００６６】
この発明に係るロケータ装置は、速度計算手段において、そのカルマンフィルタの後段にローパスフィルタを設け、このローパスフィルタを通過させることによって、カルマンフィルタにおける状態ベクトルの行列要素である距離センサのスケールファクタの変動を小さくし、それを距離センサの補正値とするようにしたものである。
【００６９】
この発明に係るロケータ装置は、車両の進行方位変化に応じた信号を出力する相対方位センサと、この相対方位センサの出力信号から車両の単位時間毎の進行方位変化を計算する方位変化計算手段を設けるとともに、速度計算手段または速度推定手段による速度から求めた単位時間当たりの移動距離と方位変化計算手段で計算した単位時間毎の進行方位変化から車両の現在位置と進行方位を計算するシステムモデルと、絶対位置観測手段で観測した車両の現在位置とシステムモデルにおける現在位置の関係を表した観測モデルに基づいて、状態ベクトルの行列要素にシステムモデルにおける現在位置と進行方位を、観測値に絶対位置観測手段で観測した車両の現在位置をそれぞれ設定したカルマンフィルタにより、状態ベクトルの行列要素である車両の現在位置と進行方位を計算する位置・方位推定手段を設け、この位置・方位推定手段にて、所定距離以上離れた複数地点または所定時間経過毎の絶対位置観測手段と位置・方位推定手段によるそれぞれの現在位置の差異の変動量を観測誤差として設定し、また所定距離以上離れた複数地点毎の絶対位置観測手段と位置・方位推定手段による両進行方位の差異の平均値をシステム誤差の行列要素である方位誤差として設定し、さらに速度計算手段または速度推定手段と絶対位置観測手段による各速度の積分値の差異をシステム誤差の行列要素である距離誤差として設定するようにしたものである。
【００７０】
この発明に係るロケータ装置は、速度計算手段または速度推定手段で求めた車両の速度が０のときに相対方位センサの出力信号を平均したものをオフセットとするオフセット検出手段を設け、方位変化計算手段にて、相対方位センサの出力信号からオフセットを引いた後にスケールファクタを乗じて車両の単位時間毎の進行方位変化を計算するとともに、その進行方位変化を積算するシステムモデルと、位置・方位推定手段で推定した進行方位とシステムモデルにおける積算方位の関係を表した観測モデルに基づいて、状態ベクトルの行列要素にシステムモデルにおける相対方位センサのオフセットとスケールファクタおよび積算方位を、観測値に位置・方位推定手段で推定した進行方位をそれぞれ設定したカルマンフィルタにより、状態ベクトルの行列要素である相対方位センサのオフセットとスケールファクタを計算して相対方位センサの補正値とし、さらに、停車毎のオフセットの変動量からシステム誤差の行列要素であるオフセット誤差を、また車両旋回時に位置・方位推定手段と方位変化計算手段の各方位情報からそれぞれ求めた車両の旋回角の差異からシステム誤差の行列要素であるスケールファクタ誤差をそれぞれ設定して、位置・方位推定手段で用いたシステム誤差の行列要素である方位誤差を観測誤差として設定するようにしたものである。
【００７１】
この発明に係るロケータ装置は、方位変化計算手段が相対方位センサのオフセットの変動を監視して、カルマンフィルタにおける状態ベクトルの行列要素である当該相対方位センサのオフセットの変動が、あらかじめ定められた所定の値以上になったか否かによって、スケールファクタの補正の有効性を判定するようにしたものである。
【００７２】
この発明に係るロケータ装置は、距離センサのスケールファクタの補正を監視して、その変化が所定割合以上となった時にロケータ装置が他の車両に載せ換えられたと判断する機能を速度計算手段に持たせたものである。
【００７３】
この発明に係るロケータ装置は、距離センサのスケールファクタを記憶する複数のメモリ領域を有する記憶手段と、利用者の操作によって記憶先であるメモリ領域の番地を指定する記憶先指定手段とを設け、指定されたメモリ領域の番地に従って、記憶手段への距離センサのスケールファクタの読み書きを行うようにしたものである。
【００７４】
この発明に係るロケータ装置は、距離センサのスケールファクタとメモリアクセス時刻とを組にして記憶する複数のメモリ領域を有した記憶手段に保持されているスケールファクタを記憶先検索手段にて検索し、計算されたスケールファクタに近い値が記憶手段に記録されている場合にはそのメモリ領域の番地を、記録されていない場合には未使用メモリ領域の番地を、さらに未使用メモリ領域がない場合にはメモリアクセス時刻が最も古いメモリ領域の番地を検索する記憶先検索手段とを設け、この記憶先検索手段にて指定された番地のメモリ領域に、計算した距離センサのスケールファクタを読み書きするとともに、そのスケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を読み書きするようにしたものである。
【００７５】
【発明の実施の形態】
以下、この発明の実施の一形態を説明する。
実施の形態１．
図１はこの発明の実施の形態１によるロケータ装置の構成を示すブロック図である。図において、１は距離センサであり、車両の走行距離に応じてパルス信号を出力する。３は絶対位置観測手段としてのＧＰＳ受信機であり、衛星からの電波を受信するＧＰＳアンテナが接続されており、当該衛星から受信した電波により、少なくとも車両の速度を観測して出力する。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器である。
【００７６】
また、この信号処理器５内において、５１１は距離センサ１からのパルス信号と、ＧＰＳ受信機３からの観測情報とに基づいて、車両の速度を計算するとともに、距離センサ１の出力信号を距離に変換するためのスケールファクタを計算する速度計算手段であり、信号処理器５はこの速度計算手段５１１によって構成されている。
【００７７】
次に動作について説明する。
ここでは、信号処理器５の速度計算手段５１１の動作について図を用いて説明する。図２はこの信号処理器５のメインルーチンの処理内容を示す流れ図であり、図３は所定時間Δｔ毎に実行される割込処理１の処理内容を示す流れ図、図４は距離センサ１から信号が出力されたときに実行される割込処理２の処理内容を示す流れ図、図５はＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３の処理内容を示す流れ図である。
【００７８】
図２に示したメインルーチンは、信号処理器５の速度計算手段５１１の動作を管理するものである。まずステップＳＴ２０１で全ての処理を初期化する。次にステップＳＴ２０２において、割込処理１に係る処理開始のタイミングか否かを処理開始フラグを参照して判断する。その結果、当該処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ２０３へ進み、クリアされていれば処理開始タイミングでないとして、その処理開始フラグがセットされるまで待機する。ステップＳＴ２０３では次回の処理のために処理開始フラグをクリアした後、ステップＳＴ２０４に進む。ステップＳＴ２０４では、まず所定時間ΔＴ毎に実行される割込処理２においてカウントした、距離センサ１の出力信号カウント値ΔＮi について、ＧＰＳ受信機３の観測周期ΔＴGPS 毎にカウント値の平均値ΔＮj を求める。そして、次に示す式（２６）により速度ＶｅｌDRj を計算するとともに、次回の処理のために出力信号カウント値ΔＮi を０にする。
【００７９】
ＶｅｌDRj ＝｜ΔＮj ×ＳＦSPDj｜／ΔＴGPS ・・・（２６）
【００８０】
また、後述する処理で使用するために、距離センサ１の出力信号カウント値ΔＮi の平均値ΔＮj とＧＰＳ速度ＶｅｌGPSjについて、所定時間分の移動平均をとった値ΔＮavejおよびＶｅｌGPSavej も計算しておく。なお、ＧＰＳ受信機３の観測周期はＧＰＳ観測情報受信フラグを参照して判断するものであり、上記式（２６）中のＳＦSPDjは距離センサ１の出力信号を距離に変換するためのスケールファクタである。
【００８１】
次にステップＳＴ２０５において、ＧＰＳ受信機３から観測情報の受信を完了したか否かについて、ＧＰＳ観測情報受信フラグを参照して再度判断する。このＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３からの観測情報の受信が完了したとしてステップＳＴ２０６へ進み、クリアされていれば、まだ受信が完了していないとしてステップＳＴ２１６へ進む。ステップＳＴ２０６では次回の処理のためにそのＧＰＳ観測情報受信フラグをクリアする。以降、ステップＳＴ２０７からステップＳＴ２１６では、距離センサ１のスケールファクタＳＦSPDjを補正するための処理が実行され、以下に説明する計算式（カルマンフィルタ）にて車両の速度ＶｅｌDRj とスケールファクタＳＦSPDjを計算する。
【００８２】
この実施の形態１におけるカルマンフィルタは、離散時刻ｊでの距離センサ１の出力信号ΔＮavejと、スケールファクタＳＦSPDjおよび加速度ａから、以下に示す式（２７）〜式（２９）を用いて速度ＶｅｌDRavejを計算するシステムモデルと、ＧＰＳ受信機３で観測した速度ＶｅｌGPSavej と上記システムモデルの速度ＶｅｌDRavejとの関係を次の式（３０）で表す観測モデルに基づいて、それぞれ以下の式（３１）〜式（３３）で示される状態方程式と、式（３４）〜式（３７）で示される観測方程式、および以下の式（３８）〜式（４２）で示されるカルマンフィルタ方程式を用いて、車両の速度ＶｅｌDRevejと距離センサ１のスケールファクタＳＦSPDjを計算する。
【００８３】
【００８４】
ＶｅｌGPSavej ＝ＶｅｌDRavej＋δＶｅｌGPSavej ・・・（３０）
【００８５】
【数３】
【００８６】
ｙj ＝Ｈ・ｘj ＋ｖj ・・・（３４）
ｙj ＝ＶｅｌGPSavej ・・・（３５）
Ｈ＝［０，１］ ・・・（３６）
ｖj ＝δＶｅｌGPSavej ・・・（３７）
【００８７】
ｘj|j ＝ｘj|j-1 ＋Ｋj ｛ｙj −（Ｈｘj|j-1 ＋ｖj ）｝ ・・・（３８）
ｘj+1|j ＝Ｆj ｘj|j ・・・（３９）
Ｋj ＝Σj|j-1ＨT［ＨΣj|j-1ＨT＋σvj 2 ］-1 ・・・（４０）
Σj|j ＝Σj|j-1 −Ｋj ＨΣj|j-1 ・・・（４１）
Σj+1|j ＝Ｆj Σj|j Ｆj T ・・・（４２）
【００８８】
なお、上記式（２７）〜式（２９）と式（３０）において、ΔＴGPS はＧＰＳ受信機の観測周期、ｊはΔＴGPS に同期した離散時刻、ＳＦSPDjは離散時刻ｊにおける距離センサ１のスケールファクタ、ＶｅｌDRavejは距離センサ１の出力信号から計算した速度の平均値、ＶｅｌGPSavej はＧＰＳ受信機２で観測した速度の平均値、ΔＮavejは離散時刻ｊ−１から離散時刻ｊの間の距離センサ１の出力信号カウント値の平均値、ａは離散時刻ｊ−１から離散時刻ｊの間の加速度、δＶｅｌGPSavej はＧＰＳ受信機２で観測した速度の平均値ＶｅｌGPSavej に含まれる誤差である。
【００８９】
また、上記式（３１）〜式（３３）および式（３４）〜式（３７）において、ｘj 、Ｆj 、ｙj 、ｖj は、離散時刻ｊにおける状態ベトクル、状態遷移行列、観測値、観測誤差であり、Ｈは観測行列である。さらに、式（３８）〜式（４２）において、ｘj|j は離散時刻ｊにおける状態ベクトルの推定量、ｘj+1|j は離散時刻ｊにおける離散時刻ｊ＋１の状態ベクトルの予測量、Ｋj はカルマンゲイン、Σj|j は離散時刻ｊにおける状態ベクトルの共分散行列の推定量、Σj+1|j は離散時刻ｊにおける離散時刻ｊ＋１の共分散行列の予測量、σvj 2 は観測誤差の分散行列である。なお、上記行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【００９０】
図２において、処理がステップＳＴ２０７に進むと、ＧＰＳ受信機３が２次元あるいは３次元測位を行っているか否かを判定する。２次元あるいは３次元測位を行っていればステップＳＴ２０８へ進み、非測位ならばステップＳＴ２１６へ進む。ステップＳＴ２０８では、ＧＰＳ受信機３の速度と距離センサ１の出力信号のそれぞれの変化率を次に示す式（４３）および式（４４）で求める。
【００９１】
ＲａｔｉｏGPSj＝ＶｅｌGPSavej ／ＶｅｌGPSavej-1 −１ ・・・（４３）
ＲａｔｉｏDRj ＝ΔＮavej／ΔＮavej-1−１ ・・・（４４）
【００９２】
そして、連続する所定時間ΔＴn における、それぞれの変化率の時間的変化に伴う勾配を、次に示す式（４５）〜式（４７）を用いて計算して、その変化率の時間的変化に伴う勾配の差異にＧＰＳ速度を乗算したものを、観測誤差として求める。
【００９３】
【数４】
【００９４】
なお、上記式（４３）および式（４４）と式（４５）〜式（４７）において、ＲａｔｉｏGPSjとＲａｔｉｏDRj はＧＰＳ速度と距離センサ１の出力信号のそれぞれの変化率であり、ＬａｍｐＲａｔｉｏGPS とＬａｍｐＲａｔｉｏDRはＧＰＳ速度と距離センサ１の出力信号の時間変化に伴うそれぞれの変化率の勾配である。また、式（４５）〜式（４７）において、ｎは所定時間ΔＴn におけるＲａｔｉｏGPSjとＲａｔｉｏDRj の計算数であり、それらの平均値がＲａｔｉｏDRave とＲａｔｉｏGPSaveである。
【００９５】
次にステップＳＴ２０９に進んで、補正が可能か否かを判定する。このステップＳＴ２０９では、ＧＰＳ速度が所定以上で、かつ変化率の時間的変化に伴う勾配の差異が所定値未満のときに補正可能と判断するもので、補正可能ならばステップＳＴ２１０へ進み、補正不可能ならばステップＳＴ２１６へ進む。ステップＳＴ２１０からステップＳＴ２１４では、上記式（２７）〜式（４２）に従って状態ベクトルの推定量Ｘj|j と予測量Ｘj+1|j 、誤差共分散の推定量Σj|j と予測量Σj+1|j 、およびカルマンゲインＫj を計算する。すなわち、ステップＳＴ２１０で状態ベクトルを推定してその推定量Ｘj|j を、ステップＳＴ２１１で状態ベクトルを予測してその予測量Ｘj+1|j を求め、ステップＳＴ２１２で誤差共分散を推定してその推定量Σj|j を、ステップＳＴ２１３で誤差共分散を予測してその予測量Σj+1|j を求め、さらにステップＳＴ２１４でカルマンゲインＫj を計算する。
【００９６】
次にステップＳＴ２１５において、このようなカルマンフィルタで計算した状態ベクトルの推定量Ｘj|j の行列要素であるスケールファクタ要素Ｘ11の値が正常範囲ならば、このスケールファクタ要素Ｘ11の値を用いて距離センサ１のスケールファクタＳＦSPDjを更新して、ステップＳＴ２１７に進む。一方、ステップＳＴ２１６では、状態ベクトルの速度に関する行列要素を初期化して、ステップＳＴ２１７に進む。ステップＳＴ２１７では状態ベクトルの行列要素である推定量の速度要素Ｘ12の値を出力した後、ステップＳＴ２０２へ戻る。
【００９７】
次に、割込処理１について説明する。
この割込処理１は所定時間Δｔ毎に起動され、図３の流れ図に示すように、そのステップＳＴ３０１において、処理開始タイミングを示す処理開始フラグをセットして、この割込処理１を終了する。
【００９８】
次に、割込処理２について説明する。
この割込処理２は距離センサ１から信号が出力されたときに起動され、図４の流れ図に示すように、そのステップＳＴ４０１において車速信号カウンタに１を加算して、この割込処理２を終了する。
【００９９】
次に、割込処理３について説明する。
この割込処理３はＧＰＳ受信機３から観測周期毎に出力された観測情報を受信して記憶するものであり、図５の流れ図に示すように、まずステップＳＴ５０１でＧＰＳ受信機３から出力された観測情報を受信して記憶する。次にステップＳＴ５０２において全ての観測情報の受信が完了したか否かを判定し、完了していればステップＳＴ５０３へ進み、完了していなければこの割込処理３を終了する。ステップＳＴ５０３では全ての観測情報の受信が完了したとして、ＧＰＳ観測信号受信フラグをセットする。
【０１００】
このように、この実施の形態１によれば、ＧＰＳ受信機３にて衛星からの電波を受信して車両の速度等を観測するとともに、距離センサ１にて車両の移動距離に応じた信号を出力し、速度計算手段５１１において、所定時間におけるＧＰＳ速度と距離センサ出力信号の平均値をそれぞれ計算して、それら各平均値の時間的変化に伴う勾配が所定値以下の時に、カルマンフィルタ方程式に従って、上記所定時間におけるＧＰＳ速度の平均値を観測値として、状態ベクトルの行列要素である距離センサ１のスケールファクタの補正と車両の速度推定を行い、また、ＧＰＳ速度と距離センサ１の出力信号の各平均値の時間的変化に伴う勾配の差異をＧＰＳ速度の平均値に乗算した値をＧＰＳ速度の誤差として使用して、誤差共分散の予測と推定の計算を行い、そして、ＧＰＳ受信機３が非測位状態の時には、カルマンフィルタの方程式に従って、距離センサ１の出力信号とスケールファクタから車両の速度を計算するようにしているので、ＧＰＳ速度の誤差が小さいと判定された時に、専用のカルマンフィルタによって、さらにＧＰＳ速度を基準にした距離センサ１のスケールファクタの補正と車両の速度の推定が行われるため、ＧＰＳ誤差が変動しても、走行開始後に迅速、かつ最適に、距離センサ１のスケールファクタを補正することができ、走行開始後に車両の速度を高い精度で推定できるという効果が得られる。
【０１０１】
実施の形態２．
次に、この発明の実施の形態２について説明する。
この実施の形態２によるロケータ装置は、速度計算手段５１１において、そのカルマンフィルタの後段にローパスフィルタを設けて、カルマンフィルタで計算した距離センサ１のスケールファクタを平滑化するようにしたものであり、その構成は図１のブロック図に示した実施の形態１の場合と同様であるため、図示および説明は省略する。
【０１０２】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態１の場合と同様であり、速度計算手段５１１のメインルーチンである図２に示した流れ図における、ステップＳＴ２１５の動作だけが実施の形態１の場合と異なるものであるため、ここではそのステップＳＴ２１５の動作についてのみ説明し、その他については説明を省略する。
【０１０３】
この実施の形態２においては、カルマンフィルタで計算した状態ベクトルの推定量の行列要素であるスケールファクタに含まれる高周波ノイズを、当該カルマンフィルタの後段にローパスフィルタを配置することによって除去し、このローパスフィルタを通過して平滑化されたスケールファクタを距離センサ１の補正値としている。すなわち、図２のステップＳＴ２１５では、次の式（４８）に示すように、状態ベクトルの推定量の行列要素であるスケールファクタ要素Ｘ11とスケールファクタの前回値ＳＦSPDj-1を重み付け平均して、スケールファクタＳＦSPDjを更新している。なお、この式（４８）において、ｗは重み付け係数である。
【０１０４】
ＳＦSPDj＝ＳＦSPDj-1＋（Ｘ11−ＳＦSPDj-1）×ｗ ・・・（４８）
【０１０５】
このように、この実施の形態２によれば、速度計算手段５１１は、カルマンフィルタの後段に設けたローパスフィルタによって、カルマンフィルタで計算した距離センサのスケールファクタを平滑化しているので、状態ベクトルの行列要素である距離センサ１のスケールファクタの変動を小さく抑えることが可能となって、車両の速度を安定的に推定できるという効果が得られる。
【０１０６】
実施の形態３．
次に、この発明の実施の形態３について説明する。
図６はこの発明の実施の形態３によるロケータ装置の構成を示すブロック図である。図において、１は図１に同一符号を付して示した実施の形態１のそれと同等の距離センサであり、車両の走行距離に応じてパルス信号を出力する。４は加速度センサであり、車両の前後方向の加速度に応じた電圧を出力する。
【０１０７】
また、５１２は加速度センサ４のオフセットを検出するオフセット検出手段であり、５１３は加速度センサ４の出力信号から車両の速度を推定する速度推定手段である。５１１は距離センサ１の出力信号から車両の速度を計算するとともに、速度推定手段５１３の出力に基づいて距離センサ１のスケールファクタを補正する速度計算手段である。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器であるが、上記速度計算手段５１１、オフセット検出手段５１２、および速度推定手段５１３によって構成されている点で、図１に同一符号を付して示した実施の形態１のそれとは異なっている。
【０１０８】
次に動作について説明する。
ここでは、信号処理器５の各手段の動作について図７を用いて説明する。この図７は信号処理器５の各手段の処理内容を示すメインルーチンの流れ図である。この他にも、図３に示した所定時間Δｔ毎に行う割込処理１と、図４に示した距離センサ１から信号が出力されたときに行う割込処理２があるが、それらは実施の形態１で説明したものと同一であるので、その説明は省略する。
【０１０９】
図７に示すメインルーチンは、信号処理器５のオフセット検出手段５１２、速度推定手段５１３、および速度計算手段５１１の各動作を管理するものである。まずステップＳＴ７０１で全ての処理を初期化する。次にステップＳＴ７０２において、割込処理１に係る処理開始タイミングか否かを、処理開始フラグを参照して判断する。その結果、当該処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ７０３へ進み、クリアされていれば処理開始タイミングではないとして、処理開始フラグがセットされるまで待機する。ステップＳＴ７０３では次回の処理のためにその処理開始フラグをクリアする。次にステップＳＴ７０４にて、速度計算手段５１１による速度計算の処理を実行する。すなわち、所定時間毎に割込処理２でカウントした距離センサ１の出力信号カウント値ΔＮi から、前述の式（２６）を用いて速度ＶｅｌDRi を計算するとともに、次回の処理のために出力信号カウント値ΔＮi を０にする。
【０１１０】
次にステップＳＴ７０５に進み、加速度センサ４より出力される、車両の前後方向の加速度に応じた電圧をＡ／Ｄ変換する。次にステップＳＴ７０６にて、オフセット検出手段５１２による加速度センサ４のオフセット検出の処理を実行する。すなわち、所定時間分の加速度センサ４の出力信号の履歴を保存しておき、距離センサ１の出力信号が０になる状態が所定時間以上継続した時に、加速度センサ４の出力信号の履歴として保存されている値の平均値を求め、この平均値を加速度センサ４のオフセットとして保持する。次にステップＳＴ７０７において、速度推定手段５１３による速度推定の処理を実行する。すなわち、オフセット検出手段５１２で加速度センサ４のオフセットが更新された時には推定速度ＶｅｌACC を０にし、それ以外は、前述の式（２０）にて加速度を求めた後、式（２１）で速度ＶｅｌACCを推定する。なお、加速度センサ４のスケールファクタはこの実施の形態３に限り固定値とした。
【０１１１】
その後ステップＳＴ７０８に進み、速度計算手段５１１において距離センサ１のスケールファクタ補正の処理を実行する。すなわち、オフセット検出手段５１２で加速度センサ４のオフセットが更新されると、それ以後所定時間が経過するまでの間は、速度推定手段５１３で推定した速度ＶｅｌACC に速度計算手段５１１で計算した速度ＶｅｌDRが合うように距離センサ１のスケールファクタを補正する。最後にステップＳＴ７０９において、速度計算手段５１１よりその速度ＶｅｌDRを出力した後ステップＳＴ７０２へ戻る。
【０１１２】
このように、この実施の形態３によれば、加速度センサ４の出力信号の変動量が所定値以下の時にその出力信号を平均した値をオフセットとし、当該加速度センサ４の出力信号からオフセットを減算した後、加速度センサ４のスケールファクタを乗算して車両の単位時間毎の加速度を計算し、その加速度を積算して速度を推定し、その推定された速度と距離センサ１の出力信号から計算した速度とが一致するように距離センサ１のスケールファクタを補正して、距離センサ１の出力信号から速度を計算するようにしているので、ＧＰＳ受信機３が非測位状態となり易い場所を走行しても、距離センサ１のスケールファクタを走行開始後に直ちに補正することができ、走行開始後に車両の速度を高い精度で推定することができるという効果が得られる。
【０１１３】
実施の形態４．
次に、この発明の実施の形態４について説明する。
図８はこの発明の実施の形態４によるロケータ装置の構成を示すブロック図である。図において、３は図１に同一符号を付して示した実施の形態１のそれに相当する、絶対位置観測手段としてのＧＰＳ受信機であり、衛星からの電波を受信するＧＰＳアンテナを有し、衛星からの電波により少なくとも車両の速度を観測して出力する。４は図６に同一符号を付して示した実施の形態３のそれと同等の加速度センサであり、車両の前後方向の加速度に応じた電圧を出力する。
【０１１４】
また、５１２は加速度センサ４のオフセットを検出する、図６に同一符号を付して示した実施の形態３のそれと同等のオフセット検出手段である。５１３は車両の速度を計算するとともに、加速度センサ４の出力信号を加速度に変換する際に使用するオフセットとスケールファクタの補正も行う点で、図６に同一符号を付して示した実施の形態３のそれとは異なった速度推定手段である。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器であるが、上記オフセット検出手段５１２、および速度推定手段５１３によって構成されている点で、図１および図６に同一符号を付して示した実施の形態１あるいは実施の形態３のそれとは異なっている。
【０１１５】
次に動作について説明する。
ここでは、信号処理器５のオフセット検出手段５１２と速度推定手段５１３の動作について図９を用いて説明する。この図９は信号処理器５の速度推定手段５１３の処理内容を示すメインルーチンの流れ図である。この他にも、図３に示した所定時間Δｔ毎に行う割込処理１と、図５に示したＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３があるが、それらは実施の形態１で説明したものと同一であるので、その説明は省略する。
【０１１６】
図９に示すメインルーチンは、信号処理器５のオフセット検出手段５１２と速度推定手段５１３の動作を管理するものである。まずステップＳＴ９０１で全ての処理を初期化する。次にステップＳＴ９０２において、割込処理１に係る処理開始タイミングか否かを処理開始フラグを参照して判断する。当該処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ９０３へ進み、クリアされていれば処理開始タイミングでないとして、当該処理開始フラグがセットされるまで待機する。ステップＳＴ９０３では次回の処理のためにその処理開始フラグをクリアする。次にステップＳＴ９０４において、所定時間分の加速度センサ４の出力信号の標準偏差を計算し、当該標準偏差が所定値以下であれば停車状態だと判断し、そうでなけば走行状態だと判断する。ステップＳＴ９０５では上記ステップＳＴ９０４の判断結果に基づいて、車両が走行中であるか否かを判定する。その結果、走行中でなかった場合にはステップＳＴ９０６へ進み、走行中であった場合にはステップＳＴ９０７へ進む。
【０１１７】
このステップＳＴ９０６はオフセット検出手段５１２の処理を示すもので、加速度センサ４の所定時間分の出力信号を平均した値をオフセットとして設定するとともに、推定速度を０とした後、ステップＳＴ９２０へ進む。一方、ステップＳＴ９０７からステップＳＴ９１９は速度推定手段５１３の処理を示すもので、以下に説明するする計算式（カルマンフィルタ）によって車両の速度ＶｅｌDRi 、加速度センサ４のオフセットＯＦACC 、およびスケールファクタＳＦACC を算出した後、ステップＳＴ９２０へ進む。ステップＳＴ９２０では速度推定手段５１３より速度ＶｅｌDRi を出力してステップＳＴ９０２へ戻る。
【０１１８】
この実施の形態４におけるカルマンフィルタは、加速度センサ４の出力信号ＳＧACC と、オフセットＯＦACC およびスケールファクタＳＦACC から、以下に示す式（４９）〜式（５０）を用いて速度ＶｅｌDRi を計算するシステムモデルと、ＧＰＳ受信機３で観測した速度情報ＶｅｌGPSiと上記システムモデルの速度ＶｅｌDRi の関係を、次の式（５１）で表した観測モデルに基づいて、それぞれ以下の式（５２）〜式（５６）で示される状態方程式と、式（５７）〜式（６０）で示される観測方程式、および以下の式（６１）〜式（６５）で示されるカルマンフィルタ方程式を用いて、車両の速度ＶｅｌDRi と、加速度センサ４のオフセットＯＦACC およびスケールファクタＳＦACC を計算する。
【０１１９】
ＶｅｌDRi ＝ＶｅｌDRi-1 ＋（ＳＧACCi−ＯＦACCi）×ＳＦACCi×ΔＴ・・・（４９）
ＯＦACCi＝ＯＦACCi-1＋δＯＦACCi ・・・（５０）
【０１２０】
ＶｅｌGPSi＝ＶｅｌRRi ＋δＶｅｌGPSi ・・・（５１）
【０１２１】
【数５】
【０１２２】
ｙi ＝Ｈ・ｘi ＋ｖi ・・・（５７）
ｙi ＝ＶｅｌGPSavei ・・・（５８）
Ｈ＝［１ ０ ０］ ・・・（５９）
ｖi ＝δＶｅｌGPSavei ・・・（６０）
【０１２３】
ｘi|i ＝ｘi|i-1 ＋Ｋi ｛ｙi −（Ｈｘi|i-1 ＋ｖi ）｝ ・・・（６１）
ｘi+1|i ＝Ｆi ｘi|i ＋Ｇi ωi ・・・（６２）
Ｋi ＝Σi|i-1 ＨT ［ＨΣi|i-1 ＨT ＋σvi 2 ］-1 ・・・（６３）
Σi|i ＝Σi|i-1 −Ｋi ＨΣi|i-1 ・・・（６４）
Σi+1|i ＝Ｆi Σi|i Ｆi T＋Ｇi σwi 2 Ｇi T ・・・（６５）
【０１２４】
なお、上記式（４９）〜式（５０）と式（５１）において、ｉは離散時刻、Δｔは所定時間、ＳＧACCiとＯＦACCiおよびＳＦACCiは離散時刻ｉにおける加速度センサ４の出力信号とオフセットおよびスケールファクタ、ＶｅｌDRi は加速度センサ４の出力信号から計算した速度、ＶｅｌGPSiはＧＰＳ受信機３で観測した速度、δＶｅｌGPSiはＶｅｌGPSiに含まれる誤差、δＯＦACCiはＯＦACCiに含まれる誤差である。
【０１２５】
また、上記式（５２）〜式（５６）および式（５７）〜式（６０）において、ｘi ，ωi ，Ｆi ，Ｇi ，ｙi ，ｖi は、離散時刻ｉにおける状態ベトクル、システム誤差、状態遷移行列、駆動行列、観測値、観測雑音であり、Ｈは観測行列はある。さらに、式（６１）〜式（６５）において、ｘi|i は離散時刻ｉにおける状態ベクトルの推定量、ｘi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の状態ベクトルの予測量、Ｋi はカルマンゲイン、Σi|i は離散時刻ｉにおける状態ベクトルの共分散行列の推定量、Σi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の共分散行列の予測量、σwi 2 はシステム誤差の分散行列、σvi 2 は観測誤差の分散行列である。なお、上記行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【０１２６】
図９において、処理がステップＳＴ９０７に進むと、ＧＰＳ受信機３から観測情報の受信を完了したか否かについて、ＧＰＳ観測情報受信フラグを参照して判断する。このＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３からの観測情報の受信を完了したものとしてステップＳＴ９０８へ進み、クリアされていればまだ受信が完了していないものとしてステップＳＴ９１２へ進む。ステップＳＴ９０８ではＧＰＳ受信機３が２次元あるいは３次元測位をしているか否かを判定する。２次元あるいは３次元測位を行っていればステップＳＴ９０９へ進み、非測位ならばステップＳＴ９１２へ進む。
【０１２７】
ステップＳＴ９０９では前述の式（４９）〜式（６５）に従って状態ベクトルの推定量ｘi|i を計算する。次にステップＳＴ９１０において、カルマンフィルタを通じて状態ベクトルの行列要素であるオフセット要素Ｘ12の修正量、すなわち推定値から予測値を減算した値の標準偏差を求め、これをシステム誤差ωであるオフセット誤差として設定する。次にステップＳＴ９１１にて、状態ベクトルの行列要素である速度要素ｘ11とＧＰＳ速度ＶｅｌGPSiの差異の標準偏差を求め、これを観測誤差として設定した後、ステップＳＴ９１２に進む。
【０１２８】
ステップＳＴ９１２では前述の式（４９）〜式（６５）に従って、状態ベクトルの予測量ｘi+1|i を計算する。次にステップＳＴ９１３において、ステップＳＴ９０７と同様にＧＰＳ観測情報受信フラグを参照して、ＧＰＳ受信機３から観測情報の受信が完了したか否かを再度判断する。ＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３から観測情報の受信を完了したものとしてステップＳＴ９１４へ進み、クリアされていればステップＳＴ９１９へ進む。ステップＳＴ９１４では次回の処理のためにそのＧＰＳ観測情報受信フラグをクリアする。
【０１２９】
次にステップＳＴ９１５において、ステップＳＴ９０８と同様にＧＰＳ受信機３が２次元あるいは３次元測位をしているか否かを判定する。２次元あるいは３次元測位を行っていればステップＳＴ９１６へ進み、非測位ならばステップＳＴ９２０へ進む。ステップＳＴ９１６では誤差共分散の推定量Σi|i を、ステップＳＴ９１７では誤差共分散の予測量Σi+1|i を、ステップＳＴ９１８ではカルマンゲインＫi を、それぞれ前述の式（４９）〜式（６５）に従って計算する。次にステップＳＴ９１９において、状態ベクトルの推定量の行列要素である速度要素ｘ11、オフセット要素ｘ12、スケールファクタ要素ｘ13に基づいて、推定速度ＶｅｌDRi 、オフセットＯＦACCi、およびスケールファクタＳＦACCIの更新を行った後、ステップＳＴ９２０へ進む。
【０１３０】
このように、この実施の形態４によれば、加速度センサ４の出力信号の変動量が所定値以下の時に加速度センサ４の出力信号を平均した値をオフセットとし、カルマンフィルタ方程式に従ってＧＰＳ速度を観測値として、状態ベクトルの行列要素である速度、オフセット、およびスケールファクタを推定し、ＧＰＳ受信機３が非測位状態の時にカルマンフィルタ方程式に従って、加速度センサ４の出力信号からオフセットを差し引いた値に加速度センサ４のスケールファクタを乗じて車両の単位時間毎の加速度を計算し、さらにその加速度を積算して速度を推定しているので、ＧＰＳ速度と加速度センサ４のオフセットの誤差が変動しても、加速度センサ４のオフセットとスケールファクタの補正を走行開始後に迅速、かつ最適に補正することができ、走行開始後に車両の速度を高精度で推定できるばかりか、車両に装着された距離センサを使用していないので、このロケータ装置を車両へ容易に着脱できるなどの効果が得られる。
【０１３１】
実施の形態５．
次に、この発明の実施の形態５について説明する。
図１０はこの発明の実施の形態５によるロケータ装置の構成を示すブロック図である。図において、１は図１に同一符号を付して示した実施の形態１のそれと同等の距離センサであり、車両の走行距離に応じてパルス信号を出力する。２は相対方位センサであり、車両のヨーレートに応じた電圧を出力するもので、この実施の形態５ではジャイロが用いられている。３は図１に同一符号を付して示した実施の形態１のそれに相当する、絶対位置観測手段としてのＧＰＳ受信機であり、衛星からの電波を受信するＧＰＳアンテナを有して、衛星からの電波により車両の現在位置を観測して出力する。
【０１３２】
５１１ａは距離センサ１の出力信号より車両の速度および移動距離を計算する距離・速度計算手段で、図１もしくは図６に符号５１１を付して示した、実施の形態１または実施の形態３における速度計算手段に相当するのものである。５１４は相対方位センサ２の出力信号から当該相対方位センサ２のオフセットを検出するオフセット検出手段であり、５１５は相対方位センサ２の出力信号から車両の進行方位の変化を計算する方位変化計算手段である。５１６はＧＰＳ受信機３からの観測情報、距離・速度計算手段５１１ａの出力信号、および方位変化計算手段５１５の出力信号に基づいて車両の現在位置と進行方位を推定する位置・方位推定手段である。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の進行方位と現在位置を演算するコンピュータを含んだ信号処理器であり、これら距離・速度計算手段５１１ａ、オフセット検出手段５１４、方位変化計算手段５１５、および位置・方位推定手段５１６から構成されている点で、図１，図６，および図８に同一符号を付して示された、上記実施の形態１から実施の形態４におけるそれとは異なっている。
【０１３３】
次に動作について説明する。
ここでは、信号処理器５の各手段の動作について図を用いて説明する。図１１は信号処理器５の処理内容を示すメインルーチンの流れ図であり、図１２は位置・方位推定手段５１６の処理内容を示す流れ図である。その他にも、図３に示した所定時間Δｔ毎に行う割込処理１、図４に示した距離センサ１から信号が出力されたときに行う割込処理２、および図５に示したＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３があるが、それらは実施の形態１で説明したものと同一であるので、その説明は省略する。また、図１３は位置・方位推定手段５１６による車両の現在位置の推定例を示す説明図である。
【０１３４】
図１１に示すメインルーチンは、信号処理器５の全手段の動作を管理するものである。まずステップＳＴ１１０１で全ての処理を初期化する。次にステップＳＴ１１０２において、処理開始タイミングか否かを処理開始フラグを参照して判断する。処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ１１０３へ進み、クリアされていれば処理開始タイミングではないとして、この処理開始フラグがセットされるまで待機する。ステップＳＴ１１０３では次回の処理のために同フラグをクリアする。次にステップＳＴ１１０４において、距離・速度計算手段５１１ａが、所定時間Δｔ毎に割込処理２でカウントした距離センサ１の出力信号カウント値ΔＮi から、移動距離Δｄi と速度ＶｅｌDRi を次に示す式（６６）と前述した式（２６）により計算し、計算が終了すると、次回の処理のために出力信号カウント値ΔＮi を０にする。
【０１３５】
Δｄi ＝Δｄi-1 ＋ΔＮi ×ＳＦSPDi ・・・（６６）
【０１３６】
次にステップＳＴ１１０５において、オフセット検出手段５１４によるオフセットＯＦGYRiの計算処理を行う。すなわち、ステップＳＴ１１０４で計算した速度ＶｅｌDRi が０（車両停車中）の時は、停車中における相対方位センサ２の所定時間分以上の出力信号ＳＧを平均したものをオフセットＯＦGYRiとする。次にステップＳＴ１１０６に進み、方位変化計算手段５１５にて進行方位変化Δθi の計算処理を行う。すなわち、所定時間Δｔ毎に計測した相対方位センサ２の出力信号ＳＧGYRiから、次に示す式（６７）を用いて進行方位変化Δθi を計算する。なお、この式（６７）中のＯＦGYRiおよびＳＦGYRiは、オフセットと、出力信号を角速度に変換するためのスケールファクタである。
【０１３７】
Δθi ＝Δθi-1 ＋（ＳＧGYRi−ＯＦGYRi）×ＳＦGYRi×Δｔ・・・（６７）
【０１３８】
次にステップＳＴ１１０７において、ＧＰＳ受信機３から観測情報の受信が完了したか否かを、ＧＰＳ観測情報受信フラグを参照して判断する。このＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３から観測情報の受信が完了したものとしてステップＳＴ１１０８へ進み、クリアされていればステップＳＴ１１０２へ戻る。ステップＳＴ１１０８では次回の処理のためにＧＰＳ観測情報受信フラグをクリアする。次にステップＳＴ１１０９において、位置・方位推定手段５１６による位置・方位の推定処理を行う。すなわち、以下に説明する計算式（カルマンフィルタ）にて車両の現在位置（λi ，φi ）と進行方位θi を推定する。次にステップＳＴ１１１０において、その車両の現在位置（λi ，φi ）と進行方位θi 、および速度ＶｅｌDri の出力を行う。次にステップＳＴ１１１１に進んで、ＧＰＳ受信機３から観測情報を受信する間の移動距離Δｄi と進行方位変化Δθi を０にクリアしてステップＳＴ１１０２に戻る。
【０１３９】
図１０に示した位置・方位推定手段５１６による、この実施の形態５におけるカルマンフィルタは、車両の移動距離Δｄi および進行方位変化Δθi から現在位置（λi ，φi ）と進行方位θi を、次に示す式（６８）〜式（７３）で計算するシステムモデルと、ＧＰＳ位置（λGi，φGi）と上記システムモデルの車両の現在位置（λi ，φi ）の関係を次に示す式（７４）〜式（７５）で表す観測モデルに基づくものである。
【０１４０】
【０１４１】
λGi＝λi ＋δλGi ・・・（７４）
φGi＝φi ＋δφGi ・・・（７５）
【０１４２】
なお、上記式（６８）〜式（７３）と式（７４）〜式（７５）において、ｉは離散時刻であり、λi-1 とφi-1 は離散時刻ｉ-１における車両の現在位置の経度と緯度、θi-1 は離散時刻ｉ-１における車両の進行方位である。ＳＦd →λ とＳＦd →φ は移動距離の単位［ｍ］を経度方向または緯度方向の移動量［”］へ変換する係数である。また、δλi とδφi は車両の現在位置の経度誤差と緯度誤差で、δΔθi とδΔｄi はΔθi とΔｄi の誤差である。また、δλGiとδφGiはＧＰＳ位置λGiとφGiの誤差である。
【０１４３】
そして次の式（７６）〜式（８０）で示される状態方程式と、次の式（８１）〜式（８４）で示される観測方程式、および次の式（８５）〜式（８９）で示されるカルマンフィルタ方程式を用いて、車両の現在位置（λi ，φi ）と進行方位θi の計算を行う。
【０１４４】
【数６】
【０１４５】
【数７】
【０１４６】
Ｘi|i ＝Ｘi|i-1 ＋Ｋi ｛Ｙi −（ＨＸi|i-1 ＋ｖi ）｝ ・・・（８５）
Ｘi+1|i ＝Ｆi Ｘi|i ＋Ｇi ｗi ・・・（８６）
Ｋi ＝Σi|i-1 ＨT ［ＨΣi|i-1 ＨT ＋Σvi］-1 ・・・（８７）
Σi|i ＝Σi|i-1 −Ｋi ＨΣi|i-1 ・・・（８８）
Σi+1|i ＝Ｆi Σi|i Ｆi T＋Ｇi ΣwiＧi T ・・・（８９）
【０１４７】
なお、上記式（７６）〜式（８０）および式（８１）〜式（８４）において、ｘi ，Ｆi ，Ｇi ，ｗi ，ｙi ，ｖi は、離散時刻ｉにおける状態ベトクル、状態遷移行列、駆動行列、システム雑音、観測値、観測雑音であり、Ｈは観測行列である。また、上記式（８５）〜式（８９）において、ｘi|i は離散時刻ｉにおける状態ベクトルの推定量、ｘi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の状態ベクトルの予測量、Ｋi はカルマンゲイン、Σi|i は離散時刻ｉにおける状態ベクトルの共分散行列の推定量、Σi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の共分散行列の予測量、Σviは観測誤差の共分散行列、Σwiはシステム誤差の共分散行列である。ここで、行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【０１４８】
次に、図１２の流れ図に従って、位置・方位推定手段５１６の処理内容をより詳細に説明する。まずステップＳＴ１２０１において、距離・速度計算手段５１１ａで計算した速度ＶｅｌDRi が０より大きいか否かを判定する。その結果、０であれば停車中と判断してこの位置・方位の推定処理を終了し、０より大きければステップＳＴ１２０２へ進む。ステップＳＴ１２０２では、ＧＰＳ受信機３が２次元あるいは３次元測位状態で、かつ前回カルマンフィルタにより車両の現在位置と進行方位を推定してから所定の距離以上移動したか否かを判定する。判定の結果、ＧＰＳ受信機３にて２次元あるいは３次元測位状態で、かつ前回カルマンフィルタにより現在位置と進行方位を推定してから所定の距離以上移動した場合にはステップＳＴ１２０３へ進み、逆にＧＰＳ受信機３にて非測位状態か、あるいは前回カルマンフィルタにより現在位置と進行方位を計算してから所定の距離以上移動していない場合にはステップＳＴ１２０５へ進む。
【０１４９】
ステップＳＴ１２０３では、所定時間におけるＧＰＳ位置（λGi，φGi）とカルマンフィルタによる現在位置（λi ，φi ）の位置の差分、すなわち緯度差と経度差のそれぞれの標準偏差を計算してその標準偏差を観測誤差ｖi とするとともに、観測誤差の共分散行列Σviを計算する。次にステップＳＴ１２０４において、ＧＰＳ速度の積分値と距離センサ１から求めた距離Δｄi の差分を、システム誤差の行列要素である距離誤差要素δΔｄi とするとともに、所定の距離だけ離れた複数地点でのＧＰＳ位置と車両の現在位置の軌跡の両移動方向の方向差を、システム誤差の行列要素である方位誤差要素δΔθi とした後、ステップＳＴ１２０６に進む。一方、ステップＳＴ１２０５ではカルマンゲインＫi を０にクリアして、ステップＳＴ１２０６に進む。
【０１５０】
ステップＳＴ１２０６ではＧＰＳ位置を観測値ｙi として、前述の式（６８）〜式（８９）に従って状態ベクトルの推定量ｘi|i を計算するが、ここで、相対方位センサ２のオフセット変動量に比例した値よりカルマンゲインＫi の全行列要素が小さくなるようにカルマンゲインＫi の全行列要素を調節して、状態ベクトルの推定量Ｘi|i を計算する。次にステップＳＴ１２０７において、前述の式（８５）〜式（８９）により状態ベクトルの予測量Ｘi+1|i を計算する。次にステップＳＴ１２０８において、ステップＳＴ１２０２と同様に、ＧＰＳ受信機３が２次元あるいは３次元測位状態で、かつ前回カルマンフィルタにより現在位置（λi ，φi ）と進行方位θi を計算してから所定の距離以上移動したか否かを判定する。ＧＰＳ受信機３にて２次元あるいは３次元測位しており、かつ前回カルマンフィルタにより現在位置（λi ，φi ）と進行方位θi を計算してから所定の距離以上移動した場合はステップＳＴ１２０９へ進み、そうでなければこの位置・方位推定の処理を終了する。
【０１５１】
ステップＳＴ１２０９では誤差共分散の推定量Σi|i を、ステップＳＴ１２１０では誤差共分散の予測量Σi+1|i を、ステップＳＴ１２１１ではカルマンゲインＫi を、それぞれ前述の式（８５）〜式（８９）を用いて計算し、この位置・方位推定の処理を終了する。
【０１５２】
次に、図１３を用いて、位置・方位推定手段５１６における車両の現在位置の推定動作について説明する。図１３（ａ）は車両が直進走行する途中でＧＰＳ位置の分散が大きくなった時に、車両の現在位置が推定される様子を示したものである。この場合にはシステム誤差の行列要素である方位誤差要素と距離誤差要素がともに小さい状態であったことを想定している。この時、位置・方位推定手段５１６はＧＰＳ位置の分散が大きくなった区間では、観測誤差であるＧＰＳ位置の誤差が大きくなったことを検出するので、ＧＰＳ位置の分散が大きくなった区間のみカルマンゲインを小さく計算する。その結果、ＧＰＳ位置の分散が小さい区間では車両の現在位置がＧＰＳ位置へ寄るように推定するが、ＧＰＳ位置の分散が大きい区間では車両の現在位置がＧＰＳ位置へ寄る量を小さく抑えて推定する。これにより、カルマンフィルタで推定した現在位置の軌跡は、図１３（ａ）に点線で示すものとなる。
【０１５３】
また図１３（ｂ）は車両が直進走行する途中で相対方位センサ２のオフセットがドリフトした時に、車両の現在位置が推定される様子を示したものである。この場合にはＧＰＳ位置の分散が小さい状態であったことを想定している。この時、位置・方位推定手段５１６はＧＰＳ位置の分散が小さい一方で、単位時間毎の進行方位変化が偏った値となって方位誤差が発生したことを検出するので、カルマンゲインを大きく計算する。その結果、相対方位センサ２に生じたドリフトの影響を受けずに車両の現在位置をＧＰＳ位置の方へ寄せるように推定する。これにより、カルマンフィルタで推定した現在位置の軌跡は、図１３（ｂ）に点線で示すものとなる。なお、カルマンフィルタを使用しない一般的な自律航法を用いた場合には、相対方位センサ２に生じたドリフトの影響を受けて、図１３（ｂ）に一点鎖線で示すように、車両の現在位置が偏った方向に徐々に旋回する。
【０１５４】
また図１３（ｃ）は相対方位センサ２のスケールファクタが真値より小さ目の時に車両が右折した場合の、車両の現在位置が推定される様子を示したものである。この場合にはＧＰＳ位置の分散が小さい状態であったことを想定している。この時、位置・方位推定手段５１６はＧＰＳ位置の分散が小さい一方で、右折後に方位誤差が発生したことを検出するので、右折後にはカルマンゲインをより大きく計算する。その結果、右折時の旋回角が小さ目になることから、推定した進行方位に誤差が生じるものの、右折後に方位誤差を検出した以後は車両の現在位置をＧＰＳ位置の方へ寄せるように推定する。これにより、カルマンフィルタで推定した現在位置の軌跡は、図１３（ｃ）に点線で示すものとなる。なお、カルマンフィルタを使用しない一般的な自律航法の場合には、図１３（ｃ）に一点鎖線で示すように、旋回角不足のまま車両の現在位置が誤った方向に直進する。
【０１５５】
なお、上記説明では、実施の形態１に示したロケータ装置に、相対方位センサ２、方位変化計算手段５１５、および位置・方位推定手段５１６を付加したものを示したが、実施の形態３もしくは実施の形態４に示したロケータ装置にそれらを付加することによって実現することも可能であり、その場合にも同様の効果を奏する。
【０１５６】
このように、この実施の形態５によれば、相対方位センサ２の出力信号から車両の単位時間毎の進行方位変化を計算するとともに、カルマンフィルタ方程式により、ＧＰＳ位置を観測値として状態ベクトルの行列要素である車両の現在位置と進行方位を推定し、ＧＰＳ受信機３と位置・方位推定手段５１６による現在位置の差異の変動量を観測誤差として使用して、所定の距離以上離れた複数地点毎のＧＰＳ受信機３位置・方位推定手段５１６による進行方位の差異の平均値をシステム誤差の行列要素である方位誤差として設定し、さらに距離・速度計算手段５１１ａで求めた速度の積分値とＧＰＳ速度の積分値との差異を、システム誤差の行列要素である距離誤差として使用して、誤差共分散の予測と推定の計算を行い、ＧＰＳ受信機３が非測位状態の時には、カルマンフィルタ方程式に従って、距離・速度計算手段５１１ａによる速度から求めた単位時間当たりの移動距離と、方位変化計算手段５１５で計算した単位時間毎の進行方位の変化から車両の現在位置と進行方位を推定しているので、ＧＰＳ位置と自律航法用のセンサからの計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定することができて、信頼性が向上し、また、ＧＰＳ受信機が非測位状態でも自律航法用のセンサの計測情報から車両の現在位置と進行方位を推定できるので、トンネルや高架下の道路においても、安定的に車両の現在位置と進行方位を提供することが可能になるなどの効果が得られる。
【０１５７】
実施の形態６．
次に、この発明の実施の形態６について説明する。
図１４はこの発明の実施の形態６によるロケータ装置の構成を示すブロック図である。図において、１は距離センサ、２は相対方位センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、これらは図１０に同一符号を付して示した実施の形態５におけるそれらと同等の部分である。なお、上記ＧＰＳ受信機３は車両の現在位置とともにその進行方位も観測して出力している。また、５１１ａは距離・速度計算手段、５１４はオフセット検出手段であり、これらも図１０に同一符号を付して示したものと同等の部分である。
【０１５８】
５１５は相対方位センサ２の出力信号から車両の進行方位変化を計算する方位変化計算手段であるが、相対方位センサ２の出力信号を角速度に変換するスケールファクタの補正も行っている点で、図１０に同一符号を付して示したものとは異なっている。５１６はＧＰＳ受信機３からの観測情報、距離・速度計算手段５１１ａの出力信号、および方位変化計算手段５１５の出力信号に基づいて車両の現在位置と進行方位を推定する位置・方位推定手段であるが、相対方位センサ２のオフセットと、相対方位センサ２の出力信号を角速度に変換するスケールファクタを補正するための信号を出力している点で、図１０に同一符号を付して示したものとは異なっている。５はこれら距離・速度計算手段５１１ａ、オフセット検出手段５１４、方位変化計算手段５１５、および位置・方位推定手段５１６から構成されて、あらかじめメモリ内に記憶されている制御プログラムに従って車両の進行方位と現在位置を演算するコンピュータを含んだ信号処理器である。
【０１５９】
次に動作について説明する。
以下、信号処理器５の各手段の動作について図を用いて説明する。なお、この信号処理器５における処理内容のメインルーチンは実施の形態５の場合と同様であり、図１１に示したメインルーチンの流れ図中の、ステップＳＴ１１０６の方位変化計算処理だけが実施の形態５と異なるものである。従って、ここでは方位変化計算処理以外については、説明を省略する。また、その他にも、図３に示した所定時間Δｔ毎に行う割込処理１、図４に示した距離センサ１から信号が出力されたときに行う割込処理２、および図５に示したＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３があるが、それらは実施の形態１で説明したものと同一であるので、その説明も省略する。
【０１６０】
ここで、図１５は相対方位センサ２の出力信号から車両の進行方位変化を計算するとともに、相対方位センサ２の出力信号を角速度に変換するスケールファクタを補正する、方位変化計算手段５１５による上記方位変化計算処理の処理内容を示す流れ図である。この方位変化計算処理が開始されると、まずステップＳＴ１５０１において、所定時間Δｔ毎に計測した相対方位センサ２の出力信号ＳＧGYRiから進行方位変化Δθi を前述の式（６７）で計算する。次にステップＳＴ１５０２において、所定時間Δｔ毎の上記進行方位変化Δθi を積算して積算方位ΣΔθi を更新する。以降、ステップＳＴ１５０３からステップＳＴ１５１５において、以下に説明するカルマンフィルタにて、相対方位センサ２から求めた積算方位ΣΔθi と、位置・方位推定手段５１６で推定した進行方位θi とを比較して、相対方位センサ２のオフセットＯＦGYRiとスケールファクタＳＦGYRiの補正を行う。
【０１６１】
この実施の形態６におけるカルマンフィルタは、相対方位サンサ２の出力信号ＳＧGYRiとオフセットＯＦGYRi、およびスケールファクタＳＦGYRiから進行方位変化Δθi を計算して、次に示す式（９０）〜式（９２）を用いて積算するシステムモデルと、位置・方位推定手段５１６で推定した車両の進行方位θi と上記システムモデルによって積算方位ΣΔθi の関係を次の式（９３）で表す観測モデルとに基づくものである。
【０１６２】
【０１６３】
θi ＝ΣΔθi ＋δθi ・・・（９３）
【０１６４】
なお、これら式（９０）〜式（９３）において、ｉは離散時刻、Δｔは所定時間であり、ＳＧGYRi、ＯＦGYRi、ＳＦGYRiは離散時刻ｉにおける相対方位センサ２の出力信号とオフセットおよびスケールファクタ、δＯＦGYRiとδＳＦGYRiはオフセット誤差とスケールファクタ誤差、δθi は推定された進行方位θi の誤差である。
【０１６５】
そして、次の式（９４）〜式（９８）に示す状態方程式と、次の式（９９）〜式（１０２）で示す観測方程式、および次に示す式（１０３）〜式（１０７）のカルマンフィルタ方程式により、相対方位センサ２のオフセットＯＦGYRiとスケールファクタＳＦGYRiを計算する。
【０１６６】
【数８】
【０１６７】
ｙi ＝Ｈ・ｘi ＋ｖi ・・・（９９）
ｙi ＝θi ・・・（１００）
ｖi ＝δθi ・・・（１０１）
Ｈ＝［１ ０ ０］ ・・・（１０２）
【０１６８】
ｘi|i ＝ｘi|i-1 ＋Ｋi ｛ｙi −（Ｈｘi|i-1 ＋ｖ）｝ ・・・（１０３）
ｘi+1|i ＝Ｆi ｘi|i ＋Ｇi ｗ ・・・（１０４）
Ｋi ＝Σi|i-1 ＨT ［ＨΣi|i-1 ＨT ＋σvi 2 ］-1 ・・・（１０５）
Σi|i ＝Σi|i-1 −Ｋi ＨΣi|i-1 ・・・（１０６）
Σi+1|i ＝Ｆi Σi|i Ｆi T＋Ｇi Σｗ iＧi T ・・・（１０７）
【０１６９】
なお、上記式（９４）〜式（９８）および式（９９）〜式（１０２）において、ｘi ，Ｆi ，Ｇi ，ｗi ，ｙi ，ｖi は離散時刻ｉにおける状態ベトクル、状態遷移行列、駆動行列、システム雑音、観測信号および観測雑音であり、Ｈは観測行列である。また上記式（１０３）〜式（１０７）において、ｘi|i は離散時刻ｉにおける状態ベクトルの推定量、ｘi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の状態ベクトルの予測量、Ｋi はカルマンゲイン、Σi|i は離散時刻ｉにおける共分散行列の推定量、Σi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の共分散行列の予測量、Σwiはシステム誤差ｗi の共分散分散、およびσvi 2 は観測誤差ｖi の分散である。ここで、行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【０１７０】
図１５において、処理がステップＳＴ１５０３に進むと、ＧＰＳ受信機３から観測情報の受信を完了したか否かを、ＧＰＳ観測情報受信フラグを参照して判断する。当該ＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３から観測情報の受信が完了したとしてステップＳＴ１５０４へ進み、クリアされていればこの方位変化計算処理を終了する。ステップＳＴ１５０４では距離センサ１から求めた速度ＶｅｌDRi が０より大きいか否かを判定する。判定の結果、速度ＶｅｌDRi が０より大きければ、車両が走行中であるとしてステップＳＴ１５０６へ進み、速度ＶｅｌDRi が０ならば、ステップＳＴ１５０５に分岐して状態ベクトルの行列要素であるオフセット要素Ｘ12と積算方位ΣΔθi を初期化した後、当該方位変化計算処理を終了する。なお、初期値には、位置・方位推定処理５１６で推定した進行方位θi と、オフセット検出手段５１４で検出した相対方位センサ２のオフセットＯＦGYRiが用いられる。
【０１７１】
ステップＳＴ１５０６ではＧＰＳ受信機３が２次元あるいは３次元測位をしているか否かを判定する。その結果、ＧＰＳ受信機３が２次元あるいは３次元測位をしていればステップＳＴ１５０７へ進み、非測位ならばこの方位変化計算処理を終了する。ステップＳＴ１５０７ではＧＰＳ受信機３での２次元あるいは３次元測位を継続している時間が所定時間以上であるか否かについて判定する。判定の結果、測位の継続時間が所定時間以上ならばステップＳＴ１５０８へ進み、所定時間未満ならばステップＳＴ１５０８〜ステップＳＴ１５１０をスキップして、直接ステップＳＴ１５１１へ進む。ステップＳＴ１５０８では、位置・方位推定手段５１６で推定した進行方位θi の誤差δθi を観測誤差ｖi とし、所定時間Δｔにおける観測誤差ｖi の分散を観測誤差の分散σvi 2 として、処理をステップＳＴ１５０９に進める。
【０１７２】
ステップＳＴ１５０９では、オフセット検出手段５１４で検出した相対方位センサ２のオフセットＯＦGYRiに、オフセット勾配であるドリフトにオフセット検出後の経過時間を乗算した値を加算した後、状態ベクトルの行列要素であるオフセット要素Ｘ12から減算した値を、システム誤差ｗi の行列要素であるオフセット誤差要素δＯＦGYRiとする。また、所定時間内に車両の進行方位が所定値以上変化した場合には、車両の進行方位θi と状態ベクトルの行列要素である積算方位要素Ｘ11の方位との差を求め、さらに相対方位センサ２の進行方位変化を図示を省略したステップで別途積算した角度で除算した値に、状態ベクトルの行列要素であるスケールファクタ要素Ｘ13と所定の係数をともに乗算した値を、スケールファクタ誤差δＳＦGYRiとする。そして所定時間におけるオフセット誤差δＯＦGYRiとスケールファクタ誤差δＳＦGYRiの分散を求めてシステム誤差の共分散Σwiを計算する。
【０１７３】
次にステップＳＴ１５１０に進んで、状態ベクトルの推定量ｘi|i を計算する。次にステップＳＴ１５１１において状態ベクトルの予測量ｘi+1|i を、ステップＳＴ１５１２において誤差共分散の推定量Σi|i を、ステップＳＴ１５１３において誤差共分散の予測量Σi+1|i を、ステップＳＴ１５１４においてカルマンゲインＫi をそれぞれ計算する。なお、これらの計算は前述の式（９０）〜式（１０７）を用いて行われる。次に、ステップＳＴ１５１５において、その状態ベクトルの推定量ｘi|i に基づいて、相対方位センサ２のオフセットＯＦGYRiとスケールファクタＳＦGYRiの更新を行った後、この方位変化計算処理を終了する。
【０１７４】
このように、この実施の形態６によれば、距離・速度計算手段５１１ａで求めた車両の速度が０のときに相対方位センサ２の出力信号を平均したものをオフセットとし、カルマンフィルタ方程式に従って位置・方位推定手段５１６で推定した進行方位を観測値とし、状態ベクトルの行列要素である相対方位センサ２のオフセットとスケールファクタを推定するとともに、停車毎のオフセットの変動量からオフセットを予測した値と現時点のオフセットの差異をシステム誤差の行列要素であるオフセット誤差として使用し、車両旋回時に位置・方位推定手段５１６と方位変化計算手段５１５の各方位情報からそれぞれ求めた車両の旋回角の差異からシステム誤差の行列要素であるスケールファクタ誤差を設定しているので、推定した車両の進行方位と相対方位センサのオフセットおよびスケールファクタに誤差があっても、相対方位センサ２のオフセットとスケールファクタを最適に補正することが可能となり、ＧＰＳ受信機３が非測位状態であっても、またＧＰＳ受信機３の観測情報の誤差が大きくなる徐行運転時であっても、相対方位センサ２のオフセットとスケールファクタを常時補正できるため、相対方位センサ２から進行方位変化を高い精度で求めることができるなどの効果が得られる。
【０１７５】
実施の形態７．
次に、この発明の実施の形態７について説明する。
この実施の形態７によるロケータ装置は、実施の形態６における相対方位センサ２のオフセットが所定値を越えた場合、そのスケールファクタの補正を行わないようにしたものであり、その構成は図１４のブロック図に示した実施の形態６の場合と同様であるため、図示および説明は省略する。
【０１７６】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態６の場合と同様であり、方位変化計算手段５１５による方位変化計算処理の処理内容を示した図１５の流れ図における、ステップＳＴ１５１５の動作だけが実施の形態６と異なるものであるため、ここではそのステップＳＴ１５１５の動作についてのみ説明し、その他については説明を省略する。
【０１７７】
この実施の形態７においては、図１５の流れ図に示したステップＳＴ１５１５において、相対方位センサ２のオフセットの変動があらかじめ定められた所定の値以上であれば、カルマンフィルタでスケールファクタが所定値以上補正されないようにカルマンゲインを調整して、状態ベクトルを前述の式（９０）〜式（１０７）に従って計算するか、実施の形態６の場合と同様の計算をしてもスケールファクタを更新しないようにしている。
【０１７８】
このように、この実施の形態７によれば、カルマンフィルタにおける状態ベクトルの行列要素である相対方位センサ２のオフセットの変動があらかじめ定めた値以上になると、状態ベクトルの行列要素である相対方位センサ２のスケールファクタの補正を無効とし、相対方位センサ２のオフセットの変動が小さい時にのみ相対方位センサ２のスケールファクタの補正を行っているので、相対方位センサ２のスケールファクタの誤補正を防止することができ、その結果、進行方位の変化を高い精度で求めることが可能になるという効果が得られる。
【０１７９】
実施の形態８．
次に、この発明の実施の形態８について説明する。
この実施の形態８によるロケータ装置は、実施の形態１の速度計算手段５１１に、当該ロケータ装置が他の車両に載せ換えられたことを検出して、車両に係る情報、あるいは当該ロケータ装置の取り付けに係る情報を初期化する機能を持たせるようにしたものであり、その構成は図１のブロック図に示した実施の形態１の場合と同様であるため、図示および説明は省略する。
【０１８０】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態１の場合と同様であり、速度計算手段５１１のメインルーチンである図２に示した流れ図における、ステップＳＴ２１５とステップＳＴ２１７の動作だけが実施の形態１の場合と異なるものであるため、ここではそのステップＳＴ２１５およびステップＳＴ２１７の動作についてのみ説明し、その他については説明を省略する。
【０１８１】
この実施の形態８では、図２の流れ図に示したステップＳＴ２１５において、カルマンフィルタで計算した状態ベクトルの行列要素であるスケールファクタ要素の値が正常範囲内であれば、当該スケールファクタ要素の最新計算値を所定時間分だけ保持する。そして、このスケールファクタ要素の計算値の所定時間分の平均値に対する標準偏差の比率が所定値以下のときに、当該平均値を用いて距離センサ１のスケールファクタの更新を行う。また、上記スケールファクタが前回の値から所定の比率以上更新された場合には、ロケータ装置が他の車両へ載せ換えられたと判断して、車両に係る情報あるいは当該ロケータ装置の取り付けに係る情報を初期化する。その後、ステップＳＴ２１７において、車両の速度などとともに、このロケータ装置が他の車両へ載せかえられたか否かの判定結果を出力する。
【０１８２】
このように、この実施の形態８によれば、距離センサ１のスケールファクタが所定割合以上変化した時に、当該ロケータ装置が他の車両に載せ換えられたと判断して、車両に係る情報あるいは当該装置の取付けに係る情報を初期化する機能を速度計算手段５１１に持たせるようにしているので、ロケータ装置を複数の車両に載せ換えて使用するような場合でも、現在搭載されている車両に係る補正を直ちに行うことが可能となり、車両の挙動情報を最適に設定することができるという効果が得られる。
【０１８３】
実施の形態９．
次に、この発明の実施の形態９について説明する。
図１６はこの発明の実施の形態９によるロケータ装置の構成を示すブロック図である。図において、１は距離センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、これらは図１に同一符号を付して示した実施の形態１におけるそれらに相当する部分である。なお、上記ＧＰＳ受信機３は車両の現在位置および進行方位を観測して出力している。
【０１８４】
また、５２は車種毎に異なった距離センサ１のスケールファクタを記憶する複数のメモリ領域を有する記憶手段であり、５３はそのスケールファクタの記憶先のメモリ領域を利用者が記憶手段５２へ指示するためのスイッチを備えた記憶先指定手段である。５１１は距離センサ１の出力信号を距離に変換するためのスケールファクタを計算する速度計算手段であるが、この記憶先指定手段５３で指定された記憶手段５２のメモリ領域に、距離センサ１のスケールファクタを読み書きする点で、図１に同一符号を付して示したものとは異なっている。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器であるが、これら記憶手段５２、記憶先指定手段５３、および速度計算手段５１１から構成されている点で、図１，図６，図８，図１０，および図１４に同一符号を付して示した上記各実施の形態のそれらとは異なっている。
【０１８５】
次に動作について説明する。
以下、信号処理器５の動作について図を用いて説明する。ただし、この信号処理器５における速度計算手段５１１の動作は実施の形態１の場合と同様であるため、ここではその説明は省略し、記憶手段５２へのスケールファクタの読み書きの動作について説明する。なお、図１７は記憶手段５２のメモリマップを簡単に示した説明図である。まず、記憶先指定手段５３は利用者によって操作されるスイッチの設定状態を確認し、その設定状態に応じてあらかじめ決めておいたメモリ領域の番地を記憶手段５２に指示する。記憶手段５２は、記憶先指定手段５３より指示された番地（図１７に示すｘ番地〜ｘ＋ｎ−１番地）のメモリ領域に対して、速度計算手段５１１で求めた、車種毎に異なる距離センサ１のスケールファクタ（図１７ではＳＦ値１〜ＳＦ値ｎ）の読み書きを行う。
【０１８６】
このように、この実施の形態９によれば、車種毎に異なる距離センサ１のスケールファクタを記憶手段５２に記憶する一方で、利用者が指定した距離センサ１のスケールファクタの記憶先であるメモリ領域の番地を記憶先指定手段５３で読み取り、記憶手段５２の指定された番地のメモリ領域へ距離センサ１のスケールファクタを読み書きしているので、距離センサ１のスケールファクタを手動で切り替えた場合には、ロケータ装置を他の車両へ載せ換えた時に車両に係る補正を毎回やり直す必要がなくなり、他の車両へ載せ換えた直後から、車両の速度を高い精度で推定することができるという効果が得られる。
【０１８７】
実施の形態１０．
次に、この発明の実施の形態１０について説明する。
図１８はこの発明の実施の形態１０によるロケータ装置の構成を示すブロック図である。図において、１は距離センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、これらは図１に同一符号を付して示した実施の形態１におけるそれらに相当する部分である。なお、上記ＧＰＳ受信機３は車両の現在位置および進行方位を観測して出力している。
【０１８８】
また、５２は車種毎に異なった距離センサ１のスケールファクタとメモリアクセス時刻とを組にして記憶する複数のメモリ領域を有した記憶手段である。５４は後述する速度計算手段５１１で計算されたスケールファクタに基づいて、記憶手段５２に記憶されたスケールファクタとメモリアクセス時刻の履歴を検索し、検索結果としてのメモリ領域の番地と記憶されているスケールファクタとを、記憶手段５２と速度計算手段５１１にそれぞれ伝える記憶先検索手段である。５１１は車両の速度を計算するとともに、距離センサ１の出力信号を距離に変換するためのスケールファクタを計算する速度計算手段であるが、スケールファクタとメモリアクセス時刻の組の記憶手段５２の各メモリ領域への読み書きを行い、さらに記憶先検索手段５２に計算したスケールファクタを送り、記憶先検索手段５４の検索結果を受け取っている点で、図１６に同一符号を付して示した実施の形態９のそれとは異なっている。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含む信号処理器であるが、これら記憶手段５２、記憶先検索手段５４、および速度計算手段５１１から構成されている点で、図１，図６，図８，図１０，図１４，および図１６に同一符号を付して示した上記各実施の形態のそれとは異なっている。
【０１８９】
次に動作について説明する。
以下、信号処理器５の各手段の動作について図を用いて説明する。ただし、この信号処理器５における速度計算手段５１１については実施の形態８の場合と同様であるため、ここではその説明は省略する。なお、図１９は記憶手段５２のメモリマップを簡単に示した説明図である。
【０１９０】
速度計算手段５１１にて距離センサ１のスケールファクタを計算し、実施の形態８の場合と同様にして当該ロケータ装置が他の車両に乗せ換えられたか否かを判定する。その結果、他の車両へ載せ換えられたと判断された場合には、速度計算手段５１１は計算した距離センサ１のスケールファクタを記憶先検索手段５４に渡す。記憶先検索手段５４は、記憶手段５２の各メモリ領域に記憶されているスケールファクタの中から、その速度計算手段５１１より受け取ったスケールファクタの計算値との差異が所定値以下となるものを検索する。そして該当するものが見つかると、その検索したスケールファクタを速度計算手段５１１へ渡すとともに、検索したスケールファクタが記憶されているメモリ領域の番地を記憶手段５２に伝える。
【０１９１】
記憶手段５２は、この記憶先検索手段５４より通知された番地（図１９に示すｙ番地からｙ＋ｎ−１番地）のメモリ領域に、車種毎に異なる距離センサ１のスケールファクタ（図１９ではＳＦ値１〜ＳＦ値ｎ）と組で記憶されているメモリアクセス時刻を、現在の時刻で更新する。その後、速度計算手段５１１は記憶先検索手段５４から受け取ったスケールファクタ、すなわち過去に補正された結果を用いて車両の速度を計算する。
【０１９２】
なお、記憶先検索手段５４は、速度計算手段５１１から受け取ったスケールファクタの計算値と記憶手段５２に記憶されたスケールファクタとの差異が所定値以下となるものを、記憶手段５２のメモリ領域から検索できなかった場合には、未確認の他の車両へ当該ロケータ装置が載せ換えられたものと判断して、記憶手段５２の空いているメモリ領域の番地を記憶手段５２へ伝える。記憶手段５２は、渡された番地が付けられた空きメモリ領域に、速度計算手段５１１の計算したスケールファクタを記憶するとともに、そのスケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を記憶する。
【０１９３】
また、速度計算手段５１１から受け取ったスケールファクタの計算値と記憶手段５２に記憶されたスケールファクタとの差異が所定値以下となるものを、記憶手段５２のメモリ領域から検索できず、さらに記憶手段５２に空いているメモリ領域もなかった場合には、記憶先検索手段５４は、メモリアクセス時刻が最古になっているスケールファクタを記憶しているメモリ領域の番地を記憶手段５２に伝える。記憶記憶５２は渡された番地のメモリ領域に、速度計算手段５１１の計算したスケールファクタを記憶するとともに、そのスケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を記憶する。
【０１９４】
このように、この実施の形態１０によれば、当該ロケータ装置が他の車両へ載せ換えられたと判断された場合には、記憶手段５２に記憶したスケールファクタの中から現在の計算値に最も近いものを探し出して速度計算手段５１１に渡すとともに、記憶手段５２へ当該メモリの番地を渡し、この時、現在の計算値に対して、記憶手段５２に記憶されたスケールファクタが所定の比率以上異なっていれば未確認車両へロケータ装置が搭載されたものと判断して、記憶手段５２に対してその空きメモリ領域に当該スケールファクタを読み書きするように指示し、空いているメモリ領域がない場合には、メモリアクセス時刻が最古のメモリ領域に上書きするように記憶手段５２に指示しているので、ロケータ装置を他の車両へ載せ換えても、当該車両に係る補正を毎回やり直す必要がなくなり、他の車両へ載せ換えた直後から車両の速度を高精度で推定することが可能になるという効果が得られる。
【０１９５】
実施の形態１１．
上記実施の形態２では、速度計算手段５１１において、状態ベクトルの推定量の行列要素であるスケールファクタ要素とスケールファクタの前回の値を式（４８）を用いて重み付け平均し、スケールファクタの更新を行うものとして説明したが、状態ベクトルの推定量の行列要素であるスケールファクタ要素を平滑化してスケールファクタを更新するものであれば、上記式（４８）以外の計算式を用いてもよい。
【０１９６】
実施の形態１２．
上記実施の形態３では、速度計算手段５１１において、加速度センサ４のオフセットが更新された後、所定時間が経過するまでの間は、速度推定手段５１３で加速度センサ４の出力信号から推定した速度に速度計算手段５１１で計算した速度が合うように、距離センサ１のスケールファクタを補正するものについて説明したが、速度推定手段５１３で推定した速度の誤差が小さいものであれば、加速度センサ４のオフセットの変動量に応じて、距離センサ１のスケールファクタを補正できる所定時間を可変に設定してもよい。
【０１９７】
実施の形態１３．
上記実施の形態５、実施の形態６および実施の形態１０では、距離・速度計算手段５１１ａあるいは速度計算手段５１１において、車両の速度と移動距離を計算するために距離センサ１を使用した場合について説明したが、加速度センサ４を代わりに用いてもよい。
【０１９８】
実施の形態１４．
上記実施の形態８では、速度計算手段５１１において、距離センサ１のスケールファクタが前回の値から所定比率以上更新された場合に、当該ロケータ装置が他の車両へ載せ換えられたと判断する場合について説明したが、カルマンフィルタで計算した状態ベクトルの行列要素であるスケールファクタ要素の計算値を所定時間分保持した履歴に基づいて、例えば、所定時間分の平均値に対する標準偏差の比率が所定の値以上変化したとき、あるいは履歴中のスケールファクタ要素の時間的変化に伴う勾配が所定値以上になった時に、当該ロケータ装置が他の車両へ載せ換えられたと判断するようにしてもよい。
【０１９９】
実施の形態１５．
上記実施の形態９では、記憶先指定手段５３が、スケールファクタの記憶場所を利用者が記憶手段５２へ指示するスイッチを備えているものとして説明したが、スイッチの代わりに通信手段をさらに備えて、利用者が通信手段を介してスケールファクタの記憶場所を指定できるようにしてもよい。
【０２００】
実施の形態１６．
上記実施の形態１０では、記憶先検索手段５４は、速度計算手段５１１で計算した距離センサ１のスケールファクタと記憶手段５２に記憶されたスケールファクタの差異が、所定値以下となるものを記憶手段５２から検索するものについて説明したが、記憶手段５２に記憶されたスケールファクタに対する上記差異の比率が所定以下となるものを検索するようにしてもよい。
【０２０１】
【発明の効果】
以上のように、この発明によれば、衛星を利用して少なくとも車両の速度等を観測する絶対位置観測手段と、車両の移動距離に応じた信号を出力する距離センサを備え、速度計算手段にて車両の速度を推定するとともに、カルマンフィルタを用いて、絶対位置観測手段で観測した速度を基準にした距離センサのスケールファクタの補正と車両の速度の推定を行うように構成したので、絶対位置観測手段で観測した速度の誤差が変動しても距離センサのスケールファクタを、車両の走行開始後に、最適かつ速やかに補正することが可能となり、走行開始後に車両の速度を高精度で推定することができるロケータ装置が得られる効果がある。
【０２０２】
この発明によれば、ローパスフィルタを速度計算手段におけるカルマンフィルタの後段に設けるように構成したので、カルマンフィルタで計算した距離センサのスケールファクタが平滑化されて、それに含まれている高周波ノイズが除去されるため、距離センサのスケールファクタの変動を小さくすることができ、車両の速度を安定的に推定できるという効果がある。
【０２０５】
この発明によれば、車両の進行方位変化に応じた信号を出力する相対方位センサと、相対方位センサの出力信号から車両の単位時間毎の進行方位変化を計算する方位変化計算手段とを設け、位置・方位推定手段にて車両の現在位置と進行方位を推定するとともに、カルマンフィルタを用いて、絶対位置観測手段の観測した位置を基準にした車両の現在位置と進行方位を推定するように構成したので、専用のカルマンフィルタにて、絶対位置観測手段の観測した位置と相対方位センサ等の計測情報から車両の現在位置と進行方位が同時に推定され、絶対位置観測手段の観測した位置と相対方位センサ等の計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定できて信頼性の向上がはかれ、また、絶対位置観測手段が非測位状態でも相対方位センサ等の計測情報から車両の現在位置と進行方位を推定することが可能となって、トンネルや高架下の道路においても、安定的に車両の現在位置と進行方位を提供できるなどの効果がある。
【０２０６】
この発明によれば、相対方位センサの出力信号から当該相対方位センサのオフセットを検出するオフセット検出手段を設け、方位変化計算手段にて相対方位センサの出力信号から車両の単位時間毎の進行方位変化を計算するとともに、カルマンフィルタを用いて、位置・方位推定手段にて推定した車両の進行方位を基準にした相対方位センサのオフセットとスケールファクタの補正を行うように構成したので、推定した車両の進行方位と相対方位センサのオフセットおよびスケールファクタに誤差があっても、相対方位センサのオフセットとスケールファクタを最適に補正することが可能となり、また、絶対位置観測手段が非測位状態であっても、また徐行運転時などのように絶対位置観測手段の観測情報の誤差が大きくなる場合であっても、相対方位センサのオフセットとスケールファクタを常時補正することができるため、相対方位センサから進行方位変化を高い精度で求めることができるなどの効果がある。
【０２０７】
この発明によれば、方位変化計算手段が相対方位センサのオフセットの変動を監視して、スケールファクタ補正の有効性を判定するように構成したので相対方位センサのオフセットの変動が小さい時に相対方位センサのスケールファクタを補正することで、相対方位センサのスケールファクタの誤補正を防止することが可能となり、相対方位センサから高い精度で進行方位変化を求めることができるという効果がある。
【０２０８】
この発明によれば、距離センサのスケールファクタの補正を監視することによって、ロケータ装置が他の車両に載せ換えられたか否かを判断するように構成したので、距離センサのスケールファクタの変動量からロケータ装置が他の車両に載せ換えられたことを検出することができ、ロケータ装置が複数の車両へ載せ換えられて使用されるような場合に、現在搭載されている車両に係る補正を直ちに行うことができ、車両の挙動情報を最適に設定することが可能になるという効果がある。
【０２０９】
この発明によれば、距離センサのスケールファクタを記憶する複数のメモリ領域を有する記憶手段と、利用者が記憶先であるメモリ領域の番地を指定するための記憶先指定手段とを設け、車種毎に異なる距離センサのスケールファクタを記憶手段に記憶する一方で、距離センサのスケールファクタの記憶場所を利用者が記憶先指定手段によって切り替えるように構成したので、距離センサのスケールファクタをこの記憶先指定手段によって切り替えることにより、ロケータ装置を他の車両へ載せ換えた時に、当該車両に係る補正を毎回やり直すことが不要になり、他の車両へ載せ換えた直後から車両の速度を高い精度で推定することが可能になるという効果がある。
【０２１０】
この発明によれば、距離センサのスケールファクタとメモリアクセス時刻の組を記憶する複数のメモリ領域を有する記憶手段と、記憶手段に保持されているスケールファクタを検索する記憶先検索手段を設け、車種毎に異なる距離センサのスケールファクタを記憶手段に記憶する一方で、距離センサのスケールファクタの記憶場所を記憶先検索手段の検索結果によって自動的に切り替えるように構成したので、ロケータ装置を他の車両へ載せ換えた時に、当該車両に係る補正を毎回やり直すことが不要になり、他の車両へ載せ換えた直後から車両の速度を高い精度で推定することが可能になるという効果がある。
【図面の簡単な説明】
【図１】 この発明の実施の形態１によるロケータ装置の構成を示すブロック図である。
【図２】 この発明の実施の形態１におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図３】 この発明の実施の形態１におけるロケータ装置の動作の、割込処理１の処理内容を示す流れ図である。
【図４】 この発明の実施の形態１におけるロケータ装置の動作の、割込処理２の処理内容を示す流れ図である。
【図５】 この発明の実施の形態１におけるロケータ装置の動作の、割込処理３の処理内容を示す流れ図である。
【図６】 この発明の実施の形態３によるロケータ装置の構成を示すブロック図である。
【図７】 この発明の実施の形態３におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図８】 この発明の実施の形態４によるロケータ装置の構成を示すブロック図である。
【図９】 この発明の実施の形態４におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図１０】 この発明の実施の形態５によるロケータ装置の構成を示すブロック図である。
【図１１】 この発明の実施の形態５におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図１２】 この発明の実施の形態５におけるロケータ装置の動作の、位置・方位推定処理の処理内容を示す流れ図である。
【図１３】 この発明の実施の形態５におけるロケータ装置の動作の、位置・方位推定手段による車両の現在位置の推定例を示す説明図である。
【図１４】 この発明の実施の形態６によるロケータ装置の構成を示すブロック図である。
【図１５】 この発明の実施の形態６におけるロケータ装置の動作の、方位変化計算処理の処理内容を示す流れ図である。
【図１６】 この発明の実施の形態９によるロケータ装置の構成を示すブロック図である。
【図１７】 この発明の実施の形態９におけるロケータ装置の記憶手段のメモリマップを簡単に示した説明図である。
【図１８】 この発明の実施の形態１０によるロケータ装置の構成を示すブロック図である。
【図１９】 この発明の実施の形態１０におけるロケータ装置の記憶手段のメモリマップを簡単に示した説明図である。
【図２０】 従来のロケータ装置の一構成例を示すブロック図である。
【図２１】 上記ロケータ装置の動作の、自律航法のメインルーチンの処理内容を示す流れ図である。
【図２２】 上記ロケータ装置の動作の、進行方位変化と移動距離の演算処理の処理内容を示す流れ図である。
【図２３】 上記ロケータ装置の動作の、相対軌跡演算処理の処理内容を示す流れ図である。
【図２４】 上記ロケータ装置の動作の、絶対方位、絶対位置の演算処理の処理内容を示す流れ図である。
【図２５】 上記ロケータ装置の動作の、自律航法とＧＰＳ航法を複合化するためのカルマンフィルタの演算処理の処理内容を示す流れ図である。
【図２６】 従来のロケータ装置の他の構成例を示すブロック図である。
【図２７】 上記ロケータ装置の動作の、自律航法のメインルーチンの処理内容を示す流れ図である。
【符号の説明】
１ 距離センサ、２ 相対方位センサ、３ ＧＰＳ受信機（絶対位置観測手段）、４ 加速度センサ、５２ 記憶手段、５３ 記憶先指定手段、５４ 記憶先検索手段、５１１ 速度計算手段、５１１ａ 距離・速度計算手段（速度計算手段）、５１２，５１４ オフセット検出手段、５１３ 速度推定手段、５１５ 方位変化計算手段、５１６ 位置・方位推定手段。
【発明の属する技術分野】
この発明は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測情報と、自律航法用複合センサの計測情報を用いて、車両の現在位置と進行方位および速度等を計算する複合航法を備えたロケータ装置に関するものである。
【０００２】
【従来の技術】
図２０は例えば、特開平８−６８６５５号公報に示された従来のロケータ装置の構成の一例を示すブロック図である。図において、１は距離センサ、２は相対方位センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、５は信号処理器である。また、信号処理器５内において、５２０は距離センサ１と相対方位センサ２の出力信号を基に相対軌跡を演算する相対軌跡演算、５２１はそれらの絶対軌跡を演算する絶対軌跡演算であり、５２２は距離センサ１の距離係数、相対方位センサ２のオフセット、絶対位置および絶対方位の各補正を行うカルマンフィルタである。
【０００３】
次に動作について説明する。
なお、相対軌跡演算５２０、絶対軌跡演算５２１、およびカルマンフィルタ５２２はマイクロコンピュータによる演算処理によって実現されるものであり、以下、これらについて説明する。ここで、図２１は自律航法のメインルーチンの演算処理を示す流れ図、図２２は進行方位変化と移動距離の演算処理を示す流れ図、図２３は相対軌跡演算処理を示す流れ図、図２４は絶対方位、絶対位置の演算処理を示す流れ図であり、図２５は自律航法とＧＰＳ航法を複合化するためのカルマンフィルタの演算処理を示す流れ図である。
【０００４】
まず、図２１のステップＳＴ２１０１において、車両の進行方位変化と移動距離を計算する。この車両の進行方位変化と移動距離の演算の処理内容の詳細を図２２に示す。この処理が開始されると、ステップＳＴ２２０１において相対方位センサ２から車両の進行方位変化をまず求める。次にステップＳＴ２２０２でオフセット補正量に前回からの移動距離Ｌを掛けたものを進行方位変化から差し引き、進行方位変化のオフセット補正を行う。その後、ステップＳＴ２２０３にて距離センサ１の信号に距離係数を掛けて移動距離を算出し、当該車両の進行方位変化と移動距離の演算処理を終了する。
【０００５】
ステップＳＴ２１０１による車両の進行方位変化と移動距離の演算処理が終了すると、次にステップＳＴ２１０２において相対軌跡演算処理を行う。この相対軌跡演算の処理内容の詳細を図２３に示す。この処理が開始されると、まずステップＳＴ２３０１にて進行方位変化を積算して相対方位を更新する。次にステップＳＴ２３０２に進み、この更新した相対方位およびステップＳＴ２２０３で求めた移動距離に基づく移動ベクトルのＸ，Ｙ成分をそれまでの相対位置座標に加算して新たな相対位置座標を求め、この相対軌跡演算処理を終了する。
【０００６】
ステップＳＴ２１０２による相対軌跡の演算処理が終了すると、次にステップＳＴ２１０３において絶対方位・絶対位置の演算処理を行う。この絶対方位・絶対位置演算の処理内容の詳細を図２４に示す。この処理が開始されると、まずステップＳＴ２４０１にて進行方位変化を基に絶対方位を更新する。次にステップＳＴ２４０２に進み、この更新した絶対方位およびステップＳＴ２２０３にて求めた移動距離によって絶対位置座標の更新を行い、この絶対方位・絶対位置の演算処理を終了する。このステップＳＴ２１０３の処理にて更新された絶対方位Ａと絶対位置は、次のステップＳＴ２１０４にけるＧＰＳ受信機３の観測情報との複合化処理に利用される。
【０００７】
そのステップＳＴ２１０４によるＧＰＳ受信機３の観測情報との複合化の処理内容の詳細を図２５に示す。図２５はカルマンフィルタ５２２の演算手順を示した流れ図であるが、自律航法とＧＰＳ航法とを複合化するためのカルマンフィルタ５２２について先に説明する。カルマンフィルタ５２２は信号生成過程と観測過程に分けられる。線形システムで表されるシステムの状態Ｘ（ｔ）に対して、観測行列Ｈで関係付けられるシステムの状態Ｘ（ｔ）の一部が観測できる場合に、フィルタはＸ（ｔ）の最適な推定値を与える。ここで、ωは信号生成過程で発生するシステム誤差であり、ｖは観測過程にて発生する観測誤差である。このフィルタの入力は観測値Ｙ（ｔ）であり、出力はＸ（ｔ）の最適推定値である。時刻ｔにおいて時刻ｔまでの情報を用いた状態ベクトルの推定量Ｘ（ｔ｜ｔ）は、次に示す式（１）により求められる。
【０００８】
ここで、上記式（１）において、Ｘ（ｔ｜ｔ−１）は時刻ｔ−１における時刻ｔでの状態ベクトルの予測量、Ｋ（ｔ）はカルマンゲインであり、それぞれ次の式（２）および式（３）により表される。
【００１０】
なお、上記式（３）において、Σ（ｔ｜ｔ−１）は時刻ｔ−１における時刻ｔでの誤差共分散の予測量である。そして、Σv は観測過程で発生する観測誤差ｖの分散であり、それぞれ次に示す式（４）および式（５）によって表される。
【００１２】
ここで、上記式（４）および式（５）において、Σw は信号過程で発生するシステム誤差ωの分散であり、Ｉは単位行列である。なお、添字のT は転置行列を意味し、-1は逆行列を意味する。さらに、システム誤差ωと観測誤差ｖは平均値０の白色ガウス雑音であり、互いに無相関である。
【００１４】
このようなカルマンフィルタ５２２は、まずシステムの状態Ｘと誤差共分散Σの初期値に適当な誤差を与えてやり、新しい観測が行われる度に、以上の計算を繰り返し行うものである。そこで、上記信号生成過程の定義についてまず説明する。自律航法でのカルマンフィルタ５２２は自律航法の誤差の補正を目的とするので、状態ベクトルの推定量Ｘの要素として、オフセット誤差εＧ、絶対方位誤差εＡ、距離係数誤差εＫ、絶対位置北方向誤差εＹ、および絶対位置東方向誤差εＸの５つの誤差値を定義する。そして、この誤差値の時間的な変化を与えるものが状態遷移行列Ｆである。
【００１５】
オフセット誤差εＧは、確定的な変化はなく、前回の誤差にノイズが付加されたものとして、次に示す式（６）により表される。
【００１６】
εＧt ＝εＧt-1 ＋ω0 ・・・（６）
【００１７】
絶対方位誤差εＡは、前回のオフセット誤差εＧに前回からの経過時間を乗算して求める方位誤差とノイズが付加されるものとして、次の式（７）によって表される。
【００１８】
εＡt ＝ＴεＧt-1 ＋εＡt-1 ＋ω1 ・・・（７）
【００１９】
距離係数誤差εＫは、確定的な変化はなく、前回の距離係数誤差εＫにノイズが付加されたものとして、次の式（８）によって表される。
【００２０】
εＫt ＝εＫt-1 ＋ω2 ・・・（８）
【００２１】
絶対位置北方向誤差εＹ、および絶対位置東方向誤差εＸは、前回の誤差εＹ，εＸに、方位誤差、距離誤差によって生じる誤差が付加されるものとして、次に示す式（９）および式（１０）により表される。なお、これら両式中のＡt は真の絶対方位、Ｌは前回からの移動距離、ｔは前回からの経過時間である。
【００２２】
εＹt ＝ｓｉｎ（Ａt ＋εＡt-1 ＋εＧt-1 ・ｔ／２）・Ｌ・（１＋εＫt-1 ）−ｓｉｎ（Ａt ）・Ｌ＋εＹt-1 ・・・（９）
εＸt ＝ｃｏｓ（Ａt ＋εＡt-1 ＋εＧt-1 ・ｔ／２）・Ｌ・（１＋εＫt-1 ）−ｃｏｓ（Ａt ）・Ｌ＋εＸt-1 ・・・（１０）
【００２３】
また、上記式（９）および式（１０）を状態量で偏微分し線形化すると、信号生成過程は、次に示す式（１１）〜式（１５）により表される。
【００２４】
【数１】
なお、上記式（１１）〜式（１５）において、Ａは真の絶対方位Ａt にセンサ誤差が加わったものであり、進行方位変化から求められる。また、ω0 はオフセット誤差（温度ドリフト等によるオフセット変動分）、ω1 は絶対方位誤差（ジャイロのスケールファクタ誤差）、ω2 は距離係数誤差（経年変化）、ω3 ，ω4 は絶対位置誤差を意味する。
【００２６】
次に観測過程について説明する。
観測値は自律航法とＧＰＳ航法の両出力の差により求める。それぞれの出力には誤差が含まれるため、自律航法の誤差とＧＰＳ観測情報の誤差の和が観測値として得られる。この観測値Ｙとシステムの状態Ｘを関連付け、次に示した式（１６）〜式（１９）のように定義する。
【００２７】
【数２】
上記式（１６）〜式（１９）において、行列の要素における添字DRt は、距離センサ１、相対方位センサ２からの出力信号に基づいて、時刻ｔに自律航法にて求められた値を意味し、同様にして、添字GPStは時刻ｔにＧＰＳ受信機３から出力される値を意味する。観測値Ｙt の要素（εＡDRt −εＡGPSt）は、自律航法によって求められた絶対方位と、ＧＰＳ受信機３から出力される方位の差、すなわち自律航法により求められた絶対方位には真の絶対方位とその誤差εＡDRt が含まれており、またＧＰＳ受信機３から出力される方位も真の絶対方位とその誤差εＡGPStが含まれていることから、それらの差を取ることによって求められるものである。
【００２９】
観測値Ｙt の要素（εＫDRt −εＫGPSt）は、自律航法により求められる速度とＧＰＳ受信機３から出力される速度の差から求まる距離係数誤差であり、（自律航法による速度−ＧＰＳ受信機による速度）／（自律航法による速度）により求められる。また、要素（εＹDRt −εＹGPSt）は自律航法により求められる絶対位置のＹ成分とＧＰＳ受信機３から出力される位置のＹ成分の誤差の差であり、要素（εＸDRt −εＸGPSt）は位置のＸ成分の誤差の差である。
【００３０】
また、観測誤差ｖはＧＰＳ受信機３の誤差であり、観測誤差ｖの各要素は、以下のように求められている。まず、ドップラ周波数の計測誤差とＨＤＯＰ（Ｈｏｒｉｚｏｎｔａｌ Ｄｉｌｕｔｉｏｎ ｏｆ Ｐｒｅｓｉｓｉｏｎ）の関係により、速度精度がドップラ周波数の計測誤差×ＨＤＯＰで求められ、速度精度と車両の速度から方位精度がｔａｎ-1（速度精度／車両の速度）で求められる。これら方位精度と速度精度をそれぞれ２乗することによって、要素（−εＡGPSt）および要素（−εＫGPSt）が求められる。また、ＧＰＳ受信機３における擬似距離の計算誤差ＵＥＲＥ（Ｕｓｅｒ Ｅｑｕｉｖａｌｅｎｔ Ｒａｎｇｉｎｇ Ｅｒｒｏｒ）と上記ＨＤＯＰとの関係から、ＵＥＲＥ×ＨＤＯＰにより測位精度が求められる。要素（−εＹGPSt）と要素（−εＸGPSt）は、この測位精度を２乗することによりそれぞれ求められる。
【００３１】
次に、カルマンフィルタ５２２の処理手順を図２５の流れ図に従って説明する。まずステップＳＴ２５０１において、前回の測位または予測計算からＴ１秒経過したか否かを判定する。この判定処理はＧＰＳ受信機３で２次元あるいは３次元測位が行われる度にステップＳＴ２５０３からステップＳＴ２５０９にて自律航法の誤差を補正する処理を行うが、ＧＰＳ受信機３で２次元あるいは３次元測位できない場合には誤差が大きくなるため、それに対応した誤差の予測計算をステップＳＴ２５１０，ＳＴ２５１１にて定期的に行うために設けられたものである。
【００３２】
前回の測位または予測計算からＴ１秒経過しておらず、ステップＳＴ２５０１による判定の結果がＮｏになると、ステップＳＴ２５０２に分岐して、ＧＰＳ受信機３からの観測情報があるか否かを判定する。ＧＰＳ受信機３からの観測情報があり、ステップＳＴ２５０２の判定結果がＹｅｓになると、ステップＳＴ２５０３以降のカルマンフィルタの演算処理に進む。ステップＳＴ２５０３ではまず、観測値Ｙの計算を行う。これは、ＧＰＳ受信機３から出力される速度、位置、および方位に関する観測情報と、自律航法におけるステップＳＴ２１０３の処理にて求めた絶対方位、絶対位置と、図示していない速度演算処理により距離センサ１の信号から求めた車両の速度とから、上記式（１６）〜式（１９）で示した観測値Ｙを計算するものであり、あわせて観測過程で発生する観測誤差ｖをＧＰＳ受信機３の観測情報等をもとに計算する。
【００３３】
次にステップＳＴ２５０４において、状態遷移行列Ｆの計算を行う。これは前回の状態遷移行列Ｆの計算時点からの移動距離Ｌ、および経過時間ｔと、ステップＳＴ２４０１で求めた絶対方位Ａとから、上記式（１１）〜式（１５）に示す状態遷移行列Ｆを求めるものである。このようにして計算した観測値Ｙおよび状態遷移行列Ｆを基に、上記式（１）〜式（５）による計算を行ってシステムの状態Ｘを求める。すなわち、ステップＳＴ２５０５で誤差共分散の予測計算を行い、ステップＳＴ２５０６ではカルマンゲインの計算を行う。次にステップＳＴ２５０７にて誤差共分散を計算を行い、その後、ステップＳＴ２５０８において、カルマンゲインおよび観測値に基づいて状態ベクトルの推定量を求める。この状態ベクトルの推定量は、オフセット誤差、絶対方位誤差、距離係数誤差、絶対位置北方向誤差および絶対位置東方向誤差を表している。
【００３４】
ステップＳＴ２５０８までの処理が終了するとステップＳＴ２５０９に進み、その計算結果に基づいて自律航法の誤差の修正を行う。すなわち、相対方位センサ３のオフセット補正量は、前回のオフセット補正量よりオフセット誤差εＧを減算することによって、距離センサ１の距離係数は、前回の距離係数に１から距離係数誤差εＫを引いたものを乗算することによって、絶対方位は前回の絶対方位より絶対方位誤差εＡを減算することによって、北方向絶対位置は前回の北方向絶対位置より絶対位置北方向誤差εＹを、東方向絶対位置は前回の東方向絶対位置より絶対位置東方向誤差εＸをそれぞれ減算することによって、自律航法の誤差補正が行われる。これらの処理は、ＧＰＳ受信機３からの観測情報を受信する度に繰り返し実行して、上記誤差修正を行うものである。
【００３５】
また、信号生成過程で発生するシステム誤差ωの行列の各要素、すなわち、オフセット誤差ω0 、絶対方位誤差ω1 、距離係数誤差ω2 、絶対位置誤差ω3 ，ω4 は、それぞれ次のように求められる。まず、オフセット誤差ω0 と絶対方位誤差ω1 には規定値を設定する。その後、車両の挙動状態に応じて発生する所定の誤差発生要因毎の誤差分散の総和として絶対方位誤差ω1 を再計算する。また、絶対位置誤差ω3 ，ω4 については、誤差共分散行列の要素（σAA 2 ）が所定値以下の場合のみに、絶対方位誤差による位置誤差の発生量の大きさをＸ，Ｙ軸に分割した値をもとに計算する。
【００３６】
また、従来のロケータ装置において、装置に内蔵した加速度センサから移動距離を計測するものが提案されている。加速度センサは、加速度ａACC が０の時でも、出力信号ＳＧACC が所定の電圧値（［ｍＶ］、以下、オフセットＯＦACC という）を示すように調整されたもので、加速時にはオフセットＯＦACCより大きな電圧値が、また減速時にはオフセットＯＦACC より小さな電圧値が出力される。この加速度センサの出力信号ＳＧACC から加速度ａACC を求めるには、次の式（２０）に示すように、出力信号ＳＧACC からオフセットＯＦACC を減算した後に、電圧値を加速度の単位に変換するスケールファクタＳＦACC （［ｍ／Ｓ2 ／ｍＶ］）を乗算すればよい。なお、式（２０）の添字ACC は加速度センサ関連のデータを指すものであり、ｉは時間を示すものである。また、ΔＴは加速度センサのサンプリング周期である。
【００３７】
ａACCi＝（ＳＧACCi−ＯＦACCi）×ＳＦACCi×ΔＴ ・・・（２０）
【００３８】
しかしながら、現実には温度変化等の要因によりオフセットＯＦACC とスケールファクタＳＦACC が変動するので、オフセットＯＦACC とスケールファクタＳＦACC を適宜適正な値に補正することが必要とされる。加速度センサの補正については、例えば特開平１０−３０７０３２号公報で提案されているので、ここではその内容を簡単に説明する。
【００３９】
図２６はそのような従来のロケータ装置の構成を示すブロック図であり、図において、１は距離センサ、２は相対方位センサ、３はＧＰＳ受信機、４は車両の前後方向の加速度を計測する上記加速度センサであり、５は信号処理器である。また、信号処理器５内において、５０は距離センサ１、相対方位センサ２、ＧＰＳ受信機３、加速度センサ４の各出力信号をインタフェースするためのインタフェース回路であり、５１はあらかじめメモリ領域に格納されている制御プログラムに従って、車両の挙動情報を推定するとともに、上記各センサの補正を行うマイクロコンピュータである。
【００４０】
次に動作について説明する。
ここで、図２７は上記マイクロコンピュータ５１の全体の処理動作の概要を示す流れ図であり、以下、この図２７に基づいて説明する。ロケータ装置に電源が投入されると、ステップＳＴ２７０１において、まず初期値設定が行われる。次にステップＳＴ２７０２で、加速度センサ４の出力信号に基づく前後方向の加速度の算出と車両の速度の推定を行う。次にステップＳＴ２７０３で、ＧＰＳ受信機３の観測した車両の現在位置、進行方位、速度（以下、ＧＰＳ位置、ＧＰＳ方位、ＧＰＳ速度という）等の観測情報を受信し、さらにステップＳＴ２７０４において、相対方位センサ２の出力信号から車両の進行方位変化を算出する。次にステップＳＴ２７０５に進み、これらの算出値に基づいて車両の走行距離を算出し、ＧＰＳ受信機３および各種センサの出力信号に基づいて車両の現在位置等を推定する。
【００４１】
その後、ステップＳＴ２７０６にて相対方位センサ２の出力信号に基づいて停車状態判定処理と発車状態判定処理を行うか、あるいは、ステップＳＴ２７０７にて相対方位センサ２の出力信号に基づいて停車時または走行中におけるオフセットの更新処理を行う。さらに、ステップＳＴ２７０８にて加速度センサ４の出力信号に基づく停車状態判定処理と発車状態判定処理を行うか、あるいはステップＳＴ２７０９にて加速度センサ４の出力信号に基づいて停車時または走行中のオフセットの更新処理を行う。次にステップＳＴ２７１０に進み、ＧＰＳ受信機３の観測データを基準にして、加速度センサ４の出力信号から推定した速度のリセット処理を行う。次にステップＳＴ２７１１において、ＧＰＳ受信機３の観測情報を基準にして加速度センサ４のスケールファクタ補正を行う。以後、ステップＳＴ２７０２からステップＳＴ２７１１の処理を繰り返し実行する。
【００４２】
次に、加速度センサ４に関する動作の説明を補足する。まずステップＳＴ２７０２において、加速度センサ４の出力信号ＳＧACCに基づく前後方向の加速度ａACCと車両の速度ＶｅｌACC を、上記式（２０）と次の式（２１）を用いてそれぞれ演算し、また、ステップＳＴ２７０５における走行距離Δｄ（［ｍ］）を、次に示す式（２２）にて演算する。
【００４３】
ＶｅｌACCi＝ＶｅｌACCI-1＋ａACCi×ΔＴ ・・・（２１）
Δｄ＝ＶｅｌACCi×ΔＴ＋１／２×ａACCi×ΔＴ2 ・・・（２２）
【００４４】
次に、ステップＳＴ２７０６あるいはステップＳＴ２７０８で行う停車状態判定処理は、加速度センサ４あるいは相対方位センサ２の出力信号の一定時間における標準偏差がそれぞれ所定値以下になった場合、あるいはＧＰＳ測位時にＧＰＳ速度が０ｋｍ／ｈであった場合に停車状態であると判定する。さらに、所定時間における加速度センサ４の出力信号の平均値が負となる減速状態の後で、加速度センサ４の出力信号が所定値以上変化した場合についても停車状態と判定する。一方、発車状態判定処理は、停車状態であると判定している場合に、加速度センサ４あるいは相対方位センサ２の一定時間における出力信号の変化量が所定値より大きくなった時、あるいはＧＰＳ測位中にＧＰＳ速度が０ｋｍ／ｈでなくなった時には発車状態であると判定する。
【００４５】
また、ステップＳＴ２７０９のオフセット値の更新は、前後方向の加速度が発生していない停車状態と等速度走行時に実施するもので、加速度センサ４の出力信号の平均値をオフセットとして設定する。ここで、等速度走行状態とは、加速度センサ４の出力信号により推定した速度が３０ｋｍ／ｈ以上かつ、推定した速度と進行方位変化の変化量が少ない状態であり、ＧＰＳ測位中にはさらにＧＰＳ速度の変化量も少ないことも判定条件に加えている。
【００４６】
ステップＳＴ２７１０では、次の式（２３）および式（２４）に示すように、加速度センサ４による推定速度ＶｅｌACC をＧＰＳ速度ＶｅｌGPS に基づいて修正する。ただし、ｋの最大値は１．０であるものとする。
【００４７】
ＶｅｌACCi＝ＶｅｌACCi＋（ＶｅｌGPSi−ＶｅｌACCi）×ｋ ・・・（２３）
ｋ＝ＶｅｌGPSi／３０ ・・・（２４）
【００４８】
ステップＳＴ２７１１のスケールファクタ補正処理は、等速度走行状態、かつ加速度センサ４の出力信号から算出された加速度の変化量が少ない状態（等加速度走行状態）であって、相対方位センサ２の出力信号から算出された角速度の変化量が少ない状態（等角加速度走行状態）、かつＧＰＳ速度の変化量である加速度の変化量が少ない状態（等加速度走行状態）である場合において実行するものであり、走行状態における加速度センサ４の推定速度ＶｅｌACCiの平均値とＧＰＳ速度ＶｅｌGPSiの平均値を比較して、次の式（２５）に示すように、ＧＰＳ速度の平均値ＶｅｌGPSavei に加速度センサ４の推定速度の平均値ＶｅｌACCavei が合うように加速度センサ４のスケールファクタＳＦACCiを修正する。
【００４９】
ＳＦACCi＝ＶｅｌGPSavei ／ＶｅｌACCavei ×ＳＦACCi-1 ・・・（２５）
【００５０】
【発明が解決しようとする課題】
従来のロケータ装置（特開平８−６８６５５号公報に示されたロケータ装置）は以上のように構成されているので、カルマンフィルタ５２２にてオフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を求めて自律航法におけるそれぞれの補正が行われ、カルマンフィルタ５２２におけるシステム誤差と観測誤差の精度に応じて、カルマンフィルタ５２２で求めるオフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差の精度が決まるものであり、観測誤差の算出に用いるＨＤＯＰとＵＥＲＥは、利用者が位置精度を推定するための指標であるので、その時々の誤差を直接表すことができず、観測誤差を高い精度で設定することがきないという課題があった。
【００５１】
また、システム誤差に関しては、オフセット誤差ω0 と絶対方位誤差ω1 には規定値を設定し、車両の挙動状態に応じて発生する所定の誤差発生要因毎の誤差分散の総和として、絶対方位誤差ω1 を再計算しているため、絶対方位誤差ω1 を高精度で設定できないという課題があり、また絶対位置誤差ω3 ，ω4 についても、誤差共分散行列の方位誤差に関する要素が所定値以下の場合のみに、精度が高くない絶対方位誤差によって位置誤差の発生量の大きさをＸ，Ｙ軸に分割した値として計算しているため、その精度も高く設定することができないという課題があった。
【００５２】
さらに、オフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を状態ベクトルとして同時に計算しているため、システム誤差と観測誤差のどれか一つの要素が不正確だった場合には、状態ベクトルの他の行列要素もカルマンゲインで決まる量だけ値に反映され誤差を生じるという問題があり、従って、このカルマンフィルタ５２２では、各誤差の特徴に合わせた検出および設定ができないという課題があった。
【００５３】
さらに、ＧＰＳ速度は低速走行時に精度が低下することがあるが、これに関する対策が距離係数誤差の計算では示されておらず、またＧＰＳ速度の分散は、距離センサ１の出力信号から計算した速度の分散より大きいので、計算される距離係数誤差の分散が大きくなると考えられ、一方、相対方位センサ２に関しては、オフセット誤差のみが補正対象となっており、相対方位センサ２としてジャイロを使用する場合には、標準値に対して最大で約±２０％も変動し得るスケールファクタに関して考慮していないため、スケールファクタ誤差が原因で生じた方位誤差もオフセット誤差として誤って補正してしまうといった課題もあった。
【００５４】
さらに、特開平１０−３０７０３２号公報に示されたロケータ装置は、加速度センサ４の出力信号から車両の速度を推定し、また加速度センサ４のオフセットに生じたドリフトや、当該ロケータ装置取付け時の傾きに起因する加速度センサ４のスケールファクタの重力影響を補正するために、ＧＰＳ速度を基準にして加速度センサ４のオフセットとスケールファクタを補正するもので、この補正を行う車両の状態としては、停車状態、等速度走行状態、等加速度状態、あるいは等角速度状態の車両が安定した走行状態であることが求められているが、信号機の多い市街地等では、車両が安定した走行状態になることが少ないことから、加速度センサ４のオフセットとスケールファクタの補正が不十分になるばかりか、車両の走行中にドリフトが大きく生じた時にオフセットの補正が不十分だった場合には、たとえ前後方向の加速度が一定であってもオフセットが徐々に変化して、出力信号からオフセットを減算した値が一定でなくなってしまい、車両が加速あるいは減速されている状態にあると誤認識して、さらに補正ができなくなるという課題があった。
【００５５】
また、車両が安定な走行状態になったときに、ＧＰＳ速度が３０ｋｍ／ｈ以上であれば、加速度センサ４による推定速度とスケールファクタの補正量が大きくなっていたため、ＧＰＳ速度の誤差の影響を受けて推定速度とスケールファクタの精度も低下するという課題もあった。
【００５６】
この発明は上記のような課題を解決するためになされたもので、距離センサの出力信号から車両の速度または移動距離を計測する際に、車両により大きく異なる距離センサのスケールファクタを、走行開始後に迅速に補正できるロケータ装置を得ることを目的とする。
【００５７】
また、この発明は、距離センサの出力から車両の速度または移動距離を計測する際に、車両によって大きく異なる距離センサのスケールファクタを、走行開始後に安定して補正できるロケータ装置を得ることを目的とする。
【００５８】
さらに、この発明は、ＧＰＳ受信機が非測位状態となり易い場所を走行していても、走行開始後に直ちにスケールファクタを補正できるロケータ装置を得ることを目的とする。
【００５９】
さらに、この発明は、車両への着脱を容易に行えるロケータ装置を得ることを目的とする。
【００６０】
さらに、この発明は、ＧＰＳ航法と自律航法の複合化に際して、一つの計算方法でＧＰＳ受信機の観測情報と自律航法用のセンサの計測情報から、車両の現在位置と進行方位を簡単、かつ高精度で推定できるロケータ装置を得ることを目的とする。
【００６１】
さらに、この発明は、ジャイロ等の角速度センサ、または相対方位センサのオフセットとスケールファクタの補正を適確に行えるロケータ装置を得ることを目的とする。
【００６２】
さらに、この発明は、スケールファクタの誤補正を防止できるロケータ装置を得ることを目的とする。
【００６３】
さらに、この発明は、ロケータ装置が複数の車両へ載せ換えられて使用される際に、他の車両へ載せ換えられたことを判定できるとともに、載せ換えられたと判定された場合には、現在の車両に係る補正を直ちに行えるロケータ装置を得ることを目的とする。
【００６４】
さらに、この発明は、他の車両へ載せ換えられたときに、車両に係る補正を毎回やり直すことが不要なロケータ装置を得ることを目的とする。
【００６５】
【課題を解決するための手段】
この発明に係るロケータ装置は、絶対位置観測手段と、距離センサを備えるとともに、距離センサの出力信号にスケールファクタを乗算して車両の速度を計算するシステムモデルと、絶対位置観測手段で観測した車両の速度とそのシステムモデルにおける速度との関係を表した観測モデルに基づいて、状態ベクトルの行列要素にシステムモデルにおける速度とスケールファクタを、観測値に絶対位置観測手段で観測した速度をそれぞれ設定したカルマンフィルタによって、状態ベクトルの行列要素である速度とスケールファクタを計算する速度計算手段を備え、速度計算手段におけるシステムモデルと観測モデルでは、距離センサの出力信号と絶対位置観測手段で観測した速度の各平均値を扱い、またその平均値の時間的変化に伴う勾配の差異を絶対位置観測手段で観測した速度の平均値に乗算した値を観測誤差として設定するようにしたものである。
【００６６】
この発明に係るロケータ装置は、速度計算手段において、そのカルマンフィルタの後段にローパスフィルタを設け、このローパスフィルタを通過させることによって、カルマンフィルタにおける状態ベクトルの行列要素である距離センサのスケールファクタの変動を小さくし、それを距離センサの補正値とするようにしたものである。
【００６９】
この発明に係るロケータ装置は、車両の進行方位変化に応じた信号を出力する相対方位センサと、この相対方位センサの出力信号から車両の単位時間毎の進行方位変化を計算する方位変化計算手段を設けるとともに、速度計算手段または速度推定手段による速度から求めた単位時間当たりの移動距離と方位変化計算手段で計算した単位時間毎の進行方位変化から車両の現在位置と進行方位を計算するシステムモデルと、絶対位置観測手段で観測した車両の現在位置とシステムモデルにおける現在位置の関係を表した観測モデルに基づいて、状態ベクトルの行列要素にシステムモデルにおける現在位置と進行方位を、観測値に絶対位置観測手段で観測した車両の現在位置をそれぞれ設定したカルマンフィルタにより、状態ベクトルの行列要素である車両の現在位置と進行方位を計算する位置・方位推定手段を設け、この位置・方位推定手段にて、所定距離以上離れた複数地点または所定時間経過毎の絶対位置観測手段と位置・方位推定手段によるそれぞれの現在位置の差異の変動量を観測誤差として設定し、また所定距離以上離れた複数地点毎の絶対位置観測手段と位置・方位推定手段による両進行方位の差異の平均値をシステム誤差の行列要素である方位誤差として設定し、さらに速度計算手段または速度推定手段と絶対位置観測手段による各速度の積分値の差異をシステム誤差の行列要素である距離誤差として設定するようにしたものである。
【００７０】
この発明に係るロケータ装置は、速度計算手段または速度推定手段で求めた車両の速度が０のときに相対方位センサの出力信号を平均したものをオフセットとするオフセット検出手段を設け、方位変化計算手段にて、相対方位センサの出力信号からオフセットを引いた後にスケールファクタを乗じて車両の単位時間毎の進行方位変化を計算するとともに、その進行方位変化を積算するシステムモデルと、位置・方位推定手段で推定した進行方位とシステムモデルにおける積算方位の関係を表した観測モデルに基づいて、状態ベクトルの行列要素にシステムモデルにおける相対方位センサのオフセットとスケールファクタおよび積算方位を、観測値に位置・方位推定手段で推定した進行方位をそれぞれ設定したカルマンフィルタにより、状態ベクトルの行列要素である相対方位センサのオフセットとスケールファクタを計算して相対方位センサの補正値とし、さらに、停車毎のオフセットの変動量からシステム誤差の行列要素であるオフセット誤差を、また車両旋回時に位置・方位推定手段と方位変化計算手段の各方位情報からそれぞれ求めた車両の旋回角の差異からシステム誤差の行列要素であるスケールファクタ誤差をそれぞれ設定して、位置・方位推定手段で用いたシステム誤差の行列要素である方位誤差を観測誤差として設定するようにしたものである。
【００７１】
この発明に係るロケータ装置は、方位変化計算手段が相対方位センサのオフセットの変動を監視して、カルマンフィルタにおける状態ベクトルの行列要素である当該相対方位センサのオフセットの変動が、あらかじめ定められた所定の値以上になったか否かによって、スケールファクタの補正の有効性を判定するようにしたものである。
【００７２】
この発明に係るロケータ装置は、距離センサのスケールファクタの補正を監視して、その変化が所定割合以上となった時にロケータ装置が他の車両に載せ換えられたと判断する機能を速度計算手段に持たせたものである。
【００７３】
この発明に係るロケータ装置は、距離センサのスケールファクタを記憶する複数のメモリ領域を有する記憶手段と、利用者の操作によって記憶先であるメモリ領域の番地を指定する記憶先指定手段とを設け、指定されたメモリ領域の番地に従って、記憶手段への距離センサのスケールファクタの読み書きを行うようにしたものである。
【００７４】
この発明に係るロケータ装置は、距離センサのスケールファクタとメモリアクセス時刻とを組にして記憶する複数のメモリ領域を有した記憶手段に保持されているスケールファクタを記憶先検索手段にて検索し、計算されたスケールファクタに近い値が記憶手段に記録されている場合にはそのメモリ領域の番地を、記録されていない場合には未使用メモリ領域の番地を、さらに未使用メモリ領域がない場合にはメモリアクセス時刻が最も古いメモリ領域の番地を検索する記憶先検索手段とを設け、この記憶先検索手段にて指定された番地のメモリ領域に、計算した距離センサのスケールファクタを読み書きするとともに、そのスケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を読み書きするようにしたものである。
【００７５】
【発明の実施の形態】
以下、この発明の実施の一形態を説明する。
実施の形態１．
図１はこの発明の実施の形態１によるロケータ装置の構成を示すブロック図である。図において、１は距離センサであり、車両の走行距離に応じてパルス信号を出力する。３は絶対位置観測手段としてのＧＰＳ受信機であり、衛星からの電波を受信するＧＰＳアンテナが接続されており、当該衛星から受信した電波により、少なくとも車両の速度を観測して出力する。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器である。
【００７６】
また、この信号処理器５内において、５１１は距離センサ１からのパルス信号と、ＧＰＳ受信機３からの観測情報とに基づいて、車両の速度を計算するとともに、距離センサ１の出力信号を距離に変換するためのスケールファクタを計算する速度計算手段であり、信号処理器５はこの速度計算手段５１１によって構成されている。
【００７７】
次に動作について説明する。
ここでは、信号処理器５の速度計算手段５１１の動作について図を用いて説明する。図２はこの信号処理器５のメインルーチンの処理内容を示す流れ図であり、図３は所定時間Δｔ毎に実行される割込処理１の処理内容を示す流れ図、図４は距離センサ１から信号が出力されたときに実行される割込処理２の処理内容を示す流れ図、図５はＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３の処理内容を示す流れ図である。
【００７８】
図２に示したメインルーチンは、信号処理器５の速度計算手段５１１の動作を管理するものである。まずステップＳＴ２０１で全ての処理を初期化する。次にステップＳＴ２０２において、割込処理１に係る処理開始のタイミングか否かを処理開始フラグを参照して判断する。その結果、当該処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ２０３へ進み、クリアされていれば処理開始タイミングでないとして、その処理開始フラグがセットされるまで待機する。ステップＳＴ２０３では次回の処理のために処理開始フラグをクリアした後、ステップＳＴ２０４に進む。ステップＳＴ２０４では、まず所定時間ΔＴ毎に実行される割込処理２においてカウントした、距離センサ１の出力信号カウント値ΔＮi について、ＧＰＳ受信機３の観測周期ΔＴGPS 毎にカウント値の平均値ΔＮj を求める。そして、次に示す式（２６）により速度ＶｅｌDRj を計算するとともに、次回の処理のために出力信号カウント値ΔＮi を０にする。
【００７９】
ＶｅｌDRj ＝｜ΔＮj ×ＳＦSPDj｜／ΔＴGPS ・・・（２６）
【００８０】
また、後述する処理で使用するために、距離センサ１の出力信号カウント値ΔＮi の平均値ΔＮj とＧＰＳ速度ＶｅｌGPSjについて、所定時間分の移動平均をとった値ΔＮavejおよびＶｅｌGPSavej も計算しておく。なお、ＧＰＳ受信機３の観測周期はＧＰＳ観測情報受信フラグを参照して判断するものであり、上記式（２６）中のＳＦSPDjは距離センサ１の出力信号を距離に変換するためのスケールファクタである。
【００８１】
次にステップＳＴ２０５において、ＧＰＳ受信機３から観測情報の受信を完了したか否かについて、ＧＰＳ観測情報受信フラグを参照して再度判断する。このＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３からの観測情報の受信が完了したとしてステップＳＴ２０６へ進み、クリアされていれば、まだ受信が完了していないとしてステップＳＴ２１６へ進む。ステップＳＴ２０６では次回の処理のためにそのＧＰＳ観測情報受信フラグをクリアする。以降、ステップＳＴ２０７からステップＳＴ２１６では、距離センサ１のスケールファクタＳＦSPDjを補正するための処理が実行され、以下に説明する計算式（カルマンフィルタ）にて車両の速度ＶｅｌDRj とスケールファクタＳＦSPDjを計算する。
【００８２】
この実施の形態１におけるカルマンフィルタは、離散時刻ｊでの距離センサ１の出力信号ΔＮavejと、スケールファクタＳＦSPDjおよび加速度ａから、以下に示す式（２７）〜式（２９）を用いて速度ＶｅｌDRavejを計算するシステムモデルと、ＧＰＳ受信機３で観測した速度ＶｅｌGPSavej と上記システムモデルの速度ＶｅｌDRavejとの関係を次の式（３０）で表す観測モデルに基づいて、それぞれ以下の式（３１）〜式（３３）で示される状態方程式と、式（３４）〜式（３７）で示される観測方程式、および以下の式（３８）〜式（４２）で示されるカルマンフィルタ方程式を用いて、車両の速度ＶｅｌDRevejと距離センサ１のスケールファクタＳＦSPDjを計算する。
【００８３】
ＶｅｌGPSavej ＝ＶｅｌDRavej＋δＶｅｌGPSavej ・・・（３０）
【００８５】
【数３】
ｙj ＝Ｈ・ｘj ＋ｖj ・・・（３４）
ｙj ＝ＶｅｌGPSavej ・・・（３５）
Ｈ＝［０，１］ ・・・（３６）
ｖj ＝δＶｅｌGPSavej ・・・（３７）
【００８７】
ｘj|j ＝ｘj|j-1 ＋Ｋj ｛ｙj −（Ｈｘj|j-1 ＋ｖj ）｝ ・・・（３８）
ｘj+1|j ＝Ｆj ｘj|j ・・・（３９）
Ｋj ＝Σj|j-1ＨT［ＨΣj|j-1ＨT＋σvj 2 ］-1 ・・・（４０）
Σj|j ＝Σj|j-1 −Ｋj ＨΣj|j-1 ・・・（４１）
Σj+1|j ＝Ｆj Σj|j Ｆj T ・・・（４２）
【００８８】
なお、上記式（２７）〜式（２９）と式（３０）において、ΔＴGPS はＧＰＳ受信機の観測周期、ｊはΔＴGPS に同期した離散時刻、ＳＦSPDjは離散時刻ｊにおける距離センサ１のスケールファクタ、ＶｅｌDRavejは距離センサ１の出力信号から計算した速度の平均値、ＶｅｌGPSavej はＧＰＳ受信機２で観測した速度の平均値、ΔＮavejは離散時刻ｊ−１から離散時刻ｊの間の距離センサ１の出力信号カウント値の平均値、ａは離散時刻ｊ−１から離散時刻ｊの間の加速度、δＶｅｌGPSavej はＧＰＳ受信機２で観測した速度の平均値ＶｅｌGPSavej に含まれる誤差である。
【００８９】
また、上記式（３１）〜式（３３）および式（３４）〜式（３７）において、ｘj 、Ｆj 、ｙj 、ｖj は、離散時刻ｊにおける状態ベトクル、状態遷移行列、観測値、観測誤差であり、Ｈは観測行列である。さらに、式（３８）〜式（４２）において、ｘj|j は離散時刻ｊにおける状態ベクトルの推定量、ｘj+1|j は離散時刻ｊにおける離散時刻ｊ＋１の状態ベクトルの予測量、Ｋj はカルマンゲイン、Σj|j は離散時刻ｊにおける状態ベクトルの共分散行列の推定量、Σj+1|j は離散時刻ｊにおける離散時刻ｊ＋１の共分散行列の予測量、σvj 2 は観測誤差の分散行列である。なお、上記行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【００９０】
図２において、処理がステップＳＴ２０７に進むと、ＧＰＳ受信機３が２次元あるいは３次元測位を行っているか否かを判定する。２次元あるいは３次元測位を行っていればステップＳＴ２０８へ進み、非測位ならばステップＳＴ２１６へ進む。ステップＳＴ２０８では、ＧＰＳ受信機３の速度と距離センサ１の出力信号のそれぞれの変化率を次に示す式（４３）および式（４４）で求める。
【００９１】
ＲａｔｉｏGPSj＝ＶｅｌGPSavej ／ＶｅｌGPSavej-1 −１ ・・・（４３）
ＲａｔｉｏDRj ＝ΔＮavej／ΔＮavej-1−１ ・・・（４４）
【００９２】
そして、連続する所定時間ΔＴn における、それぞれの変化率の時間的変化に伴う勾配を、次に示す式（４５）〜式（４７）を用いて計算して、その変化率の時間的変化に伴う勾配の差異にＧＰＳ速度を乗算したものを、観測誤差として求める。
【００９３】
【数４】
なお、上記式（４３）および式（４４）と式（４５）〜式（４７）において、ＲａｔｉｏGPSjとＲａｔｉｏDRj はＧＰＳ速度と距離センサ１の出力信号のそれぞれの変化率であり、ＬａｍｐＲａｔｉｏGPS とＬａｍｐＲａｔｉｏDRはＧＰＳ速度と距離センサ１の出力信号の時間変化に伴うそれぞれの変化率の勾配である。また、式（４５）〜式（４７）において、ｎは所定時間ΔＴn におけるＲａｔｉｏGPSjとＲａｔｉｏDRj の計算数であり、それらの平均値がＲａｔｉｏDRave とＲａｔｉｏGPSaveである。
【００９５】
次にステップＳＴ２０９に進んで、補正が可能か否かを判定する。このステップＳＴ２０９では、ＧＰＳ速度が所定以上で、かつ変化率の時間的変化に伴う勾配の差異が所定値未満のときに補正可能と判断するもので、補正可能ならばステップＳＴ２１０へ進み、補正不可能ならばステップＳＴ２１６へ進む。ステップＳＴ２１０からステップＳＴ２１４では、上記式（２７）〜式（４２）に従って状態ベクトルの推定量Ｘj|j と予測量Ｘj+1|j 、誤差共分散の推定量Σj|j と予測量Σj+1|j 、およびカルマンゲインＫj を計算する。すなわち、ステップＳＴ２１０で状態ベクトルを推定してその推定量Ｘj|j を、ステップＳＴ２１１で状態ベクトルを予測してその予測量Ｘj+1|j を求め、ステップＳＴ２１２で誤差共分散を推定してその推定量Σj|j を、ステップＳＴ２１３で誤差共分散を予測してその予測量Σj+1|j を求め、さらにステップＳＴ２１４でカルマンゲインＫj を計算する。
【００９６】
次にステップＳＴ２１５において、このようなカルマンフィルタで計算した状態ベクトルの推定量Ｘj|j の行列要素であるスケールファクタ要素Ｘ11の値が正常範囲ならば、このスケールファクタ要素Ｘ11の値を用いて距離センサ１のスケールファクタＳＦSPDjを更新して、ステップＳＴ２１７に進む。一方、ステップＳＴ２１６では、状態ベクトルの速度に関する行列要素を初期化して、ステップＳＴ２１７に進む。ステップＳＴ２１７では状態ベクトルの行列要素である推定量の速度要素Ｘ12の値を出力した後、ステップＳＴ２０２へ戻る。
【００９７】
次に、割込処理１について説明する。
この割込処理１は所定時間Δｔ毎に起動され、図３の流れ図に示すように、そのステップＳＴ３０１において、処理開始タイミングを示す処理開始フラグをセットして、この割込処理１を終了する。
【００９８】
次に、割込処理２について説明する。
この割込処理２は距離センサ１から信号が出力されたときに起動され、図４の流れ図に示すように、そのステップＳＴ４０１において車速信号カウンタに１を加算して、この割込処理２を終了する。
【００９９】
次に、割込処理３について説明する。
この割込処理３はＧＰＳ受信機３から観測周期毎に出力された観測情報を受信して記憶するものであり、図５の流れ図に示すように、まずステップＳＴ５０１でＧＰＳ受信機３から出力された観測情報を受信して記憶する。次にステップＳＴ５０２において全ての観測情報の受信が完了したか否かを判定し、完了していればステップＳＴ５０３へ進み、完了していなければこの割込処理３を終了する。ステップＳＴ５０３では全ての観測情報の受信が完了したとして、ＧＰＳ観測信号受信フラグをセットする。
【０１００】
このように、この実施の形態１によれば、ＧＰＳ受信機３にて衛星からの電波を受信して車両の速度等を観測するとともに、距離センサ１にて車両の移動距離に応じた信号を出力し、速度計算手段５１１において、所定時間におけるＧＰＳ速度と距離センサ出力信号の平均値をそれぞれ計算して、それら各平均値の時間的変化に伴う勾配が所定値以下の時に、カルマンフィルタ方程式に従って、上記所定時間におけるＧＰＳ速度の平均値を観測値として、状態ベクトルの行列要素である距離センサ１のスケールファクタの補正と車両の速度推定を行い、また、ＧＰＳ速度と距離センサ１の出力信号の各平均値の時間的変化に伴う勾配の差異をＧＰＳ速度の平均値に乗算した値をＧＰＳ速度の誤差として使用して、誤差共分散の予測と推定の計算を行い、そして、ＧＰＳ受信機３が非測位状態の時には、カルマンフィルタの方程式に従って、距離センサ１の出力信号とスケールファクタから車両の速度を計算するようにしているので、ＧＰＳ速度の誤差が小さいと判定された時に、専用のカルマンフィルタによって、さらにＧＰＳ速度を基準にした距離センサ１のスケールファクタの補正と車両の速度の推定が行われるため、ＧＰＳ誤差が変動しても、走行開始後に迅速、かつ最適に、距離センサ１のスケールファクタを補正することができ、走行開始後に車両の速度を高い精度で推定できるという効果が得られる。
【０１０１】
実施の形態２．
次に、この発明の実施の形態２について説明する。
この実施の形態２によるロケータ装置は、速度計算手段５１１において、そのカルマンフィルタの後段にローパスフィルタを設けて、カルマンフィルタで計算した距離センサ１のスケールファクタを平滑化するようにしたものであり、その構成は図１のブロック図に示した実施の形態１の場合と同様であるため、図示および説明は省略する。
【０１０２】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態１の場合と同様であり、速度計算手段５１１のメインルーチンである図２に示した流れ図における、ステップＳＴ２１５の動作だけが実施の形態１の場合と異なるものであるため、ここではそのステップＳＴ２１５の動作についてのみ説明し、その他については説明を省略する。
【０１０３】
この実施の形態２においては、カルマンフィルタで計算した状態ベクトルの推定量の行列要素であるスケールファクタに含まれる高周波ノイズを、当該カルマンフィルタの後段にローパスフィルタを配置することによって除去し、このローパスフィルタを通過して平滑化されたスケールファクタを距離センサ１の補正値としている。すなわち、図２のステップＳＴ２１５では、次の式（４８）に示すように、状態ベクトルの推定量の行列要素であるスケールファクタ要素Ｘ11とスケールファクタの前回値ＳＦSPDj-1を重み付け平均して、スケールファクタＳＦSPDjを更新している。なお、この式（４８）において、ｗは重み付け係数である。
【０１０４】
ＳＦSPDj＝ＳＦSPDj-1＋（Ｘ11−ＳＦSPDj-1）×ｗ ・・・（４８）
【０１０５】
このように、この実施の形態２によれば、速度計算手段５１１は、カルマンフィルタの後段に設けたローパスフィルタによって、カルマンフィルタで計算した距離センサのスケールファクタを平滑化しているので、状態ベクトルの行列要素である距離センサ１のスケールファクタの変動を小さく抑えることが可能となって、車両の速度を安定的に推定できるという効果が得られる。
【０１０６】
実施の形態３．
次に、この発明の実施の形態３について説明する。
図６はこの発明の実施の形態３によるロケータ装置の構成を示すブロック図である。図において、１は図１に同一符号を付して示した実施の形態１のそれと同等の距離センサであり、車両の走行距離に応じてパルス信号を出力する。４は加速度センサであり、車両の前後方向の加速度に応じた電圧を出力する。
【０１０７】
また、５１２は加速度センサ４のオフセットを検出するオフセット検出手段であり、５１３は加速度センサ４の出力信号から車両の速度を推定する速度推定手段である。５１１は距離センサ１の出力信号から車両の速度を計算するとともに、速度推定手段５１３の出力に基づいて距離センサ１のスケールファクタを補正する速度計算手段である。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器であるが、上記速度計算手段５１１、オフセット検出手段５１２、および速度推定手段５１３によって構成されている点で、図１に同一符号を付して示した実施の形態１のそれとは異なっている。
【０１０８】
次に動作について説明する。
ここでは、信号処理器５の各手段の動作について図７を用いて説明する。この図７は信号処理器５の各手段の処理内容を示すメインルーチンの流れ図である。この他にも、図３に示した所定時間Δｔ毎に行う割込処理１と、図４に示した距離センサ１から信号が出力されたときに行う割込処理２があるが、それらは実施の形態１で説明したものと同一であるので、その説明は省略する。
【０１０９】
図７に示すメインルーチンは、信号処理器５のオフセット検出手段５１２、速度推定手段５１３、および速度計算手段５１１の各動作を管理するものである。まずステップＳＴ７０１で全ての処理を初期化する。次にステップＳＴ７０２において、割込処理１に係る処理開始タイミングか否かを、処理開始フラグを参照して判断する。その結果、当該処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ７０３へ進み、クリアされていれば処理開始タイミングではないとして、処理開始フラグがセットされるまで待機する。ステップＳＴ７０３では次回の処理のためにその処理開始フラグをクリアする。次にステップＳＴ７０４にて、速度計算手段５１１による速度計算の処理を実行する。すなわち、所定時間毎に割込処理２でカウントした距離センサ１の出力信号カウント値ΔＮi から、前述の式（２６）を用いて速度ＶｅｌDRi を計算するとともに、次回の処理のために出力信号カウント値ΔＮi を０にする。
【０１１０】
次にステップＳＴ７０５に進み、加速度センサ４より出力される、車両の前後方向の加速度に応じた電圧をＡ／Ｄ変換する。次にステップＳＴ７０６にて、オフセット検出手段５１２による加速度センサ４のオフセット検出の処理を実行する。すなわち、所定時間分の加速度センサ４の出力信号の履歴を保存しておき、距離センサ１の出力信号が０になる状態が所定時間以上継続した時に、加速度センサ４の出力信号の履歴として保存されている値の平均値を求め、この平均値を加速度センサ４のオフセットとして保持する。次にステップＳＴ７０７において、速度推定手段５１３による速度推定の処理を実行する。すなわち、オフセット検出手段５１２で加速度センサ４のオフセットが更新された時には推定速度ＶｅｌACC を０にし、それ以外は、前述の式（２０）にて加速度を求めた後、式（２１）で速度ＶｅｌACCを推定する。なお、加速度センサ４のスケールファクタはこの実施の形態３に限り固定値とした。
【０１１１】
その後ステップＳＴ７０８に進み、速度計算手段５１１において距離センサ１のスケールファクタ補正の処理を実行する。すなわち、オフセット検出手段５１２で加速度センサ４のオフセットが更新されると、それ以後所定時間が経過するまでの間は、速度推定手段５１３で推定した速度ＶｅｌACC に速度計算手段５１１で計算した速度ＶｅｌDRが合うように距離センサ１のスケールファクタを補正する。最後にステップＳＴ７０９において、速度計算手段５１１よりその速度ＶｅｌDRを出力した後ステップＳＴ７０２へ戻る。
【０１１２】
このように、この実施の形態３によれば、加速度センサ４の出力信号の変動量が所定値以下の時にその出力信号を平均した値をオフセットとし、当該加速度センサ４の出力信号からオフセットを減算した後、加速度センサ４のスケールファクタを乗算して車両の単位時間毎の加速度を計算し、その加速度を積算して速度を推定し、その推定された速度と距離センサ１の出力信号から計算した速度とが一致するように距離センサ１のスケールファクタを補正して、距離センサ１の出力信号から速度を計算するようにしているので、ＧＰＳ受信機３が非測位状態となり易い場所を走行しても、距離センサ１のスケールファクタを走行開始後に直ちに補正することができ、走行開始後に車両の速度を高い精度で推定することができるという効果が得られる。
【０１１３】
実施の形態４．
次に、この発明の実施の形態４について説明する。
図８はこの発明の実施の形態４によるロケータ装置の構成を示すブロック図である。図において、３は図１に同一符号を付して示した実施の形態１のそれに相当する、絶対位置観測手段としてのＧＰＳ受信機であり、衛星からの電波を受信するＧＰＳアンテナを有し、衛星からの電波により少なくとも車両の速度を観測して出力する。４は図６に同一符号を付して示した実施の形態３のそれと同等の加速度センサであり、車両の前後方向の加速度に応じた電圧を出力する。
【０１１４】
また、５１２は加速度センサ４のオフセットを検出する、図６に同一符号を付して示した実施の形態３のそれと同等のオフセット検出手段である。５１３は車両の速度を計算するとともに、加速度センサ４の出力信号を加速度に変換する際に使用するオフセットとスケールファクタの補正も行う点で、図６に同一符号を付して示した実施の形態３のそれとは異なった速度推定手段である。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器であるが、上記オフセット検出手段５１２、および速度推定手段５１３によって構成されている点で、図１および図６に同一符号を付して示した実施の形態１あるいは実施の形態３のそれとは異なっている。
【０１１５】
次に動作について説明する。
ここでは、信号処理器５のオフセット検出手段５１２と速度推定手段５１３の動作について図９を用いて説明する。この図９は信号処理器５の速度推定手段５１３の処理内容を示すメインルーチンの流れ図である。この他にも、図３に示した所定時間Δｔ毎に行う割込処理１と、図５に示したＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３があるが、それらは実施の形態１で説明したものと同一であるので、その説明は省略する。
【０１１６】
図９に示すメインルーチンは、信号処理器５のオフセット検出手段５１２と速度推定手段５１３の動作を管理するものである。まずステップＳＴ９０１で全ての処理を初期化する。次にステップＳＴ９０２において、割込処理１に係る処理開始タイミングか否かを処理開始フラグを参照して判断する。当該処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ９０３へ進み、クリアされていれば処理開始タイミングでないとして、当該処理開始フラグがセットされるまで待機する。ステップＳＴ９０３では次回の処理のためにその処理開始フラグをクリアする。次にステップＳＴ９０４において、所定時間分の加速度センサ４の出力信号の標準偏差を計算し、当該標準偏差が所定値以下であれば停車状態だと判断し、そうでなけば走行状態だと判断する。ステップＳＴ９０５では上記ステップＳＴ９０４の判断結果に基づいて、車両が走行中であるか否かを判定する。その結果、走行中でなかった場合にはステップＳＴ９０６へ進み、走行中であった場合にはステップＳＴ９０７へ進む。
【０１１７】
このステップＳＴ９０６はオフセット検出手段５１２の処理を示すもので、加速度センサ４の所定時間分の出力信号を平均した値をオフセットとして設定するとともに、推定速度を０とした後、ステップＳＴ９２０へ進む。一方、ステップＳＴ９０７からステップＳＴ９１９は速度推定手段５１３の処理を示すもので、以下に説明するする計算式（カルマンフィルタ）によって車両の速度ＶｅｌDRi 、加速度センサ４のオフセットＯＦACC 、およびスケールファクタＳＦACC を算出した後、ステップＳＴ９２０へ進む。ステップＳＴ９２０では速度推定手段５１３より速度ＶｅｌDRi を出力してステップＳＴ９０２へ戻る。
【０１１８】
この実施の形態４におけるカルマンフィルタは、加速度センサ４の出力信号ＳＧACC と、オフセットＯＦACC およびスケールファクタＳＦACC から、以下に示す式（４９）〜式（５０）を用いて速度ＶｅｌDRi を計算するシステムモデルと、ＧＰＳ受信機３で観測した速度情報ＶｅｌGPSiと上記システムモデルの速度ＶｅｌDRi の関係を、次の式（５１）で表した観測モデルに基づいて、それぞれ以下の式（５２）〜式（５６）で示される状態方程式と、式（５７）〜式（６０）で示される観測方程式、および以下の式（６１）〜式（６５）で示されるカルマンフィルタ方程式を用いて、車両の速度ＶｅｌDRi と、加速度センサ４のオフセットＯＦACC およびスケールファクタＳＦACC を計算する。
【０１１９】
ＶｅｌDRi ＝ＶｅｌDRi-1 ＋（ＳＧACCi−ＯＦACCi）×ＳＦACCi×ΔＴ・・・（４９）
ＯＦACCi＝ＯＦACCi-1＋δＯＦACCi ・・・（５０）
【０１２０】
ＶｅｌGPSi＝ＶｅｌRRi ＋δＶｅｌGPSi ・・・（５１）
【０１２１】
【数５】
ｙi ＝Ｈ・ｘi ＋ｖi ・・・（５７）
ｙi ＝ＶｅｌGPSavei ・・・（５８）
Ｈ＝［１ ０ ０］ ・・・（５９）
ｖi ＝δＶｅｌGPSavei ・・・（６０）
【０１２３】
ｘi|i ＝ｘi|i-1 ＋Ｋi ｛ｙi −（Ｈｘi|i-1 ＋ｖi ）｝ ・・・（６１）
ｘi+1|i ＝Ｆi ｘi|i ＋Ｇi ωi ・・・（６２）
Ｋi ＝Σi|i-1 ＨT ［ＨΣi|i-1 ＨT ＋σvi 2 ］-1 ・・・（６３）
Σi|i ＝Σi|i-1 −Ｋi ＨΣi|i-1 ・・・（６４）
Σi+1|i ＝Ｆi Σi|i Ｆi T＋Ｇi σwi 2 Ｇi T ・・・（６５）
【０１２４】
なお、上記式（４９）〜式（５０）と式（５１）において、ｉは離散時刻、Δｔは所定時間、ＳＧACCiとＯＦACCiおよびＳＦACCiは離散時刻ｉにおける加速度センサ４の出力信号とオフセットおよびスケールファクタ、ＶｅｌDRi は加速度センサ４の出力信号から計算した速度、ＶｅｌGPSiはＧＰＳ受信機３で観測した速度、δＶｅｌGPSiはＶｅｌGPSiに含まれる誤差、δＯＦACCiはＯＦACCiに含まれる誤差である。
【０１２５】
また、上記式（５２）〜式（５６）および式（５７）〜式（６０）において、ｘi ，ωi ，Ｆi ，Ｇi ，ｙi ，ｖi は、離散時刻ｉにおける状態ベトクル、システム誤差、状態遷移行列、駆動行列、観測値、観測雑音であり、Ｈは観測行列はある。さらに、式（６１）〜式（６５）において、ｘi|i は離散時刻ｉにおける状態ベクトルの推定量、ｘi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の状態ベクトルの予測量、Ｋi はカルマンゲイン、Σi|i は離散時刻ｉにおける状態ベクトルの共分散行列の推定量、Σi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の共分散行列の予測量、σwi 2 はシステム誤差の分散行列、σvi 2 は観測誤差の分散行列である。なお、上記行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【０１２６】
図９において、処理がステップＳＴ９０７に進むと、ＧＰＳ受信機３から観測情報の受信を完了したか否かについて、ＧＰＳ観測情報受信フラグを参照して判断する。このＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３からの観測情報の受信を完了したものとしてステップＳＴ９０８へ進み、クリアされていればまだ受信が完了していないものとしてステップＳＴ９１２へ進む。ステップＳＴ９０８ではＧＰＳ受信機３が２次元あるいは３次元測位をしているか否かを判定する。２次元あるいは３次元測位を行っていればステップＳＴ９０９へ進み、非測位ならばステップＳＴ９１２へ進む。
【０１２７】
ステップＳＴ９０９では前述の式（４９）〜式（６５）に従って状態ベクトルの推定量ｘi|i を計算する。次にステップＳＴ９１０において、カルマンフィルタを通じて状態ベクトルの行列要素であるオフセット要素Ｘ12の修正量、すなわち推定値から予測値を減算した値の標準偏差を求め、これをシステム誤差ωであるオフセット誤差として設定する。次にステップＳＴ９１１にて、状態ベクトルの行列要素である速度要素ｘ11とＧＰＳ速度ＶｅｌGPSiの差異の標準偏差を求め、これを観測誤差として設定した後、ステップＳＴ９１２に進む。
【０１２８】
ステップＳＴ９１２では前述の式（４９）〜式（６５）に従って、状態ベクトルの予測量ｘi+1|i を計算する。次にステップＳＴ９１３において、ステップＳＴ９０７と同様にＧＰＳ観測情報受信フラグを参照して、ＧＰＳ受信機３から観測情報の受信が完了したか否かを再度判断する。ＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３から観測情報の受信を完了したものとしてステップＳＴ９１４へ進み、クリアされていればステップＳＴ９１９へ進む。ステップＳＴ９１４では次回の処理のためにそのＧＰＳ観測情報受信フラグをクリアする。
【０１２９】
次にステップＳＴ９１５において、ステップＳＴ９０８と同様にＧＰＳ受信機３が２次元あるいは３次元測位をしているか否かを判定する。２次元あるいは３次元測位を行っていればステップＳＴ９１６へ進み、非測位ならばステップＳＴ９２０へ進む。ステップＳＴ９１６では誤差共分散の推定量Σi|i を、ステップＳＴ９１７では誤差共分散の予測量Σi+1|i を、ステップＳＴ９１８ではカルマンゲインＫi を、それぞれ前述の式（４９）〜式（６５）に従って計算する。次にステップＳＴ９１９において、状態ベクトルの推定量の行列要素である速度要素ｘ11、オフセット要素ｘ12、スケールファクタ要素ｘ13に基づいて、推定速度ＶｅｌDRi 、オフセットＯＦACCi、およびスケールファクタＳＦACCIの更新を行った後、ステップＳＴ９２０へ進む。
【０１３０】
このように、この実施の形態４によれば、加速度センサ４の出力信号の変動量が所定値以下の時に加速度センサ４の出力信号を平均した値をオフセットとし、カルマンフィルタ方程式に従ってＧＰＳ速度を観測値として、状態ベクトルの行列要素である速度、オフセット、およびスケールファクタを推定し、ＧＰＳ受信機３が非測位状態の時にカルマンフィルタ方程式に従って、加速度センサ４の出力信号からオフセットを差し引いた値に加速度センサ４のスケールファクタを乗じて車両の単位時間毎の加速度を計算し、さらにその加速度を積算して速度を推定しているので、ＧＰＳ速度と加速度センサ４のオフセットの誤差が変動しても、加速度センサ４のオフセットとスケールファクタの補正を走行開始後に迅速、かつ最適に補正することができ、走行開始後に車両の速度を高精度で推定できるばかりか、車両に装着された距離センサを使用していないので、このロケータ装置を車両へ容易に着脱できるなどの効果が得られる。
【０１３１】
実施の形態５．
次に、この発明の実施の形態５について説明する。
図１０はこの発明の実施の形態５によるロケータ装置の構成を示すブロック図である。図において、１は図１に同一符号を付して示した実施の形態１のそれと同等の距離センサであり、車両の走行距離に応じてパルス信号を出力する。２は相対方位センサであり、車両のヨーレートに応じた電圧を出力するもので、この実施の形態５ではジャイロが用いられている。３は図１に同一符号を付して示した実施の形態１のそれに相当する、絶対位置観測手段としてのＧＰＳ受信機であり、衛星からの電波を受信するＧＰＳアンテナを有して、衛星からの電波により車両の現在位置を観測して出力する。
【０１３２】
５１１ａは距離センサ１の出力信号より車両の速度および移動距離を計算する距離・速度計算手段で、図１もしくは図６に符号５１１を付して示した、実施の形態１または実施の形態３における速度計算手段に相当するのものである。５１４は相対方位センサ２の出力信号から当該相対方位センサ２のオフセットを検出するオフセット検出手段であり、５１５は相対方位センサ２の出力信号から車両の進行方位の変化を計算する方位変化計算手段である。５１６はＧＰＳ受信機３からの観測情報、距離・速度計算手段５１１ａの出力信号、および方位変化計算手段５１５の出力信号に基づいて車両の現在位置と進行方位を推定する位置・方位推定手段である。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の進行方位と現在位置を演算するコンピュータを含んだ信号処理器であり、これら距離・速度計算手段５１１ａ、オフセット検出手段５１４、方位変化計算手段５１５、および位置・方位推定手段５１６から構成されている点で、図１，図６，および図８に同一符号を付して示された、上記実施の形態１から実施の形態４におけるそれとは異なっている。
【０１３３】
次に動作について説明する。
ここでは、信号処理器５の各手段の動作について図を用いて説明する。図１１は信号処理器５の処理内容を示すメインルーチンの流れ図であり、図１２は位置・方位推定手段５１６の処理内容を示す流れ図である。その他にも、図３に示した所定時間Δｔ毎に行う割込処理１、図４に示した距離センサ１から信号が出力されたときに行う割込処理２、および図５に示したＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３があるが、それらは実施の形態１で説明したものと同一であるので、その説明は省略する。また、図１３は位置・方位推定手段５１６による車両の現在位置の推定例を示す説明図である。
【０１３４】
図１１に示すメインルーチンは、信号処理器５の全手段の動作を管理するものである。まずステップＳＴ１１０１で全ての処理を初期化する。次にステップＳＴ１１０２において、処理開始タイミングか否かを処理開始フラグを参照して判断する。処理開始フラグがセットされていれば、処理開始タイミングであるとしてステップＳＴ１１０３へ進み、クリアされていれば処理開始タイミングではないとして、この処理開始フラグがセットされるまで待機する。ステップＳＴ１１０３では次回の処理のために同フラグをクリアする。次にステップＳＴ１１０４において、距離・速度計算手段５１１ａが、所定時間Δｔ毎に割込処理２でカウントした距離センサ１の出力信号カウント値ΔＮi から、移動距離Δｄi と速度ＶｅｌDRi を次に示す式（６６）と前述した式（２６）により計算し、計算が終了すると、次回の処理のために出力信号カウント値ΔＮi を０にする。
【０１３５】
Δｄi ＝Δｄi-1 ＋ΔＮi ×ＳＦSPDi ・・・（６６）
【０１３６】
次にステップＳＴ１１０５において、オフセット検出手段５１４によるオフセットＯＦGYRiの計算処理を行う。すなわち、ステップＳＴ１１０４で計算した速度ＶｅｌDRi が０（車両停車中）の時は、停車中における相対方位センサ２の所定時間分以上の出力信号ＳＧを平均したものをオフセットＯＦGYRiとする。次にステップＳＴ１１０６に進み、方位変化計算手段５１５にて進行方位変化Δθi の計算処理を行う。すなわち、所定時間Δｔ毎に計測した相対方位センサ２の出力信号ＳＧGYRiから、次に示す式（６７）を用いて進行方位変化Δθi を計算する。なお、この式（６７）中のＯＦGYRiおよびＳＦGYRiは、オフセットと、出力信号を角速度に変換するためのスケールファクタである。
【０１３７】
Δθi ＝Δθi-1 ＋（ＳＧGYRi−ＯＦGYRi）×ＳＦGYRi×Δｔ・・・（６７）
【０１３８】
次にステップＳＴ１１０７において、ＧＰＳ受信機３から観測情報の受信が完了したか否かを、ＧＰＳ観測情報受信フラグを参照して判断する。このＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３から観測情報の受信が完了したものとしてステップＳＴ１１０８へ進み、クリアされていればステップＳＴ１１０２へ戻る。ステップＳＴ１１０８では次回の処理のためにＧＰＳ観測情報受信フラグをクリアする。次にステップＳＴ１１０９において、位置・方位推定手段５１６による位置・方位の推定処理を行う。すなわち、以下に説明する計算式（カルマンフィルタ）にて車両の現在位置（λi ，φi ）と進行方位θi を推定する。次にステップＳＴ１１１０において、その車両の現在位置（λi ，φi ）と進行方位θi 、および速度ＶｅｌDri の出力を行う。次にステップＳＴ１１１１に進んで、ＧＰＳ受信機３から観測情報を受信する間の移動距離Δｄi と進行方位変化Δθi を０にクリアしてステップＳＴ１１０２に戻る。
【０１３９】
図１０に示した位置・方位推定手段５１６による、この実施の形態５におけるカルマンフィルタは、車両の移動距離Δｄi および進行方位変化Δθi から現在位置（λi ，φi ）と進行方位θi を、次に示す式（６８）〜式（７３）で計算するシステムモデルと、ＧＰＳ位置（λGi，φGi）と上記システムモデルの車両の現在位置（λi ，φi ）の関係を次に示す式（７４）〜式（７５）で表す観測モデルに基づくものである。
【０１４０】
λGi＝λi ＋δλGi ・・・（７４）
φGi＝φi ＋δφGi ・・・（７５）
【０１４２】
なお、上記式（６８）〜式（７３）と式（７４）〜式（７５）において、ｉは離散時刻であり、λi-1 とφi-1 は離散時刻ｉ-１における車両の現在位置の経度と緯度、θi-1 は離散時刻ｉ-１における車両の進行方位である。ＳＦd →λ とＳＦd →φ は移動距離の単位［ｍ］を経度方向または緯度方向の移動量［”］へ変換する係数である。また、δλi とδφi は車両の現在位置の経度誤差と緯度誤差で、δΔθi とδΔｄi はΔθi とΔｄi の誤差である。また、δλGiとδφGiはＧＰＳ位置λGiとφGiの誤差である。
【０１４３】
そして次の式（７６）〜式（８０）で示される状態方程式と、次の式（８１）〜式（８４）で示される観測方程式、および次の式（８５）〜式（８９）で示されるカルマンフィルタ方程式を用いて、車両の現在位置（λi ，φi ）と進行方位θi の計算を行う。
【０１４４】
【数６】
【数７】
Ｘi|i ＝Ｘi|i-1 ＋Ｋi ｛Ｙi −（ＨＸi|i-1 ＋ｖi ）｝ ・・・（８５）
Ｘi+1|i ＝Ｆi Ｘi|i ＋Ｇi ｗi ・・・（８６）
Ｋi ＝Σi|i-1 ＨT ［ＨΣi|i-1 ＨT ＋Σvi］-1 ・・・（８７）
Σi|i ＝Σi|i-1 −Ｋi ＨΣi|i-1 ・・・（８８）
Σi+1|i ＝Ｆi Σi|i Ｆi T＋Ｇi ΣwiＧi T ・・・（８９）
【０１４７】
なお、上記式（７６）〜式（８０）および式（８１）〜式（８４）において、ｘi ，Ｆi ，Ｇi ，ｗi ，ｙi ，ｖi は、離散時刻ｉにおける状態ベトクル、状態遷移行列、駆動行列、システム雑音、観測値、観測雑音であり、Ｈは観測行列である。また、上記式（８５）〜式（８９）において、ｘi|i は離散時刻ｉにおける状態ベクトルの推定量、ｘi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の状態ベクトルの予測量、Ｋi はカルマンゲイン、Σi|i は離散時刻ｉにおける状態ベクトルの共分散行列の推定量、Σi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の共分散行列の予測量、Σviは観測誤差の共分散行列、Σwiはシステム誤差の共分散行列である。ここで、行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【０１４８】
次に、図１２の流れ図に従って、位置・方位推定手段５１６の処理内容をより詳細に説明する。まずステップＳＴ１２０１において、距離・速度計算手段５１１ａで計算した速度ＶｅｌDRi が０より大きいか否かを判定する。その結果、０であれば停車中と判断してこの位置・方位の推定処理を終了し、０より大きければステップＳＴ１２０２へ進む。ステップＳＴ１２０２では、ＧＰＳ受信機３が２次元あるいは３次元測位状態で、かつ前回カルマンフィルタにより車両の現在位置と進行方位を推定してから所定の距離以上移動したか否かを判定する。判定の結果、ＧＰＳ受信機３にて２次元あるいは３次元測位状態で、かつ前回カルマンフィルタにより現在位置と進行方位を推定してから所定の距離以上移動した場合にはステップＳＴ１２０３へ進み、逆にＧＰＳ受信機３にて非測位状態か、あるいは前回カルマンフィルタにより現在位置と進行方位を計算してから所定の距離以上移動していない場合にはステップＳＴ１２０５へ進む。
【０１４９】
ステップＳＴ１２０３では、所定時間におけるＧＰＳ位置（λGi，φGi）とカルマンフィルタによる現在位置（λi ，φi ）の位置の差分、すなわち緯度差と経度差のそれぞれの標準偏差を計算してその標準偏差を観測誤差ｖi とするとともに、観測誤差の共分散行列Σviを計算する。次にステップＳＴ１２０４において、ＧＰＳ速度の積分値と距離センサ１から求めた距離Δｄi の差分を、システム誤差の行列要素である距離誤差要素δΔｄi とするとともに、所定の距離だけ離れた複数地点でのＧＰＳ位置と車両の現在位置の軌跡の両移動方向の方向差を、システム誤差の行列要素である方位誤差要素δΔθi とした後、ステップＳＴ１２０６に進む。一方、ステップＳＴ１２０５ではカルマンゲインＫi を０にクリアして、ステップＳＴ１２０６に進む。
【０１５０】
ステップＳＴ１２０６ではＧＰＳ位置を観測値ｙi として、前述の式（６８）〜式（８９）に従って状態ベクトルの推定量ｘi|i を計算するが、ここで、相対方位センサ２のオフセット変動量に比例した値よりカルマンゲインＫi の全行列要素が小さくなるようにカルマンゲインＫi の全行列要素を調節して、状態ベクトルの推定量Ｘi|i を計算する。次にステップＳＴ１２０７において、前述の式（８５）〜式（８９）により状態ベクトルの予測量Ｘi+1|i を計算する。次にステップＳＴ１２０８において、ステップＳＴ１２０２と同様に、ＧＰＳ受信機３が２次元あるいは３次元測位状態で、かつ前回カルマンフィルタにより現在位置（λi ，φi ）と進行方位θi を計算してから所定の距離以上移動したか否かを判定する。ＧＰＳ受信機３にて２次元あるいは３次元測位しており、かつ前回カルマンフィルタにより現在位置（λi ，φi ）と進行方位θi を計算してから所定の距離以上移動した場合はステップＳＴ１２０９へ進み、そうでなければこの位置・方位推定の処理を終了する。
【０１５１】
ステップＳＴ１２０９では誤差共分散の推定量Σi|i を、ステップＳＴ１２１０では誤差共分散の予測量Σi+1|i を、ステップＳＴ１２１１ではカルマンゲインＫi を、それぞれ前述の式（８５）〜式（８９）を用いて計算し、この位置・方位推定の処理を終了する。
【０１５２】
次に、図１３を用いて、位置・方位推定手段５１６における車両の現在位置の推定動作について説明する。図１３（ａ）は車両が直進走行する途中でＧＰＳ位置の分散が大きくなった時に、車両の現在位置が推定される様子を示したものである。この場合にはシステム誤差の行列要素である方位誤差要素と距離誤差要素がともに小さい状態であったことを想定している。この時、位置・方位推定手段５１６はＧＰＳ位置の分散が大きくなった区間では、観測誤差であるＧＰＳ位置の誤差が大きくなったことを検出するので、ＧＰＳ位置の分散が大きくなった区間のみカルマンゲインを小さく計算する。その結果、ＧＰＳ位置の分散が小さい区間では車両の現在位置がＧＰＳ位置へ寄るように推定するが、ＧＰＳ位置の分散が大きい区間では車両の現在位置がＧＰＳ位置へ寄る量を小さく抑えて推定する。これにより、カルマンフィルタで推定した現在位置の軌跡は、図１３（ａ）に点線で示すものとなる。
【０１５３】
また図１３（ｂ）は車両が直進走行する途中で相対方位センサ２のオフセットがドリフトした時に、車両の現在位置が推定される様子を示したものである。この場合にはＧＰＳ位置の分散が小さい状態であったことを想定している。この時、位置・方位推定手段５１６はＧＰＳ位置の分散が小さい一方で、単位時間毎の進行方位変化が偏った値となって方位誤差が発生したことを検出するので、カルマンゲインを大きく計算する。その結果、相対方位センサ２に生じたドリフトの影響を受けずに車両の現在位置をＧＰＳ位置の方へ寄せるように推定する。これにより、カルマンフィルタで推定した現在位置の軌跡は、図１３（ｂ）に点線で示すものとなる。なお、カルマンフィルタを使用しない一般的な自律航法を用いた場合には、相対方位センサ２に生じたドリフトの影響を受けて、図１３（ｂ）に一点鎖線で示すように、車両の現在位置が偏った方向に徐々に旋回する。
【０１５４】
また図１３（ｃ）は相対方位センサ２のスケールファクタが真値より小さ目の時に車両が右折した場合の、車両の現在位置が推定される様子を示したものである。この場合にはＧＰＳ位置の分散が小さい状態であったことを想定している。この時、位置・方位推定手段５１６はＧＰＳ位置の分散が小さい一方で、右折後に方位誤差が発生したことを検出するので、右折後にはカルマンゲインをより大きく計算する。その結果、右折時の旋回角が小さ目になることから、推定した進行方位に誤差が生じるものの、右折後に方位誤差を検出した以後は車両の現在位置をＧＰＳ位置の方へ寄せるように推定する。これにより、カルマンフィルタで推定した現在位置の軌跡は、図１３（ｃ）に点線で示すものとなる。なお、カルマンフィルタを使用しない一般的な自律航法の場合には、図１３（ｃ）に一点鎖線で示すように、旋回角不足のまま車両の現在位置が誤った方向に直進する。
【０１５５】
なお、上記説明では、実施の形態１に示したロケータ装置に、相対方位センサ２、方位変化計算手段５１５、および位置・方位推定手段５１６を付加したものを示したが、実施の形態３もしくは実施の形態４に示したロケータ装置にそれらを付加することによって実現することも可能であり、その場合にも同様の効果を奏する。
【０１５６】
このように、この実施の形態５によれば、相対方位センサ２の出力信号から車両の単位時間毎の進行方位変化を計算するとともに、カルマンフィルタ方程式により、ＧＰＳ位置を観測値として状態ベクトルの行列要素である車両の現在位置と進行方位を推定し、ＧＰＳ受信機３と位置・方位推定手段５１６による現在位置の差異の変動量を観測誤差として使用して、所定の距離以上離れた複数地点毎のＧＰＳ受信機３位置・方位推定手段５１６による進行方位の差異の平均値をシステム誤差の行列要素である方位誤差として設定し、さらに距離・速度計算手段５１１ａで求めた速度の積分値とＧＰＳ速度の積分値との差異を、システム誤差の行列要素である距離誤差として使用して、誤差共分散の予測と推定の計算を行い、ＧＰＳ受信機３が非測位状態の時には、カルマンフィルタ方程式に従って、距離・速度計算手段５１１ａによる速度から求めた単位時間当たりの移動距離と、方位変化計算手段５１５で計算した単位時間毎の進行方位の変化から車両の現在位置と進行方位を推定しているので、ＧＰＳ位置と自律航法用のセンサからの計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定することができて、信頼性が向上し、また、ＧＰＳ受信機が非測位状態でも自律航法用のセンサの計測情報から車両の現在位置と進行方位を推定できるので、トンネルや高架下の道路においても、安定的に車両の現在位置と進行方位を提供することが可能になるなどの効果が得られる。
【０１５７】
実施の形態６．
次に、この発明の実施の形態６について説明する。
図１４はこの発明の実施の形態６によるロケータ装置の構成を示すブロック図である。図において、１は距離センサ、２は相対方位センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、これらは図１０に同一符号を付して示した実施の形態５におけるそれらと同等の部分である。なお、上記ＧＰＳ受信機３は車両の現在位置とともにその進行方位も観測して出力している。また、５１１ａは距離・速度計算手段、５１４はオフセット検出手段であり、これらも図１０に同一符号を付して示したものと同等の部分である。
【０１５８】
５１５は相対方位センサ２の出力信号から車両の進行方位変化を計算する方位変化計算手段であるが、相対方位センサ２の出力信号を角速度に変換するスケールファクタの補正も行っている点で、図１０に同一符号を付して示したものとは異なっている。５１６はＧＰＳ受信機３からの観測情報、距離・速度計算手段５１１ａの出力信号、および方位変化計算手段５１５の出力信号に基づいて車両の現在位置と進行方位を推定する位置・方位推定手段であるが、相対方位センサ２のオフセットと、相対方位センサ２の出力信号を角速度に変換するスケールファクタを補正するための信号を出力している点で、図１０に同一符号を付して示したものとは異なっている。５はこれら距離・速度計算手段５１１ａ、オフセット検出手段５１４、方位変化計算手段５１５、および位置・方位推定手段５１６から構成されて、あらかじめメモリ内に記憶されている制御プログラムに従って車両の進行方位と現在位置を演算するコンピュータを含んだ信号処理器である。
【０１５９】
次に動作について説明する。
以下、信号処理器５の各手段の動作について図を用いて説明する。なお、この信号処理器５における処理内容のメインルーチンは実施の形態５の場合と同様であり、図１１に示したメインルーチンの流れ図中の、ステップＳＴ１１０６の方位変化計算処理だけが実施の形態５と異なるものである。従って、ここでは方位変化計算処理以外については、説明を省略する。また、その他にも、図３に示した所定時間Δｔ毎に行う割込処理１、図４に示した距離センサ１から信号が出力されたときに行う割込処理２、および図５に示したＧＰＳ受信機３から観測周期毎に出力された観測情報を受信する割込処理３があるが、それらは実施の形態１で説明したものと同一であるので、その説明も省略する。
【０１６０】
ここで、図１５は相対方位センサ２の出力信号から車両の進行方位変化を計算するとともに、相対方位センサ２の出力信号を角速度に変換するスケールファクタを補正する、方位変化計算手段５１５による上記方位変化計算処理の処理内容を示す流れ図である。この方位変化計算処理が開始されると、まずステップＳＴ１５０１において、所定時間Δｔ毎に計測した相対方位センサ２の出力信号ＳＧGYRiから進行方位変化Δθi を前述の式（６７）で計算する。次にステップＳＴ１５０２において、所定時間Δｔ毎の上記進行方位変化Δθi を積算して積算方位ΣΔθi を更新する。以降、ステップＳＴ１５０３からステップＳＴ１５１５において、以下に説明するカルマンフィルタにて、相対方位センサ２から求めた積算方位ΣΔθi と、位置・方位推定手段５１６で推定した進行方位θi とを比較して、相対方位センサ２のオフセットＯＦGYRiとスケールファクタＳＦGYRiの補正を行う。
【０１６１】
この実施の形態６におけるカルマンフィルタは、相対方位サンサ２の出力信号ＳＧGYRiとオフセットＯＦGYRi、およびスケールファクタＳＦGYRiから進行方位変化Δθi を計算して、次に示す式（９０）〜式（９２）を用いて積算するシステムモデルと、位置・方位推定手段５１６で推定した車両の進行方位θi と上記システムモデルによって積算方位ΣΔθi の関係を次の式（９３）で表す観測モデルとに基づくものである。
【０１６２】
θi ＝ΣΔθi ＋δθi ・・・（９３）
【０１６４】
なお、これら式（９０）〜式（９３）において、ｉは離散時刻、Δｔは所定時間であり、ＳＧGYRi、ＯＦGYRi、ＳＦGYRiは離散時刻ｉにおける相対方位センサ２の出力信号とオフセットおよびスケールファクタ、δＯＦGYRiとδＳＦGYRiはオフセット誤差とスケールファクタ誤差、δθi は推定された進行方位θi の誤差である。
【０１６５】
そして、次の式（９４）〜式（９８）に示す状態方程式と、次の式（９９）〜式（１０２）で示す観測方程式、および次に示す式（１０３）〜式（１０７）のカルマンフィルタ方程式により、相対方位センサ２のオフセットＯＦGYRiとスケールファクタＳＦGYRiを計算する。
【０１６６】
【数８】
ｙi ＝Ｈ・ｘi ＋ｖi ・・・（９９）
ｙi ＝θi ・・・（１００）
ｖi ＝δθi ・・・（１０１）
Ｈ＝［１ ０ ０］ ・・・（１０２）
【０１６８】
ｘi|i ＝ｘi|i-1 ＋Ｋi ｛ｙi −（Ｈｘi|i-1 ＋ｖ）｝ ・・・（１０３）
ｘi+1|i ＝Ｆi ｘi|i ＋Ｇi ｗ ・・・（１０４）
Ｋi ＝Σi|i-1 ＨT ［ＨΣi|i-1 ＨT ＋σvi 2 ］-1 ・・・（１０５）
Σi|i ＝Σi|i-1 −Ｋi ＨΣi|i-1 ・・・（１０６）
Σi+1|i ＝Ｆi Σi|i Ｆi T＋Ｇi Σｗ iＧi T ・・・（１０７）
【０１６９】
なお、上記式（９４）〜式（９８）および式（９９）〜式（１０２）において、ｘi ，Ｆi ，Ｇi ，ｗi ，ｙi ，ｖi は離散時刻ｉにおける状態ベトクル、状態遷移行列、駆動行列、システム雑音、観測信号および観測雑音であり、Ｈは観測行列である。また上記式（１０３）〜式（１０７）において、ｘi|i は離散時刻ｉにおける状態ベクトルの推定量、ｘi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の状態ベクトルの予測量、Ｋi はカルマンゲイン、Σi|i は離散時刻ｉにおける共分散行列の推定量、Σi+1|i は離散時刻ｉにおける離散時刻ｉ＋１の共分散行列の予測量、Σwiはシステム誤差ｗi の共分散分散、およびσvi 2 は観測誤差ｖi の分散である。ここで、行列の各要素は行列式を計算すれば求まるものなので、ここではその説明は省略する。
【０１７０】
図１５において、処理がステップＳＴ１５０３に進むと、ＧＰＳ受信機３から観測情報の受信を完了したか否かを、ＧＰＳ観測情報受信フラグを参照して判断する。当該ＧＰＳ観測情報受信フラグがセットされていれば、ＧＰＳ受信機３から観測情報の受信が完了したとしてステップＳＴ１５０４へ進み、クリアされていればこの方位変化計算処理を終了する。ステップＳＴ１５０４では距離センサ１から求めた速度ＶｅｌDRi が０より大きいか否かを判定する。判定の結果、速度ＶｅｌDRi が０より大きければ、車両が走行中であるとしてステップＳＴ１５０６へ進み、速度ＶｅｌDRi が０ならば、ステップＳＴ１５０５に分岐して状態ベクトルの行列要素であるオフセット要素Ｘ12と積算方位ΣΔθi を初期化した後、当該方位変化計算処理を終了する。なお、初期値には、位置・方位推定処理５１６で推定した進行方位θi と、オフセット検出手段５１４で検出した相対方位センサ２のオフセットＯＦGYRiが用いられる。
【０１７１】
ステップＳＴ１５０６ではＧＰＳ受信機３が２次元あるいは３次元測位をしているか否かを判定する。その結果、ＧＰＳ受信機３が２次元あるいは３次元測位をしていればステップＳＴ１５０７へ進み、非測位ならばこの方位変化計算処理を終了する。ステップＳＴ１５０７ではＧＰＳ受信機３での２次元あるいは３次元測位を継続している時間が所定時間以上であるか否かについて判定する。判定の結果、測位の継続時間が所定時間以上ならばステップＳＴ１５０８へ進み、所定時間未満ならばステップＳＴ１５０８〜ステップＳＴ１５１０をスキップして、直接ステップＳＴ１５１１へ進む。ステップＳＴ１５０８では、位置・方位推定手段５１６で推定した進行方位θi の誤差δθi を観測誤差ｖi とし、所定時間Δｔにおける観測誤差ｖi の分散を観測誤差の分散σvi 2 として、処理をステップＳＴ１５０９に進める。
【０１７２】
ステップＳＴ１５０９では、オフセット検出手段５１４で検出した相対方位センサ２のオフセットＯＦGYRiに、オフセット勾配であるドリフトにオフセット検出後の経過時間を乗算した値を加算した後、状態ベクトルの行列要素であるオフセット要素Ｘ12から減算した値を、システム誤差ｗi の行列要素であるオフセット誤差要素δＯＦGYRiとする。また、所定時間内に車両の進行方位が所定値以上変化した場合には、車両の進行方位θi と状態ベクトルの行列要素である積算方位要素Ｘ11の方位との差を求め、さらに相対方位センサ２の進行方位変化を図示を省略したステップで別途積算した角度で除算した値に、状態ベクトルの行列要素であるスケールファクタ要素Ｘ13と所定の係数をともに乗算した値を、スケールファクタ誤差δＳＦGYRiとする。そして所定時間におけるオフセット誤差δＯＦGYRiとスケールファクタ誤差δＳＦGYRiの分散を求めてシステム誤差の共分散Σwiを計算する。
【０１７３】
次にステップＳＴ１５１０に進んで、状態ベクトルの推定量ｘi|i を計算する。次にステップＳＴ１５１１において状態ベクトルの予測量ｘi+1|i を、ステップＳＴ１５１２において誤差共分散の推定量Σi|i を、ステップＳＴ１５１３において誤差共分散の予測量Σi+1|i を、ステップＳＴ１５１４においてカルマンゲインＫi をそれぞれ計算する。なお、これらの計算は前述の式（９０）〜式（１０７）を用いて行われる。次に、ステップＳＴ１５１５において、その状態ベクトルの推定量ｘi|i に基づいて、相対方位センサ２のオフセットＯＦGYRiとスケールファクタＳＦGYRiの更新を行った後、この方位変化計算処理を終了する。
【０１７４】
このように、この実施の形態６によれば、距離・速度計算手段５１１ａで求めた車両の速度が０のときに相対方位センサ２の出力信号を平均したものをオフセットとし、カルマンフィルタ方程式に従って位置・方位推定手段５１６で推定した進行方位を観測値とし、状態ベクトルの行列要素である相対方位センサ２のオフセットとスケールファクタを推定するとともに、停車毎のオフセットの変動量からオフセットを予測した値と現時点のオフセットの差異をシステム誤差の行列要素であるオフセット誤差として使用し、車両旋回時に位置・方位推定手段５１６と方位変化計算手段５１５の各方位情報からそれぞれ求めた車両の旋回角の差異からシステム誤差の行列要素であるスケールファクタ誤差を設定しているので、推定した車両の進行方位と相対方位センサのオフセットおよびスケールファクタに誤差があっても、相対方位センサ２のオフセットとスケールファクタを最適に補正することが可能となり、ＧＰＳ受信機３が非測位状態であっても、またＧＰＳ受信機３の観測情報の誤差が大きくなる徐行運転時であっても、相対方位センサ２のオフセットとスケールファクタを常時補正できるため、相対方位センサ２から進行方位変化を高い精度で求めることができるなどの効果が得られる。
【０１７５】
実施の形態７．
次に、この発明の実施の形態７について説明する。
この実施の形態７によるロケータ装置は、実施の形態６における相対方位センサ２のオフセットが所定値を越えた場合、そのスケールファクタの補正を行わないようにしたものであり、その構成は図１４のブロック図に示した実施の形態６の場合と同様であるため、図示および説明は省略する。
【０１７６】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態６の場合と同様であり、方位変化計算手段５１５による方位変化計算処理の処理内容を示した図１５の流れ図における、ステップＳＴ１５１５の動作だけが実施の形態６と異なるものであるため、ここではそのステップＳＴ１５１５の動作についてのみ説明し、その他については説明を省略する。
【０１７７】
この実施の形態７においては、図１５の流れ図に示したステップＳＴ１５１５において、相対方位センサ２のオフセットの変動があらかじめ定められた所定の値以上であれば、カルマンフィルタでスケールファクタが所定値以上補正されないようにカルマンゲインを調整して、状態ベクトルを前述の式（９０）〜式（１０７）に従って計算するか、実施の形態６の場合と同様の計算をしてもスケールファクタを更新しないようにしている。
【０１７８】
このように、この実施の形態７によれば、カルマンフィルタにおける状態ベクトルの行列要素である相対方位センサ２のオフセットの変動があらかじめ定めた値以上になると、状態ベクトルの行列要素である相対方位センサ２のスケールファクタの補正を無効とし、相対方位センサ２のオフセットの変動が小さい時にのみ相対方位センサ２のスケールファクタの補正を行っているので、相対方位センサ２のスケールファクタの誤補正を防止することができ、その結果、進行方位の変化を高い精度で求めることが可能になるという効果が得られる。
【０１７９】
実施の形態８．
次に、この発明の実施の形態８について説明する。
この実施の形態８によるロケータ装置は、実施の形態１の速度計算手段５１１に、当該ロケータ装置が他の車両に載せ換えられたことを検出して、車両に係る情報、あるいは当該ロケータ装置の取り付けに係る情報を初期化する機能を持たせるようにしたものであり、その構成は図１のブロック図に示した実施の形態１の場合と同様であるため、図示および説明は省略する。
【０１８０】
次に動作について説明する。
なお、その動作についても、基本的には実施の形態１の場合と同様であり、速度計算手段５１１のメインルーチンである図２に示した流れ図における、ステップＳＴ２１５とステップＳＴ２１７の動作だけが実施の形態１の場合と異なるものであるため、ここではそのステップＳＴ２１５およびステップＳＴ２１７の動作についてのみ説明し、その他については説明を省略する。
【０１８１】
この実施の形態８では、図２の流れ図に示したステップＳＴ２１５において、カルマンフィルタで計算した状態ベクトルの行列要素であるスケールファクタ要素の値が正常範囲内であれば、当該スケールファクタ要素の最新計算値を所定時間分だけ保持する。そして、このスケールファクタ要素の計算値の所定時間分の平均値に対する標準偏差の比率が所定値以下のときに、当該平均値を用いて距離センサ１のスケールファクタの更新を行う。また、上記スケールファクタが前回の値から所定の比率以上更新された場合には、ロケータ装置が他の車両へ載せ換えられたと判断して、車両に係る情報あるいは当該ロケータ装置の取り付けに係る情報を初期化する。その後、ステップＳＴ２１７において、車両の速度などとともに、このロケータ装置が他の車両へ載せかえられたか否かの判定結果を出力する。
【０１８２】
このように、この実施の形態８によれば、距離センサ１のスケールファクタが所定割合以上変化した時に、当該ロケータ装置が他の車両に載せ換えられたと判断して、車両に係る情報あるいは当該装置の取付けに係る情報を初期化する機能を速度計算手段５１１に持たせるようにしているので、ロケータ装置を複数の車両に載せ換えて使用するような場合でも、現在搭載されている車両に係る補正を直ちに行うことが可能となり、車両の挙動情報を最適に設定することができるという効果が得られる。
【０１８３】
実施の形態９．
次に、この発明の実施の形態９について説明する。
図１６はこの発明の実施の形態９によるロケータ装置の構成を示すブロック図である。図において、１は距離センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、これらは図１に同一符号を付して示した実施の形態１におけるそれらに相当する部分である。なお、上記ＧＰＳ受信機３は車両の現在位置および進行方位を観測して出力している。
【０１８４】
また、５２は車種毎に異なった距離センサ１のスケールファクタを記憶する複数のメモリ領域を有する記憶手段であり、５３はそのスケールファクタの記憶先のメモリ領域を利用者が記憶手段５２へ指示するためのスイッチを備えた記憶先指定手段である。５１１は距離センサ１の出力信号を距離に変換するためのスケールファクタを計算する速度計算手段であるが、この記憶先指定手段５３で指定された記憶手段５２のメモリ領域に、距離センサ１のスケールファクタを読み書きする点で、図１に同一符号を付して示したものとは異なっている。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含んだ信号処理器であるが、これら記憶手段５２、記憶先指定手段５３、および速度計算手段５１１から構成されている点で、図１，図６，図８，図１０，および図１４に同一符号を付して示した上記各実施の形態のそれらとは異なっている。
【０１８５】
次に動作について説明する。
以下、信号処理器５の動作について図を用いて説明する。ただし、この信号処理器５における速度計算手段５１１の動作は実施の形態１の場合と同様であるため、ここではその説明は省略し、記憶手段５２へのスケールファクタの読み書きの動作について説明する。なお、図１７は記憶手段５２のメモリマップを簡単に示した説明図である。まず、記憶先指定手段５３は利用者によって操作されるスイッチの設定状態を確認し、その設定状態に応じてあらかじめ決めておいたメモリ領域の番地を記憶手段５２に指示する。記憶手段５２は、記憶先指定手段５３より指示された番地（図１７に示すｘ番地〜ｘ＋ｎ−１番地）のメモリ領域に対して、速度計算手段５１１で求めた、車種毎に異なる距離センサ１のスケールファクタ（図１７ではＳＦ値１〜ＳＦ値ｎ）の読み書きを行う。
【０１８６】
このように、この実施の形態９によれば、車種毎に異なる距離センサ１のスケールファクタを記憶手段５２に記憶する一方で、利用者が指定した距離センサ１のスケールファクタの記憶先であるメモリ領域の番地を記憶先指定手段５３で読み取り、記憶手段５２の指定された番地のメモリ領域へ距離センサ１のスケールファクタを読み書きしているので、距離センサ１のスケールファクタを手動で切り替えた場合には、ロケータ装置を他の車両へ載せ換えた時に車両に係る補正を毎回やり直す必要がなくなり、他の車両へ載せ換えた直後から、車両の速度を高い精度で推定することができるという効果が得られる。
【０１８７】
実施の形態１０．
次に、この発明の実施の形態１０について説明する。
図１８はこの発明の実施の形態１０によるロケータ装置の構成を示すブロック図である。図において、１は距離センサ、３は絶対位置観測手段としてのＧＰＳ受信機であり、これらは図１に同一符号を付して示した実施の形態１におけるそれらに相当する部分である。なお、上記ＧＰＳ受信機３は車両の現在位置および進行方位を観測して出力している。
【０１８８】
また、５２は車種毎に異なった距離センサ１のスケールファクタとメモリアクセス時刻とを組にして記憶する複数のメモリ領域を有した記憶手段である。５４は後述する速度計算手段５１１で計算されたスケールファクタに基づいて、記憶手段５２に記憶されたスケールファクタとメモリアクセス時刻の履歴を検索し、検索結果としてのメモリ領域の番地と記憶されているスケールファクタとを、記憶手段５２と速度計算手段５１１にそれぞれ伝える記憶先検索手段である。５１１は車両の速度を計算するとともに、距離センサ１の出力信号を距離に変換するためのスケールファクタを計算する速度計算手段であるが、スケールファクタとメモリアクセス時刻の組の記憶手段５２の各メモリ領域への読み書きを行い、さらに記憶先検索手段５２に計算したスケールファクタを送り、記憶先検索手段５４の検索結果を受け取っている点で、図１６に同一符号を付して示した実施の形態９のそれとは異なっている。５はあらかじめメモリ内に記憶されている制御プログラムに従って車両の速度を演算するコンピュータを含む信号処理器であるが、これら記憶手段５２、記憶先検索手段５４、および速度計算手段５１１から構成されている点で、図１，図６，図８，図１０，図１４，および図１６に同一符号を付して示した上記各実施の形態のそれとは異なっている。
【０１８９】
次に動作について説明する。
以下、信号処理器５の各手段の動作について図を用いて説明する。ただし、この信号処理器５における速度計算手段５１１については実施の形態８の場合と同様であるため、ここではその説明は省略する。なお、図１９は記憶手段５２のメモリマップを簡単に示した説明図である。
【０１９０】
速度計算手段５１１にて距離センサ１のスケールファクタを計算し、実施の形態８の場合と同様にして当該ロケータ装置が他の車両に乗せ換えられたか否かを判定する。その結果、他の車両へ載せ換えられたと判断された場合には、速度計算手段５１１は計算した距離センサ１のスケールファクタを記憶先検索手段５４に渡す。記憶先検索手段５４は、記憶手段５２の各メモリ領域に記憶されているスケールファクタの中から、その速度計算手段５１１より受け取ったスケールファクタの計算値との差異が所定値以下となるものを検索する。そして該当するものが見つかると、その検索したスケールファクタを速度計算手段５１１へ渡すとともに、検索したスケールファクタが記憶されているメモリ領域の番地を記憶手段５２に伝える。
【０１９１】
記憶手段５２は、この記憶先検索手段５４より通知された番地（図１９に示すｙ番地からｙ＋ｎ−１番地）のメモリ領域に、車種毎に異なる距離センサ１のスケールファクタ（図１９ではＳＦ値１〜ＳＦ値ｎ）と組で記憶されているメモリアクセス時刻を、現在の時刻で更新する。その後、速度計算手段５１１は記憶先検索手段５４から受け取ったスケールファクタ、すなわち過去に補正された結果を用いて車両の速度を計算する。
【０１９２】
なお、記憶先検索手段５４は、速度計算手段５１１から受け取ったスケールファクタの計算値と記憶手段５２に記憶されたスケールファクタとの差異が所定値以下となるものを、記憶手段５２のメモリ領域から検索できなかった場合には、未確認の他の車両へ当該ロケータ装置が載せ換えられたものと判断して、記憶手段５２の空いているメモリ領域の番地を記憶手段５２へ伝える。記憶手段５２は、渡された番地が付けられた空きメモリ領域に、速度計算手段５１１の計算したスケールファクタを記憶するとともに、そのスケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を記憶する。
【０１９３】
また、速度計算手段５１１から受け取ったスケールファクタの計算値と記憶手段５２に記憶されたスケールファクタとの差異が所定値以下となるものを、記憶手段５２のメモリ領域から検索できず、さらに記憶手段５２に空いているメモリ領域もなかった場合には、記憶先検索手段５４は、メモリアクセス時刻が最古になっているスケールファクタを記憶しているメモリ領域の番地を記憶手段５２に伝える。記憶記憶５２は渡された番地のメモリ領域に、速度計算手段５１１の計算したスケールファクタを記憶するとともに、そのスケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を記憶する。
【０１９４】
このように、この実施の形態１０によれば、当該ロケータ装置が他の車両へ載せ換えられたと判断された場合には、記憶手段５２に記憶したスケールファクタの中から現在の計算値に最も近いものを探し出して速度計算手段５１１に渡すとともに、記憶手段５２へ当該メモリの番地を渡し、この時、現在の計算値に対して、記憶手段５２に記憶されたスケールファクタが所定の比率以上異なっていれば未確認車両へロケータ装置が搭載されたものと判断して、記憶手段５２に対してその空きメモリ領域に当該スケールファクタを読み書きするように指示し、空いているメモリ領域がない場合には、メモリアクセス時刻が最古のメモリ領域に上書きするように記憶手段５２に指示しているので、ロケータ装置を他の車両へ載せ換えても、当該車両に係る補正を毎回やり直す必要がなくなり、他の車両へ載せ換えた直後から車両の速度を高精度で推定することが可能になるという効果が得られる。
【０１９５】
実施の形態１１．
上記実施の形態２では、速度計算手段５１１において、状態ベクトルの推定量の行列要素であるスケールファクタ要素とスケールファクタの前回の値を式（４８）を用いて重み付け平均し、スケールファクタの更新を行うものとして説明したが、状態ベクトルの推定量の行列要素であるスケールファクタ要素を平滑化してスケールファクタを更新するものであれば、上記式（４８）以外の計算式を用いてもよい。
【０１９６】
実施の形態１２．
上記実施の形態３では、速度計算手段５１１において、加速度センサ４のオフセットが更新された後、所定時間が経過するまでの間は、速度推定手段５１３で加速度センサ４の出力信号から推定した速度に速度計算手段５１１で計算した速度が合うように、距離センサ１のスケールファクタを補正するものについて説明したが、速度推定手段５１３で推定した速度の誤差が小さいものであれば、加速度センサ４のオフセットの変動量に応じて、距離センサ１のスケールファクタを補正できる所定時間を可変に設定してもよい。
【０１９７】
実施の形態１３．
上記実施の形態５、実施の形態６および実施の形態１０では、距離・速度計算手段５１１ａあるいは速度計算手段５１１において、車両の速度と移動距離を計算するために距離センサ１を使用した場合について説明したが、加速度センサ４を代わりに用いてもよい。
【０１９８】
実施の形態１４．
上記実施の形態８では、速度計算手段５１１において、距離センサ１のスケールファクタが前回の値から所定比率以上更新された場合に、当該ロケータ装置が他の車両へ載せ換えられたと判断する場合について説明したが、カルマンフィルタで計算した状態ベクトルの行列要素であるスケールファクタ要素の計算値を所定時間分保持した履歴に基づいて、例えば、所定時間分の平均値に対する標準偏差の比率が所定の値以上変化したとき、あるいは履歴中のスケールファクタ要素の時間的変化に伴う勾配が所定値以上になった時に、当該ロケータ装置が他の車両へ載せ換えられたと判断するようにしてもよい。
【０１９９】
実施の形態１５．
上記実施の形態９では、記憶先指定手段５３が、スケールファクタの記憶場所を利用者が記憶手段５２へ指示するスイッチを備えているものとして説明したが、スイッチの代わりに通信手段をさらに備えて、利用者が通信手段を介してスケールファクタの記憶場所を指定できるようにしてもよい。
【０２００】
実施の形態１６．
上記実施の形態１０では、記憶先検索手段５４は、速度計算手段５１１で計算した距離センサ１のスケールファクタと記憶手段５２に記憶されたスケールファクタの差異が、所定値以下となるものを記憶手段５２から検索するものについて説明したが、記憶手段５２に記憶されたスケールファクタに対する上記差異の比率が所定以下となるものを検索するようにしてもよい。
【０２０１】
【発明の効果】
以上のように、この発明によれば、衛星を利用して少なくとも車両の速度等を観測する絶対位置観測手段と、車両の移動距離に応じた信号を出力する距離センサを備え、速度計算手段にて車両の速度を推定するとともに、カルマンフィルタを用いて、絶対位置観測手段で観測した速度を基準にした距離センサのスケールファクタの補正と車両の速度の推定を行うように構成したので、絶対位置観測手段で観測した速度の誤差が変動しても距離センサのスケールファクタを、車両の走行開始後に、最適かつ速やかに補正することが可能となり、走行開始後に車両の速度を高精度で推定することができるロケータ装置が得られる効果がある。
【０２０２】
この発明によれば、ローパスフィルタを速度計算手段におけるカルマンフィルタの後段に設けるように構成したので、カルマンフィルタで計算した距離センサのスケールファクタが平滑化されて、それに含まれている高周波ノイズが除去されるため、距離センサのスケールファクタの変動を小さくすることができ、車両の速度を安定的に推定できるという効果がある。
【０２０５】
この発明によれば、車両の進行方位変化に応じた信号を出力する相対方位センサと、相対方位センサの出力信号から車両の単位時間毎の進行方位変化を計算する方位変化計算手段とを設け、位置・方位推定手段にて車両の現在位置と進行方位を推定するとともに、カルマンフィルタを用いて、絶対位置観測手段の観測した位置を基準にした車両の現在位置と進行方位を推定するように構成したので、専用のカルマンフィルタにて、絶対位置観測手段の観測した位置と相対方位センサ等の計測情報から車両の現在位置と進行方位が同時に推定され、絶対位置観測手段の観測した位置と相対方位センサ等の計測情報の誤差が変動しても、車両の現在位置と進行方位を最適に推定できて信頼性の向上がはかれ、また、絶対位置観測手段が非測位状態でも相対方位センサ等の計測情報から車両の現在位置と進行方位を推定することが可能となって、トンネルや高架下の道路においても、安定的に車両の現在位置と進行方位を提供できるなどの効果がある。
【０２０６】
この発明によれば、相対方位センサの出力信号から当該相対方位センサのオフセットを検出するオフセット検出手段を設け、方位変化計算手段にて相対方位センサの出力信号から車両の単位時間毎の進行方位変化を計算するとともに、カルマンフィルタを用いて、位置・方位推定手段にて推定した車両の進行方位を基準にした相対方位センサのオフセットとスケールファクタの補正を行うように構成したので、推定した車両の進行方位と相対方位センサのオフセットおよびスケールファクタに誤差があっても、相対方位センサのオフセットとスケールファクタを最適に補正することが可能となり、また、絶対位置観測手段が非測位状態であっても、また徐行運転時などのように絶対位置観測手段の観測情報の誤差が大きくなる場合であっても、相対方位センサのオフセットとスケールファクタを常時補正することができるため、相対方位センサから進行方位変化を高い精度で求めることができるなどの効果がある。
【０２０７】
この発明によれば、方位変化計算手段が相対方位センサのオフセットの変動を監視して、スケールファクタ補正の有効性を判定するように構成したので相対方位センサのオフセットの変動が小さい時に相対方位センサのスケールファクタを補正することで、相対方位センサのスケールファクタの誤補正を防止することが可能となり、相対方位センサから高い精度で進行方位変化を求めることができるという効果がある。
【０２０８】
この発明によれば、距離センサのスケールファクタの補正を監視することによって、ロケータ装置が他の車両に載せ換えられたか否かを判断するように構成したので、距離センサのスケールファクタの変動量からロケータ装置が他の車両に載せ換えられたことを検出することができ、ロケータ装置が複数の車両へ載せ換えられて使用されるような場合に、現在搭載されている車両に係る補正を直ちに行うことができ、車両の挙動情報を最適に設定することが可能になるという効果がある。
【０２０９】
この発明によれば、距離センサのスケールファクタを記憶する複数のメモリ領域を有する記憶手段と、利用者が記憶先であるメモリ領域の番地を指定するための記憶先指定手段とを設け、車種毎に異なる距離センサのスケールファクタを記憶手段に記憶する一方で、距離センサのスケールファクタの記憶場所を利用者が記憶先指定手段によって切り替えるように構成したので、距離センサのスケールファクタをこの記憶先指定手段によって切り替えることにより、ロケータ装置を他の車両へ載せ換えた時に、当該車両に係る補正を毎回やり直すことが不要になり、他の車両へ載せ換えた直後から車両の速度を高い精度で推定することが可能になるという効果がある。
【０２１０】
この発明によれば、距離センサのスケールファクタとメモリアクセス時刻の組を記憶する複数のメモリ領域を有する記憶手段と、記憶手段に保持されているスケールファクタを検索する記憶先検索手段を設け、車種毎に異なる距離センサのスケールファクタを記憶手段に記憶する一方で、距離センサのスケールファクタの記憶場所を記憶先検索手段の検索結果によって自動的に切り替えるように構成したので、ロケータ装置を他の車両へ載せ換えた時に、当該車両に係る補正を毎回やり直すことが不要になり、他の車両へ載せ換えた直後から車両の速度を高い精度で推定することが可能になるという効果がある。
【図面の簡単な説明】
【図１】 この発明の実施の形態１によるロケータ装置の構成を示すブロック図である。
【図２】 この発明の実施の形態１におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図３】 この発明の実施の形態１におけるロケータ装置の動作の、割込処理１の処理内容を示す流れ図である。
【図４】 この発明の実施の形態１におけるロケータ装置の動作の、割込処理２の処理内容を示す流れ図である。
【図５】 この発明の実施の形態１におけるロケータ装置の動作の、割込処理３の処理内容を示す流れ図である。
【図６】 この発明の実施の形態３によるロケータ装置の構成を示すブロック図である。
【図７】 この発明の実施の形態３におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図８】 この発明の実施の形態４によるロケータ装置の構成を示すブロック図である。
【図９】 この発明の実施の形態４におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図１０】 この発明の実施の形態５によるロケータ装置の構成を示すブロック図である。
【図１１】 この発明の実施の形態５におけるロケータ装置の動作の、メインルーチンの処理内容を示す流れ図である。
【図１２】 この発明の実施の形態５におけるロケータ装置の動作の、位置・方位推定処理の処理内容を示す流れ図である。
【図１３】 この発明の実施の形態５におけるロケータ装置の動作の、位置・方位推定手段による車両の現在位置の推定例を示す説明図である。
【図１４】 この発明の実施の形態６によるロケータ装置の構成を示すブロック図である。
【図１５】 この発明の実施の形態６におけるロケータ装置の動作の、方位変化計算処理の処理内容を示す流れ図である。
【図１６】 この発明の実施の形態９によるロケータ装置の構成を示すブロック図である。
【図１７】 この発明の実施の形態９におけるロケータ装置の記憶手段のメモリマップを簡単に示した説明図である。
【図１８】 この発明の実施の形態１０によるロケータ装置の構成を示すブロック図である。
【図１９】 この発明の実施の形態１０におけるロケータ装置の記憶手段のメモリマップを簡単に示した説明図である。
【図２０】 従来のロケータ装置の一構成例を示すブロック図である。
【図２１】 上記ロケータ装置の動作の、自律航法のメインルーチンの処理内容を示す流れ図である。
【図２２】 上記ロケータ装置の動作の、進行方位変化と移動距離の演算処理の処理内容を示す流れ図である。
【図２３】 上記ロケータ装置の動作の、相対軌跡演算処理の処理内容を示す流れ図である。
【図２４】 上記ロケータ装置の動作の、絶対方位、絶対位置の演算処理の処理内容を示す流れ図である。
【図２５】 上記ロケータ装置の動作の、自律航法とＧＰＳ航法を複合化するためのカルマンフィルタの演算処理の処理内容を示す流れ図である。
【図２６】 従来のロケータ装置の他の構成例を示すブロック図である。
【図２７】 上記ロケータ装置の動作の、自律航法のメインルーチンの処理内容を示す流れ図である。
【符号の説明】
１ 距離センサ、２ 相対方位センサ、３ ＧＰＳ受信機（絶対位置観測手段）、４ 加速度センサ、５２ 記憶手段、５３ 記憶先指定手段、５４ 記憶先検索手段、５１１ 速度計算手段、５１１ａ 距離・速度計算手段（速度計算手段）、５１２，５１４ オフセット検出手段、５１３ 速度推定手段、５１５ 方位変化計算手段、５１６ 位置・方位推定手段。
Claims (8)
- 衛星を利用して、少なくとも車両の速度を観測する絶対位置観測手段と、
前記車両の移動距離に応じた信号を出力する距離センサと、
前記距離センサの出力信号にスケールファクタを乗算して車両の速度を計算するシステムモデルと、前記絶対位置観測手段で観測した車両の速度と前記システムモデルにおける速度の関係を表した観測モデルとに基づいて、状態ベクトルの行列要素には前記システムモデルにおける速度とスケールファクタを設定し、観測値には前記絶対位置観測手段で観測した速度を設定したカルマンフィルタにより、状態ベクトルの行列要素である速度とスケールファクタの計算を行うとともに、前記システムモデルと観測モデルでは、所定時間における、前記距離センサの出力信号および前記絶対位置観測手段で観測した速度のそれぞれの平均値を用いて、当該各平均値の時間的変化に伴う勾配の差異を、前記絶対位置観測手段で観測した速度の平均値に乗算し、その値を観測誤差として設定する、速度計算手段とを備えたロケータ装置。 - 速度計算手段は、カルマンフィルタの後段に、状態ベクトルの行列要素であるスケールファクタに含まれる高周波ノイズを除去するローパスフィルタを設けて、
前記ローパスフィルタを通過した後のスケールファクタを距離センサの補正値とするものであることを特徴とする請求項１記載のロケータ装置。 - 車両の進行方位の変化に応じた信号を出力する相対方位センサと、
前記相対方位センサの出力信号から車両の単位時間毎の進行方位の変化を計算する方位変化計算手段と、
前記車両の現在位置と進行方位を推定する位置・方位推定手段とを設け、
前記位置・方位推定手段が、速度計算手段または速度推定手段による速度から求めた単位時間当たりの移動距離と、前記方位変化計算手段で計算した単位時間毎の進行方位変化から車両の現在位置と進行方位を計算するシステムモデルと、絶対位置観測手段で観測した車両の現在位置と前記システムモデルにおける現在位置の関係を表した観測モデルに基づいて、状態ベクトルの行列要素には前記システムモデルにおける現在位置と進行方位を設定し、観測値には前記絶対位置観測手段で観測した車両の現在位置を設定したカルマンフィルタにより、状態ベクトルの行列要素である車両の現在位置と進行方位の計算を行うとともに、所定の距離以上離れた複数地点または所定時間経過毎の前記絶対位置観測手段による現在位置と当該位置・方位推定手段による現在位置の差異の変動量を観測誤差として設定し、所定の距離以上離れた複数地点毎の、前記絶対位置観測手段による進行方位と当該位置・方位推定手段による進行方位の差異の平均値を、システム誤差の行列要素である方位誤差として設定して、前記速度計算手段または前記速度推定手段とによる速度の積分値と前記絶対位置観測手段による速度の積分値の差異を、システム誤差の行列要素である距離誤差として設定するものであることを特徴とする請求項１または請求項２記載のロケータ装置。 - 速度計算手段または速度推定手段で求めた車両の速度が０のときに相対方位センサの出力信号を平均したものをオフセットとするオフセット検出手段を設け、
方位変化計算手段が、前記相対方位センサの出力信号からオフセットを引いた後にスケールファクタを乗算して車両の単位時間毎の進行方位変化を計算するとともに、当該進行方位の変化を積算するシステムモデルと、位置・方位推定手段で推定した進行方位と前記システムモデルにおける積算方位の関係を表した観測モデルに基づいて、状態ベクトルの行列要素にはシステムモデルにおける前記相対方位センサのオフセットとスケールファクタおよび積算方位を設定し、観測値には前記位置・方位推定手段で推定した進行方位を設定したカルマンフィルタにより、状態ベクトルの行列要素である前記相対方位センサのオフセットとスケールファクタを計算して前記相対方位センサの補正値とし、停車毎のオフセットの変動量からシステム誤差の行列要素であるオフセット誤差を設定し、また車両旋回時に前記位置・方位推定手段と前記方位変化計算手段の各方位情報からそれぞれ求めた車両の旋回角の差異からシステム誤差の行列要素であるスケールファクタ誤差を設定し、前記位置・方位推定手段で用いたシステム誤差の行列要素である方位誤差を観測誤差として設定するものであることを特徴とする請求項３記載のロケータ装置。 - 方位変化計算手段が、相対方位センサのオフセットの変動を監視して、前記相対方位センサのスケールファクタの補正の有効性を判定するものであることを特徴とする請求項４記載のロケータ装置。
- 速度計算手段が、当該速度計算手段で計算した距離センサのスケールファクタの変化が所定割合以上となった時に、当該ロケータ装置が他の車両に載せ換えられたと判断するものであることを特徴とする請求項３または請求項４記載のロケータ装置。
- 速度計算手段で計算した距離センサのスケールファクタを記憶する複数のメモリ領域を有する記憶手段と、
前記記憶手段のメモリ領域の番地を利用者が指定するための記憶先指定手段とを設け、
前記記憶手段は、前記記憶先指定手段によって指定された番地のメモリ領域に、前記距離センサのスケールファクタを読み書きすることを特徴とする請求項１または請求項２記載のロケータ装置。 - 速度計算手段で計算した距離センサのスケールファクタとメモリアクセス時刻を組にして記憶する複数のメモリ領域を有する記憶手段と、
前記速度計算手段で計算されたスケールファクタに近い値が前記記憶手段に記録されているか否かを検索し、記録されている場合には記録されているメモリ領域の番地を、記録されていない場合には未使用のメモリ領域の番地を、さらに未使用のメモリ領域もない場合にはメモリアクセス時刻の記録が最古のメモリ領域の番地を検索する記憶先検索手段とを設け、
前記記憶手段は、前記記憶先検索手段より指定された番地のメモリ領域に、前記速度計算手段で計算した距離センサのスケールファクタを読み書きするとともに、当該スケールファクタと組で記憶されるメモリアクセス時刻として、現在の時刻を読み書きすることを特徴とする請求項６記載のロケータ装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP15439699A JP3727489B2 (ja) | 1999-06-01 | 1999-06-01 | ロケータ装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP15439699A JP3727489B2 (ja) | 1999-06-01 | 1999-06-01 | ロケータ装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2000346661A JP2000346661A (ja) | 2000-12-15 |
| JP3727489B2 true JP3727489B2 (ja) | 2005-12-14 |
Family
ID=15583238
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP15439699A Expired - Lifetime JP3727489B2 (ja) | 1999-06-01 | 1999-06-01 | ロケータ装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3727489B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE112017001242T5 (de) | 2016-03-09 | 2018-12-13 | Mitsubishi Electric Corporation | Positionierungsvorrichtung und Positionierungsverfahren |
Families Citing this family (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005195395A (ja) * | 2004-01-05 | 2005-07-21 | Mitsubishi Electric Corp | 移動体加速度・距離推定回路、測位航法用擬似距離推定回路、移動体測位装置及び移動体測位方法 |
| JP4593341B2 (ja) * | 2005-03-30 | 2010-12-08 | クラリオン株式会社 | ナビゲーションシステム、その制御方法及び制御プログラム |
| WO2006109470A1 (ja) * | 2005-03-31 | 2006-10-19 | Pioneer Corporation | 推定位置算出装置、推定位置算出方法、推定位置算出プログラム、および記録媒体 |
| JP3875714B2 (ja) * | 2006-03-09 | 2007-01-31 | 三菱電機株式会社 | 移動体加速度・距離推定回路、移動体測位装置及び移動体測位方法 |
| JP5487890B2 (ja) * | 2009-11-12 | 2014-05-14 | 株式会社Ｊｖｃケンウッド | 傾斜角検出装置、傾斜角検出方法、および傾斜角検出プログラム |
| JP5810898B2 (ja) * | 2011-12-26 | 2015-11-11 | 株式会社デンソー | 車両姿勢推定装置、及び、盗難通知装置 |
| JP6330309B2 (ja) * | 2013-12-12 | 2018-05-30 | セイコーエプソン株式会社 | 信号処理装置、検出装置、センサー、電子機器及び移動体 |
| US10215785B2 (en) | 2013-12-12 | 2019-02-26 | Seiko Epson Corporation | Signal processing device, detection device, sensor, electronic apparatus and moving object |
| JP6417659B2 (ja) * | 2013-12-12 | 2018-11-07 | セイコーエプソン株式会社 | 信号処理装置、検出装置、センサー、電子機器及び移動体 |
| KR101481355B1 (ko) * | 2013-12-30 | 2015-01-09 | 현대자동차주식회사 | 전압 동기화 방법 및 시스템 |
| KR102326060B1 (ko) * | 2014-10-31 | 2021-11-12 | 현대모비스 주식회사 | 차량 속도 보정 장치 및 그에 의한 속도 보정 계수 산출 방법 |
| KR102250107B1 (ko) * | 2014-10-31 | 2021-05-12 | 현대모비스 주식회사 | 차량 속도 보정 장치 및 그에 의한 속도 보정 계수 산출 방법 |
| JP6834301B2 (ja) * | 2016-09-28 | 2021-02-24 | いすゞ自動車株式会社 | 区間決定装置及び区間決定方法 |
| JP6516042B2 (ja) * | 2018-05-11 | 2019-05-22 | セイコーエプソン株式会社 | 信号処理装置、検出装置、センサー、電子機器及び移動体 |
| JP7186414B2 (ja) * | 2018-12-07 | 2022-12-09 | 学校法人早稲田大学 | 移動物体の速度検出システム、速度検出装置及びそのプログラム |
| JP7053087B1 (ja) * | 2020-10-30 | 2022-04-12 | 株式会社スマートドライブ | 移動体挙動情報取得方法、移動体挙動情報取得装置及びプログラム |
| CN112577526B (zh) * | 2020-12-29 | 2023-10-13 | 武汉中海庭数据技术有限公司 | 一种多传感器融合定位的置信度计算方法及系统 |
| JP7318995B1 (ja) | 2022-03-24 | 2023-08-01 | 株式会社スマートドライブ | 移動体挙動情報取得方法、移動体挙動情報取得装置及びプログラム |
| CN115291262B (zh) * | 2022-10-09 | 2023-01-31 | 北京索云科技股份有限公司 | 卫星定位数据校正方法、装置、电子设备及存储介质 |
-
1999
- 1999-06-01 JP JP15439699A patent/JP3727489B2/ja not_active Expired - Lifetime
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| DE112017001242T5 (de) | 2016-03-09 | 2018-12-13 | Mitsubishi Electric Corporation | Positionierungsvorrichtung und Positionierungsverfahren |
| US10761215B2 (en) | 2016-03-09 | 2020-09-01 | Mitsubishi Electric Corporation | Positioning device and positioning method |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2000346661A (ja) | 2000-12-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3727489B2 (ja) | ロケータ装置 | |
| EP0716315A1 (en) | Vehicle heading correcting apparatus | |
| US7286933B2 (en) | GPS/dead-reckoning combination system and operating method thereof | |
| EP0496538B1 (en) | Vehicle heading correction apparatus | |
| JP4746374B2 (ja) | 位置補正方法及びナビゲーション装置 | |
| US5483456A (en) | Navigation system and a method of calculating GPS measuring deviation | |
| US6097337A (en) | Method and apparatus for dead reckoning and GIS data collection | |
| EP1586859A1 (en) | Walker navigation device and program | |
| US9683849B2 (en) | Vehicle navigation system with adaptive gyroscope bias compensation | |
| KR100526571B1 (ko) | 오프-보드 네비게이션 시스템 및 그의 오차 보정 방법 | |
| JPH10111137A (ja) | Ｇｐｓナビゲーション装置 | |
| KR100575933B1 (ko) | 가속도계와 경로안내 데이터를 이용한 이동체의속력측정방법 및 그 장치 | |
| JPH0531924B2 (ja) | ||
| KR100557081B1 (ko) | 복합항법 시스템에서 지.피.에스 수신기의 시간 지연 추정방법 및 그 장치 | |
| CN1576791B (zh) | 车辆用导航系统 | |
| JP2577160B2 (ja) | 車両位置検出装置 | |
| JP2013108961A (ja) | 測位装置及びプログラム | |
| EP0601712A1 (en) | Navigation system | |
| JP3569015B2 (ja) | Ｇｐｓナビゲーション装置 | |
| KR20090049500A (ko) | 차량의 주행 기록 데이터를 이용한 위치 추정 장치 및 방법 | |
| JP2685624B2 (ja) | 移動体用ナビゲーション装置 | |
| JP4688534B2 (ja) | 加速度の較正方法、及びナビゲーション装置 | |
| JP2907937B2 (ja) | 移動体の位置検出装置 | |
| JP2603766B2 (ja) | 方位検出方法および方位検出装置 | |
| JP2794807B2 (ja) | 移動体位置検知装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20040901 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20040907 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20041027 |
|
| 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: 20050830 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20050928 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 3727489 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: 20091007 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091007 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101007 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111007 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121007 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131007 Year of fee payment: 8 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| EXPY | Cancellation because of completion of term |