JP3645945B2 - Gyro drift correction method and correction circuit - Google Patents

Description

【００６４】
すなわち、Ｎ個の遅延回路４１１（遅延時間Ｔ）と、Ｎ個の加算器４１２を有し、入力信号（ＩＮ）は、先ず、遅延回路４１１−１で時間Ｔだけ遅延され、加算器４１２−２に入力される。また、加算器４１２−２には入力信号（ＩＮ）が入力されこれら２つの信号が加算される。
次に、入力信号は遅延回路４１１−２で時間２Ｔだけ遅延され加算器４１２−２に入力される。また、加算器４１２−２には加算器４１２−１の結果が入力されこれら２つの信号が加算される。
同様に、入力信号は遅延回路４１１−ｉで時間ｉ・Ｔだけ遅延され加算器４１２−ｉに入力される。また、加算器４１２−ｉには加算器４１２（ｉ−１）の結果が入力されこれら２つの信号が加算される。これにより、時系列データに対し現在から過去のＮ回分のデータの和が計算される。
【００６５】
図４のドリフト補正回路４０では、Ｎ点和の演算回路４１，５１の出力を減算回路４２に加え差出力を得て、コンパレータ４３に入力する。
コンパレータ４３はもう一方の入力データ（積分回路４４の出力）との比較を行い、２つの入力の差が一定範囲ε内（ε≧０）であれば０（ゼロ）を出力し、差出力側が大きければ正の値を、その反対なら負の値を出力する。この値を図７（ｂ）に示すような積分回路４４に入力して積分を行い、その出力をコンパレータ４３に戻す。
このような動作により積分回路４４の出力には差出力に追従した出力が得られる。なお、クロックパルス発生器４５とカウンタ回路４６により時間データが得られる。
上記出力を除算回路４７で時間で除算することにより変動の小さいドリフト補正値が得られる。このドリフト補正値を減算器４８に入力し、データＡからの差Ｆ＝Ａ−Ｅを得る。この出力Ｆをジャイロ補正値としてデットレコニング手段８に与える。
【００６６】
＜実施例５＞
本実施例は一時的に演算回路４１，５１の出力に誤差が大きく発生した場合に、これにリミッタをかける回路を示す。
図５で、ドリフト補正回路５０はナビゲーション装置１００のＡ／Ｄ変換手段６，デットレコニング手段８，及びマップマッチング手段７に接続する回路である。なお、本実施例では、説明上、ナビゲーション装置１００の各構成部分１〜１１は図８と同様とする。
ドリフト補正回路５０は、図４のドリフト補正回路４０において除算回路４７の代りにＦＩＲフィルタ５５を用いるものであり、コンパレータ４３と積分回路４４を用いることによりドリフト補正値の一時的な変動を抑制する。
【００６７】
＜実施例６＞
本実施例は一時的に演算回路４１，５１の出力に誤差が大きく発生した場合に、これにリミッタをかける回路を示す。
図６で、ドリフト補正回路６０はナビゲーション装置１００のＡ／Ｄ変換手段６，デットレコニング手段８，及びマップマッチング手段７に接続する回路である。なお、本実施例では、説明上、ナビゲーション装置１００の各構成部分１〜１１は図８と同様とする。
ドリフト補正回路６０は、図４のドリフト補正回路４０において積分回路４４の代りにＮ点和の演算回路６１を用い、時間による除算を行う除算回路４７を定数により除算を行う除算回路６２としたものであり、コンパレータ４３を用いることによりドリフト補正値の一時的な変動を抑制する。
【００６８】
上記実施例３〜実施例６でドリフト補正値の変動を小さくする回路（図３〜図６のドリフト補正回路３０，４０，５０，６０）を示したが、実施例１の場合と同様にプログラムによりこれらの回路を実現することができる。
【００６９】
＜実施例７＞
図９は本発明のナビゲーション装置の他の実施例の構成を示すブロック図であり、ＧＰＳ受信機２０１，距離センサ２０２，地磁気センサ２０３，ジャイロセンサ２０４，デットレコニング手段２０６，マップマッチング手段２０７，地図データ２１０，表示車両方位選択手段２１１，表示装置２１２は前述の実施例１〜６で説明したナビゲーション装置の構成と同様であり、本実施例でナビゲーション装置２００はそれらに積分手段２２１〜オフセット補正手段２３５からなるオフセット補正部を付加した構成をなす。また、オフセット補正手段は望ましくはナビゲーション装置２００のナビゲーション動作を実行するナビゲーションプログラムの一部分（サブルーチン）としてＲＯＭ（図示せず）に格納され、ナビゲーション装置２００のナビゲーション動作の実行時に取り出されてナビゲーション装置２００のＣＰＵ（図示せず）により実行制御される。
【００７０】
図９に示すように、ナビゲーション装置２００はＧＰＳ受信機２０１，距離センサ２０２，地磁気センサ２０３やジャイロセンサ２０４のような方位センサ等から情報を得てデットレコニング手段２０６及びマップマッチング手段２０７等による処理を経て自車位置を推定して地図データ２１０を検索し、表示データ作成手段２１１によりオフセット補正に用いる表示車両方位をＧＰＳまたはマップマッチングの状態から判定して選択し表示データを作成して、表示装置２１２上に地図を描画し、図１０の例に示すように地図３００上に自車位置マーク３０１を表示する。
【００７１】
デットレコニング手段２０６では距離センサ２０２からの移動距離と方位センサからの方位変化量θを用いて次式により移動ベクトルを求める。
推定車両方位は前回の方位をθ0 とするとθ0 ＋θとなる。
Δｘ＝１ｃｏｓ（θ0 ＋ θ）
Δｙ＝１ｓｉｎ（θ0 ＋ θ）
この（ｘ，ｙ）が推測位置であり、マップマッチング手段２０７の入力データとなる。
マップマッチング手段２０７では上記推測位置と地図データ２０９を用いて地図上の自車位置を推定する。
マップマッチング手段２０７は、まず、車両が直進しているか旋回しているかを判定する。
直進している場合には車両の進行方位に近い道路を選択する。次に、過去にマップマッチングでできた道路に接続される道路を選択する。そして、推測位置から最も近い道路を１本選択し、この選択された道路に推測位置から垂線を下し、道路との交点を自車位置とする。
旋回している場合には、車両の方位変化の過程と道路の方位変化の形状を比較して道路を１本選択し、この選択された道路に推測位置から垂線を下し、道路との交点を自車位置とする。
ただし、進行方向に近い道路がなかったり、接続されている道路がなかったり、道路の形状が一致しなかった場合には自車位置を推測位置とする。
【００７２】
さらに、道路を見付けた場合にはマップマッチングの車両方位を道路の方位とする。自車位置を推測位置とした場合には、マップマッチングの車両方位を道路の方位とする。自車位置を推測位置とした場合にはθ0 ＋θをマップマッチング処理で得た車両方位とする。
ここまででマップマッチング処理が完了するが、ＧＳＰデータが信頼できるときには次の処理を行う。
マップマッチング処理の結果得た自車位置座標とＧＰＳデータの座標が大きくずれている場合には自車位置のデータを捨ててＧＰＳデータを自車位置とし、ＧＰＳデータの方位データを表示車両方位とする。
このようにして、センサとＧＰＳから自車位置と表示車両方位が得られるが、これでも正確に実際の車両方位が得られるとは限らない。例えば駐車場でナビゲーションシステムの電源を切ったままターンテーブルで車両を回転させたり、立体駐車場で旋回を繰返した場合等ではＧＰＳの方位データ、地図データ、ジャイロセンサのデータのいずれもが使えない。これに対しては表示車両方位のマニアル修正が有効となる。ナビゲーションシステムでは一般的に「自車位置修正」、「自車方位修正」の機能が備えられており、これらは地図表示画面で自車位置と表示車両方位を自車マークを使って表示しているとき、実際の位置または方位と表示が一致していないと運転者が認識した場合に情報入力手段により運転者の推定した位置または方位に修正を行う機能であり、この機能を用いることにより前述の様な特殊な状態の場合でも正確な表示車両方位が決められる。
このようにマップマッチング処理により自車位置とマップマッチングの車両方位が決定されると自車位置を中心とした地図データ２１０を読み出しこのデータにより地図描画手段２１１”が表示装置２１２上に地図を描画する。
自車位置マーク３０１は地図上に推定した自車位置を示すと共に、車両の進行する方位（表示方位と呼ぶ）を示している。
この表示方法はナビゲーション装置が動作している間は常時１つの値を保持し、常に新しい情報で更新されているので、マップマッチングできない時でも方位センサ、ＧＰＳ等からのデータにより更新される。
【００７３】
本発明におけるジャイロの補正方法について図１のナビゲーション装置２００のオフセット補正手段および図１１のフローチャートを基に以下説明する。
ステップＳ１０１では、ナビゲーション装置２００から表示車両方位を取得し、記憶手段２３１により記憶しステップＳ１０２に移行する（ここで、記憶した値をＭとする）。
ステップＳ１０２ではジャイロ２０４の出力を積分しジャイロ方位としてステップＳ１０３に移行する（ここでのジャイロ方位をＧとする）。
ステップＳ１０３では一定時間Ｔだけ経過したか否かを調べ経過していない場合はステップＳ１０２に戻りジャイロ出力の積分を繰返す。経過した場合はステップＳ１０４に移行する。
ステップＳ１０４では一定時間が経過した時点の表示車両方位を取得し、値Ｍとの差（方位差）Ｄを方位差検出手段２３２により計算しステップＳ１０５に移行する（Ｄ＝現在車両方位−Ｍ）。
ステップＳ１０５では方位差Ｄとジャイロ方位Ｇの差（誤差）Ｅを誤差検出手段２３３により求めステップＳ１０６に移行する（Ｅ＝Ｄ−Ｇ）。
誤差Ｅからオフセット算出手段２３４によりオフセット補正値を計算してステップＳ１０７に移行する。オフセット算出手段２３４はＥの値をステップＳ２での経過時間Ｔで除算することにより角速度データに変換してオフセット補正値Ｏを得る（Ｏ＝Ｅ／Ｔ）。ここで、ステップＳ２での経過時間Ｔはオフセット算出手段２３４による除算演算で誤差が増加しないように選ばれる。
オフセット補正手段２３５によりジャイロ２０４の出力からオフセット補正値Ｏを減算して補正を行う（補正済ジャイロ出力＝補正前ジャイロ出力−Ｏ）。
【００７４】
＜具体例＞
図１２は上述のジャイロ出力のオフセット補正方法の説明図であり、図では処理や入出力機器を矩形のブロックで示し、データを円で示してある。また、説明上、ナビゲーション装置２００’の各構成部分２０１〜２３５は図９とほぼ同様であり、記号の同じ各構成部分の機能及び構成は図９の場合と同様である（表示データ作成手段２１１は表示車両方位選択手段２１１’および地図描画手段２１１”を含むものとする）。
本発明のジャイロ出力のオフセット補正では、ナビゲーション装置２００’の表示方位のデータを基準方位として使用しオフセット値を制御するため、ナビゲーション装置２００’の内部状態の影響を受ける。よってナビゲーション装置２００’の状態も含めて説明を行う。また、ナビゲーション装置２００’ではその内部状態がＧＰＳ受信機２０１の測位中か否か、マップマッチングが可能か否かでデータの処理方法が異なってくるので、これらを場合分けして説明する。
【００７５】
（１）ＧＰＳ測位中の場合：
表示車両方位選択手段２１１’は自律航法で求めた座標とＧＰＳ受信により求めた座標に大きな誤差がある場合には、ＧＰＳ受信の状態がよければＧＰＳからの座標を現在位置と推定する。ＧＰＳの受信状態が悪ければ自律航法の座標を現在位置とする。
ＧＰＳの座標を現在位置とした場合、ジャイロの補正は以下のように実行される。
（ａ）方位センサ（地磁気センサ２０３，ジャイロ２０４）および距離センサからのデータを捨ててデットレコニング手段２０６及びマップマッチング手段２０７で求めた座標と方位データを捨てる。そして、ＧＰＳ受信により求めた座標と方位データを現在位置のデータとしこれにより地図の表示を行う。
（ｂ）上記表示車両方位データと記憶手段２３１に記憶されているデータから方位差検出手段２３２により差を求め方位差とする。記憶手段２３１にはナビゲーション装置２００’の起動時の表示車両方位データおよびそのときの時刻を記憶しておく。或いは、走行中に停車してジャイロ２０４のキャリブレーションを実施したときの表示車両方位データおよびそのときの時刻を記憶しておいてもよい。
（ｃ）積分手段２２１では、上記記憶手段２３１にデータを記憶した時刻から積分を開始し、ジャイロ２０４の角速度データから角度データを得る。
（ｄ）誤差検出手段２３３では、上記（ｂ）の方位データと（ｃ）の角度データから差を求め、これを角度誤差とする。
（ｅ）オフセット算出手段２３４では（ｄ）の角度誤差を時間データで除算しオフセット誤差を求める。時間データとしては現在時刻から（ｂ）のデータを記憶した時刻を減算した経過時間を用いる。
（ｆ）オフセット補正手段２３４ではジャイロからの角速度データから（ｅ）のオフセット誤差を減算することにより誤差を除去する。
【００７６】
次に、ＧＰＳ測位中の場合について具体的な数値で補正の過程を示す。
（ａ’）ＧＰＳの方位データを北を基準として時計回りに３６０度で表すとき、現在の方位データを４８度と仮定する。
（ｂ’）記憶手段２３１の方位データが４８度で時刻が８時３２分０秒であったと仮定すると、方位差は次式で求められる。
（方位差）＝（表示車両方位データ）−（記憶データ）＝４８−４８＝０
（ｃ’）ジャイロ２０４の角速度データをＥｓｉｎωｔ＋０．０１と仮定する。つまり、振幅Ｅで正弦波で角速度が発生しオフセットが０．０１ｖあるとする。
この角速度データを１０００秒間積分するとωｔ＝２ｎπとなると仮定すれば（ｎ：整数）、ｓｉｎωｔ＝０となり、ｓｉｎωｔの積分値もゼロとなりオフセット分（０．０１ｖ×１０００＝１０ｖ）だけが残る。
このデータを変換感度２０ｍｖ／ｄｅｇで角度に換算すると、
１０ｖ／（２０ｍｖ／ｄｅｇ）＝５００ｄｅｇ
となる。これが角度データとなる。
（ｄ’）角度誤差は（ｂ’）と（ｃ’）から５００度となる。すなわち、
（角度誤差）＝（角度データ）−（方位差）＝５００−０＝５００ｄｅｇ
（ｅ’）現在時刻を８時４８分４０秒と仮定すると、時間データは１６分４０秒＝１０００秒となり、この値で角度誤差を除算する。すなわち、

