JP4776570B2 - Navigation apparatus, angular velocity correction apparatus and method using tilt angle - Google Patents

Navigation apparatus, angular velocity correction apparatus and method using tilt angle Download PDF

Info

Publication number
JP4776570B2
JP4776570B2 JP2007058131A JP2007058131A JP4776570B2 JP 4776570 B2 JP4776570 B2 JP 4776570B2 JP 2007058131 A JP2007058131 A JP 2007058131A JP 2007058131 A JP2007058131 A JP 2007058131A JP 4776570 B2 JP4776570 B2 JP 4776570B2
Authority
JP
Japan
Prior art keywords
sensor
inclination angle
tilt angle
gps
acceleration
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2007058131A
Other languages
Japanese (ja)
Other versions
JP2008216226A (en
Inventor
隆行 渡辺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Alpine Electronics Inc
Original Assignee
Alpine Electronics Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Alpine Electronics Inc filed Critical Alpine Electronics Inc
Priority to JP2007058131A priority Critical patent/JP4776570B2/en
Publication of JP2008216226A publication Critical patent/JP2008216226A/en
Application granted granted Critical
Publication of JP4776570B2 publication Critical patent/JP4776570B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Description

本発明は、ナビゲーション装置、傾斜角を用いた角速度補正装置および方法に関するものである。   The present invention relates to a navigation device, an angular velocity correction device using an inclination angle, and a method.

一般に、車載用のナビゲーション装置では、自立航法センサやＧＰＳ（Global Positioning System）受信機などを用いて車両の現在位置を検出し、その近傍の地図データを記録媒体から読み出して画面上に表示する。そして、画面上の所定箇所に自車位置を示す自車位置マークを重ね合わせて表示することにより、車両が現在どこを走行しているのかを一目で分かるようにしている。   In general, in-vehicle navigation devices detect the current position of a vehicle using a self-contained navigation sensor, a GPS (Global Positioning System) receiver, or the like, read out map data in the vicinity of the vehicle, and display it on a screen. Then, the vehicle position mark indicating the vehicle position is superimposed and displayed at a predetermined location on the screen, so that it can be seen at a glance where the vehicle is currently traveling.

自立航法センサは、所定走行距離毎に１個のパルスを出力して車両の移動距離を検出する車速センサ（距離センサ）と、車両の回転角度（移動方位）を検出する振動ジャイロ等の角速度センサとを含んでいる。自立航法センサは、これらの車速センサおよび角速度センサによって車両の相対位置および相対方位を検出するようになっている。   The self-contained navigation sensor includes a vehicle speed sensor (distance sensor) that outputs a single pulse for each predetermined travel distance to detect the travel distance of the vehicle, and an angular velocity sensor such as a vibration gyro that detects the rotation angle (movement direction) of the vehicle. Including. The self-contained navigation sensor detects the relative position and relative direction of the vehicle by using these vehicle speed sensor and angular velocity sensor.

ここで、角速度センサは、角速度を検出しようとする平面が、実際に角速度センサが回転する平面と平行である必要がある。両者の平面が互いに平行でなくなると、角速度センサの出力感度が本来の感度からずれてしまう。これを感度誤差と呼ぶ。   Here, in the angular velocity sensor, the plane on which the angular velocity is to be detected needs to be parallel to the plane on which the angular velocity sensor actually rotates. If the two planes are not parallel to each other, the output sensitivity of the angular velocity sensor deviates from the original sensitivity. This is called a sensitivity error.

一般的に、角速度センサは、これを水平に設置したときの出力感度で正しく進行方向が判断できるように調整してある。そのため、角速度センサの角度が水平でなくなった場合には、角速度センサに加わる角速度と出力感度との関係が変化して、車両の進行方向を正しく検出することができなくなってしまう。そこで、角速度センサの水平方向に対する傾斜分の感度補正を行うことが必要になる。   Generally, the angular velocity sensor is adjusted so that the traveling direction can be correctly determined by the output sensitivity when the sensor is installed horizontally. For this reason, when the angle of the angular velocity sensor is not horizontal, the relationship between the angular velocity applied to the angular velocity sensor and the output sensitivity changes, and the traveling direction of the vehicle cannot be detected correctly. Therefore, it is necessary to perform sensitivity correction for the inclination of the angular velocity sensor in the horizontal direction.

従来、この感度補正は、以下のようにして行っていた。すなわち、ＧＰＳ衛星から電波を良好に受信できているときに、ＧＰＳ受信機により絶対方位として得られるＧＰＳ方位と、角速度センサにより得られる角速度から計算されるジャイロ方位とを比較し、角速度がＧＰＳ方位の変化量と一致するように、角速度センサの感度を決定している。   Conventionally, this sensitivity correction has been performed as follows. That is, when radio waves are successfully received from GPS satellites, the GPS azimuth obtained as an absolute azimuth by the GPS receiver is compared with the gyro azimuth calculated from the angular velocity obtained by the angular velocity sensor. The sensitivity of the angular velocity sensor is determined so as to coincide with the amount of change.

また、簡単な構成で車両の移動状況を認識するものとして、加速度センサを用いた技術も存在する。この加速度センサは、車両が停止している状態（車速センサの出力がゼロである状態）での出力値をオフセット値（基準電圧値）として算出し、加速度センサからの出力値とオフセット値との差分に加速度センサの感度を乗算して、加速度データを求めている（例えば、特許文献１参照）。これは一般的に、ゼロ点補正と呼ばれている。
特開２００３−３０７５２４号公報 In addition, there is a technique using an acceleration sensor for recognizing a moving state of a vehicle with a simple configuration. This acceleration sensor calculates the output value when the vehicle is stopped (state where the output of the vehicle speed sensor is zero) as an offset value (reference voltage value), and calculates the output value from the acceleration sensor and the offset value. The difference is multiplied by the sensitivity of the acceleration sensor to obtain acceleration data (see, for example, Patent Document 1). This is generally called zero point correction.
JP 2003-307524 A

ところが、加速度センサの出力電圧値は、周辺温度の影響によって大きく変動する。そのため、ゼロ点補正を行えない車両の走行中に加速度センサが周辺温度の影響を受けると、前回の停車時に取得したオフセット値がゼロ点の真の電圧値から大きく外れてしまうといった、いわゆるゼロ点誤差を生じる。上記特許文献１では、このゼロ点誤差を補正する手法について開示している。   However, the output voltage value of the acceleration sensor varies greatly due to the influence of the ambient temperature. Therefore, if the acceleration sensor is affected by the ambient temperature while the vehicle cannot perform zero point correction, the offset value obtained when the vehicle stopped last time will deviate significantly from the true voltage value of the zero point. An error is generated. In the above-mentioned Patent Document 1, a method for correcting this zero point error is disclosed.

しかしながら、上記従来の技術では、ＧＰＳ受信機で衛星からの電波を受信できない環境では、角速度センサの感度補正を行うことができない。また、特許文献１に記載の技術では、車両の傾斜角がほぼ０°ときでなければ加速度センサのゼロ点補正を行うことができない。そのため、立体駐車場など、車両の傾きが大きく、ＧＰＳ電波を受信できない環境を走行した場合には、角速度センサの感度補正や加速度センサのゼロ点補正ができないため、２次元平面として描画された地図上の自車位置が正しい位置から徐々にずれてしまうという問題があった。また、従来の技術では、感度補正とゼロ点補正とが別々のアルゴリズムで行われており、処理が極めて煩雑であるという問題もあった。   However, in the above conventional technique, the sensitivity correction of the angular velocity sensor cannot be performed in an environment where the GPS receiver cannot receive radio waves from the satellite. In the technique described in Patent Document 1, the zero point correction of the acceleration sensor cannot be performed unless the vehicle tilt angle is approximately 0 °. Therefore, a map drawn as a two-dimensional plane because the sensitivity of the angular velocity sensor and the zero point of the acceleration sensor cannot be corrected when the vehicle is traveling in an environment where the inclination of the vehicle is large and GPS signals cannot be received, such as a multilevel parking lot. There was a problem that the upper vehicle position gradually shifted from the correct position. Further, in the conventional technique, sensitivity correction and zero point correction are performed by separate algorithms, and there is a problem that processing is extremely complicated.

本発明は、このような問題を解決するために成されたものであり、車両が傾いているときでも簡単なアルゴリズムで感度補正とゼロ点補正を行うことができ、それによって自律航法精度を向上させることができるようにすることを目的とする。
また、本発明は、車両の傾きが存在し、ＧＰＳ電波を受信できない環境においても、簡単なアルゴリズムで感度補正とゼロ点補正を行うことができ、それによって自律航法精度を向上させることができるようにすることを目的とする。 The present invention has been made to solve such a problem, and even when the vehicle is tilted, it is possible to perform sensitivity correction and zero point correction with a simple algorithm, thereby improving autonomous navigation accuracy. The purpose is to be able to make it.
Further, the present invention can perform sensitivity correction and zero point correction with a simple algorithm even in an environment where a vehicle has a tilt and GPS signals cannot be received, thereby improving autonomous navigation accuracy. The purpose is to.

上記した課題を解決するために、本発明では、水平方向に対する加速度センサの傾斜角を表した加速度傾斜角を算出するとともに、水平方向に対する車両の傾斜角を表したＧＰＳ傾斜角をＧＰＳ受信機の出力信号を用いて算出する。さらにこれらの加速度傾斜角およびＧＰＳ傾斜角に基づいて、車両の走行方向に対する角速度センサの傾斜角を表したセンサ傾斜角と、水平方向に対する車両の傾斜角を表した車両傾斜角とを算出する。そして、これらのセンサ傾斜角と車両傾斜角とに基づいて、角速度センサより出力される角速度を補正するようにしている。   In order to solve the above-described problems, in the present invention, an acceleration inclination angle representing the inclination angle of the acceleration sensor with respect to the horizontal direction is calculated, and a GPS inclination angle representing the inclination angle of the vehicle with respect to the horizontal direction is calculated by the GPS receiver. Calculate using the output signal. Further, based on the acceleration inclination angle and the GPS inclination angle, a sensor inclination angle that represents the inclination angle of the angular velocity sensor with respect to the traveling direction of the vehicle and a vehicle inclination angle that represents the inclination angle of the vehicle with respect to the horizontal direction are calculated. The angular velocity output from the angular velocity sensor is corrected based on the sensor inclination angle and the vehicle inclination angle.

また、本発明の他の態様では、算出されたセンサ傾斜角を記憶しておくセンサ傾斜角記憶部を備え、算出されるＧＰＳ傾斜角の信頼性を判定し、信頼性がないと判定された場合に、新たにセンサ傾斜角を算出せずに、センサ傾斜角記憶部に記憶されている過去のセンサ傾斜角を使用して、角速度センサより出力される角速度を補正するようにしている。   In another aspect of the present invention, a sensor tilt angle storage unit that stores the calculated sensor tilt angle is provided, the reliability of the calculated GPS tilt angle is determined, and it is determined that there is no reliability. In this case, the angular velocity output from the angular velocity sensor is corrected by using the past sensor inclination angle stored in the sensor inclination angle storage unit without newly calculating the sensor inclination angle.

上記のように構成した本発明によれば、加速度センサの出力値を使用して算出される加速度傾斜角には加速度センサのゼロ点誤差が含まれているが、これをそのまま用いるのではなく、加速度センサの傾斜角がセンサ傾斜角と車両傾斜角とに分解して計算される。このときセンサ傾斜角は、ゼロ点誤差や感度誤差とは無関係のＧＰＳ傾斜角を用いて計算されるので、ゼロ点誤差が含まれない正しいセンサ傾斜角と車両傾斜角とが求められる。これにより、車両が傾いているときでも、角速度センサの感度誤差と同時に加速度センサのゼロ点誤差も補正することができ、自律航法精度を向上させることができる。しかも、角速度センサの感度誤差と加速度センサのゼロ点誤差とを別々のアルゴリズムで補正しているのではなく、同じ演算の中で同時に補正することができる。つまり、簡単なアルゴリズムで感度補正とゼロ点補正を行うことができる。   According to the present invention configured as described above, the acceleration tilt angle calculated using the output value of the acceleration sensor includes the zero point error of the acceleration sensor, but this is not used as it is. The inclination angle of the acceleration sensor is calculated by being decomposed into the sensor inclination angle and the vehicle inclination angle. At this time, since the sensor tilt angle is calculated using a GPS tilt angle that is unrelated to the zero point error and the sensitivity error, a correct sensor tilt angle and vehicle tilt angle not including the zero point error are obtained. Thereby, even when the vehicle is tilted, the zero point error of the acceleration sensor can be corrected simultaneously with the sensitivity error of the angular velocity sensor, and the autonomous navigation accuracy can be improved. Moreover, the sensitivity error of the angular velocity sensor and the zero point error of the acceleration sensor are not corrected by separate algorithms, but can be corrected simultaneously in the same calculation. That is, sensitivity correction and zero point correction can be performed with a simple algorithm.

本発明の他の態様によれば、ＧＰＳ電波を受信できない環境になると、算出されるＧＰＳ傾斜角に信頼性がないと判定され、センサ傾斜角記憶部に記憶されている過去のセンサ傾斜角を使用して、角速度の補正が行われる。しかも、この過去のセンサ傾斜角は、ＧＰＳ傾斜角に信頼性があると判定されたときに計算されたものであり、上述のように加速度センサのゼロ点誤差が取り除かれた正しい値である。このため、ＧＰＳ電波を受信できない環境においても、簡単なアルゴリズムで感度補正とゼロ点補正を行うことができ、それによって自律航法精度を向上させることができる。   According to another aspect of the present invention, in an environment where GPS radio waves cannot be received, it is determined that the calculated GPS tilt angle is not reliable, and past sensor tilt angles stored in the sensor tilt angle storage unit are determined. In use, the angular velocity is corrected. In addition, the past sensor tilt angle is calculated when it is determined that the GPS tilt angle is reliable, and is a correct value from which the zero point error of the acceleration sensor is removed as described above. For this reason, even in an environment where GPS radio waves cannot be received, sensitivity correction and zero point correction can be performed with a simple algorithm, thereby improving autonomous navigation accuracy.

以下、本発明の一実施形態を図面に基づいて説明する。図１は、本実施形態による角速度補正装置を適用したナビゲーション装置の要部構成例を示すブロック図である。図２は、本実施形態で用いる各傾斜角の説明図である。   Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1 is a block diagram illustrating a configuration example of a main part of a navigation device to which the angular velocity correction device according to the present embodiment is applied. FIG. 2 is an explanatory diagram of each inclination angle used in the present embodiment.

図１に示すように、本実施形態のナビゲーション装置は、加速度センサ１、車速センサ（距離センサ）２、ＧＰＳ受信機３、角速度センサ（振動ジャイロ）４およびプロセッサ５を備えている。そして、プロセッサ５が加速度センサ１、車速センサ２、ＧＰＳ受信機３および角速度センサ４の出力値を用いて、ナビゲーション装置が搭載される車両の位置および方位を検出するように成されている。ここで、加速度センサ１および角速度センサ４は、例えば図２に示すように同一基板上に搭載され、共に同じ傾斜角で車両内に設置されている。   As shown in FIG. 1, the navigation device of this embodiment includes an acceleration sensor 1, a vehicle speed sensor (distance sensor) 2, a GPS receiver 3, an angular velocity sensor (vibration gyro) 4, and a processor 5. The processor 5 detects the position and direction of the vehicle on which the navigation device is mounted using the output values of the acceleration sensor 1, the vehicle speed sensor 2, the GPS receiver 3, and the angular velocity sensor 4. Here, the acceleration sensor 1 and the angular velocity sensor 4 are mounted on the same board as shown in FIG. 2, for example, and both are installed in the vehicle at the same inclination angle.

プロセッサ５は、本発明の傾斜角を用いた角速度補正装置に相当するものであり、以下に説明するような機能構成を備えている。加速度傾斜角算出部１１は、加速度センサ１より出力される加速度Ａacc、車速センサ２より出力される車速Ｖcarの単位時間当たりの変化量から求められる加速度Ａcar、および重力加速度Ｇに基づいて、水平方向（傾斜のない地平線の方向）に対する加速度センサ１の傾斜角を表した加速度傾斜角θaccを算出する。   The processor 5 corresponds to the angular velocity correction device using the tilt angle of the present invention, and has a functional configuration as described below. The acceleration inclination angle calculation unit 11 performs horizontal direction based on the acceleration Aacc output from the acceleration sensor 1, the acceleration Acar obtained from the amount of change per unit time of the vehicle speed Vcar output from the vehicle speed sensor 2, and the gravitational acceleration G. The acceleration inclination angle θacc representing the inclination angle of the acceleration sensor 1 with respect to (the direction of the horizon without inclination) is calculated.

図２に示すように、加速度センサ１より出力される加速度Ａaccは、加速度センサ１が水平方向に対して傾斜している分だけ重力加速度Ｇの影響を受け、重力加速度Ｇの正弦成分Ｇ・sin(θacc)の分だけ、車速変化による加速度Ａcarよりも値が小さくなる。つまり、Ｇ・sin(θacc)＝Ａcar−Ａaccの関係が成り立つ。これにより、加速度傾斜角θaccは、θacc＝sin-1{(Ａcar−Ａacc)／Ｇ}なる演算によって求めることができる。 As shown in FIG. 2, the acceleration Aacc output from the acceleration sensor 1 is affected by the gravitational acceleration G as much as the acceleration sensor 1 is inclined with respect to the horizontal direction, and the sine component G · sin of the gravitational acceleration G The value is smaller than the acceleration Acar due to the change in vehicle speed by the amount of (θacc). That is, the relationship G · sin (θacc) = Acar−Aacc holds. Thereby, the acceleration inclination angle θacc can be obtained by calculation of θacc = sin −1 {(Acar−Aacc) / G}.

ＧＰＳ傾斜角算出部１２は、ＧＰＳ受信機３で複数のＧＰＳ衛星（図示せず）から受信する電波のドップラー効果による周波数の変化に基づいて、水平方向（傾斜のない地平線の方向）に対する車両の傾斜角を表したＧＰＳ傾斜角θgpsを算出する。具体的には、ＧＰＳ傾斜角算出部１２は、図２に示すように、緯度方向（Ｘ方向）、経度方向（Ｙ方向）および高さ方向（Ｚ方向）の３次元測位処理を行って、各方向の速度成分Ｘv，Ｙv，Ｚvに基づいてθgps＝tan-1{Ｚv／√(Ｘv2＋Ｙv2)}なる演算によってＧＰＳ傾斜角gpsを求める。 The GPS tilt angle calculation unit 12 is based on a change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites (not shown) by the GPS receiver 3, and the vehicle tilts in the horizontal direction (the direction of the horizon with no tilt). A GPS tilt angle θgps representing the tilt angle is calculated. Specifically, as shown in FIG. 2, the GPS tilt angle calculation unit 12 performs a three-dimensional positioning process in the latitude direction (X direction), the longitude direction (Y direction), and the height direction (Z direction) Based on the velocity components Xv, Yv, and Zv in each direction, the GPS tilt angle gps is obtained by calculation of θgps = tan −1 {Zv / √ (Xv 2 + Yv 2 )}.

センサ傾斜角算出部１３は、加速度傾斜角算出部１１により算出された加速度傾斜角θaccおよびＧＰＳ傾斜角算出部１２により算出されたＧＰＳ傾斜角θgpsに基づいて、車両の走行方向に対する角速度センサ４の傾斜角を表したセンサ傾斜角θsensorを算出する。具体的には、センサ傾斜角算出部１３は、θsensor＝θacc−θgpsなる演算によってセンサ傾斜角θsensorを算出する。   The sensor inclination angle calculation unit 13 is based on the acceleration inclination angle θacc calculated by the acceleration inclination angle calculation unit 11 and the GPS inclination angle θgps calculated by the GPS inclination angle calculation unit 12. A sensor inclination angle θsensor representing the inclination angle is calculated. Specifically, the sensor inclination angle calculation unit 13 calculates the sensor inclination angle θsensor by an operation of θsensor = θacc−θgps.

車両傾斜角算出部１４は、加速度傾斜角算出部１１により算出された加速度傾斜角θaccおよびセンサ傾斜角算出部１３により算出されたセンサ傾斜角θsensorに基づいて、水平方向（傾斜のない地平線の方向）に対する車両の傾斜角を表した車両傾斜角θcarを算出する。具体的には、車両傾斜角算出部１４は、θcar＝θacc−θsensorなる演算によって車両傾斜角θcarを算出する。   The vehicle inclination angle calculation unit 14 generates a horizontal direction (the direction of the horizon without inclination) based on the acceleration inclination angle θacc calculated by the acceleration inclination angle calculation unit 11 and the sensor inclination angle θsensor calculated by the sensor inclination angle calculation unit 13. The vehicle inclination angle θcar representing the inclination angle of the vehicle with respect to) is calculated. Specifically, the vehicle inclination angle calculation unit 14 calculates the vehicle inclination angle θcar by calculation of θcar = θacc−θsensor.

