JP3791074B2 - Navigation device - Google Patents

Description

【０００６】
【数２】

【０００７】
ここで、車両１２がピッチ方向に傾いている場合、加速度センサ１の出力データには重力加速度のピッチ成分が含まれるため、加速度をそのまま用いて計算しては移動距離算出に誤差が生じる。そこで、次に示すような重力加速度成分除去のための補正を速度補正手段６で行う。
【０００８】
速度補正手段６では、ジャイロセンサ２からの出力データ（ピッチ角速度データ）に対して時間軸で積分演算を行う。つまり、（数３）の演算を行うことにより、車両１２のピッチ角（積算ピッチ角）αｎを得る。ただし、ジャイロセンサ２からデータの得られる時間周期をΔｔ、時刻ｎにおける車両１２のピッチ角速度をωｎ、時刻ｎで時間周期Δｔにおける車両１２のピッチ角をαｎ、初期ピッチ角をα０とする。
【０００９】
【数３】

【００１０】
この（数３）により求められたピッチ角αｎを用いて（数４）により加速度センサ１出力データの補正を行う。ただし、補正された時刻ｎにおける車両１２の加速度Ａｎ、加速度センサ１から出力された車両１２の加速度をａｎ、重力加速度をｇとする。
【００１１】
【数４】

【００１２】
（数４）のＡｎを（数１）、（数２）のａｎに代入して、時刻ｎにおける車両１２の速度Ｖｎと時間周期Δｔにおける車両１２の移動距離ΔＤｎとを得る。これを（数５）、（数６）として示す。
【００１３】
【数５】

【００１４】
【数６】

【００１５】
次に、移動方位算出手段７は、所定の時間周期（所定微小時間）Δｔ毎にジャイロセンサ３から出力されるデータ（ヨー角速度データ）に対して時間軸で積分演算を行う。つまり、（数７）の演算を行うことにより、車両１２の方位角（積算ヨー角）θｎを得る。ただし、ジャイロセンサ３からデータの得られる時間周期をΔｔ、時刻ｎにおける車両１２の方位角速度（ヨー角速度）をωｎ、時刻ｎで時間周期Δｔにおける車両１２の移動方位角をθｎ、初期方位角をθ０とする。
【００１６】
【数７】

【００１７】
図５はナビゲーション装置における位置演算を説明するための位置演算の概念図である。相対位置演算手段８は、前述したように車両１２の移動距離ΔＤｎおよび移動方位角θｎを求めると、図５に示す考え方（移動距離に移動方位角を乗算して所定時間ΔｔにおけるＸ、Ｙ方向の移動距離を求めるという考え方）を用いて、（数８）、（数９）に基づき、車両１２の初期位置から累積演算して車両１２の位置を求める。ただし、時刻ｎにおける車両１２の位置座標を（ｘｎ、ｙｎ）、初期位置を（ｘ０、ｙ０）とする。なお、移動距離算出において、水平面となすピッチ角の影響については考慮されていないが、これは簡単に演算可能であると共にわずかなピッチ角では誤差はあまり生じないので、説明は省略する。
【００１８】
【数８】

【００１９】
【数９】