（ｆ’）ジャイロ２０４の角速度データからオフセット誤差を減算すると、

となり、直流分のオフセットが除去できる。
【００７７】
（２）マップマッチング中の場合：
表示車両方位選択手段２１１’は、ＧＰＳの受信ができない場合、或いはＧＰＳの受信状態が良くてＧＰＳからの座標とマップマッチング手段２０７で求めた座標が近似している場合にはマップマッチング手段２０７で求めた座標を現在位置と推定する。そして、マップマッチング手段２０７は自車位置のある道路を見付けた場合にジャイロ２０４の補正を次のようにして実行する（道路が見付からなかった場合には後述の（３）によりジャイロの補正を行う）。
（ａ）ジャイロ２０４からの角速度データはオフセット補正が行われる。この補正はオフセット誤差データを角速度データに加算することであり、初期値がゼロで時系列に沿って以下の処理により求められるオフセット誤差を用いる。
（ｂ）オフセットの補正された角速度データは積分手段２２１により積分されて角度データとなる。また、地磁気センサ２０３の値が正しいと考えられる場合には地磁気センサ２０３からの角度データを使用する。
（ｃ）距離センサからの距離データと（ｂ）の角度データからデットレコニング手段２０６により推測位置を求める。また、角度データをそのまま推測方位とする。
（ｄ）推測位置と推測方位からマップマッチング手段２０６により自車位置のある道路を探す。道路を見付けた場合にはその道路上で自車位置を決定し、その道路の方向を車両方位として角度データを捨てる。
（ｅ）上記（ｄ）で求められた自車位置を表示車両方位選択手段２１１’でＧＰＳ受信機２０１からの座標と比較する。ＧＰＳの受信状態が良くて２つの座標が遠い場合には前述した（１）の処理を行い、近い場合には上記（ｄ）の自車位置を表示車両位置とし、車両方向を表示車両方位として地図描画手段２１１”により地図の表示を行う。
（ｆ）上記表示車両方位データと記憶手段２３１に記憶されているデータから方位差検出手段２３２により差を求め方位差とする。記憶手段２３１にはナビゲーション装置２００’の起動時の表示車両方位データおよびそのときの時刻を記憶しておく。或いは、走行中に停車してジャイロ２０４のキャリブレーションを実施したときの表示車両方位データおよびそのときの時刻を記憶しておいてもよい。
（ｇ）積分手段２２１では、上記記憶手段２３１にデータを記憶した時刻から積分を開始し、ジャイロ２０４の角速度データから角度データを得る。
（ｈ）誤差検出手段２３３では、（ｆ）の方位データと（ｇ）の角度データから差を求め、これを角度誤差とする。
（ｉ）オフセット算出手段２３４では（ｈ）の角度誤差を時間データで除算しオフセット誤差を求める。時間データとしては現在時刻から（ｆ）のデータを記憶した時刻を減算した経過時間を用いる。
（ｊ）オフセット補正手段２３４ではジャイロからの角速度データから（ｉ）のオフセット誤差を減算することにより誤差を除去する。
【００７８】
次に、マップマッチング中の場合について具体的な数値で補正の過程を示す。
（ａ’）ジャイロ２０４の角速度データをＥｓｉｎωｔ＋０．０１と仮定する。
（ｂ’）この角速度データを１秒間積分した結果を変換感度２０ｍｖ／ｄｅｇで角度に換算すると、１秒間の回転量をθｄｅｇとして、
θ＋０．０１ｖ／（２０ｍｖ／ｄｅｇ）＝θ＋０．５ｄｅｇ
となり、また、θ＝｛∫（Ｅｓｉｎωｔ）ｄｔ｝／０．０２より求められる（但し積分∫はここではΔｔ＝０から１までの積分を意味する）。
すなわち、ジャイロ２０４の回転量とオフセットによる０．５度の誤差が加算されて角度データとなる。
（ｃ’）推測方位データは前回の方位をθ0として、θ0＋θ＋０．５度となる。
（ｄ’）ここでは道路の方向が（θ0＋θ）度であると仮定する。
（ｅ’）このとき表示車両方位は（θ0＋θ）度となる。
（ｆ’）記憶手段２３１の方位データが０度で時刻が８時３２分０秒であったと仮定すると、方位差は次式により求められる。

（ｇ’）積分手段２３１では（ｂ’）と同じ結果（θ＋０．５度）となる。
（ｈ’）角度誤差は（ｆ）と（ｇ）より、

（ｉ’）上記のような角度誤差の発生が１０００秒継続すると、現在時刻が８時４８分４０秒となり角度誤差も１０００倍の５００度となる。この角度誤差からオフセット誤差を求める。すなわち、

（ｊ’）ジャイロ２０４の角速度データからオフセット誤差を減算すると、

となり、直流分のオフセットが除去される。
【００７９】
（３）デットレコニング中の場合：
表示車両方位選択手段２１１’は、ＧＰＳが受信できない場合、或いは、ＧＰＳの受信状態が良くてＧＰＳからの座標とデットレコニング手段２０６で求めた座標が近い場合にデットレコニング手段２０６による推測位置を現在位置と推定する（但し、マップマッチングで道路がみつからない場合）。
（ａ）ジャイロ２０４からの角速度データに対してはオフセット補正が行われる。この補正は、オフセット誤差データを角速度データに加算することであり、初期値がゼロで時系列に沿って以下の処理により求められるオフセット誤差を用いる。
（ｂ）オフセットの補正された角速度データは積分手段２２１により積分されて角度データとなる。また、地磁気センサ２０３の値が正しいと考えられる場合には地磁気センサ２０３からの角度データを使用する。
（ｃ）距離センサからの距離データと（ｂ）の角度データからデットレコニング手段２０６により推測位置を求める。また、角度データをそのまま推測方位とする。そしてマップマッチング手段２０７でこの位置に対する道路がみつからない場合に推測位置と推測方位を自車位置と車両方位とする。
（ｄ）上記（ｃ）で求められた自車位置を表示車両方位選択手段２１１’でＧＰＳ受信機２０１からの座標と比較する。ＧＰＳの受信状態が良くて２つの座標が遠い場合には上述した（１）の処理を行うこととなり、近い場合には上記（ｃ）の自車位置を表示車両位置とし、車両方向を表示車両方位として地図描画手段２１１”により地図の表示を行う。
（ｅ）上記表示車両方位データと記憶手段２３１に記憶されているデータから方位差検出手段２３２により差を求め方位差とする。記憶手段２３１にはナビゲーション装置２００’の起動時の表示車両方位データおよびそのときの時刻を記憶しておく。或いは、走行中に停車してジャイロ２０４のキャリブレーションを実施したときの表示車両方位データおよびそのときの時刻を記憶しておいてもよい。
（ｆ）積分手段２２１では、上記記憶手段２３１にデータを記憶した時刻から積分を開始し、ジャイロ２０４の角速度データから角度データを得る。
（ｇ）誤差検出手段２３３では、（ｅ）の方位データと（ｆ）の角度データから差を求め、これを角度誤差とする。
（ｈ）オフセット算出手段２３４では（ｇ）の角度誤差を時間データで除算しオフセット誤差を求める。時間データとしては現在時刻から（ｅ）のデータを記憶した時刻を減算した経過時間を用いる。
（ｉ）オフセット補正手段２３４ではジャイロからの角速度データから（ｈ）のオフセット誤差を減算することにより誤差を除去する。
【００８０】
次に、マップマッチング中の場合について具体的な数値で補正の過程を示す。
（ａ’）ジャイロ２０４の角速度データをＥｓｉｎωｔ＋０．０１と仮定する。
（ｂ’）この角速度データを１秒間積分した結果を変換感度２０ｍｖ／ｄｅｇで角度に換算すると、１秒間の回転量をθｄｅｇとして、
θ＋０．０１ｖ／（２０ｍｖ／ｄｅｇ）＝θ＋０．５ｄｅｇ
となり、また、θ＝｛∫（Ｅｓｉｎωｔ）ｄｔ｝／０．０２より求められる（但し積分∫はここではΔｔ＝０から１までの積分を意味する）。
すなわち、ジャイロ２０４の回転量とオフセットによる０．５度の誤差が加算されて角度データとなる。
（ｃ’）推測方位データは前回の方位をθ0として、θ0＋θ＋０．５度となる。
（ｄ’）表示車両方位もθ0＋θ＋０．５度となる。
（ｅ’）方位差は次式により求まる。

（ｆ’）積分手段２３１では上記（ｂ’）と同じ結果（θ＋０．５ｄｅｇ）となる。
（ｇ’）角度誤差は（ｅ）と（ｆ）よりゼロとなる。すなわち、

【００８１】
以上のようにデットレコニングによる推測データしか使えない場合には角度誤差がゼロとなり、オフセット誤差もゼロとなって補正ができないこととなる。これは地図データのない場所を走行することに相当し、補正のための基準となるデータが何もないときに起こる。
なお、この場合でも、地磁気センサ２０３からの角度データがあれば（ｂ’）で角度データがθ度となり、（ｇ’）で角度誤差が０．５度となってオフセット補正が可能になる。
さらに、所定時間以上前述の（１）または（２）の状態で積分手段２３１にオフセットを含んだ角速度データが積分された状態で保存されていた場合、一時的に地図データのない場所を走行して方位差の変化分と積分手段２３１の変化分が一致する場合にも、誤差検出手段２３３から角度誤差が生じるのでオフセットの補正が可能である。
【００８２】
この場合の過程を前記（ａ）の各速度データが１０００秒経過した例をもとに説明する。
（ｅ’）初期状態よりジャイロ２０４の回転量が０度とすると、

（ｆ’）積分手段２３１でもオフセット分だけが残る。すなわち、
０．０１ｖ×１０００／（０．０２ｖ／ｄｅｇ）＝５００ｄｅｇ
（ｇ’）角度誤差は（ｅ”）と（ｆ）より、

（ｈ’）経過時間を１０００秒とすると、

（ｉ’）オフセット補正では以下のようにオフセットが除去される。