角速度補正部１５は、センサ傾斜角算出部１３により算出されたセンサ傾斜角θsensorおよび車両傾斜角算出部１４により算出された車両傾斜角θcarに基づいて、角速度センサ４より出力される角速度を補正する。具体的には、補正前の角速度をωgyro、補正後の角速度をωgyro’とした場合、角速度補正部１５は、ωgyro’＝ωgyro÷cos(θsensor＋θcar)なる演算によって角速度ωgyroの補正を行う。   The angular velocity correction unit 15 corrects the angular velocity output from the angular velocity sensor 4 based on the sensor inclination angle θsensor calculated by the sensor inclination angle calculation unit 13 and the vehicle inclination angle θcar calculated by the vehicle inclination angle calculation unit 14. . Specifically, when the angular velocity before correction is ωgyro and the angular velocity after correction is ωgyro ′, the angular velocity correction unit 15 corrects the angular velocity ωgyro by calculation of ωgyro ′ = ωgyro ÷ cos (θsensor + θcar).

なお、演算式上ではθsensor＋θcar＝θaccとなるが、加速度センサ１の出力値から加速度傾斜角算出部１１により求められる加速度傾斜角θaccと、θsensor＋θcarの演算によって求められる傾斜角（以下、加速度傾斜角θaccと区別するためにθacc’と記す）とは、正確には一致しない。加速度傾斜角算出部１１により求められる加速度傾斜角θaccには加速度センサ１のゼロ点誤差が含まれているのに対し、θsensor＋θcarの演算によって求まる傾斜角θacc’からはゼロ点誤差が取り除かれているからである。   In the calculation formula, θsensor + θcar = θacc. However, the acceleration inclination angle θacc obtained by the acceleration inclination angle calculation unit 11 from the output value of the acceleration sensor 1 and the inclination angle obtained by the calculation of θsensor + θcar (hereinafter referred to as acceleration inclination angle θacc). Is not exactly the same as θacc '). The acceleration inclination angle θacc obtained by the acceleration inclination angle calculation unit 11 includes the zero point error of the acceleration sensor 1, whereas the zero point error is removed from the inclination angle θacc ′ obtained by the calculation of θsensor + θcar. Because.

すなわち、センサ傾斜角θsensorは、ゼロ点誤差とは無関係のＧＰＳ傾斜角θgpsを用いて計算されるので、ゼロ点誤差が含まれない正しい値となる。また、車両傾斜角θcarは、この正しい値のセンサ傾斜角θsensorを用いて計算されるので、車両傾斜角θcarも正しい値となる。したがって、θsensor＋θcarの演算によって求まる傾斜角θacc’は、加速度センサ１のゼロ点誤差の影響を受けない正しい値となる。   In other words, the sensor tilt angle θsensor is calculated using the GPS tilt angle θgps that is unrelated to the zero point error, and thus becomes a correct value that does not include the zero point error. Further, since the vehicle inclination angle θcar is calculated using this correct value of the sensor inclination angle θsensor, the vehicle inclination angle θcar is also a correct value. Therefore, the inclination angle θacc ′ obtained by the calculation of θsensor + θcar is a correct value that is not affected by the zero point error of the acceleration sensor 1.

走行距離補正部１６は、車両傾斜角算出部１４により算出された車両傾斜角θcarに基づいて、車速センサ２の出力値から計算される車両の走行距離を補正する。具体的には、補正前の走行距離をＬcar、補正後の走行距離をＬcar’とした場合、走行距離補正部１６は、Ｌcar’＝Ｌcar×cos(θcar)なる演算によって走行距離Ｌcarの補正を行う。   The travel distance correction unit 16 corrects the travel distance of the vehicle calculated from the output value of the vehicle speed sensor 2 based on the vehicle tilt angle θcar calculated by the vehicle tilt angle calculation unit 14. Specifically, when the travel distance before correction is Lcar and the travel distance after correction is Lcar ′, the travel distance correction unit 16 corrects the travel distance Lcar by the calculation of Lcar ′ = Lcar × cos (θcar). Do.

加速度傾斜角信頼性判定部２１は、車速センサ２より出力される車速Ｖcarに基づいて、加速度傾斜角算出部１１により算出される加速度傾斜角θaccの信頼性を判定する。車速Ｖcarが遅い状況では路面の振動の影響を受けやすいので、算出される加速度傾斜角θaccの信頼性が低下する。そこで、加速度傾斜角信頼性判定部２１は、例えば車速Ｖcarが１０ｋｍ／ｈ以上かどうかを判定し、１０ｋｍ／ｈ以上のときは加速度傾斜角θaccの信頼性があると判断し、１０ｋｍ／ｈより遅いときは加速度傾斜角θaccの信頼性がないと判断する。   The acceleration inclination angle reliability determination unit 21 determines the reliability of the acceleration inclination angle θacc calculated by the acceleration inclination angle calculation unit 11 based on the vehicle speed Vcar output from the vehicle speed sensor 2. In a situation where the vehicle speed Vcar is slow, it is easily affected by road surface vibration, so that the reliability of the calculated acceleration inclination angle θacc decreases. Therefore, the acceleration inclination angle reliability determination unit 21 determines, for example, whether the vehicle speed Vcar is 10 km / h or more, and determines that the acceleration inclination angle θacc is reliable when the vehicle speed Vcar is 10 km / h or more. When it is slow, it is determined that the acceleration inclination angle θacc is not reliable.

加速度傾斜角信頼性判定部２１は、加速度傾斜角θaccの信頼性があると判断したときは、内部に持つ加速度傾斜角信頼フラグをオンにする。また、加速度傾斜角θaccの信頼性がないと判断したときは、内部に持つ加速度傾斜角信頼フラグをオフにする。加速度傾斜角算出部１１は、加速度傾斜角信頼性判定部２１により信頼性ありと判定されたときにのみ、加速度傾斜角θaccを算出する。   When the acceleration inclination angle reliability determination unit 21 determines that the acceleration inclination angle θacc is reliable, the acceleration inclination angle reliability determination unit 21 turns on the acceleration inclination angle reliability flag included therein. When it is determined that the acceleration inclination angle θacc is not reliable, the acceleration inclination angle reliability flag held inside is turned off. The acceleration tilt angle calculation unit 11 calculates the acceleration tilt angle θacc only when the acceleration tilt angle reliability determination unit 21 determines that there is reliability.