【００２０】
相対位置演算手段８からは車両１２の位置データが表示制御手段１１に出力される。表示制御手段１１は、車両１２の位置データを受け取ると、地図記憶部１０から、所定の領域のデジタル地図データを読み出し、表示部９に出力する。
【００２１】
このように、従来のナビゲーション装置では、加速度センサ１による速度算出を精度良く行うために、車両１２のピッチ角を検出するためのジャイロセンサ２からの出力データ（ピッチ角速度データ）により、加速度センサ１からの出力データ（加速度データ）に対して補正を行う。
【００２２】
【発明が解決しようとする課題】
しかしながら、上記従来のナビゲーション装置では、加速度センサ１からの加速度に対して補正を行うためのピッチ角速度を検出するジャイロセンサ２からの出力にドリフト成分があると、ジャイロセンサ２により車両のピッチ角速度を検出しようとする場合、速度補正手段６はジャイロセンサ２の出力データを積算していくため、ジャイロセンサ２の出力ドリフトまで積算されることになり、積算ピッチ角の累積誤差が増大し、速度精度（従って移動距離算出精度）や移動方位角精度が非常に悪くなるという問題点を有していた。
【００２３】
このナビゲーション装置では、積算ピッチ角の累積誤差の増大を防止することができることが要求されている。
【００２４】
本発明は、積算ピッチ角の累積誤差の増大を防止することができるナビゲーション装置を提供することを目的とする。
【００２５】
【課題を解決するための手段】
この課題を解決するために本発明によるナビゲーション装置は、移動体の加速度を検出する加速度センサと、加速度センサ出力の加速度の補正を行う速度補正手段を有し、前記補正された加速度から移動体の移動速度を求め、移動速度を積算して移動体の移動距離を算出する移動距離算出手段と、移動体のピッチ角速度を検出するピッチ角速度検出部とを有するナビゲーション装置であって、検出されたピッチ角速度を所定微小時間だけ積算して得られた所定微小時間積算ピッチ角と基準値とを比較して所定微小時間積算ピッチ角を補正するピッチ角判定手段を備えるように構成した。
【００２６】
これにより、積算ピッチ角の累積誤差の増大を防止することができるナビゲーション装置が得られる。
【００２７】
【発明の実施の形態】
本発明の請求項１に記載の発明は、移動体の加速度を検出する加速度センサと、加速度センサ出力の加速度の補正を行う速度補正手段を有し、前記補正された加速度から移動体の移動速度を求め、移動速度を積算して移動体の移動距離を算出する移動距離算出手段と、移動体のピッチ角速度を検出するピッチ角速度検出部とを有するナビゲーション装置であって、検出されたピッチ角速度を所定微小時間だけ積算して得られた所定微小時間積算ピッチ角と基準値とを比較して所定微小時間積算ピッチ角を補正するピッチ角判定手段を備えることとしたものであり、所定微小時間積算ピッチ角が基準値を越えているか否かに応じて補正されるという作用を有する。
【００２８】
請求項２に記載の発明は、請求項１に記載の発明において、ピッチ角判定手段は、所定微小時間積算ピッチ角が基準値を越えたとき、所定微小時間積算ピッチ角を基準値に補正することとしたものであり、所定微小時間積算ピッチ角が基準値を越えたときには基準値に等しい角度に補正されるという作用を有する。
【００２９】
請求項３に記載の発明は、請求項１に記載の発明において、ピッチ角判定手段は、所定微小時間積算ピッチ角が基準値を越えたとき、所定微小時間積算ピッチ角を測定時間当りの平均値に補正することとしたものであり、所定微小時間積算ピッチ角が基準値を越えたときには測定時間当りの平均値たとえば走行時間における平均値に等しい角度に補正されるという作用を有する。
【００３０】
請求項４に記載の発明は、請求項１に記載の発明において、ピッチ角判定手段は、所定微小時間積算ピッチ角が基準値を越えたとき、所定微小時間積算ピッチ角を加速度センサ出力の加速度から推測される値に補正することとしたものであり、所定微小時間積算ピッチ角が基準値を越えたときには加速度センサ出力の加速度から推測される値に等しい角度に補正されるという作用を有する。
【００３１】
請求項５に記載の発明は、請求項１乃至４のいずれか１に記載の発明において、ピッチ角判定手段は、ＣＤ−ＲＯＭに記憶された地図の高度変化情報に基づいて基準値を設定することとしたものであり、基準値が適正な値に設定されるという作用を有する。
【００３２】
以下、本発明の実施の形態について、図１、図２を用いて説明する。
（実施の形態１）
図１は本発明の実施の形態１によるナビゲーション装置を示すブロック図である。図１において、加速度センサ１、ジャイロセンサ２、３、移動距離算出手段５、速度補正手段６、移動方位算出手段７、相対位置演算手段８、表示部９、地図記憶部１０、表示制御手段１１は図３と同様のものなので、同一符号を付し、説明は省略する。Ｂは演算、制御の処理を行う中央制御部、４はジャイロセンサ２の出力データ（ピッチ角速度データ）を所定微小時間だけ積算して得られた所定微小時間積算ピッチ角と基準値（閾値）とを比較してジャイロセンサ２の出力データを補正するピッチ角判定手段である。
【００３３】
以上のように構成されたナビゲーション装置について、その動作を説明する。まず、移動距離算出手段５は、加速度センサ１の出力データ（加速度データ）から車両１２の速度を求め、この車両速度を積分することにより車両１２の移動距離を算出し、移動方位算出手段７は、ジャイロセンサ３の出力データ（方位角速度データ）を入力し、この方位角速度を積分することにより車両１２の方位角を算出する。さらに、相対位置演算手段８は、移動距離算出手段５と移動方位算出手段７から出力される車両移動距離と車両方位角から車両１２の位置データを算出し、表示制御手段１１に出力する。このとき、速度補正手段６は、ジャイロセンサ２の出力データから算出した車両１２のピッチ角を用いて、加速度センサ１の出力データを補正する。これらの動作は従来と同様である。
【００３４】
ジャイロセンサ２の出力には通常ドリフト成分が含まれているが、従来は、速度補正手段６でそのドリフト成分も積算され、累積された誤差成分を含んだ車両ピッチ角により加速度センサ１の出力データが補正されていた。このため、移動距離算出手段５により算出した車両１２の移動距離は実際の車両１２の移動距離とは異なって誤差を含むことになり、また、その誤差が累積により増大してしまうという問題点を有していた。このような誤差を除去するためにピッチ角判定手段４を設けた。
【００３５】
次に、ピッチ角判定手段４におけるドリフト成分除去のための補正動作について説明する。時間周期（所定微小時間）Δｔにジャイロセンサ２から得られるピッチ角速度の積分値（所定微小時間積算ピッチ角）をＩｎ、基準値（閾値）をＩｘとする。この閾値Ｉｘは、補正動作が最適動作となるように、実験等により求めた起こり得る最大ピッチ角速度を基準に設定した基準値である。ピッチ角判定手段４は、Ｉｎ≦Ｉｘの場合は、車両１２に通常有り得るピッチ角が発生しているとみなし、ジャイロセンサ２の出力データの積算値（所定微小時間積算ピッチ角）を補正せずに速度補正手段６へ出力する。また、ピッチ角判定手段４は、Ｉｎ＞Ｉｘの場合は、ジャイロセンサ２の出力にドリフト成分を含んでいるため、通常有り得ないピッチ角速度をジャイロセンサ２が出力しているとみなし、次にパターン１〜パターン３として示すような補正値を速度補正手段６へ出力する。どの補正値を選択するかについては、過去の実験データ等からピッチ角の増減がどのような傾向にあるかを判別して決定される。
【００３６】
まずパターン１について説明する。この場合、ピッチ角判定手段４は、ジャイロセンサ２から得られる出力データの積算値（所定微小時間積算ピッチ角）を閾値Ｉｘに補正して（Ｉｎ＝Ｉｘとして）速度補正手段６へ出力する。速度補正手段６は、加速度センサ１出力の加速度をＩｎ＝Ｉｘに基づいて（数４）を用いて補正する。
【００３７】
次にパターン２について説明する。この場合、ピッチ角判定手段４は、ジャイロセンサ２から得られるピッチ角速度の所定微小時間における積算値である所定微小時間積算ピッチ角を測定時間当りの平均値（例えば走行時間における平均値）に補正して速度補正手段６へ出力する。
【００３８】
最後にパターン３について説明する。この場合、ピッチ角判定手段４は、ジャイロセンサ２から得られるピッチ角速度の所定微小時間における積算値である所定微小時間積算ピッチ角を加速度センサ１から推測される値に補正して速度補正手段６へ出力する。例えば加速度センサ１の出力加速度が一定でＩｎ＞Ｉｘの時は、実際は加速度が減少しながらジャイロセンサ２のドリフトが増加していることになるので、所定微小時間積算ピッチ角を減少させていくように補正する。また、加速度センサ１の出力加速度が増加してＩｎ＞Ｉｘの時は、実際は加速度が増加しながらジャイロセンサ２にドリフトが生じていることになるので、所定微小時間積算ピッチ角を閾値Ｉｘで一定になるように補正する。このとき閾値ＩｘとしてＣＤ−ＲＯＭ等の記憶手段に記憶された地図の高度変化情報を用いてもよい。
【００３９】
図２はピッチ角判定手段４の動作を示すフローチャートである。まず、ジャイロセンサ２から出力される所定微小時間のピッチ角速度を積算し、所定微小時間積算ピッチ角Ｉｎを得る（Ｓ１）。次に、所定微小時間積算ピッチ角Ｉｎを閾値Ｉｘとを比較し（Ｓ２）、Ｉｎ≦Ｉｘか否かを判定する（Ｓ３）。Ｉｎ≦Ｉｘの場合には移動距離算出手段５は所定微小時間積算ピッチ角Ｉｎに対して補正をかけることなく（Ｓ４）、速度補正および移動距離算出を行う（Ｓ５）。Ｉｎ≦Ｉｘでない場合には移動距離算出手段５は所定微小時間積算ピッチ角Ｉｎに対して上記パターン１〜３のいずれかのパターンにより補正を行う（Ｓ６）。これを走行時間中、繰り返す（Ｓ２〜Ｓ６）。
【００４０】
このように、ジャイロセンサ２の出力がドリフト成分を含んでいると判定されたときには（Ｉｎ＞Ｉｘのときには）、ピッチ角判定手段４は、パターン１〜３のいずれかにより所定微小時間積算ピッチ角の補正を行うようにしたので、ジャイロセンサ２にドリフトが発生している場合でもドリフト成分がピッチ角として積算されることがなく、したがって、ドリフト成分に基づく積算ピッチ角の誤差累積を防止することができ、移動距離算出における誤差増大を防止することができる。
【００４１】
ジャイロセンサ３についてもジャイロセンサ２と同様のことが言える。この場合は図１のピッチ角判定手段４に相当するものとしてヨー角判定手段（図示せず）をジャイロセンサ３と移動方位算出手段７との間に配置する。ヨー角判定手段は、所定微小時間積算ヨー角が閾値以下か否かを比較し、閾値以下の場合は、車両１２に通常有り得るピッチ角が発生しているとみなし、ジャイロセンサ３の出力データの積算値（所定微小時間積算ヨー角）を補正せずに移動方位算出手段７へ出力する。また、ヨー角判定手段は、所定微小時間積算ヨー角が閾値以下か否かを比較し、閾値を越えている場合は、ジャイロセンサ３の出力にドリフト成分を含んでいるため、通常有り得ないヨー角速度をジャイロセンサ３が出力しているとみなし、所定パターンの補正値を移動方位算出手段７へ出力する。
【００４２】
このように、ジャイロセンサ３の出力がドリフト成分を含んでいると判定されたときには、ヨー角判定手段は、所定パターンにより所定微小時間積算ヨー角の補正を行うようにしたので、ジャイロセンサ３にドリフトが発生している場合でもドリフト成分がヨー角として積算されることがなく、従って、ドリフト成分に基づく積算ヨー角の誤差累積を防止することができ、移動距離算出における誤差増大を防止することができる。
【００４３】
なお、本実施の形態では、中央制御部Ｂの各手段４〜８、１１はハードウェア構成として説明したが、本発明はこれに限らず、各手段４〜８、１１をソフトウェアで構成すること、すなわちＣＰＵがプログラムを実行することにより各手段４〜８、１１を実現するようにしてもよい。
【００４４】
以上のように本実施の形態によれば、ジャイロセンサ２、３の出力がドリフト成分を含んでいると判定されたときには、ピッチ角判定手段４、ヨー角判定手段は、パターン１〜３のいずれかにより又は所定パターンにより所定微小時間積算ピッチ角、所定微小時間積算ヨー角の補正を行うようにしたので、ジャイロセンサ２、３にドリフトが発生している場合でもドリフト成分がピッチ角、ヨー角として積算されることがなく、従って、ドリフト成分に基づく積算ピッチ角、積算ヨー角の誤差累積を防止することができ、移動距離算出における誤差増大を防止することができる。
【００４５】
【発明の効果】
以上のように本発明のナビゲーション装置によれば、所定微小時間積算ピッチ角が基準値を越えているか否かに応じて所定微小時間積算ピッチ角を補正することができるので、積算ピッチ角の累積誤差が増大しないように補正することができるという有利な効果が得られる。
【００４６】
また、ピッチ角判定手段は、所定微小時間積算ピッチ角が基準値を越えたとき、所定微小時間積算ピッチ角を基準値に補正することにより、所定微小時間積算ピッチ角が基準値を越えたとき発生していると推定されるドリフト分を上記基準値により減少させることができるので、積算ピッチ角の累積誤差の増大を防止することができるという有利な効果が得られる。
【００４７】
さらに、ピッチ角判定手段は、所定微小時間積算ピッチ角が基準値を越えたとき、所定微小時間積算ピッチ角を測定時間当りの平均値に補正することにより、所定微小時間積算ピッチ角が基準値を越えたとき発生していると推定されるドリフト分を上記平均値により減少させることができるので、積算ピッチ角の累積誤差の増大を防止することができるという有利な効果が得られる。
【００４８】
さらに、ピッチ角判定手段は、所定微小時間積算ピッチ角が基準値を越えたとき、所定微小時間積算ピッチ角を加速度センサ出力の加速度から推測される値に補正することにより、所定微小時間積算ピッチ角が基準値を越えたとき発生していると推定されるドリフト分を加速度に基づいて減少させることができるので、積算ピッチ角の累積誤差の増大を防止することができるという有利な効果が得られる。
【００４９】
さらに、ピッチ角判定手段は、ＣＤ−ＲＯＭに記憶された地図の高度変化情報に基づいて基準値を設定することにより、基準値を適正な値に設定することができるという有利な効果が得られる。
【図面の簡単な説明】
【図１】本発明の実施の形態１によるナビゲーション装置を示すブロック図
【図２】図１のナビゲーション装置の動作を示すフローチャート
【図３】従来のナビゲーション装置を示すブロック図
【図４】ナビゲーション装置の車両搭載状態を示す車載概念図
【図５】位置演算の概念図
【符号の説明】
１ 加速度センサ
２、３ ジャイロセンサ
４ ピッチ角判定手段
５ 移動距離算出手段
６ 速度補正手段
７ 移動方位算出手段
８ 相対位置演算手段
９ 表示部
１０ 地図記憶部[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a navigation device that calculates the position of a moving body, such as a vehicle, using an acceleration sensor.
[0002]
[Prior art]
FIG. 3 is a block diagram showing a conventional navigation apparatus. In FIG. 3, A is a central control unit, 1 is an acceleration sensor that detects acceleration in the traveling direction of the vehicle without contact, and 2 is a gyro sensor (pitch angular velocity detection) that detects an angular velocity (pitch angular velocity) in the pitch operation direction of the vehicle. Part), 3 is a gyro sensor for detecting an angular velocity (yaw angular velocity) in the yaw motion direction of the vehicle, 5 is a moving distance calculating means for calculating the moving distance of the vehicle from the acceleration of the acceleration sensor 1 output, and 6 is an output of the gyro sensor 2 A speed correction means for calculating the pitch angle from the pitch angular speed and correcting the speed of the pitch component with respect to the acceleration of the acceleration sensor 1 output, 7 is the vehicle azimuth angle (yaw angle) from the yaw angular speed detected by the gyro sensor 3. A moving azimuth calculating means 8 for calculating, a moving distance and a moving azimuth angle of the vehicle outputted from the moving distance calculating means 5 and the moving azimuth calculating means 7 Relative position calculation means for calculating the current vehicle position by adding to the previous vehicle position, 9 a display unit for displaying vehicle position information, map data, and the like, 10 a map storage unit for storing digital map data , 11 reads out map data of a predetermined area corresponding to the position of the vehicle from the map storage unit 10 based on the position information of the vehicle output from the relative position calculation means 8, and displays the position information of the vehicle and the map data. The display control means outputs to the unit 9, and the means 5 to 8 and 11 constitute the central control unit A.
[0003]
FIG. 4 is an in-vehicle conceptual diagram showing a vehicle-mounted state of the navigation device. In FIG. 4, the acceleration sensor 1 and the gyro sensors 2 and 3 are the same as those in FIG. Reference numeral 12 denotes a vehicle, and reference numeral 13 denotes an apparatus main body including a central control unit A, a display unit 9 and a map storage unit 10. As shown in FIG. 4, the acceleration sensor 1 is arranged on the vehicle 12 so that the acceleration in the traveling direction of the vehicle 12 can be detected, and the gyro sensor 2 can detect the pitch angular velocity of the vehicle 12. The gyro sensor 3 is arranged so that the yaw angular velocity of the vehicle 12 can be detected.
[0004]
The operation of the navigation device configured and arranged as described above will be described. As shown in FIG. 3, the position of the vehicle 12 is calculated by the relative position calculation means 8 from the output data of the acceleration sensor 1 and the gyro sensors 2 and 3. First, the movement distance calculation means 5 performs an integration operation twice on the time axis with respect to data (acceleration data) output from the acceleration sensor 1 every predetermined period (predetermined minute time) Δt. That is, by calculating (Equation 1) and (Equation 2), the speed vn and the movement distance Δdn of the vehicle 12 are obtained. However, the time period for obtaining data from the acceleration sensor 1 is Δt, the acceleration of the vehicle 12 at time n is an, the speed of the vehicle 12 at time n is vn, the moving distance of the vehicle 12 at the time period Δt is Δdn, The initial speed is V0.
[0005]
[Expression 1]