【００８３】
ここで、地図データのない場所へ入ると次のようになる。
（ｅ”）表示車両方位データがＥｓｉｎωｔ＋０．０１となりオフセット分だけを考えると、
（方位差）＝４８＋（０．０１／０．０２）−４８＝０．５ｄｅｇ
（ｆ”）積分手段２３１の値は、
５００＋（０．０１／０．０２）＝５００．５ｄｅｇ
（ｇ”）角度誤差は、
（角度誤差）＝５００．５−０．５＝５００ｄｅｇ
（ｈ”）オフセット誤差は、
（オフセット誤差）＝（５００／１００１）×０．０２≒０．００９９９ｖ
（ｉ”）オフセット補正は、

となり、徐々に補正値が減少していくが、その期間、積分されたデータにより補正がされる。
【００８４】
【発明の効果】
以上説明したように本発明によれば、車両が停止状態或いは移動状態のいずれであってもドリフトの補正が可能となるので、高速道路や信号機のない道路での方位検出が正確になり、その結果ナビゲーション装置の表示性能が向上する。
また、直線道路がない場合でも、ドリフトの除去されたデータをナビゲーション装置に入力するので修正自体が不要となる。
更に、事前に地図データのある場所を走行して車両及びジャイロセンサの温度変化がなくなっている場合に補正値が安定してくる。この場合、車が地図データのない場所に移動しても補正値は大きく変化しないため、単純に補正値をホールドするだけでドリフトの補正が可能となり、ナビゲーション装置の現在位置表示性能が向上する。
更にまた、第１２〜第１３の発明ではドリフト補正回路にリミッタを設けることにより一時的な変動を抑制することができる。
また、第１４〜１７の発明では車両がマップマッチング状態かどうかに拘らず、補正が可能である。また、地図のない場所では地磁気センサ，ＧＰＳ受信機からの情報により補正が可能である。更に、リンク方位ではない連続的に変化する表示車両方位を使っているため、一時的な誤差を生じない。
また、第１７の発明では、多くの情報からその時点で一番正確と判定された情報をナビゲーション装置が選択して得た表示車両方位を用いているので個々の情報の欠点を補った精度の高いジャイロのドリフト補正が実現される。
【図面の簡単な説明】
【図１】本発明に基づくジャイロのドリフト補正方法の一実施例を示すフローチャートである。
【図２】本発明に基づくジャイロのドリフト補正回路の一実施例の構成を示すブロック図である。
【図３】本発明に基づくジャイロのドリフト補正回路の他の実施例の構成を示すブロック図である。
【図４】本発明に基づくジャイロのドリフト補正回路の他の実施例の構成を示すブロック図である。
【図５】本発明に基づくジャイロのドリフト補正回路の他の実施例の構成を示すブロック図である。
【図６】本発明に基づくジャイロのドリフト補正回路の他の実施例の構成を示すブロック図である。
【図７】Ｎ点和の演算回路及び積分回路の構成例を示すブロック図である。
【図８】従来のナビゲーション装置の構成例を示すブロック図である。
【図９】本発明のナビゲーション装置の一実施例の構成を示すブロック図である。
【図１０】地図の表示例である。
【図１１】ジャイロの補正方法の一例を示すフローチャートである。
【図１２】ジャイロ出力のオフセット補正方法の説明図である。
【符号の説明】
３，２０４ ジャイロセンサ
６ Ａ／Ｄ変換回路（変換手段）
７，２０８ マップマッチング手段
８，２０７ デットレコニング手段
２０，３０，４０，５０，６０ ドリフト補正回路
２１ ＦＩＲフィルタ（第１のフィルタ手段）
２２，２３ 減算器（減算手段）
２５ 微分回路（第２のフィルタ手段）
２６ ＦＩＲフィルタ（第２のフィルタ手段）
３１ 遅延回路（遅延手段）
４１，５１ Ｎ点和の演算回路
４２，４８ 減算器（減算手段）
４３ コンパレータ（補正値応答遅延手段）
４４ 積分回路（補正値応答遅延手段）
４７ 除算回路（補正値応答遅延手段）
６１ Ｎ点和の演算回路（補正値応答遅延手段）
６２ 定数除算回路（補正値応答遅延手段）
１００，２００，２００’ ナビゲーション装置
２０１ ＧＰＳ受信機
２０３ 距離センサ
２１０ 地図データ
２１１ 表示データ作成手段
２１１’ 表示車両方位選択手段
２１２ 表示装置
２２１，２２２ 積分手段
２３１ 記憶手段
２３２ 方位差検出手段
２３３ 誤差検出手段
２３４ オフセット算出手段
２３５ オフセット補正手段[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a gyro used for a navigation system of a mobile object, and more particularly to a drift correction method and a drift correction circuit.
[0002]
[Prior art]
A mobile device, for example, a car navigation device, measures an azimuth change by an angle sensor, displays the vehicle position on a map displayed on the display device using the result, and makes the driver recognize the current position. The purpose is that.
[0003]
FIG. 8 is a block diagram showing a configuration example of a main part of the above-described vehicle-mounted navigation device 100, and the A / D conversion means 6 converts the output voltage of the gyro sensor (piezoelectric vibration gyro (angle sensor)) 3 by the A / D conversion means 6. The output data (digital signal) is input to the dead reckoning means 8.
Then, the data from the speed sensor 2 as the distance sensor is taken into the dead reckoning means 8 through the interface 5 and the dead reckoning means 8 performs the dead reckoning process, and the result (coordinate data) is given to the map matching means 7. The map matching means 7 is extracted from the map data search means 9 storing the map data based on the coordinate data from the GPS 1, compares the coordinate data obtained by the dead reckoning means 8, and the map and vehicle position are drawn by the drawing means 10. (The in-vehicle navigation device 100 uses a navigation system that uses GPS data as an example. However, even if the navigation system does not use GPS data, the gyro sensor 3, A The configuration between the / D conversion means 6 and the dead reckoning means 8 is the same).
[0004]
The map matching means 7, the dead reckoning means 8, the search means 9, and the drawing means 10 are normally performed by a program (hereinafter referred to as a navigation program), and the execution of these navigation programs and the control of the entire navigation apparatus are performed by a control unit (not shown). Z).
[0005]
In the conventional navigation device, when position display is performed in a short cycle (for example, a cycle of 1 to 2 seconds), dead reckoning processing and map matching processing (described later) are performed in that cycle, and a piezoelectric vibration gyro is used as an angle sensor. When used, the output voltage of the piezoelectric vibration gyro is sampled at regular intervals, A / D converted, taken into a microcomputer, and integrated with a period of one second to obtain an integrated value as an angular change amount for one second. In addition, a new coordinate is obtained by obtaining a movement vector from the angle change amount and the movement distance from the distance sensor and adding this to the previous coordinate. Also, a new direction is obtained by adding the amount of angle change to the previous direction (dead reckoning process).
[0006]
Further, the vehicle position is corrected on the display map by comparing the map data of the navigation device with the coordinates of the vehicle (map matching process).
However, even when the piezoelectric output gyro is stopped or when the sensor output should be zero, such as when running in a straight line, drift (drift) due to the influence of temperature, humidity, etc. occurs, so the output voltage is completely zero. However, there was a problem that the error would increase if integration was continued.
[0007]
And as a correction method for such drift,
(1) Japanese Examined Patent Publication No. 58-39360 discloses that a DC component X (volt) is obtained by measuring the output voltage of a piezoelectric vibration gyro while the vehicle is stopped after a certain period of time. A technique for removing drift by subtracting X from the output voltage while traveling, that is, a method of measuring and correcting at regular intervals using the voltage at the time of stop as a reference voltage, is disclosed.
(2) Japanese Patent Application Laid-Open No. 6-42976 discloses a technique for determining the direction of a vehicle when traveling on a straight road during map matching processing, that is, based on map information (offset correction of gyro output). The method of correcting the orientation according to (1) is disclosed. Furthermore,
(3) In Japanese Patent Laid-Open No. 4-34623, the first link information is stored from the map data of the road in which the vehicle is estimated to be traveling while the vehicle is in the map matching state. A technique is disclosed in which a link azimuth difference is calculated from the second link azimuth at the time when it has elapsed, and an offset value is estimated from an error from the gyro azimuth change amount during this period to correct the gyro drift. In this technique, when GPS azimuth data is obtained, this data is compared with the azimuth data obtained from the gyro output, and an offset value is obtained and corrected.
[0008]
[Problems to be solved by the invention]
However, the characteristics of the piezoelectric vibration gyro drift correction method according to the above prior art are as follows:
(1) Correct the output voltage of the piezoelectric vibration gyro when the car is running using the output voltage component of the piezoelectric vibration gyro when the car is stationary.
(2) When the vehicle goes straight, the angle after integration is corrected using the road direction based on the stored map information.
(3) When the vehicle is in the map matching state, the link direction obtained from the gyro output is compared with the link direction obtained from the map matching, and the difference is accumulated to obtain the drift correction value.
There are problems as shown in the following (1), (2), and (3).
[0009]
(1) Regarding point (1) above, there is no opportunity to stop on highways and roads without traffic lights (for example, mountain narrow roads) and voltage measurement at rest cannot be performed.
(2) Regarding point (2) above, if there is no straight road, it cannot be corrected where there is no map data. Further, even if correction is possible, the correction target is integration data and not sensor data (for example, output of a piezoelectric vibration gyro).
(3) Since point (3) above is based on the premise of map matching, it cannot be corrected where there is no map data, and the current navigation device has all the map data that the vehicle can pass through. Because it is not, there is a low possibility of correction.
In addition, since the vehicle is constantly moving, there is a high possibility that the vehicle will temporarily move to a place without a map even in the map matching state, and it is difficult to maintain the map matching state.
Furthermore, even in the map matching state, the link direction and the traveling direction of the vehicle are not always the same. For example,
a. If you operate the steering wheel to change lanes, the link direction and the traveling direction of the vehicle will not match,
b. When entering the parking area on an expressway, etc., it is unlikely that the link direction matches the direction of travel of the vehicle.
c. In the case of a turning angle, the digital map data assumes that the turning angle is expressed by two links, one link is assumed to be 0 degrees and the other one link is assumed to be 90 degrees for a turn of 90 degrees. The actual vehicle direction changes continuously as time goes from 0 degree → 1 degree → 2 degrees →. → 89 degrees → 90 degrees, whereas the link direction is 0 degrees as described above. Therefore, if the offset value is estimated from the link azimuth and the actual vehicle azimuth, a large error occurs temporarily, which adversely affects the gyro drift correction.
Furthermore, the GPS azimuth data increases when the speed of the vehicle is slow, and cannot be calculated when the vehicle stops, and is not calculated when radio waves from the satellite do not enter the GPS receiver. A point that becomes possible.
Here, as a problem to solve the above problem, if drift correction can be realized even if the vehicle cannot be stopped, drift does not occur, so the accuracy of the map matching process is improved. Moreover, even if the vehicle moves from a place where there is map data to a place where the map data does not exist, the current position display performance of the navigation device can be improved if the drift correction can be continued.
[0010]
The present invention was made for the purpose of solving the above problems and solving the above problems,
(A) Providing a drift correction method and drift correction circuit for a piezoelectric vibration gyro capable of correcting drift without stopping, and
(B) An object of the present invention is to provide a gyro drift correction circuit capable of continuing drift correction even when a vehicle moves from a place where map data exists to a place where map data does not exist.
(C) Furthermore, a gyro drift correction method and a drift correction circuit capable of correcting the gyro even when the vehicle is not in the map matching state and there is no map data, and
(D) Providing a drift correction method and drift correction circuit for a gyro that does not cause a temporary error when obtaining a correction value; and
(E) An object of the present invention is to provide a gyro drift correction method and a drift correction circuit capable of correcting the gyro offset even when accurate azimuth data cannot be obtained from the GPS.
[0011]
[Means for Solving the Problems]
In order to achieve the above object, a gyro drift correction method according to a first aspect of the present invention is a mobile navigation device including a gyro sensor for detecting a change in angle and a map matching means.
(1) Obtain digital data at regular intervals from the output signal from the gyro sensor, determine a first moving average value within a predetermined time from the digital data,
(2) The vehicle orientation data obtained by the map matching process is taken out at regular time intervals to obtain a difference value, and a second moving average value within a predetermined time is obtained based on the difference value.
(3) The difference between the first moving average value and the second moving average value is obtained as a drift correction value,
(4) The difference between the digital data and the drift correction value is calculated to obtain corrected gyro data.
[0012]
The second aspect of the invention is the gyro drift correction method of the first aspect of the invention, further storing the drift correction value obtained in step (3) and storing the corrected gyro data in step (4). The drift correction value is extracted and used.
[0013]
According to a third aspect of the present invention, in the gyro drift correction method according to the first or second aspect of the invention, the moving body moves to a location outside the range defined in the map data stored in the storage unit of the navigation device of the moving body. In this case, the updating of the drift correction value is stopped and the drift correction is continued.
[0014]
A gyro drift correction circuit according to a fourth aspect of the present invention is a mobile navigation device including a gyro sensor for detecting a change in angle and a map matching means. The output data from the gyro sensor is input to the gyro sensor for every predetermined time. First computing means for obtaining a first moving average value, and second computing means for obtaining a second moving average value for every predetermined time by inputting a difference value of azimuth data of the moving body from the map matching means. First subtracting means for subtracting the second moving average amount from the first moving average amount to obtain a drift correction amount; and corrected gyro data by subtracting the drift correction amount from the output data from the gyro sensor. And a second subtracting means for obtaining.
[0015]
According to a fifth invention, in the circuit of the fourth invention, a delay means for delaying output data from the gyro sensor for a predetermined time is provided on the input side of the first calculation means and the second calculation means. Features.
[0016]
A gyro drift correction circuit according to a sixth aspect of the present invention is a mobile navigation device including a gyro sensor that detects a change in angle and a map matching means. The gyro sensor includes N times of past output data from the gyro sensor. A first sum calculating means for calculating the first sum data; and a second sum for calculating the second sum data for the past N times from the present of the difference value of the orientation data of the moving object from the map matching means. Calculating means; first subtracting means for subtracting the second sum data from the first sum data to obtain a difference output; comparing means for comparing the difference output with input data; and integrating the output of the comparing means The integrated output is returned to the comparing means as the input data, and the integrated output is divided by time to obtain a drift correction value, and the integrated output is output from the output data from the gyro sensor. A second subtraction means for obtaining a corrected gyro data by subtracting the bets correction value, characterized by comprising a.
[0017]
A gyro drift correction circuit according to a seventh aspect of the present invention is a mobile navigation device including a gyro sensor that detects a change in angle and a map matching means, and outputs N times of past output data from the gyro sensor. A first sum calculating means for calculating the first sum data; and a second sum for calculating the second sum data for the past N times from the present of the difference value of the orientation data of the moving object from the map matching means. Calculating means; first subtracting means for subtracting the second sum data from the first sum data to obtain a difference output; comparing means for comparing the difference output with input data; and integrating the output of the comparing means The integrated output is returned to the comparing means as the input data, and the integrated output is input to the FIR filter to obtain a drift correction value, and the output data from the gyro sensor. A second subtraction means for obtaining a corrected gyro data by subtracting the count on drift correction value, characterized by comprising a.
[0018]
A gyro drift correction circuit according to an eighth aspect of the present invention is a mobile navigation device including a gyro sensor that detects a change in angle and a map matching means, and outputs N times of past output data from the gyro sensor. A first sum calculating means for calculating the first sum data; and a second sum for calculating the second sum data for the past N times from the present of the difference value of the orientation data of the moving object from the map matching means. A computing means; a first subtracting means for subtracting the second sum data from the first sum data to obtain a difference output; a comparing means for comparing the difference output with the input data; and output data of the comparing means. The third sum data for the past N times from the present is calculated, the third sum data is returned to the comparison means as the input data, and the third sum data is divided by a predetermined constant to obtain a drift correction value. And obtaining arithmetic means, characterized by comprising a second subtracting means for obtaining a corrected gyro data by subtracting the drift correction value from the output data from the gyro sensor.
[0019]
According to a ninth invention, in the invention of the fourth circuit, the first and second arithmetic means are FIR filters.
[0020]
A gyro drift correction circuit according to a tenth aspect of the present invention includes a gyro sensor that detects an angle change, a storage unit that stores map information, a conversion unit that obtains voltage change data from the gyro sensor output, and an output and distance of the conversion unit. Detconing means for obtaining coordinate data by obtaining the coordinate change amount from the movement distance amount obtained from the sensor and adding the current change amount to the previous coordinate, and moving based on the azimuth data from the dead reckoning means and the map information A gyro drift correction circuit for a mobile navigation device including a map matching means for obtaining a current position of the body, wherein voltage change data from the conversion means is input to obtain a first moving average amount within a predetermined time. Based on the difference value between the first filter means to obtain and the azimuth data of the moving body from the map matching means, the second moving average amount within a predetermined time is obtained. Second filtering means, first subtracting means for obtaining a difference between the first moving average amount and the second moving average amount, obtaining a difference between the voltage change data and the output of the first subtracting means, and And second subtracting means for giving to the dead reckoning means.
[0021]
According to an eleventh aspect of the invention, in the drift correction circuit of the tenth aspect of the present invention, clock pulse generating means for providing time information to the first and second filter means, and a storage unit of the navigation device for the mobile object. Switch means for cutting off time information from the clock pulse generation means so as not to update the drift correction value when moving to a location outside the range defined in the map data stored in the map data.
[0022]
According to a twelfth aspect of the present invention, in the drift correction circuit according to the tenth or eleventh aspect of the present invention, a delay corresponding to a difference value output time in the map matching means is further provided between the conversion means and the first filter means. Means are provided.
[0023]
A thirteenth aspect of the invention is a gyro sensor that detects an angle change, a storage unit that stores map information, a conversion unit that obtains voltage change data from a gyro sensor output, an output of the conversion unit, and a movement obtained from the distance sensor Detconing means for obtaining coordinate data by obtaining coordinate change amount from distance amount and adding current change amount to previous coordinate, and obtaining current position of moving body based on coordinate data and map information from dead reckoning means A gyro drift correction circuit for a navigation device of a mobile object including a map matching means, wherein a first N for calculating a sum of data for the past N times from the present time by inputting voltage change data from the conversion means. Point sum calculation means, differentiation means for obtaining the azimuth data of the moving body from the map matching means and obtaining a difference value of the azimuth data, and the output of the differentiation means are input. A second N-point sum calculating means for calculating the sum of the past N data from the present time; a first subtracting means for outputting a difference between the outputs of the first and second N-point sum calculating means; Correction value response delay means for obtaining a drift correction value by delaying the response of the drift correction value based on the output of the first subtracting means by the difference value output time in the map matching means, and subtracting the drift correction value from the voltage change data And a second subtracting means.
[0024]
According to a fourteenth aspect of the present invention, there is provided a gyro drift correction method comprising: a gyro sensor for detecting an angle change; a dead reckoning unit; and a map matching unit. It is characterized by including.
(1) Obtain the displayed vehicle direction and store it with the time,
(2) The output from the gyro sensor is integrated to obtain a gyro direction. If a certain time has elapsed, step (3) is executed, and if not, the above integration is repeated.
(3) The display vehicle orientation at the time when the predetermined time has elapsed is acquired, and the orientation difference is obtained.
(4) Obtain the difference between the azimuth difference and the gyro azimuth, and obtain the offset correction value by dividing the difference by the predetermined time.
(5) The gyro output is corrected by subtracting the offset correction value from the gyro output.
[0025]
According to a fifteenth aspect of the present invention, in the gyro drift correction method according to the fourteenth aspect of the present invention, when the coordinates obtained by the map matching means are estimated as the own vehicle position, and the own vehicle position is determined on the road by the estimated value, the If the vehicle direction cannot be determined on the road from the estimated value obtained by the map matching means, the direction of the road is used as the display vehicle direction, and the estimated position obtained by the dead reckoning process is used as the current position. It is characterized by that.
[0026]
According to a sixteenth aspect of the present invention, in the gyro drift correction method of the fourteenth aspect, when the difference between the coordinates obtained by autonomous navigation and the coordinates obtained from the GPS is large, the direction obtained from the GPS is set as the display vehicle direction, and the GPS reception is performed. When the difference between the coordinates obtained from the GPS and the coordinates obtained by the map matching means is small, the coordinates obtained by the map matching means are estimated as the own vehicle position, and the own vehicle position is determined on the road by the estimated value. When determined, the direction of the road is used as the vehicle direction, and if the vehicle position cannot be determined on the road based on the estimated value obtained by the map matching means, the vehicle direction using the estimated position obtained by the dead reckoning process as the current position. Is the display vehicle direction.
[0027]
According to a seventeenth aspect of the present invention, there is provided a gyro drift correction apparatus, which is obtained by autonomous navigation in a mobile navigation apparatus including a GPS receiver, a gyro sensor that detects a change in angle, dead reckoning means, and map matching means. When the difference between the coordinates obtained from the GPS and the coordinates obtained from the GPS is large, the direction obtained from the GPS is used as the display vehicle direction, and when the GPS cannot be received, or the difference between the coordinates obtained from the GPS and the coordinates obtained by the map matching means is small. Sometimes the coordinates obtained by the map matching means are estimated as the vehicle position, and when the vehicle position is determined on the road by the estimated value, the direction of the road is set as the display vehicle direction, and the estimation obtained by the map matching means If the vehicle position cannot be determined on the road by the value, the vehicle direction is displayed using the estimated position obtained by the dead reckoning process as the current position. Display vehicle direction selecting means for performing the display, storage means for obtaining the display vehicle direction from the display vehicle direction selecting means and storing it together with the time, integration means for integrating the output from the gyro sensor for a certain period of time to obtain gyro bearing data, and the constant After a lapse of time, a direction difference detection means for obtaining a display vehicle direction at that time and obtaining a direction difference from the stored display vehicle direction data, a difference between the direction difference and the gyro direction is obtained, and the difference is calculated at the predetermined time. An offset correction value calculating unit that obtains an offset correction value by dividing, and an offset correction unit that subtracts the offset correction value from the gyro output to correct the gyro output.
[0028]
According to an eighteenth aspect of the present invention, in the gyro drift correction circuit according to the seventeenth aspect of the present invention, the gyro drift correction circuit further includes a geomagnetic sensor, and the corrected gyro is determined when the vehicle position cannot be determined on the road by the estimated value obtained by the map matching means. Instead of the output, the angle data value from the geomagnetic sensor is used as an input of the dead reckoning means.
[0029]
BEST MODE FOR CARRYING OUT THE INVENTION
In the gyro drift correction method of the first invention, digital data is obtained from the output signal from the gyro sensor at regular intervals, a first moving average value within a predetermined time is obtained from the digital data, and map matching processing is performed. The obtained azimuth data of the vehicle is taken out at regular time intervals to obtain a difference value thereof, a second moving average value within a predetermined time is obtained based on the difference value, and the first moving average value and the second moving average value are obtained. Since the difference between the moving average value is obtained and used as a drift correction value, and the difference between the digital data and the drift correction value is calculated to obtain corrected gyro data, the drift correction value is obtained regardless of the (stop or move) state of the vehicle. be able to.
[0030]
In the drift correction method of the second invention, in the first invention, the obtained drift correction value is stored, and the stored drift correction value is extracted and used when calculating the corrected gyro data. When moving to a place where there is no map data, such as when located in a place other than the range specified in the above, the previous correction value stored is used as the calculation data of the corrected gyro data of this time.
[0031]
In the gyro drift correction method of the third aspect of the invention, in the first or second aspect of the invention, the range specified by the mobile object in the map data stored in the storage unit of the navigation device of the mobile object (for example, the map data Since the drift correction value is not updated and the drift correction is continued when the vehicle is located on a place other than the road defined in (1), corrected gyro data is calculated based on the previous correction value.
[0032]
In the fourth aspect of the invention, the first and second moving average values are obtained at fixed time intervals from the output data from the gyro sensor and the azimuth data from the map matching means, and the drift value is obtained by subtracting both to correct. Obtain completed gyro data.
[0033]
According to a fifth aspect, in the fourth aspect, the output data from the gyro sensor is delayed for a predetermined time so as to correspond to the calculation time in the map matching means.
[0034]
In the sixth invention, instead of the first and second moving average amounts in the fourth invention, the first and second sum data for the past N times from the present are calculated and the difference output is compared, Integration and time division are performed to obtain a drift correction value.
[0035]
In the seventh invention, instead of the time division in the sixth invention, the calculation by the FIR filter is performed.
[0036]
In the eighth invention, instead of integration and time division in the sixth invention, the third sum data for the past N times of the comparison output data from the present is calculated, and the third sum data is divided by a constant. To obtain a drift correction value.
[0037]
In the ninth invention, the first and second moving average amounts in the sixth invention are obtained by the FIR filter.
[0038]
In the gyro drift correction circuit of the tenth aspect of the invention, the first filter means inputs voltage change data from the conversion means to obtain a first moving average amount within a predetermined time, and the second filter means maps it. A second moving average amount within a predetermined time is obtained from the matching unit based on the difference value of the azimuth data of the moving body, and the difference between the first moving average amount and the second moving average amount is obtained by the first subtracting unit. (Drift correction value) can be obtained, and the drift correction value can be obtained regardless of the (stop or move) state of the vehicle. Further, since the difference between the voltage change data and the output of the first subtracting means is obtained by the second subtracting means and given to the dead reckoning means, more accurate azimuth can be determined by the dead reckoning means.
[0039]
The gyro drift correction circuit according to an eleventh aspect of the present invention is the drift correction circuit according to the tenth aspect of the present invention, when the mobile body is located at a place other than the range specified in the map data stored in the storage unit of the navigation device of the mobile body. Since updating of the value is stopped and drift correction is continued, corrected gyro data is calculated based on the previous correction value.
[0040]
A gyro drift correction circuit according to a twelfth aspect of the present invention is the delay means according to the tenth or eleventh aspect, wherein the delay corresponding to the difference value output time in the map matching means is performed between the conversion means and the first filter means. Therefore, a large fluctuation of the drift correction value that can occur within the difference output time by the map matching means is suppressed.
[0041]
The gyro drift correction circuit according to the thirteenth aspect of the present invention provides the difference value output by the map matching means by the correction value response delay means for obtaining the drift correction value by delaying the response of the drift correction value by the difference value output time in the map matching means. Large fluctuations in the drift correction value that can occur in time are suppressed.
[0042]
In the gyro drift correction method of the fourteenth aspect of the invention, the display vehicle direction is obtained and stored along with the time, and the output from the gyro sensor is integrated to obtain the gyro direction. The display vehicle direction is acquired and the direction difference is obtained. Next, a difference between the azimuth difference and the gyro azimuth is obtained, and the difference is divided by the predetermined time to obtain an offset correction value, and the offset correction value is subtracted from the gyro output to correct the gyro output.
[0043]
According to a fifteenth aspect of the present invention, in the gyro drift correction method according to the fourteenth aspect of the present invention, when the vehicle position cannot be determined on the road by the estimated value obtained by the map matching means, the estimated position obtained by the dead reckoning process is determined as the current position. Assuming that the vehicle direction is the display vehicle direction, the offset correction value is obtained and the gyro output is corrected.
[0044]
According to a sixteenth aspect of the present invention, in the gyro drift correction method according to the fourteenth aspect of the present invention, any one of the azimuth obtained from GPS, the vehicle azimuth obtained by the map matching means, and the vehicle direction obtained by the dead reckoning process is set as the current GPS or The gyro output is corrected by obtaining an offset correction value as a display vehicle direction selected according to the map matching state.
[0045]
A gyro drift correction device according to a seventeenth aspect of the present invention obtains a display vehicle direction from the display vehicle direction selection means and stores it together with time, integrates the output from the gyro sensor for a certain period of time to obtain gyro direction data, and displays the vehicle direction Is obtained, and a difference between the orientation difference and the gyro orientation is obtained and divided by time to obtain an offset correction value. Then, the gyro output is corrected by subtracting the offset correction value from the gyro output. This makes it possible to use the display vehicle orientation obtained by the navigation device selecting the most accurate information at that time from a large amount of information, and to accurately compensate for the gyro drift that compensates for the shortcomings of individual information. Is realized. In a place where there is no map data, only data from the direction sensor and GPS direction data can be used to obtain the displayed vehicle direction. Therefore, the displayed vehicle direction is the direction data from the direction sensor, or the GPS direction data itself when the GPS is receivable and reliable. Therefore, when the display vehicle direction is obtained from the direction sensor data, if the output of the gyro is used as the direction sensor, the output of the error detection means becomes zero. However, if the vehicle is traveling in a place with map data before moving to a place without map data, the offset value is accumulated in the integrating means during that time.
On the other hand, since the offset difference is not accumulated in the azimuth difference detection means, offset correction can be performed for a while after the vehicle moves to a place where there is no map data. Then, the offset correction amount decreases with time and eventually becomes zero. Therefore, after traveling in a place with map data, the offset is corrected even if traveling in a place without map data for a short time.
Further, when GPS azimuth data is obtained in the middle of the process, the offset value accumulated in the map matching up to that point is discarded, so that the offset can be corrected even when traveling for a long time without map data.
[0046]
In the eighteenth aspect, since the geomagnetic sensor is used as the azimuth sensor, the integrated value of the gyro output follows the angle offset of the geomagnetic sensor. Therefore, integration control is performed, and the gyro offset is corrected to zero.
[0047]
【Example】
In the gyro drift correction method according to the present invention, taking the navigation device 100 of FIG. 8 as an example, the output signal from the gyro sensor 3 is A / D converted at regular intervals to obtain data A, and from the data A A moving average value B for a predetermined time t is obtained (however, a correction value E described later is not calculated when there is no map data).
Next, the azimuth data of the vehicle is extracted from the map matching means 7 of the navigation device 100 at regular intervals, the difference value C is obtained, and the moving average value D for a predetermined time t is obtained from the difference value C.
A difference (B−D) is obtained from the above two moving average values B and D and set as a correction value E. Then, a difference (EA) between the data A and the correction value E is calculated to obtain corrected gyro data F. The correction value E is stored every time, and the stored correction value is taken out when calculating the corrected gyro data F.
[0048]
<Example 1>
FIG. 2 is a flowchart showing an embodiment of a gyro drift correction method according to the present invention, which is an example realized by a computer program (for the sake of explanation, FIG. 8 shows a configuration of a navigation apparatus that executes the program of this embodiment. As an example).
Here, the gyro drift program is stored in a ROM (not shown) as a part (subroutine) of the navigation program for executing the navigation operation of the navigation device 100, and is taken out and executed when the navigation operation of the navigation device 100 is executed. Execution is controlled by a CPU (not shown) of the apparatus 100.
[0049]
In FIG. 1, in step S1, digital data obtained by A / D converting the output signal from the gyro sensor 3 every predetermined time is read as data A, and the process proceeds to step S2.
In step S2, it is determined whether or not map data near the current position is obtained from the map data search means 9 via the map matching means 7 (the presence or absence of map data). If map data is obtained, the process proceeds to step S3. If the map data cannot be obtained, the process proceeds to step S7.
In step S3, a moving average value for 10 minutes is calculated from the data A obtained in step S1, and the result is set to B, and the process proceeds to step S4.
In step S4, the vehicle orientation data from the map matching means 7 of the navigation device 100 (the vehicle orientation data of the map matching means 7 is used for drawing the vehicle position because the navigation device 100 uses the vehicle orientation data) is fixed. The difference from the previous azimuth data stored in the memory (not shown) taken out every time is obtained as a difference value C, and the process proceeds to step S5.
[0050]
In step S5, a moving average value for 10 minutes is calculated from the difference value C, the result is set to D, and the process proceeds to step S6.
In step S6, a correction value E = BD is calculated, the result is stored in a memory (not shown), and the process proceeds to step S7.
In step S7, the correction value E stored in the memory is taken out, and the difference (EA) between the data A and the correction value E is calculated to obtain corrected gyro data F.
The correction value E is saved every time, and the saved correction value is taken out when calculating the corrected gyro data F. Therefore, if there is no map data in the determination of step S2, the previous correction value E stored is saved. Is used as the calculation data of the corrected gyro data F at this time. Then, the obtained corrected gyro data F is delivered to the dead reckoning means 8 of the navigation apparatus 100 as corrected data of the gyro sensor output.
[0051]
<Example 2>
2A is a block diagram showing a configuration of an embodiment of a gyro drift correction circuit according to the present invention. The drift correction circuit 20 includes an A / D conversion means 6, a dead reckoning means 8, And a circuit connected to the map matching means 7. In the present embodiment, for the sake of explanation, the components 1 to 11 of the navigation device 100 are the same as those in FIG.
In the drift correction circuit 20, 21 is a first FIR filter, 22 and 23 are subtractors, 24 is an interface, 25 is a differentiation circuit, 26 is a second FIR filter, 27 is a clock generator, and 28 is a switch.
2B is a configuration example of the FIR filters 21 and 26 of the drift correction circuit 20, and FIG. 2C is a configuration example of the differentiation circuit 25 of the drift correction circuit 20.
[0052]
In FIG. 2, the output of the gyro sensor 3 is A / D converted by the A / D conversion means 6 at regular time intervals, the digital data A is obtained and input to the first FIR filter 21, and the output is B.
Next, the azimuth data of the vehicle is input from the map matching means 7 of the navigation device 100 to the differentiation circuit 25 through the interface circuit 24 at regular time intervals to obtain the output C, and the output C is input to the second FIR filter 26. The output is D. These two filter outputs are input to the subtractor 22 to obtain a drift correction value E = BD.
Then, the data A and the correction value E from the A / D conversion means 6 are added to the subtracter 22 to obtain an output F = AE. This output F is given to the dead reckoning means 8 as a gyro correction value.
[0053]
Here, as shown in the example of FIG. 2A, the clock is supplied from the clock generator 27 to the first FIR filter 21 and the second FIR filter 26, but when there is no map data. The switch 28 is turned OFF to stop the clock supply, and the correction value E is held. Thereby, when there is no map data, the subtracter 23 is given the data A and the previous correction value E.
[0054]
As shown in the block diagram of FIG. 2B, the FIR filters 21 and 26 have N delay circuits 221 (delay time T), N + 1 multipliers 222, and N adders 223. The signal (IN) is first multiplied by the coefficient h0 by the multiplication circuit 222-0 and input to the adder 223-1 in the first-order filter stage.
Further, the input signal (IN) is delayed by the time T by the delay circuit 221-1, multiplied by the coefficient h1 by the multiplication circuit 222-1 and input to the adder 223-1, and these two signals are added.
[0055]
In the second-order filter stage, the input signal (IN) is delayed by the time 2T by the delay circuit 221-2, multiplied by the coefficient h2 by the multiplication circuit 222-2, and input to the adder 223-2. The adder 223-2 receives the result of the adder 222-2 and adds these two signals.
[0056]
Similarly, in the i-th filter stage, the input signal (IN) is delayed by the time i · T by the delay circuit 221-i, multiplied by the coefficient hi from the multiplication circuit 222-i, and input to the adder 223-i. The adder 223-i receives the result of the adder 222- (i-1) and adds these two signals.
Thus, the moving average value can be obtained by appropriately selecting the coefficients h0 to Hn by using the FIR filter.
[0057]
The differentiation circuit 25 includes a delay circuit 251 (delay time T) and a subtraction circuit 252, as shown in the example of FIG. 2C. One of the input signals (IN) is added to the positive terminal of the subtractor 252, and the input signal ( IN) is delayed by time T through the delay circuit 251 and added to the negative terminal of the subtractor 252 to obtain a difference value.
[0058]
<Example 3>
In the drift correction method of the first embodiment and the drift correction circuit 20 of the second embodiment, it is assumed that a difference value corresponding to the curve is immediately obtained from the map matching means 7 when the vehicle curves at the intersection.
However, the calculation by the map matching means 7 requires about 1 to 5 seconds, and the drift correction value may fluctuate greatly during this period.
Accordingly, in this embodiment, a configuration example of a drift correction circuit to which a drift correction value fluctuation suppressing function within a difference value calculation time by a map matching process required when a vehicle turns an intersection will be described with reference to the block diagram of FIG. .
[0059]
FIG. 3 is a block diagram showing a configuration of an embodiment of a gyro drift correction circuit according to the present invention. The drift correction circuit 30 includes an A / D conversion means 6, a dead reckoning means 8 and a map matching means of the navigation apparatus 100. 7 is a circuit connected to 7. In the present embodiment, for the sake of explanation, the components 1 to 11 of the navigation device 100 are the same as those in FIG.
[0060]
In the drift correction circuit 30, 21 is a first FIR filter and corresponds to the first filter means, 22 and 23 are subtractors, 24 is an interface, 25 is a differentiation circuit, 26 is a second FIR filter, and 31 is The configuration of the delay circuit, from the first FIR filter 21 to the second FIR filter 26, is the same as that of the drift correction circuit 20 of FIG. In the present embodiment, the differentiating circuit 25 and the second FIR filter constitute second filter means.
In the drift correction circuit 30 of this embodiment, a delay time is given to the data A from the A / D conversion means 6 by the delay circuit 31 and the data A is delayed by a time corresponding to the calculation time of the map matching processing by the map matching means 7.
This reduces and suppresses large fluctuations in the drift correction value that may occur within the time corresponding to the difference output by the map matching means. The other processes are the same as those of the drift correction circuit 30 in FIG.
[0061]
<Example 4>
This embodiment shows a circuit that applies a limiter when a large error occurs in the outputs of the arithmetic circuits 41 and 51 temporarily.
In FIG. 4, the drift correction circuit 40 is a circuit connected to the A / D conversion means 6, the dead reckoning means 8, and the map matching means 7 of the navigation device 100. In the present embodiment, for the sake of explanation, the components 1 to 11 of the navigation device 100 are the same as those in FIG.
[0062]
In the drift correction circuit 40, 41 and 51 are N point sum calculation circuits, 42 is a subtraction circuit, 43 is a comparator, 44 is an integration circuit, 45 is a clock pulse generation circuit, 46 is a counter circuit, 47 is a division circuit, and 48 is A subtractor, 49 is an interface, and 50 is a differentiation circuit. In this embodiment, the comparator 43, the integrator 44, the clock pulse generation circuit 45, the counter circuit 46, and the division circuit 47 constitute a limiter.
In the drift correction circuit 40, the FIR filter (first FIR filter 21 and second FIR filter 26) of FIG. 2 is divided into N-point sum calculation circuits 41 and 51 and a multiplication circuit (actually a division circuit 47). In the meantime, the comparator 43 and the integration circuit 44 are used to suppress temporary fluctuations in the drift correction value.
Here, the N-point sum calculation circuits 41 and 51 are circuits having the configuration shown in FIG. 7A, and are circuits that perform calculation according to the following equation (1) for the N-point finite discrete time x (n). is there.
[0063]
[Expression 1]

[0064]
That is, it has N delay circuits 411 (delay time T) and N adders 412, and the input signal (IN) is first delayed by the delay circuit 411-1 by time T, and the adder 412- 2 is input. The adder 412-2 receives the input signal (IN) and adds these two signals.
Next, the input signal is delayed by time 2T by the delay circuit 411-2 and input to the adder 412-2. The adder 412-2 receives the result of the adder 412-1 and adds these two signals.
Similarly, the input signal is delayed by time i · T by the delay circuit 411-i and input to the adder 412-i. The adder 412-i receives the result of the adder 412 (i-1) and adds these two signals. As a result, the sum of the data for the past N times from the present to the time series data is calculated.
[0065]
In the drift correction circuit 40 of FIG. 4, the outputs of the N-point sum calculation circuits 41 and 51 are added to the subtraction circuit 42 to obtain a difference output, which is input to the comparator 43.
The comparator 43 compares with the other input data (output of the integration circuit 44), and outputs 0 (zero) if the difference between the two inputs is within a certain range ε (ε ≧ 0). If the value is large, a positive value is output. If the value is opposite, a negative value is output. This value is input to an integration circuit 44 as shown in FIG. 7B to perform integration, and the output is returned to the comparator 43.
By such an operation, an output following the difference output is obtained as the output of the integrating circuit 44. Time data is obtained by the clock pulse generator 45 and the counter circuit 46.
By dividing the output by time by the division circuit 47, a drift correction value having a small fluctuation can be obtained. This drift correction value is input to the subtractor 48, and a difference F = AE from the data A is obtained. This output F is given to the dead reckoning means 8 as a gyro correction value.
[0066]
<Example 5>
This embodiment shows a circuit that applies a limiter when a large error occurs in the outputs of the arithmetic circuits 41 and 51 temporarily.
In FIG. 5, the drift correction circuit 50 is a circuit connected to the A / D conversion means 6, the dead reckoning means 8, and the map matching means 7 of the navigation device 100. In the present embodiment, for the sake of explanation, the components 1 to 11 of the navigation device 100 are the same as those in FIG.
The drift correction circuit 50 uses the FIR filter 55 instead of the division circuit 47 in the drift correction circuit 40 of FIG. 4, and suppresses temporary fluctuation of the drift correction value by using the comparator 43 and the integration circuit 44. .
[0067]
<Example 6>
This embodiment shows a circuit that applies a limiter when a large error occurs in the outputs of the arithmetic circuits 41 and 51 temporarily.
In FIG. 6, the drift correction circuit 60 is a circuit connected to the A / D conversion means 6, the dead reckoning means 8, and the map matching means 7 of the navigation device 100. In the present embodiment, for the sake of explanation, the components 1 to 11 of the navigation device 100 are the same as those in FIG.
The drift correction circuit 60 uses an N-point sum calculation circuit 61 instead of the integration circuit 44 in the drift correction circuit 40 of FIG. 4, and a division circuit 47 that performs division by time is a division circuit 62 that performs division by a constant. Thus, by using the comparator 43, temporary fluctuation of the drift correction value is suppressed.
[0068]
Although the circuits (drift correction circuits 30, 40, 50, and 60 in FIGS. 3 to 6) for reducing the fluctuation of the drift correction value are shown in the third to sixth embodiments, the program is the same as in the first embodiment. Thus, these circuits can be realized.
[0069]
<Example 7>
FIG. 9 is a block diagram showing the configuration of another embodiment of the navigation apparatus of the present invention. The GPS receiver 201, distance sensor 202, geomagnetic sensor 203, gyro sensor 204, dead reckoning means 206, map matching means 207, map The data 210, the display vehicle direction selection means 211, and the display device 212 are the same as the configuration of the navigation device described in the first to sixth embodiments. In this embodiment, the navigation device 200 includes the integration means 221 to the offset correction means. The configuration is such that an offset correction unit consisting of 235 is added. The offset correction means is preferably stored in a ROM (not shown) as a part (subroutine) of a navigation program for executing the navigation operation of the navigation device 200, and is taken out when the navigation operation of the navigation device 200 is executed and taken out. CPU (not shown) controls execution.
[0070]
As shown in FIG. 9, the navigation apparatus 200 obtains information from a GPS receiver 201, a distance sensor 202, an orientation sensor such as a geomagnetic sensor 203, a gyro sensor 204, and the like, and processes by a dead reckoning means 206, a map matching means 207, and the like. The vehicle position is estimated through the map data 210, the display data creation means 211 determines the display vehicle direction to be used for offset correction from the GPS or map matching state, selects it, creates display data, and displays it. A map is drawn on the device 212, and the vehicle position mark 301 is displayed on the map 300 as shown in the example of FIG.
[0071]
The dead reckoning means 206 uses the movement distance from the distance sensor 202 and the azimuth change amount θ from the azimuth sensor to obtain a movement vector by the following equation.
The estimated vehicle direction is θ0 + θ, where θ0 is the previous direction.
Δx = 1cos (θ0 + θ)
Δy = 1sin (θ0 + θ)
This (x, y) is the estimated position and becomes input data of the map matching means 207.
The map matching means 207 estimates the vehicle position on the map using the estimated position and the map data 209.
The map matching means 207 first determines whether the vehicle is traveling straight or turning.
If the vehicle is traveling straight, a road close to the traveling direction of the vehicle is selected. Next, the road connected to the road made by the map matching in the past is selected. Then, one road closest to the estimated position is selected, a perpendicular is drawn from the estimated position to the selected road, and the intersection with the road is set as the vehicle position.
If the vehicle is turning, select one road by comparing the direction change process of the vehicle and the direction change of the road, and make a perpendicular line from the estimated position to the selected road. Is the vehicle position.
However, if there is no road close to the traveling direction, there is no connected road, or the shapes of the roads do not match, the own vehicle position is set as the estimated position.
[0072]
Further, when a road is found, the map matching vehicle direction is set as the road direction. When the vehicle position is the estimated position, the vehicle direction for map matching is the road direction. When the vehicle position is the estimated position, θ0 + θ is set as the vehicle direction obtained by the map matching process.
The map matching process is completed so far, but the following process is performed when the GSP data is reliable.
When the vehicle position coordinates obtained as a result of the map matching process and the coordinates of the GPS data are greatly deviated, the data of the vehicle position is discarded and the GPS data is set as the vehicle position, and the direction data of the GPS data is displayed as the vehicle direction To do.
In this way, the vehicle position and the displayed vehicle orientation can be obtained from the sensor and the GPS, but even in this case, the actual vehicle orientation is not always obtained accurately. For example, if the vehicle is rotated on a turntable while the navigation system is turned off in a parking lot, or if a turn is repeated in a multi-story parking lot, none of GPS direction data, map data, or gyro sensor data can be used. . For this, manual correction of the displayed vehicle direction is effective. Navigation systems generally have the functions of “correcting the vehicle position” and “correcting the vehicle direction”, which are displayed on the map display screen using the vehicle mark. When the driver recognizes that the display does not match the actual position or direction, the information input means corrects the position or direction estimated by the driver. Even in the case of such a special state, an accurate display vehicle direction can be determined.
Thus, when the vehicle position and the vehicle direction of the map matching are determined by the map matching process, the map data 210 centering on the vehicle position is read and the map drawing means 211 "draws the map on the display device 212 by this data. To do.
The own vehicle position mark 301 indicates the estimated own vehicle position on the map, and also indicates the direction in which the vehicle travels (referred to as the display direction).
Since this display method always holds one value while the navigation device is operating and is always updated with new information, it is updated with data from the direction sensor, GPS, etc. even when map matching is not possible.
[0073]
A gyro correction method according to the present invention will be described below based on the offset correction means of the navigation apparatus 200 of FIG. 1 and the flowchart of FIG.
In step S101, the display vehicle direction is acquired from the navigation device 200, stored in the storage means 231, and the process proceeds to step S102 (here, the stored value is M).
In step S102, the output of the gyro 204 is integrated and converted to a gyro orientation, and the process proceeds to step S103 (the gyro orientation here is G).
In step S103, it is checked whether or not the fixed time T has elapsed. If not, the process returns to step S102 and the integration of the gyro output is repeated. If it has elapsed, the process proceeds to step S104.
In step S104, the displayed vehicle orientation when a certain time has elapsed is acquired, a difference (azimuth difference) D from the value M is calculated by the orientation difference detection means 232, and the process proceeds to step S105 (D = current vehicle orientation−M). .
In step S105, a difference (error) E between the azimuth difference D and the gyro azimuth G is obtained by the error detection means 233, and the process proceeds to step S106 (E = DG).
An offset correction value is calculated from the error E by the offset calculation means 234, and the process proceeds to step S107. The offset calculating means 234 divides the value of E by the elapsed time T in step S2 to convert it into angular velocity data to obtain an offset correction value O (O = E / T). Here, the elapsed time T in step S2 is selected so that the error does not increase by the division calculation by the offset calculation means 234.
The offset correction means 235 performs correction by subtracting the offset correction value O from the output of the gyro 204 (corrected gyro output = gyro output before correction−O).
[0074]
<Specific example>
FIG. 12 is an explanatory diagram of the gyro output offset correction method described above. In the figure, processing and input / output devices are indicated by rectangular blocks, and data is indicated by circles. Further, for the sake of explanation, the constituent parts 201 to 235 of the navigation device 200 ′ are substantially the same as those in FIG. 9, and the functions and configurations of the constituent parts having the same symbols are the same as those in FIG. Includes display vehicle orientation selection means 211 ′ and map drawing means 211 ″).
In the offset correction of the gyro output according to the present invention, since the offset value is controlled using the display orientation data of the navigation device 200 ′ as the reference orientation, it is affected by the internal state of the navigation device 200 ′. Therefore, description will be made including the state of the navigation device 200 ′. In the navigation apparatus 200 ′, the data processing method differs depending on whether the internal state of the GPS receiver 201 is being measured or not, and whether map matching is possible.
[0075]
(1) During GPS positioning:
When there is a large error between the coordinates obtained by autonomous navigation and the coordinates obtained by GPS reception, the display vehicle orientation selection means 211 ′ estimates the coordinates from GPS as the current position if the GPS reception state is good. If the GPS reception state is bad, the coordinates of autonomous navigation are set as the current position.
When the GPS coordinates are the current position, the gyro correction is executed as follows.
(A) The data from the direction sensor (geomagnetic sensor 203, gyro 204) and the distance sensor are discarded, and the coordinates and direction data obtained by the dead reckoning means 206 and the map matching means 207 are discarded. Then, coordinates and orientation data obtained by GPS reception are used as current position data, thereby displaying a map.
(B) A difference is obtained by the azimuth difference detection means 232 from the displayed vehicle azimuth data and the data stored in the storage means 231 to obtain an azimuth difference. The storage means 231 stores the display vehicle direction data when the navigation device 200 ′ is activated and the time at that time. Or you may memorize | store the display vehicle azimuth | direction data and time at that time when stopping and carrying out the calibration of the gyro 204 during driving | running | working.
(C) The integration unit 221 starts integration from the time when the data is stored in the storage unit 231, and obtains angle data from the angular velocity data of the gyro 204.
(D) The error detection means 233 obtains a difference from the azimuth data (b) and the angle data (c), and uses this as the angle error.
(E) In the offset calculation means 234, the angle error in (d) is divided by the time data to obtain the offset error. As the time data, an elapsed time obtained by subtracting the time at which the data (b) is stored from the current time is used.
(F) The offset correction means 234 removes the error by subtracting the offset error of (e) from the angular velocity data from the gyro.
[0076]
Next, the correction process will be described with specific numerical values for the case of GPS positioning.
(A ′) When the GPS azimuth data is represented by 360 degrees clockwise with respect to the north, the current azimuth data is assumed to be 48 degrees.
(B ′) Assuming that the azimuth data in the storage means 231 is 48 degrees and the time is 8: 32: 0, the azimuth difference is obtained by the following equation.
(Directional difference) = (Display vehicle direction data) − (Storage data) = 48−48 = 0
(C ′) The angular velocity data of the gyro 204 is assumed to be Esinωt + 0.01. That is, it is assumed that an angular velocity is generated by a sine wave with an amplitude E and an offset is 0.01v.
If this angular velocity data is integrated for 1000 seconds, assuming that ωt = 2nπ (n: integer), sin ωt = 0, and the integral value of sin ωt is also zero, leaving only an offset (0.01 v × 1000 = 10 v).
When this data is converted into an angle with a conversion sensitivity of 20 mv / deg,
10v / (20mv / deg) = 500deg
It becomes. This is angle data.
(D ′) The angle error is 500 degrees from (b ′) and (c ′). That is,
(Angle error) = (angle data) − (azimuth difference) = 500−0 = 500 deg
(E ′) Assuming that the current time is 8:48:40, the time data is 16 minutes 40 seconds = 1000 seconds, and the angle error is divided by this value. That is,