ＧＰＳ傾斜角信頼性判定部２２は、ＧＰＳ受信機３による電波の受信の受信状態に基づいて、ＧＰＳ傾斜角算出部１２により算出されるＧＰＳ傾斜角θgpsの信頼性を判定する。ＧＰＳ傾斜角算出部１２は、４つ以上のＧＰＳ衛星から電波を受信できて、３次元測位処理を行うことが可能なときにのみ、ＸＹＺ方向の速度ベクトルを計算することができる。そこで、ＧＰＳ傾斜角信頼性判定部２２は、３次元測位処理を行うことが可能かどうかに応じて、それが可能でないときはＧＰＳ傾斜角θgpsの信頼性がないと判断する。ＧＰＳ傾斜角算出部１２は、ＧＰＳ傾斜角信頼性判定部２２により信頼性ありと判定されたときにのみ、ＧＰＳ傾斜角θgpsを算出する。   The GPS tilt angle reliability determination unit 22 determines the reliability of the GPS tilt angle θgps calculated by the GPS tilt angle calculation unit 12 based on the reception state of radio wave reception by the GPS receiver 3. The GPS tilt angle calculation unit 12 can calculate velocity vectors in the XYZ directions only when radio waves can be received from four or more GPS satellites and three-dimensional positioning processing can be performed. Therefore, the GPS tilt angle reliability determination unit 22 determines that the GPS tilt angle θgps is not reliable when it is not possible depending on whether or not the three-dimensional positioning process can be performed. The GPS tilt angle calculation unit 12 calculates the GPS tilt angle θgps only when the GPS tilt angle reliability determination unit 22 determines that there is reliability.

ＧＰＳ傾斜角信頼性判定部２２は、３次元測位処理を行うことが可能と判断した場合は、更に次の内容を判定する。すなわち、ＧＰＳ受信機３で複数のＧＰＳ衛星から受信する電波のドップラー効果による周波数の変化から計算した車両の速度（以下、ＧＰＳ速度Ｖgpsと称する）に基づいて、ＧＰＳ傾斜角算出部１２により算出されるＧＰＳ傾斜角θgpsの信頼性を判定する。   If the GPS tilt angle reliability determination unit 22 determines that the three-dimensional positioning process can be performed, the GPS tilt angle reliability determination unit 22 further determines the following contents. That is, it is calculated by the GPS tilt angle calculation unit 12 based on the vehicle speed (hereinafter referred to as GPS speed Vgps) calculated from the change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites by the GPS receiver 3. The reliability of the GPS tilt angle θgps is determined.

車速が遅い状況では、算出されるＧＰＳ傾斜角θgpsの精度が上がらない。また、ＧＰＳ傾斜角θgpsを求める演算式から分かるように、ＧＰＳ傾斜角θgpsを求める際には垂直速度Ｚvを水平速度√(Ｘv2＋Ｙv2)により除算する必要があるので、車速がゼロの停車時はＧＰＳ傾斜角θgpsを求めることができない。そこで、ＧＰＳ傾斜角信頼性判定部２２は、例えばＧＰＳ速度Ｖgpsが１０ｋｍ／ｈ以上かどうかを判定し、１０ｋｍ／ｈ以上のときはＧＰＳ傾斜角θgpsの信頼性があると判断し、１０ｋｍ／ｈより遅いときはＧＰＳ傾斜角θgpsの信頼性がないと判断する。 In a situation where the vehicle speed is low, the accuracy of the calculated GPS tilt angle θgps does not increase. Further, as can be seen from the calculation formula for determining the GPS tilt angle θgps, when the GPS tilt angle θgps is determined, it is necessary to divide the vertical speed Zv by the horizontal speed √ (Xv 2 + Yv 2 ). At times, the GPS tilt angle θgps cannot be obtained. Therefore, the GPS tilt angle reliability determination unit 22 determines, for example, whether the GPS speed Vgps is 10 km / h or more, and determines that the GPS tilt angle θgps is reliable when the GPS speed Vgps is 10 km / h or more. When it is later, it is determined that the GPS tilt angle θgps is not reliable.

また、ＧＰＳ傾斜角信頼性判定部２２は、ＧＰＳ傾斜角算出部１２により実際に算出されたＧＰＳ傾斜角θgpsを取得し、当該ＧＰＳ傾斜角θgpsの絶対値が所定値以下か否かでＧＰＳ傾斜角θgpsの信頼性を判定する。日本の道路には１０度より大きい傾斜は存在しない。そこで、ＧＰＳ傾斜角信頼性判定部２２は、例えばＧＰＳ傾斜角θgpsが１０度以下かどうかを判定し、１０度以下のときはＧＰＳ傾斜角θgpsの信頼性があると判断し、１０度より大きいときはＧＰＳ傾斜角θgpsの信頼性がないと判断する。   The GPS tilt angle reliability determination unit 22 acquires the GPS tilt angle θgps actually calculated by the GPS tilt angle calculation unit 12, and determines whether the GPS tilt angle θgps is equal to or less than a predetermined value. The reliability of the angle θgps is determined. There is no slope greater than 10 degrees on Japanese roads. Therefore, the GPS tilt angle reliability determination unit 22 determines, for example, whether the GPS tilt angle θgps is 10 degrees or less, and determines that the GPS tilt angle θgps is reliable when the GPS tilt angle θgps is 10 degrees or less. When it is determined that the GPS tilt angle θgps is not reliable.

さらに、ＧＰＳ傾斜角信頼性判定部２２は、ＧＰＳ傾斜角算出部１２により実際に算出されたＧＰＳ傾斜角θgpsを取得し、当該ＧＰＳ傾斜角θgpsの標準偏差が所定値以下か否かでＧＰＳ傾斜角θgpsの信頼性を判定する。例えば、ＧＰＳ傾斜角θgpsの標準偏差が３度以下であれば、ＧＰＳ傾斜角算出部１２により算出されるＧＰＳ傾斜角θgpsの値にバラツキは少ないと考えられるので、ＧＰＳ傾斜角θgpsの信頼性があると判断する。一方、標準偏差が３度より大きいときはＧＰＳ傾斜角θgpsの信頼性がないと判断する。   Further, the GPS tilt angle reliability determination unit 22 acquires the GPS tilt angle θgps actually calculated by the GPS tilt angle calculation unit 12, and determines whether the GPS tilt angle θgps has a standard deviation equal to or less than a predetermined value. The reliability of the angle θgps is determined. For example, if the standard deviation of the GPS tilt angle θgps is 3 degrees or less, it is considered that there is little variation in the value of the GPS tilt angle θgps calculated by the GPS tilt angle calculation unit 12, so the reliability of the GPS tilt angle θgps is high. Judge that there is. On the other hand, when the standard deviation is larger than 3 degrees, it is determined that the GPS tilt angle θgps is not reliable.

上述したＧＰＳ速度Ｖgps、ＧＰＳ傾斜角θgpsの絶対値、ＧＰＳ傾斜角θgpsの標準偏差を用いた３つの判定は、何れか１つのみ行っても良いし、何れか２つの組み合わせで行っても良いし、３つ全てを行っても良い。２つ以上の判定を行ったときは、その全ての条件を満たすときはＧＰＳ傾斜角θgpsの信頼性があると判断し、少なくとも１つの条件を満たさないときはＧＰＳ傾斜角θgpsの信頼性がないと判断する。   The above-described three determinations using the GPS speed Vgps, the absolute value of the GPS tilt angle θgps, and the standard deviation of the GPS tilt angle θgps may be performed in any one or in combination of any two. And you may do all three. When two or more determinations are made, it is determined that the GPS inclination angle θgps is reliable when all the conditions are satisfied, and the GPS inclination angle θgps is not reliable when at least one condition is not satisfied. Judge.

ＧＰＳ傾斜角信頼性判定部２２は、ＧＰＳ傾斜角θgpsの信頼性があると判断したときは、内部に持つＧＰＳ傾斜角信頼フラグをオンにする。また、ＧＰＳ傾斜角θgpsの信頼性がないと判断したときは、内部に持つＧＰＳ傾斜角信頼フラグをオフにする。センサ傾斜角算出部１３は、ＧＰＳ傾斜角信頼性判定部２２によりＧＰＳ傾斜角θgpsの信頼性があると判断されたとき、すなわち、ＧＰＳ傾斜角信頼フラグがオンとなっているときにのみ、センサ傾斜角θsensorを算出して車両傾斜角算出部１４に出力する。   When the GPS inclination angle reliability determination unit 22 determines that the GPS inclination angle θgps is reliable, the GPS inclination angle reliability flag is turned on. When it is determined that the GPS tilt angle θgps is not reliable, the GPS tilt angle reliability flag held inside is turned off. The sensor tilt angle calculation unit 13 detects the sensor only when the GPS tilt angle reliability determination unit 22 determines that the GPS tilt angle θgps is reliable, that is, when the GPS tilt angle reliability flag is on. The inclination angle θsensor is calculated and output to the vehicle inclination angle calculation unit 14.

または、センサ傾斜角算出部１３は、加速度傾斜角信頼性判定部２１により加速度傾斜角θaccの信頼性があると判断され、かつ、ＧＰＳ傾斜角信頼性判定部２２によりＧＰＳ傾斜角θgpsの信頼性があると判定されたとき、すなわち、加速度傾斜角信頼フラグおよびＧＰＳ傾斜角信頼フラグの両方がオンとなっているときにのみ、センサ傾斜角θsensorを算出して車両傾斜角算出部１４に出力するようにしても良い。   Alternatively, the sensor inclination angle calculation unit 13 determines that the acceleration inclination angle reliability determination unit 21 determines that the acceleration inclination angle θacc is reliable, and the GPS inclination angle reliability determination unit 22 determines the reliability of the GPS inclination angle θgps. The sensor inclination angle θsensor is calculated and output to the vehicle inclination angle calculation unit 14 only when it is determined that there is, that is, when both the acceleration inclination angle reliability flag and the GPS inclination angle reliability flag are on. You may do it.

センサ傾斜角記憶部２３は、センサ傾斜角算出部１３により算出されたセンサ傾斜角θsensorを記憶しておくためのメモリである。センサ傾斜角算出部１３は、算出したセンサ傾斜角θsensorをこのセンサ傾斜角記憶部２３に書き込んだり、必要に応じてセンサ傾斜角記憶部２３からセンサ傾斜角θsensorを読み出したりする制御機能を有している。   The sensor tilt angle storage unit 23 is a memory for storing the sensor tilt angle θsensor calculated by the sensor tilt angle calculation unit 13. The sensor tilt angle calculation unit 13 has a control function of writing the calculated sensor tilt angle θsensor into the sensor tilt angle storage unit 23 and reading the sensor tilt angle θsensor from the sensor tilt angle storage unit 23 as necessary. ing.

センサ傾斜角算出部１３は、センサ傾斜角θsensorを算出する度に、その値をセンサ傾斜角記憶部２３に記憶する。このとき、例えば古いセンサ傾斜角θsensorに新しいセンサ傾斜角θsensorを上書きすることによって、常に最新のセンサ傾斜角θsensorをセンサ傾斜角記憶部２３に記憶しておくようにする。なお、最新のセンサ傾斜角θsensorをセンサ傾斜角記憶部２３から取り出すことができるようにしておけば、必ずしも古いセンサ傾斜角θsensorを上書きにより消去する必要はない。   The sensor tilt angle calculation unit 13 stores the value in the sensor tilt angle storage unit 23 every time the sensor tilt angle θsensor is calculated. At this time, for example, the latest sensor inclination angle θsensor is always stored in the sensor inclination angle storage unit 23 by overwriting the old sensor inclination angle θsensor with the new sensor inclination angle θsensor. If the latest sensor inclination angle θsensor can be taken out from the sensor inclination angle storage unit 23, the old sensor inclination angle θsensor need not necessarily be deleted by overwriting.

また、センサ傾斜角算出部１３は、加速度傾斜角信頼性判定部２１およびＧＰＳ傾斜角信頼性判定部２２の少なくとも一方により信頼性なしと判定されたときは、センサ傾斜角記憶部２３にセンサ傾斜角θsensorの過去の算出値が記憶されていれば、当該センサ傾斜角記憶部２３に記憶されているセンサ傾斜角θsensorを読み出して車両傾斜角算出部１４に出力する。なお、ここでは加速度傾斜角θaccの信頼性とＧＰＳ傾斜角θgpsの信頼性とを両方とも判定しているが、ＧＰＳ傾斜角θgpsの信頼性判定のみとしても良い。   The sensor tilt angle calculation unit 13 stores the sensor tilt angle in the sensor tilt angle storage unit 23 when at least one of the acceleration tilt angle reliability determination unit 21 and the GPS tilt angle reliability determination unit 22 determines that there is no reliability. If the past calculated value of the angle θsensor is stored, the sensor inclination angle θsensor stored in the sensor inclination angle storage unit 23 is read and output to the vehicle inclination angle calculation unit 14. Here, both the reliability of the acceleration tilt angle θacc and the reliability of the GPS tilt angle θgps are determined, but only the reliability determination of the GPS tilt angle θgps may be performed.