[0006]
[Expression 2]

[0007]
Here, when the vehicle 12 is tilted in the pitch direction, the output data of the acceleration sensor 1 includes the pitch component of gravity acceleration. Therefore, if the calculation is performed using the acceleration as it is, an error occurs in the calculation of the moving distance. Therefore, correction for removing the gravitational acceleration component as described below is performed by the speed correction means 6.
[0008]
The speed correction means 6 performs an integration operation on the time axis for the output data (pitch angular speed data) from the gyro sensor 2. That is, by calculating (Equation 3), the pitch angle (integrated pitch angle) αn of the vehicle 12 is obtained. However, the time period for obtaining data from the gyro sensor 2 is Δt, the pitch angular velocity of the vehicle 12 at time n is ωn, the pitch angle of the vehicle 12 at the time period Δt at time n is αn, and the initial pitch angle is α0.
[0009]
[Equation 3]

[0010]
The acceleration sensor 1 output data is corrected by (Equation 4) using the pitch angle αn obtained by (Equation 3). However, the corrected acceleration An of the vehicle 12 at time n, the acceleration of the vehicle 12 output from the acceleration sensor 1 as an, and the gravitational acceleration as g.
[0011]
[Expression 4]

[0012]
By substituting An in (Equation 4) for an in (Equation 1) and (Equation 2), the speed Vn of the vehicle 12 at time n and the movement distance ΔDn of the vehicle 12 in the time period Δt are obtained. This is shown as (Equation 5) and (Equation 6).
[0013]
[Equation 5]