(F ′) Subtracting the offset error from the angular velocity data of the gyro 204,

Thus, the offset of the direct current component can be removed.
[0077]
(2) During map matching:
The display vehicle orientation selection means 211 ′ is used by the map matching means 207 when GPS reception is not possible, or when the GPS reception state is good and the coordinates from the GPS and the coordinates obtained by the map matching means 207 are approximate. The obtained coordinates are estimated as the current position. The map matching means 207 corrects the gyro 204 when it finds the road where the vehicle is located (if no road is found, it corrects the gyro by (3) described later). ).
(A) The angular velocity data from the gyro 204 is offset-corrected. This correction is to add the offset error data to the angular velocity data, and uses an offset error obtained by the following processing along the time series with an initial value of zero.
(B) The angular velocity data with the offset corrected is integrated by the integrating means 221 to become angle data. If the value of the geomagnetic sensor 203 is considered to be correct, the angle data from the geomagnetic sensor 203 is used.
(C) The estimated position is obtained by the dead reckoning means 206 from the distance data from the distance sensor and the angle data of (b). Further, the angle data is directly used as the estimated azimuth.
(D) The map matching unit 206 searches for the road where the vehicle is located from the estimated position and the estimated direction. When a road is found, the vehicle position is determined on the road, and the angle data is discarded with the direction of the road as the vehicle direction.
(E) The own vehicle position obtained in the above (d) is compared with the coordinates from the GPS receiver 201 by the display vehicle orientation selection means 211 ′. When the GPS reception state is good and the two coordinates are far away, the process of (1) described above is performed. The map is displayed by the map drawing means 211 ".
(F) A difference is obtained from the displayed vehicle azimuth data and the data stored in the storage means 231 by the azimuth difference detection means 232 and is defined as an azimuth difference. The storage means 231 stores the display vehicle direction data when the navigation device 200 ′ is activated and the time at that time. Or you may memorize | store the display vehicle direction data and time at that time when stopping and carrying out the calibration of the gyro 204 during driving | running | working.
(G) The integration unit 221 starts integration from the time when the data is stored in the storage unit 231, and obtains angle data from the angular velocity data of the gyro 204.
(H) The error detection means 233 obtains a difference from the azimuth data (f) and the angle data (g), and uses this as an angle error.
(I) The offset calculation means 234 calculates the offset error by dividing the angle error of (h) by the time data. As the time data, an elapsed time obtained by subtracting the time when the data of (f) is stored from the current time is used.
(J) The offset correction means 234 removes the error by subtracting the offset error (i) from the angular velocity data from the gyro.
[0078]
Next, the correction process will be described with specific numerical values for the case of map matching.
(A ′) The angular velocity data of the gyro 204 is assumed to be Esinωt + 0.01.
(B ′) When the result of integrating the angular velocity data for 1 second is converted into an angle with a conversion sensitivity of 20 mv / deg, the rotation amount for 1 second is θdeg,
θ + 0.01v / (20mv / deg) = θ + 0.5deg
In addition, θ = {∫ (Esinωt) dt} /0.02 (where integral で は means an integration from Δt = 0 to 1).
That is, the rotation amount of the gyro 204 and an error of 0.5 degrees due to the offset are added to form angle data.
(C ′) The estimated azimuth data is θ0 + θ + 0.5 degrees with the previous azimuth as θ0.
(D ′) Here, it is assumed that the road direction is (θ0 + θ) degrees.
(E ′) At this time, the displayed vehicle direction is (θ0 + θ) degrees.
(F ′) Assuming that the azimuth data in the storage means 231 is 0 degrees and the time is 8: 32: 0, the azimuth difference is obtained by the following equation.