センサ傾斜角信頼性判定部２４は、センサ傾斜角算出部１３よりセンサ傾斜角θsensorが出力されたか否かでセンサ傾斜角θsensorの信頼性を判定する。上述のように、センサ傾斜角算出部１３は、加速度傾斜角信頼フラグおよびＧＰＳ傾斜角信頼フラグの両方がオンとなっているときに、センサ傾斜角θsensorを算出して車両傾斜角算出部１４に出力する。また、加速度傾斜角信頼フラグおよびＧＰＳ傾斜角信頼フラグの少なくとも一方がオフとなっていても、センサ傾斜角記憶部２３にセンサ傾斜角θsensorの過去の算出値が記憶されていれば、センサ傾斜角算出部１３は、当該センサ傾斜角記憶部２３に記憶されているセンサ傾斜角θsensorを読み出して車両傾斜角算出部１４に出力する。   The sensor tilt angle reliability determination unit 24 determines the reliability of the sensor tilt angle θsensor based on whether or not the sensor tilt angle θsensor is output from the sensor tilt angle calculation unit 13. As described above, the sensor inclination angle calculation unit 13 calculates the sensor inclination angle θsensor to the vehicle inclination angle calculation unit 14 when both the acceleration inclination angle reliability flag and the GPS inclination angle reliability flag are on. Output. Even if at least one of the acceleration inclination angle reliability flag and the GPS inclination angle reliability flag is off, if the past calculated value of the sensor inclination angle θsensor is stored in the sensor inclination angle storage unit 23, the sensor inclination angle The calculation unit 13 reads the sensor inclination angle θsensor stored in the sensor inclination angle storage unit 23 and outputs the sensor inclination angle θsensor to the vehicle inclination angle calculation unit 14.

そこでセンサ傾斜角信頼性判定部２４は、センサ傾斜角算出部１３からセンサ傾斜角θsensorが出力されたときはセンサ傾斜角θsensorの信頼性があると判断し、センサ傾斜角θsensorが出力されないときはセンサ傾斜角θsensorの信頼性がないと判断する。センサ傾斜角算出部１３からセンサ傾斜角θsensorが出力されると、センサ傾斜角信頼性判定部２４は、内部に持つセンサ傾斜角信頼フラグをオンにする。一方、センサ傾斜角算出部１３からセンサ傾斜角θsensorが出力されないときは、センサ傾斜角信頼性判定部２４は、内部に持つセンサ傾斜角信頼フラグをオフにする。   Therefore, the sensor tilt angle reliability determination unit 24 determines that the sensor tilt angle θsensor is reliable when the sensor tilt angle θsensor is output from the sensor tilt angle calculation unit 13, and when the sensor tilt angle θsensor is not output. It is determined that the sensor inclination angle θsensor is not reliable. When the sensor inclination angle θsensor is output from the sensor inclination angle calculation unit 13, the sensor inclination angle reliability determination unit 24 turns on the sensor inclination angle reliability flag that is provided inside. On the other hand, when the sensor tilt angle θsensor is not output from the sensor tilt angle calculation unit 13, the sensor tilt angle reliability determination unit 24 turns off the sensor tilt angle reliability flag provided therein.

車両傾斜角算出部１４は、センサ傾斜角信頼性判定部２４によりセンサ傾斜角θsensorの信頼性がある（センサ傾斜角信頼フラグがオン）と判断されたときに、車両傾斜角θcarを算出する。または、車両傾斜角算出部１４は、加速度傾斜角信頼性判定部２１により加速度傾斜角θaccの信頼性がある（加速度傾斜角信頼フラグがオン）と判断され、かつ、センサ傾斜角信頼性判定部２４によりセンサ傾斜角θsensorの信頼性がある（センサ傾斜角信頼フラグがオン）と判断されたときに、車両傾斜角θcarを算出するようにしても良い。   The vehicle inclination angle calculation unit 14 calculates the vehicle inclination angle θcar when the sensor inclination angle reliability determination unit 24 determines that the sensor inclination angle θsensor is reliable (the sensor inclination angle reliability flag is on). Alternatively, the vehicle inclination angle calculation unit 14 determines that the acceleration inclination angle reliability determination unit 21 determines that the acceleration inclination angle θacc is reliable (the acceleration inclination angle reliability flag is on), and the sensor inclination angle reliability determination unit When it is determined that the sensor inclination angle θsensor is reliable (the sensor inclination angle reliability flag is on), the vehicle inclination angle θcar may be calculated.

車両傾斜角信頼性判定部２５は、車両傾斜角算出部１４より車両傾斜角θcarが算出されたか否かで車両傾斜角θcarの信頼性を判定する。上述のように、車両傾斜角算出部１４は、加速度傾斜角信頼フラグおよびセンサ傾斜角信頼フラグの両方がオンとなっているときに、車両傾斜角θcarを算出して出力する。車両傾斜角算出部１４から車両傾斜角θcarが出力されると、車両傾斜角信頼性判定部２５は、内部に持つ車両傾斜角信頼フラグをオンにする。一方、車両傾斜角算出部１４から車両傾斜角θcarが出力されないときは、車両傾斜角信頼性判定部２５は、内部に持つ車両傾斜角信頼フラグをオフにする。   The vehicle inclination angle reliability determination unit 25 determines the reliability of the vehicle inclination angle θcar based on whether or not the vehicle inclination angle θcar has been calculated by the vehicle inclination angle calculation unit 14. As described above, the vehicle inclination angle calculation unit 14 calculates and outputs the vehicle inclination angle θcar when both the acceleration inclination angle reliability flag and the sensor inclination angle reliability flag are on. When the vehicle inclination angle θcar is output from the vehicle inclination angle calculation unit 14, the vehicle inclination angle reliability determination unit 25 turns on the vehicle inclination angle reliability flag held therein. On the other hand, when the vehicle inclination angle θcar is not output from the vehicle inclination angle calculation unit 14, the vehicle inclination angle reliability determination unit 25 turns off the vehicle inclination angle reliability flag held therein.

角速度補正部１５は、センサ傾斜角信頼性判定部２４によりセンサ傾斜角θsensorの信頼性がある（センサ傾斜角信頼フラグがオン）と判定され、かつ、車両傾斜角信頼性判定部２５により車両傾斜角θcarの信頼性がある（車両傾斜角信頼フラグがオン）と判定されたときに、センサ傾斜角θsensorおよび車両傾斜角θcarに基づいて、ωgyro’＝ωgyro÷cos(θsensor＋θcar)なる演算によって、角速度センサ４より出力される角速度ωgyroを補正する。   The angular velocity correction unit 15 determines that the sensor inclination angle reliability determination unit 24 determines that the sensor inclination angle θsensor is reliable (the sensor inclination angle reliability flag is on), and the vehicle inclination angle reliability determination unit 25 determines the vehicle inclination. When it is determined that the angle θcar is reliable (the vehicle inclination angle reliability flag is on), the angular velocity is calculated based on the sensor inclination angle θsensor and the vehicle inclination angle θcar by calculating ωgyro '= ωgyro ÷ cos (θsensor + θcar). The angular velocity ωgyro output from the sensor 4 is corrected.

また、角速度補正部１５は、センサ傾斜角信頼性判定部２４によりセンサ傾斜角θsensorの信頼性があると判定されているものの、車両傾斜角信頼性判定部２５により車両傾斜角θcarの信頼性がないと判定されたときには、センサ傾斜角θsensorに基づいて、ωgyro’＝ωgyro÷cos(θsensor)なる演算によって、角速度センサ４より出力される角速度ωgyroを補正する。なお、センサ傾斜角θsensorおよび車両傾斜角θcarの両方とも信頼性がないと判定されたときは、角速度補正部１５は、角速度ωgyroを補正せずにそのまま出力する。   In addition, although the angular velocity correction unit 15 is determined to have the reliability of the sensor inclination angle θsensor by the sensor inclination angle reliability determination unit 24, the reliability of the vehicle inclination angle θcar is determined by the vehicle inclination angle reliability determination unit 25. If it is determined that there is no angular velocity, the angular velocity ωgyro output from the angular velocity sensor 4 is corrected based on the sensor inclination angle θsensor by the calculation of ωgyro ′ = ωgyro ÷ cos (θsensor). When it is determined that both the sensor inclination angle θsensor and the vehicle inclination angle θcar are not reliable, the angular velocity correction unit 15 outputs the angular velocity ωgyro as it is without correction.

走行距離補正部１６は、車両傾斜角信頼性判定部２５により車両傾斜角θcarの信頼性がある（車両傾斜角信頼フラグがオン）と判定されたときに、車両傾斜角θcarに基づいて、Ｌcar’＝Ｌcar×cos(θcar)なる演算によって車両の走行距離Ｌcarの補正を行う。なお、車両傾斜角θcarの信頼性がないと判定されたときは、走行距離補正部１６は、車両の走行距離Ｌcarを補正せずにそのまま出力する。   When the vehicle inclination angle reliability determination unit 25 determines that the vehicle inclination angle θcar is reliable (the vehicle inclination angle reliability flag is on), the mileage correction unit 16 determines Lcar based on the vehicle inclination angle θcar. The travel distance Lcar of the vehicle is corrected by the calculation of '= Lcar × cos (θcar). When it is determined that the vehicle inclination angle θcar is not reliable, the travel distance correction unit 16 outputs the travel distance Lcar of the vehicle without correction.

次に、上記のように構成した本実施形態による角速度補正装置の動作を説明する。図３は、本実施形態による角速度補正装置の全体の動作例を示すフローチャートである。図３において、加速度傾斜角算出部１１は、加速度センサ１より出力される加速度Ａacc、車速センサ２より出力される車速Ｖcarの変化量から求められる加速度Ａcarおよび重力加速度Ｇに基づいて、加速度傾斜角θaccを算出する（ステップＳ１）。   Next, the operation of the angular velocity correcting device according to the present embodiment configured as described above will be described. FIG. 3 is a flowchart showing an example of the overall operation of the angular velocity correction apparatus according to the present embodiment. In FIG. 3, the acceleration tilt angle calculation unit 11 is based on the acceleration Aacc output from the acceleration sensor 1, the acceleration Acar obtained from the change amount of the vehicle speed Vcar output from the vehicle speed sensor 2, and the gravitational acceleration G. θacc is calculated (step S1).

また、ＧＰＳ傾斜角算出部１２は、ＧＰＳ受信機３で複数のＧＰＳ衛星から受信する電波のドップラー効果による周波数の変化に基づいて、ＧＰＳ傾斜角θgpsを算出する（ステップＳ２）。さらに、センサ傾斜角算出部１３は、上記ステップＳ１，Ｓ２で求めた加速度傾斜角θaccおよびＧＰＳ傾斜角θgpsに基づいて、センサ傾斜角θsensorを算出する（ステップＳ３）。   Further, the GPS tilt angle calculation unit 12 calculates the GPS tilt angle θgps based on the change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites by the GPS receiver 3 (step S2). Further, the sensor inclination angle calculation unit 13 calculates the sensor inclination angle θsensor based on the acceleration inclination angle θacc and the GPS inclination angle θgps obtained in steps S1 and S2 (step S3).

さらに、車両傾斜角算出部１４は、ステップＳ１，Ｓ３で求めた加速度傾斜角θaccおよびセンサ傾斜角θsensorに基づいて、車両傾斜角θcarを算出する（ステップＳ４）。そして、走行距離補正部１６は、ステップＳ４で求めた車両傾斜角θcarに基づいて、車両の走行距離Ｌcarを補正する（ステップＳ５）。また、角速度補正部１５は、ステップＳ３で求めたセンサ傾斜角θsensorおよびステップＳ４で求めた車両傾斜角θcarに基づいて、角速度センサ４より出力される角速度ωgyroを補正する（ステップＳ６）。   Further, the vehicle inclination angle calculation unit 14 calculates the vehicle inclination angle θcar based on the acceleration inclination angle θacc and the sensor inclination angle θsensor obtained in steps S1 and S3 (step S4). Then, the travel distance correction unit 16 corrects the travel distance Lcar of the vehicle based on the vehicle inclination angle θcar obtained in step S4 (step S5). Further, the angular velocity correction unit 15 corrects the angular velocity ωgyro output from the angular velocity sensor 4 based on the sensor inclination angle θsensor obtained in step S3 and the vehicle inclination angle θcar obtained in step S4 (step S6).

図４は、上記ステップＳ１における加速度傾斜角θaccの算出手順を示すフローチャートである。図４において、加速度傾斜角信頼性判定部２１は、車速センサ２より出力される車速Ｖcarが１０ｋｍ／ｈ以上かどうかを判定する（ステップＳ１１）。ここで、車速Ｖcarが１０ｋｍ／ｈ以上のときは、これから算出する加速度傾斜角θaccの信頼性があると判断し、１０ｋｍ／ｈより遅いときは加速度傾斜角θaccの信頼性がないと判断する。   FIG. 4 is a flowchart showing a procedure for calculating the acceleration inclination angle θacc in step S1. In FIG. 4, the acceleration inclination angle reliability determination unit 21 determines whether or not the vehicle speed Vcar output from the vehicle speed sensor 2 is 10 km / h or more (step S11). Here, when the vehicle speed Vcar is 10 km / h or more, it is determined that the acceleration inclination angle θacc calculated from now is reliable, and when it is slower than 10 km / h, it is determined that the acceleration inclination angle θacc is not reliable.