[0014]
[Formula 6]

[0015]
Next, the moving azimuth calculating means 7 performs an integration operation on the time axis with respect to data (yaw angular velocity data) output from the gyro sensor 3 every predetermined time period (predetermined minute time) Δt. That is, the azimuth angle (integrated yaw angle) θn of the vehicle 12 is obtained by performing the calculation of (Equation 7). However, the time period for obtaining data from the gyro sensor 3 is Δt, the azimuth velocity (yaw angular velocity) of the vehicle 12 at time n is ωn, the movement azimuth angle of the vehicle 12 at time n at time n is θn, and the initial azimuth is Let θ0.
[0016]
[Expression 7]

[0017]
FIG. 5 is a conceptual diagram of position calculation for explaining position calculation in the navigation device. When the relative position calculation means 8 obtains the movement distance ΔDn and the movement azimuth angle θn of the vehicle 12 as described above, the concept shown in FIG. 5 (the X and Y directions at a predetermined time Δt by multiplying the movement distance by the movement azimuth angle). Based on (Equation 8) and (Equation 9), the position of the vehicle 12 is obtained by cumulative calculation from the initial position of the vehicle 12. However, the position coordinates of the vehicle 12 at time n are (xn, yn) and the initial position is (x0, y0). In calculating the moving distance, the influence of the pitch angle formed with the horizontal plane is not taken into consideration, but this can be easily calculated, and an error does not occur so much with a small pitch angle, so the description is omitted.
[0018]
[Equation 8]