(G ′) The integrating means 231 gives the same result (θ + 0.5 degrees) as (b ′).
(H ′) The angle error is from (f) and (g),

(I ′) If the generation of the angle error as described above continues for 1000 seconds, the current time is 8:48:40, and the angle error is also 1000 times 500 degrees. An offset error is obtained from this angle error. That is,

(J ′) Subtracting the offset error from the angular velocity data of the gyro 204,

Thus, the offset of the direct current component is removed.
[0079]
(3) During dead reckoning:
The display vehicle direction selection means 211 ′ presents the estimated position by the dead reckoning means 206 when the GPS cannot be received or when the GPS reception state is good and the coordinates from the GPS are close to the coordinates obtained by the dead reckoning means 206. Estimate the location (however, if the road cannot be found by map matching).
(A) Offset correction is performed on angular velocity data from the gyro 204. This correction is to add the offset error data to the angular velocity data, and uses an offset error obtained by the following processing along the time series with an initial value of zero.
(B) The angular velocity data with the offset corrected is integrated by the integrating means 221 to become angle data. If the value of the geomagnetic sensor 203 is considered to be correct, the angle data from the geomagnetic sensor 203 is used.
(C) The estimated position is obtained by the dead reckoning means 206 from the distance data from the distance sensor and the angle data of (b). Further, the angle data is directly used as the estimated azimuth. When the map matching means 207 does not find a road for this position, the estimated position and estimated direction are set as the own vehicle position and vehicle direction.
(D) The own vehicle position obtained in the above (c) is compared with the coordinates from the GPS receiver 201 by the display vehicle direction selection means 211 ′. When the GPS reception state is good and the two coordinates are far away, the above-described process (1) is performed. The map is displayed by the map drawing means 211 "as the direction.
(E) A difference is obtained from the displayed vehicle azimuth data and the data stored in the storage means 231 by the azimuth difference detection means 232 and is defined as an azimuth difference. The storage means 231 stores the display vehicle direction data when the navigation device 200 ′ is activated and the time at that time. Or you may memorize | store the display vehicle direction data and time at that time when stopping and carrying out the calibration of the gyro 204 during driving | running | working.
(F) The integration unit 221 starts integration from the time when the data is stored in the storage unit 231, and obtains angle data from the angular velocity data of the gyro 204.
(G) The error detection means 233 obtains a difference from the azimuth data (e) and the angle data (f), and uses this as an angle error.
(H) The offset calculating means 234 calculates the offset error by dividing the angle error of (g) by the time data. As the time data, an elapsed time obtained by subtracting the time when the data of (e) is stored from the current time is used.
(I) The offset correction means 234 removes the error by subtracting the offset error of (h) from the angular velocity data from the gyro.
[0080]
Next, the correction process will be described with specific numerical values for the case of map matching.
(A ′) The angular velocity data of the gyro 204 is assumed to be Esinωt + 0.01.
(B ′) When the result of integrating the angular velocity data for 1 second is converted into an angle with a conversion sensitivity of 20 mv / deg, the rotation amount for 1 second is θdeg,
θ + 0.01v / (20mv / deg) = θ + 0.5deg
In addition, θ = {∫ (Esinωt) dt} /0.02 (where integral で は means an integration from Δt = 0 to 1).
That is, the rotation amount of the gyro 204 and an error of 0.5 degrees due to the offset are added to form angle data.
(C ′) The estimated azimuth data is θ0 + θ + 0.5 degrees with the previous azimuth as θ0.
(D ′) The displayed vehicle orientation is θ0 + θ + 0.5 degrees.
(E ′) The azimuth difference is obtained by the following equation.