加速度傾斜角θaccの信頼性があると判断されると、加速度傾斜角算出部１１は、θacc＝sin-1{(Ａcar−Ａacc)／Ｇ}なる演算によって加速度傾斜角θaccを算出する（ステップＳ１２）。そして、加速度傾斜角信頼性判定部２１は、内部に持つ加速度傾斜角信頼フラグをオンにする（ステップＳ１３）。一方、上記ステップＳ１１にて加速度傾斜角θaccの信頼性がないと判断されたときは、加速度傾斜角算出部１１による加速度傾斜角θaccの算出は行わない。この場合、加速度傾斜角信頼性判定部２１は、内部に持つ加速度傾斜角信頼フラグをオフにする（ステップＳ１４）。 If it is determined that the acceleration inclination angle θacc is reliable, the acceleration inclination angle calculation unit 11 calculates the acceleration inclination angle θacc by calculation of θacc = sin −1 {(Acar−Aacc) / G} (step S12). ). And the acceleration inclination angle reliability determination part 21 turns on the acceleration inclination angle reliability flag which it has inside (step S13). On the other hand, when it is determined in step S11 that the acceleration tilt angle θacc is not reliable, the acceleration tilt angle calculation unit 11 does not calculate the acceleration tilt angle θacc. In this case, the acceleration inclination angle reliability determination unit 21 turns off the acceleration inclination angle reliability flag held therein (step S14).

図５は、図３のステップＳ２におけるＧＰＳ傾斜角θgpsの算出手順を示すフローチャートである。図５において、まずＧＰＳ傾斜角信頼性判定部２２は、ＧＰＳ受信機３による電波の受信の受信状態に基づいて、３次元測位処理を行うことが可能かどうかを判定する（ステップＳ２１）。ここで、３次元測位処理を行うことが可能と判断した場合、ＧＰＳ傾斜角算出部１２は、θgps＝tan-1{Ｚv／√(Ｘv2＋Ｙv2)}なる演算によってＧＰＳ傾斜角θgpsを算出する（ステップＳ２２）。 FIG. 5 is a flowchart showing a procedure for calculating the GPS tilt angle θgps in step S2 of FIG. In FIG. 5, first, the GPS tilt angle reliability determination unit 22 determines whether or not the three-dimensional positioning process can be performed based on the reception state of reception of radio waves by the GPS receiver 3 (step S21). Here, when it is determined that the three-dimensional positioning process can be performed, the GPS tilt angle calculation unit 12 calculates the GPS tilt angle θgps by a calculation of θgps = tan −1 {Zv / √ (Xv 2 + Yv 2 )}. (Step S22).

次に、ＧＰＳ傾斜角信頼性判定部２２は、算出したＧＰＳ傾斜角θgpsの履歴がＮ個（例えば、Ｎ＝５）たまったかどうかを判定する（ステップＳ２３）。この判定は、ＧＰＳ傾斜角θgpsの標準偏差を算出するのに必要なサンプル数がたまったかどうかの判定である。ここで、ＧＰＳ傾斜角θgpsの履歴がＮ個たまったと判断した場合、続いてＧＰＳ傾斜角信頼性判定部２２は、次の３つの条件を全て満たすかどうか判定する（ステップＳ２４）。
１．ＧＰＳ速度Ｖgpsが１０ｋｍ／ｈ以上であること
２．算出したＧＰＳ傾斜角θgpsの絶対値が１０度以下であること
３．算出したＧＰＳ傾斜角θgpsの標準偏差が３度以下であること Next, the GPS tilt angle reliability determination unit 22 determines whether N (for example, N = 5) histories of the calculated GPS tilt angles θgps have accumulated (step S23). This determination is a determination as to whether or not the number of samples necessary to calculate the standard deviation of the GPS tilt angle θgps has been accumulated. Here, when it is determined that the history of the GPS tilt angle θgps has accumulated, the GPS tilt angle reliability determination unit 22 determines whether or not all of the following three conditions are satisfied (step S24).
1. 1. GPS speed Vgps is 10 km / h or more. 2. The absolute value of the calculated GPS tilt angle θgps is 10 degrees or less. The standard deviation of the calculated GPS tilt angle θgps is 3 degrees or less

ＧＰＳ傾斜角信頼性判定部２２は、上述の３つの条件を全て満たすと判断した場合に、ＧＰＳ傾斜角θgpsの信頼性があるとして、内部に持つＧＰＳ傾斜角信頼フラグをオンにする（ステップＳ２５）。一方、ＧＰＳ傾斜角信頼性判定部２２は、ステップＳ２１で３次元測位処理ができないと判断した場合、ステップＳ２３でＧＰＳ傾斜角θgpsの履歴数がＮ個に満たないと判断した場合、またはステップＳ２４で３つの条件のうち少なくとも１つを満たさないと判断した場合には、内部に持つＧＰＳ傾斜角信頼フラグをオフにする（ステップＳ２６）。   If it is determined that the above three conditions are all satisfied, the GPS tilt angle reliability determination unit 22 turns on the GPS tilt angle reliability flag included in the GPS tilt angle θgps (step S25). ). On the other hand, the GPS tilt angle reliability determination unit 22 determines that the three-dimensional positioning process cannot be performed in step S21, determines that the history number of GPS tilt angles θgps is less than N in step S23, or step S24. If it is determined that at least one of the three conditions is not satisfied, the GPS tilt angle reliability flag held inside is turned off (step S26).

図６は、図３のステップＳ３におけるセンサ傾斜角θsensorの算出手順を示すフローチャートである。図６において、センサ傾斜角算出部１３は、加速度傾斜角信頼フラグとＧＰＳ傾斜角信頼フラグとが共にオンになっているか否かを判定する（ステップＳ３１）。これら２つのフラグが共にオンになっていると判断された場合、センサ傾斜角算出部１３は、θsensor＝θacc−θgpsなる演算によってセンサ傾斜角θsensorを算出する（ステップＳ３２）。   FIG. 6 is a flowchart showing a calculation procedure of the sensor tilt angle θsensor in step S3 of FIG. In FIG. 6, the sensor inclination angle calculation unit 13 determines whether or not both the acceleration inclination angle reliability flag and the GPS inclination angle reliability flag are on (step S31). When it is determined that both of these two flags are on, the sensor inclination angle calculation unit 13 calculates the sensor inclination angle θsensor by the calculation θsensor = θacc−θgps (step S32).

そして、センサ傾斜角算出部１３は、算出したセンサ傾斜角θsensorをセンサ傾斜角記憶部２３に記録する（ステップＳ３３）。また、センサ傾斜角信頼性判定部２４は、センサ傾斜角θsensorの信頼性があるとして、内部に持つセンサ傾斜角信頼フラグをオンにする（ステップＳ３４）。   Then, the sensor tilt angle calculation unit 13 records the calculated sensor tilt angle θsensor in the sensor tilt angle storage unit 23 (step S33). Further, the sensor inclination angle reliability determination unit 24 turns on the sensor inclination angle reliability flag held inside, assuming that the sensor inclination angle θsensor is reliable (step S34).

一方、上記ステップＳ３１で、加速度傾斜角信頼フラグおよびＧＰＳ傾斜角信頼フラグの少なくとも一方がオフになっていると判断された場合、センサ傾斜角算出部１３は、センサ傾斜角記憶部２３にセンサ傾斜角θsensorが格納されているかどうかを見ることによって、過去に一度でもセンサ傾斜角θsensorが算出されたかどうかを判定する（ステップＳ３５）。   On the other hand, if it is determined in step S31 that at least one of the acceleration tilt angle reliability flag and the GPS tilt angle reliability flag is off, the sensor tilt angle calculation unit 13 stores the sensor tilt angle in the sensor tilt angle storage unit 23. By determining whether or not the angle θsensor is stored, it is determined whether or not the sensor inclination angle θsensor has been calculated even once in the past (step S35).

ここで、センサ傾斜角記憶部２３にセンサ傾斜角θsensorが記憶されている場合、センサ傾斜角算出部１３は、センサ傾斜角記憶部２３からセンサ傾斜角θsensorを読み出して車両傾斜角算出部１４に出力する（ステップＳ３６）。そして、センサ傾斜角信頼性判定部２４は、センサ傾斜角θsensorの信頼性があるとして、内部に持つセンサ傾斜角信頼フラグをオンにする（ステップＳ３４）。一方、センサ傾斜角記憶部２３にセンサ傾斜角θsensorが記憶されていない場合、センサ傾斜角信頼性判定部２４は、センサ傾斜角θsensorの信頼性がないとして、センサ傾斜角信頼フラグをオフにする（ステップＳ３７）。   Here, when the sensor inclination angle θsensor is stored in the sensor inclination angle storage unit 23, the sensor inclination angle calculation unit 13 reads the sensor inclination angle θsensor from the sensor inclination angle storage unit 23 and stores it in the vehicle inclination angle calculation unit 14. Output (step S36). Then, the sensor inclination angle reliability determination unit 24 turns on the sensor inclination angle reliability flag that is held inside, assuming that the sensor inclination angle θsensor is reliable (step S34). On the other hand, when the sensor tilt angle θsensor is not stored in the sensor tilt angle storage unit 23, the sensor tilt angle reliability determination unit 24 turns off the sensor tilt angle reliability flag on the assumption that the sensor tilt angle θsensor is not reliable. (Step S37).

図７は、図３のステップＳ４における車両傾斜角θcarの算出手順を示すフローチャートである。図７において、車両傾斜角算出部１４は、加速度傾斜角信頼フラグとセンサ傾斜角信頼フラグとが共にオンになっているか否かを判定する（ステップＳ４１）。これら２つのフラグが共にオンになっていると判断された場合、車両傾斜角算出部１４は、θcar＝θacc−θsensorなる演算によって車両傾斜角θcarを算出する（ステップＳ４２）。   FIG. 7 is a flowchart showing a procedure for calculating the vehicle inclination angle θcar in step S4 of FIG. In FIG. 7, the vehicle inclination angle calculation unit 14 determines whether or not both the acceleration inclination angle reliability flag and the sensor inclination angle reliability flag are on (step S41). If it is determined that both of these two flags are on, the vehicle inclination angle calculation unit 14 calculates the vehicle inclination angle θcar by the calculation θcar = θacc−θsensor (step S42).

そして、車両傾斜角信頼性判定部２５は、車両傾斜角θcarの信頼性があるとして、内部に持つ車両傾斜角信頼フラグをオンにする（ステップＳ４３）。一方、上記ステップＳ４１で、加速度傾斜角信頼フラグおよびセンサ傾斜角信頼フラグの少なくとも一方がオフになっていると判断された場合、車両傾斜角信頼性判定部２５は、車両傾斜角θcarの信頼性がないとして、車両傾斜角信頼フラグをオフにする（ステップＳ４４）。   Then, the vehicle inclination angle reliability determination unit 25 turns on the vehicle inclination angle reliability flag held inside, assuming that the vehicle inclination angle θcar is reliable (step S43). On the other hand, if it is determined in step S41 that at least one of the acceleration inclination angle reliability flag and the sensor inclination angle reliability flag is off, the vehicle inclination angle reliability determination unit 25 determines the reliability of the vehicle inclination angle θcar. If there is not, the vehicle inclination angle reliability flag is turned off (step S44).