[0019]
[Equation 9]

[0020]
The relative position calculation means 8 outputs position data of the vehicle 12 to the display control means 11. When receiving the position data of the vehicle 12, the display control unit 11 reads out digital map data of a predetermined area from the map storage unit 10 and outputs it to the display unit 9.
[0021]
Thus, in the conventional navigation device, in order to accurately calculate the speed by the acceleration sensor 1, the acceleration sensor 1 uses the output data (pitch angular velocity data) from the gyro sensor 2 for detecting the pitch angle of the vehicle 12. The output data (acceleration data) from is corrected.
[0022]
[Problems to be solved by the invention]
However, in the conventional navigation device, if there is a drift component in the output from the gyro sensor 2 that detects the pitch angular velocity for correcting the acceleration from the acceleration sensor 1, the gyro sensor 2 determines the pitch angular velocity of the vehicle. When trying to detect, since the speed correction means 6 accumulates the output data of the gyro sensor 2, it accumulates up to the output drift of the gyro sensor 2, increasing the accumulated error of the accumulated pitch angle, and speed accuracy. (Accordingly, the movement distance calculation accuracy) and the movement azimuth angle accuracy are very poor.
[0023]
This navigation device is required to be able to prevent an increase in the accumulated error of the integrated pitch angle.
[0024]
An object of the present invention is to provide a navigation device that can prevent an increase in accumulated error of accumulated pitch angles.
[0025]
[Means for Solving the Problems]
In order to solve this problem, a navigation device according to the present invention includes an acceleration sensor that detects acceleration of a moving body, and speed correction means that corrects acceleration of an acceleration sensor output. A navigation device having a moving distance calculating means for calculating a moving speed, calculating a moving distance of the moving body by adding the moving speed, and a pitch angular speed detecting unit for detecting a pitch angular speed of the moving body, wherein the detected pitch Pitch angle determination means for correcting the predetermined minute time integrated pitch angle by comparing the predetermined minute time integrated pitch angle obtained by integrating the angular velocity for a predetermined minute time with a reference value is provided.
[0026]
Thereby, the navigation apparatus which can prevent the increase in the accumulation error of the integrated pitch angle can be obtained.
[0027]
DETAILED DESCRIPTION OF THE INVENTION
The invention according to claim 1 of the present invention includes an acceleration sensor for detecting the acceleration of the moving body, and speed correction means for correcting the acceleration of the acceleration sensor output, and the moving speed of the moving body is determined from the corrected acceleration. A navigation apparatus having a movement distance calculation means for calculating a movement distance of a moving body by integrating the movement speed and a pitch angular speed detection unit for detecting a pitch angular speed of the moving body, the detected pitch angular speed being Pitch angle determination means for correcting a predetermined minute time integration pitch angle by comparing a predetermined minute time integration pitch angle obtained by integrating for a predetermined minute time and a reference value is provided. The pitch angle is corrected according to whether or not it exceeds the reference value.
[0028]
According to a second aspect of the present invention, in the first aspect of the invention, the pitch angle determining means corrects the predetermined minute time accumulated pitch angle to the reference value when the predetermined minute time accumulated pitch angle exceeds the reference value. Therefore, when the predetermined minute time integrated pitch angle exceeds the reference value, the pitch angle is corrected to an angle equal to the reference value.
[0029]
According to a third aspect of the present invention, in the first aspect of the invention, the pitch angle determining means calculates the average of the predetermined minute time integrated pitch angle per measurement time when the predetermined minute time integrated pitch angle exceeds a reference value. When the predetermined minute time integrated pitch angle exceeds the reference value, the value is corrected to an average value per measurement time, for example, an angle equal to the average value during travel time.
[0030]
According to a fourth aspect of the present invention, in the first aspect of the invention, the pitch angle determining means determines the predetermined minute time accumulated pitch angle as an acceleration of the acceleration sensor output when the predetermined minute time accumulated pitch angle exceeds a reference value. Therefore, when the predetermined minute time accumulated pitch angle exceeds the reference value, it is corrected to an angle equal to the value estimated from the acceleration of the acceleration sensor output.
[0031]
The invention according to claim 5 is the invention according to any one of claims 1 to 4, wherein the pitch angle determination means sets the reference value based on the altitude change information of the map stored in the CD-ROM. The reference value is set to an appropriate value.
[0032]
Hereinafter, embodiments of the present invention will be described with reference to FIGS.
(Embodiment 1)
FIG. 1 is a block diagram showing a navigation apparatus according to Embodiment 1 of the present invention. In FIG. 1, an acceleration sensor 1, a gyro sensor 2, 3, a movement distance calculation unit 5, a speed correction unit 6, a movement direction calculation unit 7, a relative position calculation unit 8, a display unit 9, a map storage unit 10, a display control unit 11. Is the same as that in FIG. B is a central control unit that performs calculation and control processing, 4 is a predetermined minute time integrated pitch angle obtained by integrating the output data (pitch angular velocity data) of the gyro sensor 2 for a predetermined minute time, and a reference value (threshold). Are pitch angle determination means for correcting the output data of the gyro sensor 2.
[0033]
The operation of the navigation device configured as described above will be described. First, the moving distance calculating means 5 calculates the moving distance of the vehicle 12 by obtaining the speed of the vehicle 12 from the output data (acceleration data) of the acceleration sensor 1 and integrating the vehicle speed. The moving direction calculating means 7 Then, the output data (azimuth velocity data) of the gyro sensor 3 is inputted, and the azimuth angle of the vehicle 12 is calculated by integrating the azimuth velocity. Further, the relative position calculation means 8 calculates the position data of the vehicle 12 from the vehicle movement distance and the vehicle azimuth angle output from the movement distance calculation means 5 and the movement direction calculation means 7 and outputs them to the display control means 11. At this time, the speed correction means 6 corrects the output data of the acceleration sensor 1 using the pitch angle of the vehicle 12 calculated from the output data of the gyro sensor 2. These operations are the same as in the prior art.
[0034]
Although the drift component is usually included in the output of the gyro sensor 2, conventionally, the drift component is also integrated by the speed correction means 6, and the output data of the acceleration sensor 1 is determined by the vehicle pitch angle including the accumulated error component. Was corrected. For this reason, the movement distance of the vehicle 12 calculated by the movement distance calculation means 5 is different from the actual movement distance of the vehicle 12 and includes an error, and the error increases due to accumulation. Had. In order to remove such an error, the pitch angle determination means 4 is provided.
[0035]
Next, the correction operation for removing the drift component in the pitch angle determination means 4 will be described. The integrated value (predetermined minute time integrated pitch angle) of the pitch angular velocity obtained from the gyro sensor 2 in the time period (predetermined minute time) Δt is In, and the reference value (threshold value) is Ix. The threshold value Ix is a reference value that is set based on the maximum possible pitch angular velocity obtained by experiments or the like so that the correction operation becomes the optimum operation. When In ≦ Ix, the pitch angle determination unit 4 regards that a possible pitch angle is normally generated in the vehicle 12 and does not correct the integrated value (predetermined minute time integrated pitch angle) of the output data of the gyro sensor 2. To the speed correction means 6. Further, when In> Ix, the pitch angle determination means 4 considers that the gyro sensor 2 outputs a pitch angular velocity that is not normally possible because the output of the gyro sensor 2 includes a drift component, and then the pattern 1 to 3 are output to the speed correction means 6. Which correction value is selected is determined by determining the tendency of the increase / decrease in the pitch angle from past experimental data or the like.
[0036]
First, the pattern 1 will be described. In this case, the pitch angle determination means 4 corrects the integrated value (predetermined minute time integrated pitch angle) of the output data obtained from the gyro sensor 2 to the threshold value Ix (In = Ix) and outputs it to the speed correction means 6. The speed correction means 6 corrects the acceleration of the output of the acceleration sensor 1 using (Equation 4) based on In = Ix.
[0037]
Next, the pattern 2 will be described. In this case, the pitch angle determination means 4 corrects the predetermined minute time integrated pitch angle, which is an integrated value of the pitch angular velocity obtained from the gyro sensor 2 in a predetermined minute time, to an average value per measurement time (for example, an average value in traveling time). And output to the speed correction means 6.
[0038]
Finally, pattern 3 will be described. In this case, the pitch angle determination means 4 corrects the predetermined minute time integrated pitch angle, which is an integrated value of the pitch angular speed obtained from the gyro sensor 2 in a predetermined minute time, to a value estimated from the acceleration sensor 1, thereby correcting the speed. Output to. For example, when the output acceleration of the acceleration sensor 1 is constant and In> Ix, the drift of the gyro sensor 2 actually increases while the acceleration decreases, so that the predetermined minute time integrated pitch angle is decreased. To correct. Further, when the output acceleration of the acceleration sensor 1 increases and In> Ix, the gyro sensor 2 actually drifts while the acceleration increases, so the predetermined minute time integrated pitch angle is constant at the threshold value Ix. Correct so that At this time, the altitude change information of the map stored in the storage means such as a CD-ROM may be used as the threshold value Ix.
[0039]
FIG. 2 is a flowchart showing the operation of the pitch angle determination means 4. First, the pitch angular velocity of a predetermined minute time output from the gyro sensor 2 is integrated to obtain a predetermined minute time integrated pitch angle In (S1). Next, the predetermined minute time accumulated pitch angle In is compared with a threshold value Ix (S2), and it is determined whether or not In ≦ Ix (S3). In the case of In ≦ Ix, the movement distance calculation means 5 performs speed correction and movement distance calculation (S5) without correcting the predetermined minute time integrated pitch angle In (S4). If In ≦ Ix is not satisfied, the moving distance calculating means 5 corrects the predetermined minute time integrated pitch angle In by any one of the patterns 1 to 3 (S6). This is repeated during the running time (S2 to S6).
[0040]
As described above, when it is determined that the output of the gyro sensor 2 includes a drift component (when In> Ix), the pitch angle determination unit 4 determines the predetermined minute time integrated pitch angle according to any one of the patterns 1 to 3. Therefore, even if drift occurs in the gyro sensor 2, the drift component is not integrated as a pitch angle, and therefore, it is possible to prevent the accumulated pitch angle error accumulation based on the drift component. It is possible to prevent an increase in error in calculating the movement distance.
[0041]
The same can be said for the gyro sensor 3 as with the gyro sensor 2. In this case, a yaw angle determining means (not shown) is arranged between the gyro sensor 3 and the moving direction calculating means 7 as equivalent to the pitch angle determining means 4 of FIG. The yaw angle determination means compares whether or not the predetermined minute time integrated yaw angle is equal to or smaller than a threshold value. If the yaw angle determination means is equal to or smaller than the threshold value, it is considered that a possible pitch angle is normally generated in the vehicle 12 and the output data of the gyro sensor 3 The integrated value (predetermined minute time integrated yaw angle) is output to the moving direction calculating means 7 without being corrected. Further, the yaw angle determination means compares whether or not the predetermined minute time integrated yaw angle is equal to or less than a threshold value. The angular velocity is regarded as being output by the gyro sensor 3, and a predetermined pattern correction value is output to the moving direction calculation means 7.
[0042]
As described above, when it is determined that the output of the gyro sensor 3 includes a drift component, the yaw angle determination means corrects the predetermined minute time integrated yaw angle using a predetermined pattern. Even when drift occurs, the drift component is not accumulated as the yaw angle, and therefore, accumulation of errors in the accumulated yaw angle based on the drift component can be prevented, and an increase in error in calculating the moving distance can be prevented. Can do.
[0043]
In the present embodiment, each means 4-8, 11 of the central control unit B has been described as a hardware configuration, but the present invention is not limited to this, and each means 4-8, 11 is configured by software. That is, the means 4 to 8 and 11 may be realized by the CPU executing the program.
[0044]
As described above, according to the present embodiment, when it is determined that the outputs of the gyro sensors 2 and 3 include a drift component, the pitch angle determination unit 4 and the yaw angle determination unit are any of the patterns 1 to 3. Since the predetermined minute time accumulated pitch angle and the predetermined minute time accumulated yaw angle are corrected by the predetermined pattern or the predetermined pattern, even if drift occurs in the gyro sensors 2 and 3, the drift component is the pitch angle and yaw angle. Therefore, accumulation of errors in the accumulated pitch angle and accumulated yaw angle based on the drift component can be prevented, and an increase in errors in the movement distance calculation can be prevented.
[0045]
【The invention's effect】
As described above, according to the navigation device of the present invention, the predetermined minute time accumulated pitch angle can be corrected according to whether or not the predetermined minute time accumulated pitch angle exceeds the reference value. An advantageous effect is obtained that correction can be made so that the error does not increase.
[0046]
The pitch angle determining means corrects the predetermined minute time accumulated pitch angle to the reference value when the predetermined minute time accumulated pitch angle exceeds the reference value, so that the predetermined minute time accumulated pitch angle exceeds the reference value. Since the drift amount estimated to be generated can be reduced by the reference value, an advantageous effect that an increase in the accumulated error of the accumulated pitch angle can be prevented is obtained.
[0047]
Further, the pitch angle determining means corrects the predetermined minute time accumulated pitch angle to an average value per measurement time when the predetermined minute time accumulated pitch angle exceeds the reference value, so that the predetermined minute time accumulated pitch angle becomes the reference value. Since the drift that is estimated to occur when the value exceeds the value can be reduced by the average value, an advantageous effect that an increase in the accumulated error of the accumulated pitch angle can be prevented is obtained.
[0048]
Further, the pitch angle determination means corrects the predetermined minute time accumulated pitch angle to a value estimated from the acceleration of the acceleration sensor output when the predetermined minute time accumulated pitch angle exceeds the reference value, thereby obtaining the predetermined minute time accumulated pitch angle. Since the drift that is estimated to occur when the angle exceeds the reference value can be reduced based on the acceleration, an advantageous effect of preventing an increase in the accumulated error of the accumulated pitch angle can be obtained. It is done.
[0049]
Furthermore, the pitch angle determination means can obtain an advantageous effect that the reference value can be set to an appropriate value by setting the reference value based on the altitude change information of the map stored in the CD-ROM. .
[Brief description of the drawings]
FIG. 1 is a block diagram showing a navigation device according to Embodiment 1 of the present invention. FIG. 2 is a flowchart showing the operation of the navigation device of FIG. 1. FIG. 3 is a block diagram showing a conventional navigation device. In-vehicle concept diagram showing the state of vehicle mounting [Fig.5] Conceptual diagram of position calculation [Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Acceleration sensor 2, 3 Gyro sensor 4 Pitch angle determination means 5 Movement distance calculation means 6 Speed correction means 7 Movement direction calculation means 8 Relative position calculation means 9 Display part 10 Map storage part

Claims (5)

An acceleration sensor for detecting the acceleration of the moving body and speed correction means for correcting the acceleration of the acceleration sensor output are obtained, the moving speed of the moving body is obtained from the corrected acceleration, and the moving speed is integrated to move. A navigation device having a moving distance calculating means for calculating a moving distance of a body and a pitch angular velocity detecting unit for detecting a pitch angular velocity of the moving object, and obtained by integrating the detected pitch angular velocity for a predetermined minute time. A navigation apparatus comprising pitch angle determination means for comparing the predetermined minute time accumulated pitch angle with a reference value and correcting the predetermined minute time accumulated pitch angle. The navigation device according to claim 1, wherein the pitch angle determination unit corrects the predetermined minute time accumulated pitch angle to the reference value when the predetermined minute time accumulated pitch angle exceeds the reference value. The navigation device according to claim 1, wherein the pitch angle determination means corrects the predetermined minute time integrated pitch angle to an average value per measurement time when the predetermined minute time integrated pitch angle exceeds the reference value. 2. The pitch angle determination means corrects the predetermined minute time accumulated pitch angle to a value estimated from the acceleration of the acceleration sensor output when the predetermined minute time accumulated pitch angle exceeds the reference value. Navigation device. The navigation device according to any one of claims 1 to 4, wherein the pitch angle determination means sets the reference value based on map altitude change information stored in a CD-ROM.

Images