(F ′) The integration means 231 gives the same result (θ + 0.5 deg) as the above (b ′).
(G ′) The angle error is zero from (e) and (f). That is,

[0081]
As described above, when only estimated data by dead reckoning can be used, the angle error becomes zero, and the offset error becomes zero, which cannot be corrected. This corresponds to traveling in a place without map data, and occurs when there is no data serving as a reference for correction.
Even in this case, if there is angle data from the geomagnetic sensor 203, the angle data becomes θ degrees in (b ′), and the angle error becomes 0.5 degrees in (g ′), thereby enabling offset correction.
Further, when the angular velocity data including the offset is stored in the integrating means 231 in the state (1) or (2) described above for a predetermined time or longer, the vehicle temporarily travels in a place without map data. Even when the change in the azimuth difference and the change in the integration unit 231 coincide, an angle error is generated from the error detection unit 233, so that the offset can be corrected.
[0082]
The process in this case will be described based on an example in which each speed data of (a) has passed 1000 seconds.
(E ′) If the rotation amount of the gyro 204 is 0 degree from the initial state,

(F ′) Even the integration means 231 leaves only the offset. That is,
0.01 v × 1000 / (0.02 v / deg) = 500 deg
(G ′) The angle error is from (e ″) and (f),

(H ′) If the elapsed time is 1000 seconds,