図８は、図３のステップＳ５における走行距離Ｌcarの補正手順を示すフローチャートである。図８において、走行距離補正部１６は、車両傾斜角信頼フラグがオンになっているか否かを判定する（ステップＳ５１）。そして、車両傾斜角フラグがオンになっていると判断された場合、走行距離補正部１６は、Ｌcar’＝Ｌcar×cos(θcar)なる演算によって走行距離Ｌcarの補正を行う（ステップＳ５２）。一方、車両傾斜角フラグがオフになっていると判断された場合、走行距離補正部１６は、走行距離Ｌcarの補正は行わず（Ｌcar’＝Ｌcar）、そのまま出力する（ステップＳ５３）。   FIG. 8 is a flowchart showing a procedure for correcting the travel distance Lcar in step S5 of FIG. In FIG. 8, the travel distance correction unit 16 determines whether or not the vehicle inclination angle reliability flag is turned on (step S51). If it is determined that the vehicle tilt angle flag is on, the travel distance correction unit 16 corrects the travel distance Lcar by a calculation of Lcar ′ = Lcar × cos (θcar) (step S52). On the other hand, when it is determined that the vehicle tilt angle flag is off, the travel distance correction unit 16 does not correct the travel distance Lcar (Lcar '= Lcar) and outputs it as it is (step S53).

図９は、図３のステップＳ６における角速度ωgyroの補正手順を示すフローチャートである。図９において、角速度補正部１５は、センサ斜角信頼フラグがオンになっているか否かを判定する（ステップＳ６１）。センサ傾斜角信頼フラグがオンの場合は更に、車両傾斜角フラグがオンになってか否かを判定する（ステップＳ６２）。   FIG. 9 is a flowchart showing a procedure for correcting the angular velocity ωgyro in step S6 of FIG. In FIG. 9, the angular velocity correction unit 15 determines whether or not the sensor oblique angle reliability flag is turned on (step S61). If the sensor inclination angle reliability flag is on, it is further determined whether or not the vehicle inclination angle flag is on (step S62).

そして、センサ斜角信頼フラグと車両傾斜角フラグとの両方がオンになっていると判断された場合、角速度補正部１５は、ωgyro’＝ωgyro÷cos(θsensor＋θcar)なる演算によって角速度ωgyroの補正を行う（ステップＳ６３）。また、センサ斜角信頼フラグがオンであるものの、車両傾斜角フラグがオフになっている判断された場合、角速度補正部１５は、ωgyro’＝ωgyro÷cos(θsensor)なる演算によって角速度ωgyroの補正を行う（ステップＳ６４）。さらに、センサ斜角信頼フラグと車両傾斜角フラグとの両方ともオフになっていると判断された場合、角速度補正部１５は、角速度ωgyroの補正は行わず（ωgyro’＝ωgyro）、そのまま出力する（ステップＳ６５）。   When it is determined that both the sensor oblique angle reliability flag and the vehicle inclination angle flag are turned on, the angular velocity correction unit 15 corrects the angular velocity ωgyro by calculating ωgyro ′ = ωgyro ÷ cos (θsensor + θcar). This is performed (step S63). If it is determined that the sensor oblique angle reliability flag is on but the vehicle inclination angle flag is off, the angular velocity correction unit 15 corrects the angular velocity ωgyro by calculating ωgyro ′ = ωgyro ÷ cos (θsensor). Is performed (step S64). Further, when it is determined that both the sensor oblique angle reliability flag and the vehicle inclination angle flag are off, the angular velocity correction unit 15 does not correct the angular velocity ωgyro (ωgyro ′ = ωgyro) and outputs it as it is. (Step S65).

以上詳しく説明したように、本実施形態では、加速度傾斜角θaccおよびＧＰＳ傾斜角θgpsからセンサ傾斜角θsensorと車両傾斜角θcarとを算出し、この２つの傾斜角θsensor，θcarに基づいて、角速度センサ４より出力される角速度ωgyroを補正するようにしている。   As described above in detail, in this embodiment, the sensor inclination angle θsensor and the vehicle inclination angle θcar are calculated from the acceleration inclination angle θacc and the GPS inclination angle θgps, and the angular velocity sensor is based on the two inclination angles θsensor and θcar. The angular velocity ωgyro output from 4 is corrected.

また、加速度センサ１の出力値を使用して算出される加速度傾斜角θaccには加速度センサ１のゼロ点誤差が含まれているが、本実施形態ではこれをそのまま用いて角速度ωgyroの補正をしていない。すなわち、センサ傾斜角θsensorと車両傾斜角θcarとに分解して計算し、これら２つの傾斜角θsensor，θcarに基づいて角速度ωgyroの補正を行っている。   In addition, the acceleration tilt angle θacc calculated using the output value of the acceleration sensor 1 includes the zero point error of the acceleration sensor 1, but in this embodiment, this is used as it is to correct the angular velocity ωgyro. Not. That is, the sensor inclination angle θsensor and the vehicle inclination angle θcar are decomposed and calculated, and the angular velocity ωgyro is corrected based on these two inclination angles θsensor and θcar.

このときセンサ傾斜角θsensorは、ゼロ点誤差や感度誤差とは無関係のＧＰＳ傾斜角θgpsを用いて計算されるので、ゼロ点誤差が含まれない正しいセンサ傾斜角θsensorと車両傾斜角θcarとを求めることができる。これにより、車両が傾いているときでも、角速度センサ４の感度誤差と同時に加速度センサ１のゼロ点誤差も補正することができる。   At this time, since the sensor inclination angle θsensor is calculated using the GPS inclination angle θgps that is not related to the zero point error or the sensitivity error, the correct sensor inclination angle θsensor and the vehicle inclination angle θcar that do not include the zero point error are obtained. be able to. Thereby, even when the vehicle is tilted, the zero point error of the acceleration sensor 1 can be corrected simultaneously with the sensitivity error of the angular velocity sensor 4.

さらに、角速度センサ４の感度誤差と加速度センサ１のゼロ点誤差とを別々のアルゴリズムで補正する必要がなく、同じ演算の中でこれらを同時に補正することができる。しかも、いわゆるカルマンフィルタような逐次計算を行わなくて良いので、簡単なアルゴリズムで感度補正とゼロ点補正を行うことができる。   Furthermore, it is not necessary to correct the sensitivity error of the angular velocity sensor 4 and the zero point error of the acceleration sensor 1 with separate algorithms, and these can be corrected simultaneously in the same calculation. In addition, since it is not necessary to perform a sequential calculation such as a so-called Kalman filter, sensitivity correction and zero point correction can be performed with a simple algorithm.

また、本実施形態では、ＧＰＳ傾斜角θgpsの信頼性を判定し、信頼性がないと判定された場合には、新たにセンサ傾斜角θsensorを算出せずに、センサ傾斜角記憶部２３に記憶されている過去のセンサ傾斜角θsensorを使用して角速度ωgyroを補正するようにしている。これにより、ＧＰＳ電波を受信できない環境においても角速度ωgyroの補正を行うことができる。しかも、この過去のセンサ傾斜角θsensorは、ＧＰＳ傾斜角θgpsに信頼性があると判定されたときに計算されたものであり、上述のように加速度センサ１のゼロ点誤差が取り除かれた正しい値である。よって、ＧＰＳ電波を受信できない環境でも角速度ωgyroを正確に補正することができる。   In the present embodiment, the reliability of the GPS tilt angle θgps is determined. If it is determined that the GPS tilt angle θgps is not reliable, the sensor tilt angle θsensor is not newly calculated and stored in the sensor tilt angle storage unit 23. The past sensor inclination angle θsensor is used to correct the angular velocity ωgyro. Thereby, the angular velocity ωgyro can be corrected even in an environment where GPS radio waves cannot be received. In addition, the past sensor tilt angle θsensor is calculated when it is determined that the GPS tilt angle θgps is reliable, and is a correct value from which the zero point error of the acceleration sensor 1 is removed as described above. It is. Therefore, the angular velocity ωgyro can be accurately corrected even in an environment where GPS radio waves cannot be received.

また、本実施形態では、角速度ωgyroの補正だけでなく、車両傾斜角θcarに基づいて車両の走行距離Ｌcarも補正している。このため、車両の方位のみならず、走行距離も正確に補正することができる。   In this embodiment, not only the correction of the angular velocity ωgyro but also the travel distance Lcar of the vehicle is corrected based on the vehicle inclination angle θcar. For this reason, not only the direction of the vehicle but also the travel distance can be accurately corrected.

以上のことから、本実施形態によれば、傾斜が大きく、ＧＰＳ電波を受信できない立体駐車場内においても自律航法精度を向上させることができ、自車位置の精度が格段に向上する。これにより、立体駐車場脱出後の自車位置が正確になり、正しい経路案内を素早く開始することができるようになる。   From the above, according to the present embodiment, the accuracy of autonomous navigation can be improved even in a multi-story parking lot that has a large inclination and cannot receive GPS radio waves, and the accuracy of the position of the vehicle is significantly improved. Thereby, the own vehicle position after exiting the multi-story parking lot becomes accurate, and correct route guidance can be started quickly.

図１０は、立体駐車場内および立体駐車場脱出後の自車位置を示す図であり、（ａ）は従来技術を適用した場合の例、（ｂ）は本実施形態を適用した場合の例を示している。図１０（ａ）に示す従来例の場合は、ＧＰＳ電波が受信できない駐車場内では角速度ωgyroの補正ができず、車両の走行距離Ｌcarの補正も行っていないため、駐車場内で方位ずれと位置ずれが発生している。そのため、駐車場脱出時にも方位ずれと位置ずれが存在し、車両が実際に存在する道路上に正しくマップマッチングすることができない。その状態で車両が走行を続けるため、マップマッチングミスによる位置ずれがしばらく続き、正しい道路上に自車位置が復帰するまでに多くの時間を要してしまう。   FIG. 10 is a diagram illustrating the position of the host vehicle in the multi-story parking lot and after exiting the multi-story parking lot, where (a) is an example when the conventional technology is applied, and (b) is an example when the present embodiment is applied. Show. In the case of the conventional example shown in FIG. 10A, the angular velocity ωgyro cannot be corrected in the parking lot where GPS radio waves cannot be received, and the vehicle travel distance Lcar is not corrected. Has occurred. For this reason, there are azimuth and positional deviations even when the parking lot is escaped, and map matching cannot be performed correctly on the road where the vehicle actually exists. Since the vehicle continues to run in this state, a position shift due to a map matching error continues for a while, and it takes a lot of time for the vehicle position to return to the correct road.

これに対して、図１０（ｂ）に示す本実施形態の場合は、ＧＰＳ電波が受信できない駐車場内でも角速度ωgyroの補正ができ、車両走行距離Ｌcarの補正も行っているため、駐車場内で方位ずれと位置ずれが殆ど発生しない。駐車場内ではセンサ傾斜角記憶部２３に記憶されている過去のセンサ傾斜角θsensorを使用して角速度ωgyroを補正するようにしているが、ここで使用する過去のセンサ傾斜角θsensorは駐車場に入る直前に算出された精度の高い値である。そのため、駐車場脱出時にも方位ずれと位置ずれは殆どなく、車両が実際に存在する道路上に正しくマップマッチングすることができる。よって、正しい道路上に自車位置が復帰するまでの時間が極めて短くなる。   On the other hand, in the case of the present embodiment shown in FIG. 10B, the angular velocity ωgyro can be corrected even in a parking lot where GPS radio waves cannot be received, and the vehicle travel distance Lcar is also corrected. There is almost no displacement or misalignment. In the parking lot, the past sensor inclination angle θsensor stored in the sensor inclination angle storage unit 23 is used to correct the angular velocity ωgyro, but the past sensor inclination angle θsensor used here enters the parking lot. It is a highly accurate value calculated immediately before. Therefore, there is almost no azimuth shift and position shift even when the parking lot is escaped, and map matching can be correctly performed on the road where the vehicle actually exists. Therefore, the time until the vehicle position returns on the correct road is extremely short.

また、上記実施形態によれば、加速度傾斜角θaccとＧＰＳ傾斜角θgpsとが同時に１回でも信頼性があると判定されれば、高精度なセンサ傾斜角θsensorと車両傾斜角θcarとを得ることができる。これにより、市販製品のナビゲーション装置などにおいて、ナビゲーション装置の車両への取付状態（水平方向からどの程度傾いて設置されているか）が分からないときでも、水平方向に対する取付角が正確に求まり、傾斜補正に利用することができる。   In addition, according to the above embodiment, if it is determined that the acceleration inclination angle θacc and the GPS inclination angle θgps are reliable at the same time, the highly accurate sensor inclination angle θsensor and the vehicle inclination angle θcar are obtained. Can do. This makes it possible to accurately determine the mounting angle with respect to the horizontal direction even when the navigation device of a commercial product, etc., does not know the mounting state of the navigation device to the vehicle (how much it is installed with respect to the horizontal direction). Can be used.

なお、上記実施形態は、何れも本発明を実施するにあたっての具体化の一例を示したものに過ぎず、これによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその精神、またはその主要な特徴から逸脱することなく、様々な形で実施することができる。   The above-described embodiments are merely examples of implementation in carrying out the present invention, and the technical scope of the present invention should not be interpreted in a limited manner. In other words, the present invention can be implemented in various forms without departing from the spirit or main features thereof.

本実施形態による角速度補正装置を適用したナビゲーション装置の要部構成例を示すブロック図である。It is a block diagram which shows the principal part structural example of the navigation apparatus to which the angular velocity correction apparatus by this embodiment is applied. 本実施形態で用いる各傾斜角の説明図である。It is explanatory drawing of each inclination | tilt angle used by this embodiment. 本実施形態による角速度補正装置の全体の動作例を示すフローチャートである。It is a flowchart which shows the operation example of the whole angular velocity correction apparatus by this embodiment. 図３のステップＳ１における加速度傾斜角の算出手順を示すフローチャートである。It is a flowchart which shows the calculation procedure of the acceleration inclination angle in step S1 of FIG. 図３のステップＳ２におけるＧＰＳ傾斜角の算出手順を示すフローチャートである。It is a flowchart which shows the calculation procedure of the GPS inclination angle in step S2 of FIG. 図３のステップＳ３におけるセンサ傾斜角の算出手順を示すフローチャートである。It is a flowchart which shows the calculation procedure of the sensor inclination angle in step S3 of FIG. 図３のステップＳ４における車両傾斜角の算出手順を示すフローチャートである。It is a flowchart which shows the calculation procedure of the vehicle inclination angle in step S4 of FIG. 図３のステップＳ５における車両走行距離の補正手順を示すフローチャートである。It is a flowchart which shows the correction | amendment procedure of the vehicle travel distance in step S5 of FIG. 図３のステップＳ６における角速度の補正手順を示すフローチャートである。It is a flowchart which shows the correction procedure of the angular velocity in step S6 of FIG. 本実施形態による角速度補正装置の効果を説明するための図で、立体駐車場内および立体駐車場脱出後の自車位置を示す図である。It is a figure for demonstrating the effect of the angular velocity correction apparatus by this embodiment, and is a figure which shows the own vehicle position in a multistory parking lot and after a multistory parking lot escape.

符号の説明Explanation of symbols

１ 加速度センサ
２ 車速センサ
３ ＧＰＳ受信機
４ 角速度センサ
１１ 加速度傾斜角算出部
１２ ＧＰＳ傾斜角算出部
１３ センサ傾斜角算出部
１４ 車両傾斜角算出部
１５ 角速度補正部
１６ 走行距離補正部
２１ 加速度傾斜角信頼性判定部
２２ ＧＰＳ傾斜角信頼性判定部
２３ センサ傾斜角記憶部
２４ センサ傾斜角信頼性判定部
２５ 車両傾斜角信頼性判定部 DESCRIPTION OF SYMBOLS 1 Acceleration sensor 2 Vehicle speed sensor 3 GPS receiver 4 Angular velocity sensor 11 Acceleration inclination angle calculation part 12 GPS inclination angle calculation part 13 Sensor inclination angle calculation part 14 Vehicle inclination angle calculation part 15 Angular speed correction part 16 Travel distance correction part 21 Acceleration inclination angle Reliability determination unit 22 GPS tilt angle reliability determination unit 23 Sensor tilt angle storage unit 24 Sensor tilt angle reliability determination unit 25 Vehicle tilt angle reliability determination unit

Claims (19)

ＧＰＳ受信機と角速度センサと加速度センサと車速センサとを備え、これらの出力値を用いて車両の位置および方位を検出する機能を備えたナビゲーション装置であって、上記角速度センサと上記加速度センサとが同じ傾斜角で車両内に設置されており、
上記加速度センサより出力される加速度、上記車速センサより出力される車速の変化量から求められる加速度および重力加速度に基づいて、水平方向に対する上記加速度センサの傾斜角を表した加速度傾斜角を算出する加速度傾斜角算出部と、
上記ＧＰＳ受信機で複数のＧＰＳ衛星から受信する電波のドップラー効果による周波数の変化に基づいて、水平方向に対する車両の傾斜角を表したＧＰＳ傾斜角を算出するＧＰＳ傾斜角算出部と、
上記加速度傾斜角および上記ＧＰＳ傾斜角に基づいて、上記車両の走行方向に対する上記角速度センサの傾斜角を表したセンサ傾斜角を算出するセンサ傾斜角算出部と、
上記加速度傾斜角および上記センサ傾斜角に基づいて、水平方向に対する上記車両の傾斜角を表した車両傾斜角を算出する車両傾斜角算出部と、
上記センサ傾斜角および上記車両傾斜角に基づいて、上記角速度センサより出力される角速度を補正する角速度補正部とを備えたことを特徴とするナビゲーション装置。 A navigation device comprising a GPS receiver, an angular velocity sensor, an acceleration sensor, and a vehicle speed sensor, and having a function of detecting the position and orientation of the vehicle using these output values, wherein the angular velocity sensor and the acceleration sensor are Installed in the vehicle at the same angle of inclination,
Acceleration for calculating an acceleration tilt angle representing the tilt angle of the acceleration sensor with respect to the horizontal direction based on acceleration output from the acceleration sensor, acceleration obtained from the amount of change in vehicle speed output from the vehicle speed sensor, and gravitational acceleration An inclination angle calculation unit;
A GPS tilt angle calculation unit that calculates a GPS tilt angle representing the tilt angle of the vehicle with respect to the horizontal direction based on a change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites by the GPS receiver;
A sensor inclination angle calculation unit for calculating a sensor inclination angle representing an inclination angle of the angular velocity sensor with respect to a traveling direction of the vehicle based on the acceleration inclination angle and the GPS inclination angle;
A vehicle inclination angle calculation unit for calculating a vehicle inclination angle representing the inclination angle of the vehicle with respect to a horizontal direction based on the acceleration inclination angle and the sensor inclination angle;
A navigation apparatus comprising: an angular velocity correction unit that corrects an angular velocity output from the angular velocity sensor based on the sensor inclination angle and the vehicle inclination angle. 上記車両傾斜角に基づいて、上記車速センサの出力値から計算される車両の走行距離を補正する走行距離補正部を更に備えたことを特徴とする請求項１に記載のナビゲーション装置。 The navigation apparatus according to claim 1, further comprising a travel distance correction unit that corrects a travel distance of the vehicle calculated from an output value of the vehicle speed sensor based on the vehicle inclination angle. 上記車速センサより出力される車速に基づいて、上記加速度傾斜角算出部により算出される上記加速度傾斜角の信頼性を判定する加速度傾斜角信頼性判定部を備え、
上記加速度傾斜角算出部は、上記加速度傾斜角信頼性判定部により信頼性ありと判定されたときに、上記加速度傾斜角を算出することを特徴とする請求項１に記載のナビゲーション装置。 An acceleration tilt angle reliability determination unit that determines the reliability of the acceleration tilt angle calculated by the acceleration tilt angle calculation unit based on the vehicle speed output from the vehicle speed sensor;
The navigation apparatus according to claim 1, wherein the acceleration inclination angle calculation unit calculates the acceleration inclination angle when the acceleration inclination angle reliability determination unit determines that the acceleration inclination angle is reliable. 上記ＧＰＳ受信機による電波の受信の受信状態に基づいて、上記ＧＰＳ傾斜角算出部により算出される上記ＧＰＳ傾斜角の信頼性を判定するＧＰＳ傾斜角信頼性判定部を備え、
上記ＧＰＳ傾斜角算出部は、上記ＧＰＳ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記ＧＰＳ傾斜角を算出することを特徴とする請求項１に記載のナビゲーション装置。 A GPS tilt angle reliability determination unit that determines the reliability of the GPS tilt angle calculated by the GPS tilt angle calculation unit based on a reception state of radio wave reception by the GPS receiver;
The navigation device according to claim 1, wherein the GPS tilt angle calculation unit calculates the GPS tilt angle when the GPS tilt angle reliability determination unit determines that there is reliability. 上記ＧＰＳ受信機で複数のＧＰＳ衛星から受信する電波のドップラー効果による周波数の変化から計算した上記車両の速度に基づいて、上記ＧＰＳ傾斜角算出部により算出される上記ＧＰＳ傾斜角の信頼性を判定するＧＰＳ傾斜角信頼性判定部を備え、
上記センサ傾斜角算出部は、上記ＧＰＳ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記センサ傾斜角を算出することを特徴とする請求項１に記載のナビゲーション装置。 The reliability of the GPS tilt angle calculated by the GPS tilt angle calculation unit is determined based on the speed of the vehicle calculated from the change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites by the GPS receiver. A GPS tilt angle reliability determination unit
The navigation device according to claim 1, wherein the sensor inclination angle calculation unit calculates the sensor inclination angle when the GPS inclination angle reliability determination unit determines that the sensor inclination is reliable. 上記ＧＰＳ傾斜角が所定値以下か否かで上記ＧＰＳ傾斜角の信頼性を判定するＧＰＳ傾斜角信頼性判定部を備え、
上記センサ傾斜角算出部は、上記ＧＰＳ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記センサ傾斜角を算出することを特徴とする請求項１に記載のナビゲーション装置。 A GPS tilt angle reliability determining unit that determines the reliability of the GPS tilt angle based on whether the GPS tilt angle is a predetermined value or less;
The navigation device according to claim 1, wherein the sensor inclination angle calculation unit calculates the sensor inclination angle when the GPS inclination angle reliability determination unit determines that the sensor inclination is reliable. 上記ＧＰＳ傾斜角の標準偏差が所定値以下か否かで上記ＧＰＳ傾斜角の信頼性を判定するＧＰＳ傾斜角信頼性判定部を備え、
上記センサ傾斜角算出部は、上記ＧＰＳ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記センサ傾斜角を算出することを特徴とする請求項１に記載のナビゲーション装置。 A GPS tilt angle reliability determining unit that determines the reliability of the GPS tilt angle based on whether the standard deviation of the GPS tilt angle is a predetermined value or less;
The navigation device according to claim 1, wherein the sensor inclination angle calculation unit calculates the sensor inclination angle when the GPS inclination angle reliability determination unit determines that the sensor inclination is reliable. 上記車速センサより出力される車速に基づいて、上記加速度傾斜角算出部により算出される上記加速度傾斜角の信頼性を判定する加速度傾斜角信頼性判定部を備え、
上記センサ傾斜角算出部は、上記加速度傾斜角信頼性判定部により信頼性ありと判定され、かつ、上記ＧＰＳ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記センサ傾斜角を算出することを特徴とする請求項４〜７の何れか１項に記載のナビゲーション装置。 An acceleration tilt angle reliability determination unit that determines the reliability of the acceleration tilt angle calculated by the acceleration tilt angle calculation unit based on the vehicle speed output from the vehicle speed sensor;
The sensor tilt angle calculation unit determines the sensor tilt angle when the acceleration tilt angle reliability determination unit determines that the sensor tilt angle is reliable and the GPS tilt angle reliability determination unit determines that the sensor tilt angle is reliable. The navigation device according to claim 4, wherein the navigation device calculates the navigation device. 上記センサ傾斜角算出部により算出された上記センサ傾斜角を記憶しておくセンサ傾斜角記憶部を備え、
上記センサ傾斜角算出部は、上記ＧＰＳ傾斜角信頼性判定部により信頼性なしと判定され、かつ、上記センサ傾斜角記憶部に上記センサ傾斜角の過去の算出値が記憶されているときに、上記センサ傾斜角記憶部に記憶されている上記センサ傾斜角を出力することを特徴とする請求項８に記載のナビゲーション装置。 A sensor inclination angle storage unit for storing the sensor inclination angle calculated by the sensor inclination angle calculation unit;
The sensor tilt angle calculation unit is determined to be unreliable by the GPS tilt angle reliability determination unit, and a past calculated value of the sensor tilt angle is stored in the sensor tilt angle storage unit. 9. The navigation apparatus according to claim 8, wherein the sensor tilt angle stored in the sensor tilt angle storage unit is output. 上記センサ傾斜角算出部により算出された上記センサ傾斜角を記憶しておくセンサ傾斜角記憶部を備え、
上記センサ傾斜角算出部は、上記加速度傾斜角信頼性判定部および上記ＧＰＳ傾斜角信頼性判定部の少なくとも一方により信頼性なしと判定され、かつ、上記センサ傾斜角記憶部に上記センサ傾斜角の過去の算出値が記憶されているときに、上記センサ傾斜角記憶部に記憶されている上記センサ傾斜角を出力することを特徴とする請求項８に記載のナビゲーション装置。 A sensor inclination angle storage unit for storing the sensor inclination angle calculated by the sensor inclination angle calculation unit;
The sensor tilt angle calculation unit determines that there is no reliability by at least one of the acceleration tilt angle reliability determination unit and the GPS tilt angle reliability determination unit, and stores the sensor tilt angle in the sensor tilt angle storage unit. 9. The navigation apparatus according to claim 8, wherein when the past calculated value is stored, the sensor tilt angle stored in the sensor tilt angle storage unit is output. 上記センサ傾斜角算出部より上記センサ傾斜角が出力されたか否かで上記センサ傾斜角の信頼性を判定するセンサ傾斜角信頼性判定部を備え、
上記車両傾斜角算出部は、上記センサ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記車両傾斜角を算出することを特徴とする請求項９または１０に記載のナビゲーション装置。 A sensor tilt angle reliability determination unit that determines the reliability of the sensor tilt angle based on whether the sensor tilt angle is output from the sensor tilt angle calculation unit;
The navigation device according to claim 9 or 10, wherein the vehicle inclination angle calculation unit calculates the vehicle inclination angle when the sensor inclination angle reliability determination unit determines that the vehicle inclination is reliable. 上記車両傾斜角算出部は、上記加速度傾斜角信頼性判定部により信頼性ありと判定され、かつ、上記センサ傾斜角信頼性判定部により信頼性ありと判定されたときに、上記車両傾斜角を算出することを特徴とする請求項１１に記載のナビゲーション装置。 The vehicle inclination angle calculation unit determines the vehicle inclination angle when the acceleration inclination angle reliability determination unit determines that there is reliability and the sensor inclination angle reliability determination unit determines that there is reliability. The navigation device according to claim 11, wherein the navigation device calculates. 上記車両傾斜角算出部より上記車両傾斜角が算出されたか否かで上記車両傾斜角の信頼性を判定する車両傾斜角信頼性判定部を備え、
上記角速度補正部は、上記センサ傾斜角信頼性判定部により信頼性ありと判定され、かつ、上記車両傾斜角信頼性判定部により信頼性ありと判定されたときに、上記センサ傾斜角および上記車両傾斜角に基づいて、上記角速度センサより出力される角速度を補正することを特徴とする請求項１１に記載のナビゲーション装置。 A vehicle inclination angle reliability determination unit for determining reliability of the vehicle inclination angle based on whether or not the vehicle inclination angle is calculated by the vehicle inclination angle calculation unit;
When the sensor inclination angle reliability determination unit determines that the angular velocity correction unit is reliable and the vehicle inclination angle reliability determination unit determines that there is reliability, the sensor inclination angle and the vehicle The navigation device according to claim 11, wherein the angular velocity output from the angular velocity sensor is corrected based on an inclination angle. 上記角速度補正部は、上記センサ傾斜角信頼性判定部により信頼性ありと判定され、かつ、上記車両傾斜角信頼性判定部により信頼性なしと判定されたときに、上記センサ傾斜角に基づいて、上記角速度センサより出力される角速度を補正することを特徴とする請求項１１に記載のナビゲーション装置。 The angular velocity correction unit is determined based on the sensor inclination angle when the sensor inclination angle reliability determination unit determines that there is reliability and the vehicle inclination angle reliability determination unit determines that there is no reliability. The navigation device according to claim 11, wherein the angular velocity output from the angular velocity sensor is corrected. 加速度センサより出力される加速度、車速センサより出力される車速の変化量から求められる加速度および重力加速度に基づいて、水平方向に対する上記加速度センサの傾斜角を表した加速度傾斜角を算出する加速度傾斜角算出部と、
ＧＰＳ受信機で複数のＧＰＳ衛星から受信する電波のドップラー効果による周波数の変化に基づいて、水平方向に対する車両の傾斜角を表したＧＰＳ傾斜角を算出するＧＰＳ傾斜角算出部と、
上記加速度傾斜角および上記ＧＰＳ傾斜角に基づいて、上記加速度センサと同じ傾斜角で車両内に設置された角速度センサの、上記車両の走行方向に対する傾斜角を表したセンサ傾斜角を算出するセンサ傾斜角算出部と、
上記加速度傾斜角および上記センサ傾斜角に基づいて、水平方向に対する上記車両の傾斜角を表した車両傾斜角を算出する車両傾斜角算出部と、
上記センサ傾斜角および上記車両傾斜角に基づいて、上記角速度センサより出力される角速度を補正する角速度補正部とを備えたことを特徴とする傾斜角を用いた角速度補正装置。 Based on the acceleration output from the acceleration sensor, the acceleration obtained from the change in the vehicle speed output from the vehicle speed sensor, and the gravitational acceleration, the acceleration inclination angle that calculates the inclination angle of the acceleration sensor with respect to the horizontal direction is calculated. A calculation unit;
A GPS tilt angle calculation unit that calculates a GPS tilt angle representing a tilt angle of the vehicle with respect to the horizontal direction, based on a change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites by a GPS receiver;
Based on the acceleration inclination angle and the GPS inclination angle, a sensor inclination that calculates a sensor inclination angle representing an inclination angle with respect to the traveling direction of the vehicle of an angular velocity sensor installed in the vehicle at the same inclination angle as the acceleration sensor. An angle calculator,
A vehicle inclination angle calculation unit for calculating a vehicle inclination angle representing the inclination angle of the vehicle with respect to a horizontal direction based on the acceleration inclination angle and the sensor inclination angle;
An angular velocity correction apparatus using an inclination angle, comprising: an angular velocity correction unit that corrects an angular velocity output from the angular velocity sensor based on the sensor inclination angle and the vehicle inclination angle. 上記センサ傾斜角算出部により算出された上記センサ傾斜角を記憶しておくセンサ傾斜角記憶部と、
上記ＧＰＳ傾斜角算出部により算出される上記ＧＰＳ傾斜角の信頼性を判定するＧＰＳ傾斜角信頼性判定部とを備え、
上記センサ傾斜角算出部は、上記ＧＰＳ傾斜角信頼性判定部により信頼性なしと判定され、かつ、上記センサ傾斜角記憶部に上記センサ傾斜角の過去の算出値が記憶されているときに、上記センサ傾斜角記憶部に記憶されている上記センサ傾斜角を出力することを特徴とする請求項１５に記載の傾斜角を用いた角速度補正装置。 A sensor inclination angle storage unit for storing the sensor inclination angle calculated by the sensor inclination angle calculation unit;
A GPS tilt angle reliability determination unit that determines the reliability of the GPS tilt angle calculated by the GPS tilt angle calculation unit;
The sensor tilt angle calculation unit is determined to be unreliable by the GPS tilt angle reliability determination unit, and a past calculated value of the sensor tilt angle is stored in the sensor tilt angle storage unit. 16. The angular velocity correction apparatus using an inclination angle according to claim 15, wherein the sensor inclination angle stored in the sensor inclination angle storage unit is output. 上記センサ傾斜角算出部により算出された上記センサ傾斜角を記憶しておくセンサ傾斜角記憶部と、
上記加速度傾斜角算出部により算出される上記加速度傾斜角の信頼性を判定する加速度傾斜角信頼性判定部と、
上記ＧＰＳ傾斜角算出部により算出される上記ＧＰＳ傾斜角の信頼性を判定するＧＰＳ傾斜角信頼性判定部とを備え、
上記センサ傾斜角算出部は、上記加速度傾斜角信頼性判定部および上記ＧＰＳ傾斜角信頼性判定部の少なくとも一方により信頼性なしと判定され、かつ、上記センサ傾斜角記憶部に上記センサ傾斜角の過去の算出値が記憶されているときに、上記センサ傾斜角記憶部に記憶されている上記センサ傾斜角を出力することを特徴とする請求項１５に記載の傾斜角を用いた角速度補正装置。 A sensor inclination angle storage unit for storing the sensor inclination angle calculated by the sensor inclination angle calculation unit;
An acceleration tilt angle reliability determination unit that determines the reliability of the acceleration tilt angle calculated by the acceleration tilt angle calculation unit;
A GPS tilt angle reliability determination unit that determines the reliability of the GPS tilt angle calculated by the GPS tilt angle calculation unit;
The sensor tilt angle calculation unit determines that there is no reliability by at least one of the acceleration tilt angle reliability determination unit and the GPS tilt angle reliability determination unit, and stores the sensor tilt angle in the sensor tilt angle storage unit. 16. The angular velocity correction apparatus using an inclination angle according to claim 15, wherein when the past calculated value is stored, the sensor inclination angle stored in the sensor inclination angle storage unit is output. 加速度センサより出力される加速度、車速センサより出力される車速の変化量から求められる加速度および重力加速度に基づいて、水平方向に対する上記加速度センサの傾斜角を表した加速度傾斜角を算出する第１のステップと、
ＧＰＳ受信機で複数のＧＰＳ衛星から受信する電波のドップラー効果による周波数の変化に基づいて、水平方向に対する車両の傾斜角を表したＧＰＳ傾斜角を算出する第２のステップと、
上記加速度傾斜角および上記ＧＰＳ傾斜角に基づいて、上記加速度センサと同じ傾斜角で車両内に設置された角速度センサの、上記車両の走行方向に対する傾斜角を表したセンサ傾斜角を算出する第３のステップと、
上記加速度傾斜角および上記センサ傾斜角に基づいて、水平方向に対する上記車両の傾斜角を表した車両傾斜角を算出する第４のステップと、
上記上記センサ傾斜角および上記車両傾斜角に基づいて、上記角速度センサより出力される角速度を補正する第５のステップとを有することを特徴とする傾斜角を用いた角速度補正方法。 A first acceleration inclination angle representing an inclination angle of the acceleration sensor with respect to the horizontal direction is calculated based on an acceleration output from the acceleration sensor, an acceleration obtained from a change amount of the vehicle speed output from the vehicle speed sensor, and a gravitational acceleration. Steps,
A second step of calculating a GPS inclination angle representing the inclination angle of the vehicle with respect to the horizontal direction based on a change in frequency due to the Doppler effect of radio waves received from a plurality of GPS satellites by a GPS receiver;
Based on the acceleration inclination angle and the GPS inclination angle, a sensor inclination angle representing an inclination angle of the angular velocity sensor installed in the vehicle at the same inclination angle as the acceleration sensor with respect to the traveling direction of the vehicle is calculated. And the steps
A fourth step of calculating a vehicle inclination angle representing the inclination angle of the vehicle with respect to a horizontal direction based on the acceleration inclination angle and the sensor inclination angle;
An angular velocity correction method using an inclination angle, comprising: a fifth step of correcting an angular velocity output from the angular velocity sensor based on the sensor inclination angle and the vehicle inclination angle. 上記第３のステップは、上記第１のステップで算出される上記加速度傾斜角の信頼性を判定する第６のステップと、
上記第２のステップで算出される上記ＧＰＳ傾斜角の信頼性を判定する第７のステップと、
上記加速度傾斜角および上記ＧＰＳ傾斜角の双方とも信頼性ありと判定されたときに、上記センサ傾斜角を算出する第８のステップと、
上記第８のステップで計算された上記センサ傾斜角をセンサ傾斜角記憶部に記憶する第９のステップと、
上記加速度傾斜角および上記ＧＰＳ傾斜角の少なくとも一方について信頼性なしと判定され、かつ、上記センサ傾斜角記憶部に上記センサ傾斜角の過去の算出値が記憶されているときに、上記センサ傾斜角記憶部に記憶されている上記センサ傾斜角を出力する第１０のステップとを有することを特徴とする請求項１８に記載の傾斜角を用いた角速度補正方法。 The third step includes a sixth step of determining reliability of the acceleration inclination angle calculated in the first step,
A seventh step of determining the reliability of the GPS tilt angle calculated in the second step;
An eighth step of calculating the sensor tilt angle when it is determined that both the acceleration tilt angle and the GPS tilt angle are reliable;
A ninth step of storing the sensor tilt angle calculated in the eighth step in a sensor tilt angle storage unit;
When it is determined that at least one of the acceleration tilt angle and the GPS tilt angle is unreliable and a past calculated value of the sensor tilt angle is stored in the sensor tilt angle storage unit, the sensor tilt angle The angular velocity correction method using the tilt angle according to claim 18, further comprising: a tenth step of outputting the sensor tilt angle stored in the storage unit.
JP2007058131A 2007-03-08 2007-03-08 Navigation apparatus, angular velocity correction apparatus and method using tilt angle Active JP4776570B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2007058131A JP4776570B2 (en) 2007-03-08 2007-03-08 Navigation apparatus, angular velocity correction apparatus and method using tilt angle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2007058131A JP4776570B2 (en) 2007-03-08 2007-03-08 Navigation apparatus, angular velocity correction apparatus and method using tilt angle

Publications (2)

Publication Number Publication Date
JP2008216226A JP2008216226A (en) 2008-09-18
JP4776570B2 true JP4776570B2 (en) 2011-09-21

Family

ID=39836417

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2007058131A Active JP4776570B2 (en) 2007-03-08 2007-03-08 Navigation apparatus, angular velocity correction apparatus and method using tilt angle

Country Status (1)

Country Link
JP (1) JP4776570B2 (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9864064B2 (en) 2012-06-27 2018-01-09 Mitsubishi Electric Corporation Positioning device
US10267920B2 (en) 2012-06-27 2019-04-23 Mitsubishi Electric Corporation Positioning method
US10452075B2 (en) 2016-11-30 2019-10-22 Samsung Electronics Co., Ltd. Method and apparatus for generating autonomous driving route

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5057952B2 (en) 2007-12-06 2012-10-24 アルパイン株式会社 Angular velocity correction device, correction method thereof, and navigation device
JP5115655B2 (en) 2009-05-18 2013-01-09 トヨタ自動車株式会社 Vehicle control device
JP5224063B2 (en) * 2009-06-18 2013-07-03 三菱自動車工業株式会社 Driving assistance device
WO2012050471A1 (en) * 2010-10-11 2012-04-19 General Electric Company Systems, methods, and apparatus for detecting irregular sensor signal noise
WO2012050474A1 (en) 2010-10-11 2012-04-19 General Electric Company Systems, methods, and apparatus for detecting shifts in redundant sensor signals
JP2013044562A (en) * 2011-08-22 2013-03-04 Ono Sokki Co Ltd Road surface gradient calculation device and method
JP6217504B2 (en) * 2014-04-18 2017-10-25 井関農機株式会社 Work information recording device
JP6883236B2 (en) * 2016-09-23 2021-06-09 株式会社Ｊｖｃケンウッド Mounting angle calculation device, mounting angle evaluation device, mounting angle calculation method and mounting angle evaluation method
US11427211B2 (en) * 2018-06-18 2022-08-30 Baidu Usa Llc Methods for handling sensor failures in autonomous driving vehicles
CN114802054B (en) * 2022-04-25 2023-06-13 九江精密测试技术研究所 Control system applied to vehicle-mounted high-precision linear velocity excitation device

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9864064B2 (en) 2012-06-27 2018-01-09 Mitsubishi Electric Corporation Positioning device
US10267920B2 (en) 2012-06-27 2019-04-23 Mitsubishi Electric Corporation Positioning method
US10452075B2 (en) 2016-11-30 2019-10-22 Samsung Electronics Co., Ltd. Method and apparatus for generating autonomous driving route

Also Published As

Publication number Publication date
JP2008216226A (en) 2008-09-18

Similar Documents

Publication Publication Date Title
JP4776570B2 (en) Navigation apparatus, angular velocity correction apparatus and method using tilt angle
JP5057952B2 (en) Angular velocity correction device, correction method thereof, and navigation device
JP4466705B2 (en) Navigation device
US7561961B2 (en) Vehicle position estimating apparatus and vehicle position estimating method
US20090063051A1 (en) Method And Apparatus Of Updating Vehicle Position And Orientation
JPH0518774A (en) Vehicle position-azimuth computing device
JP2009025012A (en) Acceleration sensor offset correction apparatus and method
JP2007093541A (en) Navigation device and vehicle position estimating method
JP2007155584A (en) Inertia navigation system
JP2011013204A (en) Inertial navigation system including error correction function based on navigation map
JP4931113B2 (en) Own vehicle position determination device
JP3868416B2 (en) Angular velocity correction device
JP4063149B2 (en) Vehicle angular velocity detection device
JP4848931B2 (en) Signal correction device for angular velocity sensor
JP2001194169A (en) Vehicle attitude angle detecting apparatus
JP4376738B2 (en) Apparatus and method for detecting zero point error of angular velocity sensor
US20110126647A1 (en) Rate of turn signal generator with drift compensation
JP2577160B2 (en) Vehicle position detection device
JP4884109B2 (en) Moving locus calculation method, moving locus calculation device, and map data generation method
JP2006153714A (en) Map matching method of car navigation device and car navigation system
JP2001264091A (en) Navigation system
JP3251705B2 (en) Vehicle position correction method
JP2011112591A (en) Apparatus for loading mobile body
JP3210483B2 (en) Vehicle position correction method
JP4454442B2 (en) Sensitivity error detection device for angular velocity sensor

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20090929

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20101126

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20110601

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

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20110628

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

Free format text: JAPANESE INTERMEDIATE CODE: A01

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20110628

R150 Certificate of patent or registration of utility model

Free format text: JAPANESE INTERMEDIATE CODE: R150

Ref document number: 4776570

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

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

Free format text: PAYMENT UNTIL: 20140708

Year of fee payment: 3