(I ′) In the offset correction, the offset is removed as follows.

[0083]
Here, entering a place without map data is as follows.
(E ″) The displayed vehicle heading data is Esinωt + 0.01, and considering only the offset,
(Azimuth difference) = 48 + (0.01 / 0.02) −48 = 0.5 deg
(F ″) The value of the integration means 231 is
500+ (0.01 / 0.02) = 500.5 deg
(G ") Angular error is
(Angle error) = 500.5−0.5 = 500 deg
(H ″) Offset error is
(Offset error) = (500/1001) × 0.02≈0.00999v
(I ") Offset correction is

Thus, the correction value gradually decreases, but correction is performed with the integrated data during that period.
[0084]
【The invention's effect】
As described above, according to the present invention, drift can be corrected regardless of whether the vehicle is in a stopped state or a moving state, so that the direction detection on a highway or a road without a traffic light becomes accurate. As a result, the display performance of the navigation device is improved.
Even when there is no straight road, correction data itself is not necessary because data from which drift has been removed is input to the navigation device.
Furthermore, the correction value becomes stable when the vehicle and the gyro sensor have gone out of temperature in advance in a place with map data. In this case, since the correction value does not change greatly even if the vehicle moves to a place without map data, drift can be corrected simply by holding the correction value, and the current position display performance of the navigation device is improved.
Furthermore, in the twelfth to thirteenth inventions, temporary fluctuations can be suppressed by providing a limiter in the drift correction circuit.
In the fourteenth to seventeenth inventions, correction is possible regardless of whether the vehicle is in a map matching state. Further, in a place without a map, correction can be made by information from a geomagnetic sensor and a GPS receiver. Furthermore, since a continuously changing display vehicle direction that is not a link direction is used, a temporary error does not occur.
In the seventeenth invention, the display device orientation obtained by the navigation device selecting the information determined to be most accurate at that time from a large amount of information is used. High gyro drift correction is realized.
[Brief description of the drawings]
FIG. 1 is a flowchart showing an embodiment of a gyro drift correction method according to the present invention.
FIG. 2 is a block diagram showing a configuration of an embodiment of a gyro drift correction circuit according to the present invention.
FIG. 3 is a block diagram showing the configuration of another embodiment of the gyro drift correction circuit according to the present invention.
FIG. 4 is a block diagram showing the configuration of another embodiment of the gyro drift correction circuit according to the present invention.
FIG. 5 is a block diagram showing a configuration of another embodiment of the gyro drift correction circuit according to the present invention.
FIG. 6 is a block diagram showing the configuration of another embodiment of the gyro drift correction circuit according to the present invention.
FIG. 7 is a block diagram illustrating a configuration example of an N-point sum calculation circuit and an integration circuit.
FIG. 8 is a block diagram illustrating a configuration example of a conventional navigation device.
FIG. 9 is a block diagram showing a configuration of an embodiment of the navigation apparatus of the present invention.
FIG. 10 is a display example of a map.
FIG. 11 is a flowchart illustrating an example of a gyro correction method.
FIG. 12 is an explanatory diagram of a gyro output offset correction method;
[Explanation of symbols]
3,204 Gyro sensor
6 A / D conversion circuit (conversion means)
7,208 Map matching means
8,207 Dead reckoning means
20, 30, 40, 50, 60 Drift correction circuit
21 FIR filter (first filter means)
22, 23 Subtractor (subtraction means)
25 Differentiation circuit (second filter means)
26 FIR filter (second filter means)
31 Delay circuit (delay means)
41, 51 N-point sum calculation circuit
42,48 subtractor (subtraction means)
43 Comparator (Correction value response delay means)
44 Integration circuit (correction value response delay means)
47 Dividing circuit (correction value response delay means)
61 N-point sum calculation circuit (correction value response delay means)
62 Constant division circuit (correction value response delay means)
100, 200, 200 'navigation device
201 GPS receiver
203 Distance sensor
210 Map data
211 Display data creation means
211 'display vehicle direction selection means
212 Display device
221, 222 Integration means
231 memory means
232 Direction difference detection means
233 Error detection means
234 Offset calculation means
235 Offset correction means

Claims (18)

In a navigation device for a mobile object including a gyro sensor for detecting an angle change and a map matching means,
(1) Obtain digital data at regular intervals from the output signal from the gyro sensor, determine a first moving average value within a predetermined time from the digital data,
(2) The vehicle orientation data obtained by the map matching process is taken out at regular time intervals to obtain a difference value, and a second moving average value within a predetermined time is obtained based on the difference value.
(3) The difference between the first moving average value and the second moving average value is obtained as a drift correction value,
(4) calculating a difference between the digital data and the drift correction value to obtain corrected gyro data;
A gyro drift correction method characterized by the above. 2. The gyro drift correction method according to claim 1, further comprising storing the drift correction value obtained in step (3), and extracting the stored drift correction value when calculating the corrected gyro data in step (4). A gyro drift correction method characterized by being used. 3. The gyro drift correction method according to claim 1 or 2, wherein the drift correction value is updated when the mobile body moves to a location other than the range specified in the map data stored in the storage unit of the navigation device of the mobile body. A gyro drift correction method characterized by stopping and continuing drift correction. In a navigation device for a mobile object including a gyro sensor for detecting an angle change and a map matching means,
A first computing means for inputting the output data from the gyro sensor and obtaining a first moving average value for every predetermined time;
A second computing means for inputting a difference value of the azimuth data of the moving body from the map matching means and obtaining a second moving average value for every predetermined time;
First subtracting means for subtracting the second moving average amount from the first moving average amount to obtain a drift correction amount;
Second subtracting means for subtracting a drift correction amount from output data from the gyro sensor to obtain corrected gyro data;
A gyro drift correction method characterized by comprising: 5. The gyro drift correction method according to claim 4, wherein delay means for delaying output data from the gyro sensor for a predetermined time is provided on the input side of the first calculation means and the second calculation means. . In a navigation device for a mobile object including a gyro sensor for detecting an angle change and a map matching means,
First sum calculation means for calculating first sum data for the past N times from the present output data from the gyro sensor;
Second sum calculation means for calculating second sum data for the past N times of the difference value of the orientation data of the moving body from the map matching means;
First subtraction means for subtracting the second sum data from the first sum data to obtain a difference output;
A comparison means for comparing the difference output with input data;
An arithmetic means for integrating the output of the comparison means and returning the integrated output to the comparison means as the input data and dividing the integral output by time to obtain a drift correction value;
And a second subtracting means for obtaining corrected gyro data by subtracting a drift correction value from output data from the gyro sensor. In a navigation device for a mobile object including a gyro sensor for detecting an angle change and a map matching means,
First sum calculation means for calculating first sum data for the past N times from the present output data from the gyro sensor;
Second sum calculation means for calculating second sum data for the past N times of the difference value of the orientation data of the moving body from the map matching means;
First subtraction means for subtracting the second sum data from the first sum data to obtain a difference output;
A comparison means for comparing the difference output with input data;
Computing means for integrating the output of the comparing means and returning the integrated output as the input data to the comparing means and inputting the integrated output to the FIR filter to obtain a drift correction value;
And a second subtracting means for obtaining corrected gyro data by subtracting a drift correction value from output data from the gyro sensor. In a navigation device for a mobile object including a gyro sensor for detecting an angle change and a map matching means,
First sum calculation means for calculating first sum data for the past N times from the present output data from the gyro sensor;
Second sum calculation means for calculating second sum data for the past N times of the difference value of the orientation data of the moving body from the map matching means;
First subtraction means for subtracting the second sum data from the first sum data to obtain a difference output;
A comparison means for comparing the difference output with input data;
The third sum data for the past N times of the output data of the comparison means is calculated, the third sum data is returned to the comparison means as the input data, and the third sum data is divided by a predetermined constant. Calculating means for obtaining a drift correction value,
And a second subtracting means for obtaining corrected gyro data by subtracting a drift correction value from output data from the gyro sensor. 5. The gyro drift correction circuit according to claim 4, wherein the first and second arithmetic means are FIR filters. A gyro sensor that detects an angle change, a storage unit that stores map information, a conversion unit that obtains voltage change data from the output of the gyro sensor, and a coordinate change amount obtained from the output of the conversion unit and the movement distance obtained from the distance sensor. A dead reckoning means for obtaining the coordinate data by adding the current change amount to the previous coordinates, and a map matching means for obtaining the current position of the moving body based on the coordinate data from the dead reckoning means and the map information; In a mobile navigation device including:
First filter means for inputting voltage change data from the conversion means to obtain a first moving average amount within a predetermined time;
Second filter means for obtaining a second moving average amount within a predetermined time based on the difference value of the azimuth data of the moving body from the map matching means;
First subtraction means for obtaining a difference between the first moving average amount and the second moving average amount;
A second subtracting unit that obtains a difference between the voltage change data and the output of the first subtracting unit and gives the difference to the dead reckoning unit;
A gyro drift correction circuit comprising: 11. The drift correction circuit according to claim 10, further comprising clock pulse generating means for providing time information to the first and second filter means, and map data in which the moving body is stored in a storage unit of the navigation device of the moving body. And a switch means for cutting off time information from the clock pulse generation means so that the drift correction value is not updated when the position is moved to a place other than the range specified in (1). 12. The drift correction circuit according to claim 10 or 11, further comprising delay means for performing a delay corresponding to the difference value output time in the map matching means between the conversion means and the first filter means. Gyro drift correction circuit. A gyro sensor that detects an angle change, a storage unit that stores map information, a conversion unit that obtains voltage change data from the output of the gyro sensor, and an angle change amount from an output of the conversion unit and a moving distance amount obtained from the distance sensor. First N-point sum calculating means for calculating the sum of the coordinates by adding the current change amount to the previous coordinates,
Differentiating means for obtaining azimuth data of the moving body from the map matching means and obtaining a difference value of the azimuth data;
Second N-point sum calculating means for inputting the output of the differentiating means and calculating the sum of the past N data from the present time;
First subtracting means for outputting a difference between outputs of the first and second N-point sum calculating means;
Correction value response delay means for obtaining a drift correction value by delaying the response of the drift correction value based on the output of the first subtracting means by the difference value output time in the map matching means; and the drift correction value from the voltage change data And a second subtracting means for subtracting the gyro drift correction circuit. A gyro drift correction method comprising the following steps (1) to (5) in a mobile navigation device including a gyro sensor for detecting an angle change, a dead reckoning unit, and a map matching unit. .
(1) Obtain the displayed vehicle direction and store it with the time,
(2) The output from the gyro sensor is integrated to obtain a gyro direction. If a certain time has elapsed, step (3) is executed, and if not, the above integration is repeated.
(3) The display vehicle orientation at the time when the predetermined time has elapsed is acquired, and the orientation difference is obtained.
(4) Obtain the difference between the azimuth difference and the gyro azimuth, and obtain the offset correction value by dividing the difference by the predetermined time.
(5) The gyro output is corrected by subtracting the offset correction value from the gyro output. When the coordinates obtained by the map matching means are estimated as the vehicle position and the vehicle position is determined on the road by the estimated value, the direction of the road is set as the display vehicle direction,
If the vehicle position cannot be determined on the road by the estimated value obtained by the map matching means, the difference value of the vehicle data is input with the estimated position obtained by the dead reckoning process as the current position, and the second movement at regular intervals A second computing means for obtaining an average value;
First subtracting means for subtracting the second moving average amount from the first moving average amount to obtain a drift correction amount;
Subtracting the drift correction amount from the output data from the gyro sensor and correcting the gyro data as the display vehicle direction,
The gyro drift correction method according to claim 14. When the difference between the coordinates obtained by autonomous navigation and the coordinates obtained from GPS is large, the direction obtained from GPS is set as the display vehicle direction,
When GPS cannot be received or when the difference between the coordinates obtained from the GPS and the coordinates obtained by the map matching means is small, the coordinates obtained by the map matching means are estimated as the vehicle position, and the estimated values are automatically displayed on the road. When the vehicle position is determined, the direction of the road is the display vehicle direction,
When the vehicle position cannot be determined on the road by the estimated value obtained by the map matching means, the vehicle direction is set as the display vehicle direction with the estimated position obtained by the dead reckoning process as the current position.
The gyro drift correction method according to claim 14. In a mobile navigation device including a GPS receiver, a gyro sensor for detecting a change in angle, a dead reckoning means, and a map matching means,
When the difference between the coordinates obtained by autonomous navigation and the coordinates obtained from GPS is large, the direction obtained from GPS is used as the display vehicle direction, and when GPS cannot be received, or the coordinates obtained from GPS and the coordinates obtained by the map matching means When the difference between the two is small, the coordinates obtained by the map matching means are estimated as the vehicle position, and when the vehicle position is determined on the road by the estimated value, the direction of the road is set as the display vehicle direction, and the map matching means Display vehicle direction selection means that uses the estimated position obtained by the dead reckoning process as the current position and the vehicle direction as the display vehicle direction when the vehicle position cannot be determined on the road by the estimated value obtained in
Storage means for obtaining the display vehicle orientation from the display vehicle orientation selection means and storing it with the time;
Integration means for integrating the output from the gyro sensor for a certain period of time to obtain gyro bearing data;
An azimuth difference detection means for obtaining a azimuth difference with the stored display vehicle azimuth data by acquiring the display vehicle azimuth at that time after the lapse of the predetermined time;
An offset correction value calculation means for obtaining an offset correction value by obtaining a difference between the azimuth difference and the gyro azimuth and dividing the difference by the predetermined time;
Offset correction means for correcting the gyro output by subtracting the offset correction value from the gyro output;
And a gyro drift correction circuit. Further, when the vehicle position cannot be determined on the road by the estimated value obtained by the map matching means, the angle data value from the geomagnetic sensor is input to the dead reckoning means instead of the corrected gyro output. A gyro drift correction circuit characterized by being used as:

Images