JP3884742B2

JP3884742B2 - Compass

Info

Publication number: JP3884742B2
Application number: JP2004098835A
Authority: JP
Inventors: 雄一増田
Original assignee: Japan Aviation Electronics Industry Ltd
Current assignee: Japan Aviation Electronics Industry Ltd
Priority date: 2004-03-30
Filing date: 2004-03-30
Publication date: 2007-02-21
Anticipated expiration: 2024-03-30
Also published as: JP2005283385A

Description

本発明は方位計に関し、特に車両、穴掘用掘進機などの装置に搭載され、ジャイロおよび加速度計を用いて、該車両または他の装置等の進行方位角を計測する方位計に関する。 The present invention relates to an azimuth meter, and more particularly, to an azimuth meter mounted on a device such as a vehicle or a digging machine for drilling, and using a gyroscope and an accelerometer to measure a traveling azimuth angle of the vehicle or other device.

従来、ジャイロおよび加速度計を用いて方位角を計測する方位計としては、特許文献１（特公平７−１５３８４）に、ジャイロコンパシング装置として実現された北探知システムが開示されている。このシステムにおける、ジャイロと加速度計より方位角を計測する主要機能ブロック図を図５に示す。図５に示されるように、Ｘ軸またはＹ軸の何れか一方の１軸に配置された１個のジャイロ３４と、Ｘ軸およびＹ軸の２軸に配置された加速度計３５および３６と、座標変換手段３７と、ジャイロ読取補償手段３８と、緯度推定手段３９と、切替器４０と、機首角度計算手段４１とを備えて構成されている。 Conventionally, as an azimuth meter that measures an azimuth angle using a gyro and an accelerometer, Patent Document 1 (Japanese Patent Publication No. 7-15384) discloses a north detection system that is realized as a gyro compacting device. FIG. 5 shows a main functional block diagram for measuring the azimuth angle from the gyro and the accelerometer in this system. As shown in FIG. 5, one gyro 34 arranged on one of the X axis and the Y axis, accelerometers 35 and 36 arranged on the two axes of the X axis and the Y axis, A coordinate conversion means 37, a gyro reading compensation means 38, a latitude estimation means 39, a switch 40, and a nose angle calculation means 41 are provided.

また特許文献２（特開平６−１１３４９）の方位計には、方位測定の時間短縮ならびに測定悪条件に対応できる方位計が開示されている。この従来例における、ジャイロと加速度計より方位角を計測する主要機能ブロック図は、図６に示されるように、Ｘ軸およびＹ軸に配置されたジャイロ４２および４３と、Ｘ軸およびＹ軸に配置された加速度計４４および４５と、重力加速度鉛直成分検出手段４６と、座標変換手段４７と、地球自転角速度鉛直成分検出手段４８と、方位角演算手段４９と、姿勢角演算手段５０とを備えて構成される。 Further, the compass of Patent Document 2 (Japanese Patent Laid-Open No. 6-11349) discloses a compass that can shorten the time of the azimuth measurement and can cope with bad measurement conditions. The main functional block diagram for measuring the azimuth angle from the gyro and the accelerometer in this conventional example is as shown in FIG. 6 in which the gyros 42 and 43 arranged on the X axis and the Y axis, and the X axis and the Y axis Included accelerometers 44 and 45, gravitational acceleration vertical component detection means 46, coordinate conversion means 47, earth rotation angular velocity vertical component detection means 48, azimuth angle calculation means 49, and attitude angle calculation means 50 are provided. Configured.

更に、特許文献３（特開平５−１８７８７３）には、あらゆる方向の被測定孔の傾斜角および方位角を、同時に、且つセンサ部を停止させることなく短時間にて計測することを目的とした、孔曲がり計測装置および孔曲がり計測方法が開示されている、この従来例における、ジャイロと加速度計より方位角を計測する主要機能ブロック図は、図７に示されるように、Ｘ軸、Ｙ軸およびＺ軸に配置された加速度計５１、５２および５３と、Ｘ軸、Ｙ軸およびＺ軸に配置されたにジャイロ５４、５５および５６と、第１の演算手段５７と、第２の演算手段５８と、第３の演算手段５９と、距離計６０とを備えて構成される。 Further, Patent Document 3 (Japanese Patent Laid-Open No. 5-187873) aims to measure the tilt angle and azimuth angle of the hole to be measured in all directions at the same time and without stopping the sensor unit. In this conventional example, a hole bending measuring device and a hole bending measuring method are disclosed. A main functional block diagram for measuring an azimuth angle from a gyroscope and an accelerometer is shown in FIG. And accelerometers 51, 52 and 53 arranged on the Z axis, gyroscopes 54, 55 and 56 arranged on the X axis, Y axis and Z axis, a first computing means 57, and a second computing means 58, a third computing means 59, and a distance meter 60.

他の例として、非特許文献１[日本航空宇宙工業会発行の技術書「スキューリダンダント・イナーシャルシステムの研究」（発行日：１９８５年３月），ISSN0910-1500]においては、ジャイロと加速度計より方位角を計測する主要機能ブロック図として、図８に示されるように、Ｘ軸、Ｙ軸およびＺ軸に配置されたジャイロ６１、６２および６３と、Ｘ軸、Ｙ軸およびＺ軸に配置された加速度計６４、６５および６６と、水平変換器６７と、速度ループ６８と、方位演算器６９とを備えたブロック図が示されている。 As another example, in Non-Patent Document 1 [Technical document published by Japan Aerospace Industry Association "Study on Skew Redundant Inertial System" (issue date: March 1985), ISSN0910-1500], from gyroscope and accelerometer As shown in FIG. 8, as main functional block diagrams for measuring azimuth angles, gyros 61, 62 and 63 arranged on the X, Y and Z axes, and arranged on the X, Y and Z axes. A block diagram with accelerometers 64, 65 and 66, a horizontal converter 67, a velocity loop 68, and an azimuth calculator 69 is shown.

特公平７−１５３８４Japanese Patent Publication No. 7-15384 特開平６−１１３４９JP-A-6-11349 特開平５−１８７８７３JP-A-5-187873 日本航空宇宙工業会発行の技術書「スキューリダンダント・イナーシャルシステムの研究」（発行日：１９８５年３月）， ISSN0910-1500Technical document published by the Japan Aerospace Industry Association "Study on Skew Redundant Inertial System" (issue date: March 1985), ISSN0910-1500

上述した特許文献１の方位計においては、リングレーザージャイロを２軸分設けているために、ランダムドリフトの影響を抑制する時間が1軸分に比して２倍の時間を必要とし、また入力軸を２方向に指向させるための可動部を要するために、耐環境性/信頼性に乏しいという欠点があり、加速度計出力の微分値については、特に高周波領域における振動入力等の外乱による影響を大きく受けやすく、このために角速度補正の誤差が生じ易いという欠点も存在している。また、特許文献２の方位計においては、方位計に動揺が入力されると、ベクトル値が15度/Ｈよりも大きくなるような場合には、方位計算に誤差が生じるという問題があり、特許文献３の孔曲がり計測装置および孔曲がり計測方法においては、３軸の動きに対しては誤差を持たずに孔曲がりの計測を行うことは可能であるが、ジャイロおよび加速度計をそれぞれ３個づつ設けることを必要とし、コストアップとなり高価格になるという欠点がある。更に非特許文献１においても同様に、３軸の動きに対して誤差を持たずに方位角を計測することはできるが、ジャイロと加速度計をそれぞれ３個づつ設けることが必要となり、コストアップとなって高価格になるという欠点がある。 In the azimuth meter of Patent Document 1 described above, since the ring laser gyro is provided for two axes, the time for suppressing the influence of random drift requires twice as long as that for one axis, and input is required. Since it requires a moving part to direct the axis in two directions, it has the disadvantage of poor environmental resistance / reliability, and the differential value of the accelerometer output is affected by disturbances such as vibration input especially in the high frequency range. There is also a drawback that the angular velocity correction error is likely to occur. In addition, the compass of Patent Document 2 has a problem that if the motion is input to the compass, if the vector value is larger than 15 degrees / H, an error occurs in the compass calculation. In the hole bending measuring device and the hole bending measuring method of Document 3, it is possible to measure the hole bending without error with respect to the movement of the three axes, but three gyroscopes and three accelerometers are provided. There is a drawback that it is necessary to provide it, which increases the cost and increases the price. Furthermore, similarly in Non-Patent Document 1, it is possible to measure the azimuth angle without error with respect to the movement of the three axes, but it is necessary to provide three gyroscopes and three accelerometers, which increases the cost. There is a drawback of becoming expensive.

本発明の目的は、上記の従来例の課題を解決して可動部分を除去して耐環境性/信頼性の向上を図り、装置の姿勢角変化による影響ならびに２軸に対する高周波数領域等における振動の外乱による影響を排除して、精度高い方位計測を可能にするとともに、地球自転の鉛直成分算定に際しては、ベクトル値が15度/Ｈよりも大きくなるような状態においても計算誤差を生じることなく、且つジャイロと加速度計をそれぞれ２軸のみに配置することにより、価格の低減を実現することのできる方位計を提供することにある。 The object of the present invention is to solve the above-mentioned problems of the conventional example, improve the environmental resistance / reliability by removing the movable part, and influences due to the change in the attitude angle of the apparatus, as well as vibrations in the high frequency range with respect to two axes. This eliminates the effects of external disturbances and enables high-precision azimuth measurement, while calculating the vertical component of the Earth's rotation without causing a calculation error even when the vector value is greater than 15 degrees / H. And it is providing the compass which can implement | achieve price reduction by arrange | positioning a gyro and an accelerometer to each of only two axes.

前述の課題を解決するために本発明は次の手段を提供する。 In order to solve the above-mentioned problems, the present invention provides the following means.

（１）互いに直交するＸb,Ｙb及びＺbでなる直交３軸座標系のＸb軸およびＹb軸にジャイロ及び加速度計をそれぞれ１個ずつ配置し、方位角ψを計測する方位計において、
Ｚ角速度推定器、水平変換器、速度ループ及び方位演算器を備えてなり、
前記Ｚ角速度推定器は、前記速度ループから受けるロール角φおよびピッチ角θ、並びに初期値として入力されている緯度 λに基づき、前記Ｚb軸の角速度推定値ωzb’を演算し、
前記水平変換器は、前記角速度ωxbおよびωybと、前記角速度推定値ωzb’と、ロール角φおよびピッチ角θの入力を受けて、該角速度ωxbおよびωybを、地球水平直交２軸のＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLに変換し、
前記速度ループは、前記加速度計のＸb軸の加速度ＡxbおよびＹb軸の加速度Ａybと、前記地球水平直交２軸のＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLの入力を受けて、負帰還ループを介して前記ロール角φおよびピッチ角θの更新を行い、前記Ｚ角速度推定器ならびに水平変換器に帰還するとともに、角速度推定値ωxL’および角速度推定値ωyL’を生成し、
前記方位演算器は、前記速度ループより出力される角速度推定値ωxL’および角速度推定値ωyL’の入力を受けて、これら角速度推定値ωxL’およびωyL’の比に対応する逆正接関数を用いて方位角ψを推定する演算をし、
前記Ｚ角速度推定器、水平変換器および速度ループを含む総合帰還ループを介して、前記速度ループより出力される角速度推定値ωxL’と角速度推定値ωyL’とを逐次更新し、該総合帰還ループの収束時に最終的に該速度ループより出力される角速度推定値ωxL’と角速度推定値ωyL’とを用いて、方位演算器において方位角ψを演算することを特徴とする方位計。 (1) An azimuth meter that measures an azimuth angle ψ by arranging one gyro and one accelerometer on the Xb axis and the Yb axis of an orthogonal triaxial coordinate system composed of Xb, Yb, and Zb orthogonal to each other.
Comprising a Z angular velocity estimator, horizontal converter, velocity loop and azimuth calculator,
The Z angular velocity estimator calculates the angular velocity estimated value ωzb ′ of the Zb axis based on the roll angle φ and pitch angle θ received from the velocity loop and the latitude λ input as an initial value,
The horizontal converter receives the angular velocities ωxb and ωyb, the angular velocity estimation value ωzb ′, the roll angle φ and the pitch angle θ, and converts the angular velocities ωxb and ωyb into two horizontal X-axis horizontal axes that are orthogonal to the earth. Convert to angular velocity ωxL and Y-axis horizontal angular velocity ωyL,
The velocity loop receives an input of the Xb-axis acceleration Axb and Yb-axis acceleration Ayb of the accelerometer, the X-axis horizontal angular velocity ωxL and the Y-axis horizontal angular velocity ωyL of the earth horizontal orthogonal two axes, and performs a negative feedback loop. The roll angle φ and the pitch angle θ are updated via the Z angle velocity estimator and the horizontal converter, and the angular velocity estimated value ωxL ′ and the angular velocity estimated value ωyL ′ are generated.
The azimuth calculator receives an angular velocity estimation value ωxL ′ and an angular velocity estimation value ωyL ′ output from the velocity loop, and uses an arctangent function corresponding to a ratio of the angular velocity estimation values ωxL ′ and ωyL ′. Calculate the azimuth angle ψ,
Through an overall feedback loop including the Z angular velocity estimator, a horizontal converter and a velocity loop, the angular velocity estimation value ωxL ′ and the angular velocity estimation value ωyL ′ output from the velocity loop are sequentially updated, and the overall feedback loop An azimuth meter that calculates an azimuth angle ψ in an azimuth calculator using an angular velocity estimated value ωxL ′ and an angular velocity estimated value ωyL ′ that are finally output from the velocity loop at the time of convergence.

（２）前記Ｚ角速度推定器において、初期値として入力されるλと、地球自転角速度Ωと、Ｘb軸の角速度ωxbおよびＹb軸の角速度ωybと、姿勢角φおよびθとの入力を受けて、次式を用いてＺb軸の角速度推定値ωzb’を演算出力することを特徴とする請求項1記載の方位計。
ωzb’＝(ωxb・sinθ−ωyb・cosθ・sinφ−Ω・sinλ)/(cosθ・cosφ) (2) In the Z angular velocity estimator, in response to the input of λ, the earth rotation angular velocity Ω, the angular velocity ωxb of the Xb axis, the angular velocity ωyb of the Yb axis, and the attitude angles φ and θ, 2. The compass according to claim 1, wherein the Zb-axis estimated angular velocity value ωzb ′ is calculated and output using the following equation.
ωzb '= (ωxb ・ sinθ−ωyb ・ cosθ ・ sinφ−Ω ・ sinλ) / (cosθ ・ cosφ)

上記構成の手段によれば、ジャイロと加速度計を含むセンサの構成がストラップダウンであり、Ｚ角速度推定器を介して、Ｘb軸の角速度ωxb、Ｙb軸の角速度ωyb、姿勢角φ、θおよび緯度λ等によりＺ軸角速度推定値ωzb’が得られ、これらのωxb、ωyb、姿勢角φ、θおよびＺ軸角速度推定値ωzb’の入力を受けて、水平変換器からは水平面に投影されたＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLが生成される。このＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLと、Ｘ軸加速度ＡxbおよびＹb軸加速度Ａybの入力を受けて、負帰還ループを含む速度ループからは、角速度推定値ωxL’および角速度推定値ωyL’と，更新された姿勢角φ、θが出力される。これらの角速度推定値ωxL’およびωyL’の最終推定値を用いて方位演算を行うことにより、姿勢角変化および振動の影響を受けることなく正確に方位角が計測される。 According to the means of the above configuration, the configuration of the sensor including the gyro and the accelerometer is strap-down, and the angular velocity ωxb of the Xb axis, the angular velocity ωyb of the Yb axis, the posture angle φ, θ, and the latitude are passed through the Z angular velocity estimator. The estimated Z-axis angular velocity value ωzb ′ is obtained from λ, etc., and these ωxb, ωyb, posture angle φ, θ, and Z-axis angular velocity estimate value ωzb ′ are received, and the X projected onto the horizontal plane from the horizontal converter An axial horizontal angular velocity ωxL and a Y-axis horizontal angular velocity ωyL are generated. In response to the inputs of the X-axis horizontal angular velocity ωxL and the Y-axis horizontal angular velocity ωyL, the X-axis acceleration Axb and the Yb-axis acceleration Ayb, the velocity loop including the negative feedback loop receives the angular velocity estimated value ωxL ′ and the angular velocity estimated value ωyL ′. Then, the updated attitude angles φ and θ are output. By performing the azimuth calculation using the final estimated values of these angular velocity estimated values ωxL ′ and ωyL ′, the azimuth angle is accurately measured without being affected by the posture angle change and vibration.

以上要するに、本発明には、本発明の特徴とするＺ角速度推定器により抽出されるＺb軸角速度推定値と、Ｘb軸角速度およびＹb軸角速度と、速度ループより出力される姿勢角を入力として、水平変換器より出力される水平面直交Ｘ、Ｙ２軸の角速度推定値を速度ループに入力し、該Ｚ角速度推定器、水平変換器および速度ループを含む総合帰還ループを介して、該速度ループにおいて生成されるＸ軸角速度推定値とＹ軸角速度推定値を用いて方位角を演算出力することにより、姿勢角変化および外乱の振動の影響を受けることなく正確に方位角が計測することができるという効果がある。 In short, in the present invention, the Zb-axis angular velocity estimated value extracted by the Z-angular velocity estimator, which is a feature of the present invention, the Xb-axis angular velocity and the Yb-axis angular velocity, and the attitude angle output from the velocity loop are input. The horizontal plane orthogonal X and Y 2-axis angular velocity estimates output from the horizontal converter are input to the velocity loop, and are generated in the velocity loop via a comprehensive feedback loop including the Z angular velocity estimator, horizontal transducer and velocity loop. By calculating and outputting the azimuth angle using the estimated X-axis angular velocity value and the estimated Y-axis angular velocity value, it is possible to accurately measure the azimuth angle without being affected by posture angle changes and disturbance vibrations. There is.

また、本発明には、Ｚ角速度推定器によるＺb軸の角速度推定値の抽出により、Ｚb軸に対するジャイロの配置を不要とし、これにより２軸のジャイロのみにて高精度の方位を計測することが可能になるとともに、方位計の価格を低減することができるという効果があり、さらに方位計の構成に可動部分を含まないために、耐環境性/信頼性の向上を図ることができるという効果がある。 Further, according to the present invention, the Zb axis angular velocity estimation value is extracted by the Z angular velocity estimator, thereby eliminating the need for the gyro to be arranged with respect to the Zb axis, thereby enabling high-precision azimuth measurement with only the two-axis gyro. As well as being able to reduce the price of the compass, and because it does not include moving parts in the compass configuration, it is possible to improve environmental resistance / reliability. is there.

次に、本発明の実施形態について説明する。図１は本実施形態のジャイロと加速度計より方位角を計測する主要機能ブロック図である。図1に示されるように、本実施形態は、方位計測の基準として装置に設定された直交３軸座標系のＸb軸に配置されるジャイロ１および加速度計５と、Ｙb軸に配置されるジャイロ２および加速度計６と、Ｚ軸角速度推定器３と、水平変換器４と、速度ループ７と、方位演算器８とを備えて構成される。上記の速度ループ７は、図３に示されるように、減算器９，１３および１５と、水平変換器１０および１４と、積分器１１，１６および２０と、重力加速度カップリング演算器１２と、増幅器１７〜１９と、加算器２1とを備えて構成される。なお図３においては、Ｘb軸とＹb軸の各軸に対応する速度ループ７が併せて示されているが、実際には、Ｘb軸とＹb軸の各軸に対応して速度ループ７が求められる。 Next, an embodiment of the present invention will be described. FIG. 1 is a main functional block diagram for measuring an azimuth angle from a gyro and an accelerometer according to this embodiment. As shown in FIG. 1, in this embodiment, the gyro 1 and the accelerometer 5 that are arranged on the Xb axis of the orthogonal triaxial coordinate system set in the apparatus as a reference for azimuth measurement, and the gyro that is arranged on the Yb axis. 2, accelerometer 6, Z-axis angular velocity estimator 3, horizontal converter 4, velocity loop 7, and azimuth calculator 8. As shown in FIG. 3, the velocity loop 7 includes subtractors 9, 13 and 15, horizontal converters 10 and 14, integrators 11, 16 and 20, gravity acceleration coupling calculator 12, Amplifiers 17 to 19 and an adder 21 are provided. In FIG. 3, speed loops 7 corresponding to the Xb axis and Yb axis are shown together, but actually, the speed loop 7 is obtained corresponding to the Xb axis and Yb axis. It is done.

上記の図1および図３を参照して、本実施形態の方位計測の動作原理に関わる機能動作を説明する前に、図２および図４を参照して、本発明の方位計の構成概要・装置設置条件に関わる信頼性ならびに耐環境性の優位性について説明する。本装置は車両または他の装置に搭載されて、該車両または装置の方位角を計測する装置であり、図４に示されるように、Ｘb軸のジャイロ２５、Ｙb軸のジャイロ２６、Ｘb軸の加速度計２７およびＹb軸の加速度計２８を含むセンサ部２２と、ジャイロ制御２９および高電圧電源３０を含むジャイロコントロール２３と、ＣＰＵ３１、加速度デジタイザ３２およびシリアルインタフェース３３を含むＣＰＵ&I/F２４とを備えて構成される。ジャイロ２５およびジャイロ２６としては、バイアス再現性が高いＲＬＧ（リングレーザージャイロ）などの高精度ジャイロが使用される。加速度計２７および加速度計２８は、姿勢角再現性がある加速度計、傾斜計等が使用される。ジャイロ２５および２６からの角速度は、ジャイロコントロール２３を介してＣＰＵ３１に入力され、加速度計２７および２８からの加速度は加速度デジタイザ３２を介してＣＰＵ３１に入力されて、ＣＰＵ３１において演算処理が行われる。この場合に装置に対するコマンド内容および計測地点における緯度情報等は、予めシリアルインタフェース３３を介してＣＰＵ３１に与えられており、この演算処理結果による装置の方位角および姿勢角は、シリアルインタフェース３３を介して外部に出力される。 Before describing the functional operation related to the operation principle of the azimuth measurement of the present embodiment with reference to FIG. 1 and FIG. 3 above, referring to FIG. 2 and FIG. 4, the configuration outline of the azimuth meter of the present invention The superiority of reliability and environmental resistance related to the installation conditions of equipment will be explained. This device is mounted on a vehicle or other device and measures the azimuth angle of the vehicle or device. As shown in FIG. 4, the Xb axis gyro 25, the Yb axis gyro 26, and the Xb axis A sensor unit 22 including an accelerometer 27 and a Yb-axis accelerometer 28, a gyro control 23 including a gyro control 29 and a high voltage power supply 30, and a CPU & I / F 24 including a CPU 31, an acceleration digitizer 32 and a serial interface 33 are provided. Composed. As the gyro 25 and the gyro 26, a high-precision gyro such as an RLG (ring laser gyro) having high bias reproducibility is used. As the accelerometer 27 and the accelerometer 28, an accelerometer, an inclinometer or the like having posture angle reproducibility is used. Angular velocities from the gyros 25 and 26 are input to the CPU 31 via the gyro control 23, and accelerations from the accelerometers 27 and 28 are input to the CPU 31 via the acceleration digitizer 32, and calculation processing is performed in the CPU 31. In this case, the command contents for the device, latitude information at the measurement point, and the like are given in advance to the CPU 31 via the serial interface 33, and the azimuth angle and attitude angle of the device as a result of this arithmetic processing are obtained via the serial interface 33. Output to the outside.

次に図２(a) に示されるのは、Ｘb軸がロール軸であり、Ｙb軸がピッチ軸である場合について、装置の側面より見た装置前方向と上方向に対応して配置されるジャイロ２５と加速度計２７の設定向きと、ジャイロ２６と加速度計２８の設定向きとを示す模式図であり、図２(b)は、装置の上部より見た装置前方向、装置右方向および装置左方向に対応して配置されるジャイロ２５と加速度計２７の設定向きと、ジャイロ２６と加速度計２８の設定向きとを示す模式図である。図２においては、各ジャイロおよび各加速度計には、対応する軸のＸbおよびＹbが付記されている。これらのジャイロおよび加速度計に対応する入力軸は、通常は直方体型として形成されるケースの底面に平行になるように設定されており、Ｘb軸のジャイロ２５と加速度計２７の入力軸は、前方（計測する方位方向）に向いて一致するように設定されている。またＹb軸のジャイロ２６と加速度計２８は、ジャイロ２５または加速度計２７に対して直交する方向（Ｘb軸方向を前方とする場合には、Ｙb軸は右方）を入力軸として設定される。このように、本装置の構成に関しては、回転機構を要する可動部を含むことがなく、装置運用上の信頼性ならびに耐環境性に優れているのみならず、該可動部における疲労症状等に対応する定期的なメンテナンス作業を、全く不要にすることができるという利点がある。 Next, FIG. 2 (a) shows that when the Xb axis is a roll axis and the Yb axis is a pitch axis, they are arranged corresponding to the forward direction and the upward direction as seen from the side of the apparatus. FIG. 2B is a schematic diagram showing the setting direction of the gyro 25 and the accelerometer 27, and the setting direction of the gyro 26 and the accelerometer 28. FIG. It is a schematic diagram which shows the setting direction of the gyroscope 25 and the accelerometer 27 arrange | positioned corresponding to the left direction, and the setting direction of the gyroscope 26 and the accelerometer 28. In FIG. 2, the corresponding axes Xb and Yb are appended to each gyro and each accelerometer. The input shafts corresponding to these gyros and accelerometers are usually set to be parallel to the bottom surface of the case formed as a rectangular parallelepiped, and the input shafts of the Xb-axis gyro 25 and the accelerometer 27 It is set so as to coincide with (measurement azimuth direction). The Yb-axis gyro 26 and the accelerometer 28 are set with the direction orthogonal to the gyro 25 or the accelerometer 27 (when the Xb-axis direction is the front, the Yb-axis is the right) as the input axis. As described above, the configuration of the apparatus does not include a movable part that requires a rotation mechanism, and is not only excellent in reliability and environmental resistance in operation of the apparatus, but also supports fatigue symptoms in the movable part. There is an advantage that the regular maintenance work to be performed can be completely eliminated.

以下においては、図１および図３を参照して、本実施形態の動作原理に関わる機能動作について説明する。図１において、Ｘb軸のジャイロ１より検出出力される角速度ωxbとＹb軸のジャイロ２より検出出力される角速度ωybは、Ｚ角速度推定器３と水平変換器４に入力される。また、Ｘb軸の加速度計５より検出出力される加速度ＡxbとＹb軸の加速度計６より検出出力される加速度Ａybは、直接速度ループ７に入力される。Ｚ角速度推定器３においては、方位計測中には方位が不変であるとする条件を基にして、前記角速度ωxbおよび角速度ωybの入力と、速度ループ７より出力される更新された姿勢角φおよびθの入力を受けて、予め初期値として与えられている計測地点の緯度λを含む次式による演算処理が行われて、Ｘb軸、Ｙb軸の直交２軸に対応するＺb軸の角速度推定値ωzb’が演算出力される。

In the following, with reference to FIG. 1 and FIG. 3, a functional operation related to the operation principle of the present embodiment will be described. In FIG. 1, an angular velocity ωxb detected and output from the Xb axis gyro 1 and an angular velocity ωyb detected and output from the Yb axis gyro 2 are input to the Z angular velocity estimator 3 and the horizontal converter 4. The acceleration Axb detected and output from the Xb-axis accelerometer 5 and the acceleration Ayb detected and output from the Yb-axis accelerometer 6 are directly input to the velocity loop 7. In the Z angular velocity estimator 3, the angular velocity ωxb and the angular velocity ωyb are input based on the condition that the azimuth is unchanged during the azimuth measurement, and the updated attitude angle φ and In response to the input of θ, an arithmetic processing according to the following expression including the latitude λ of the measurement point given in advance as an initial value is performed, and an estimated angular velocity value of the Zb axis corresponding to two orthogonal axes of the Xb axis and the Yb axis ωzb ′ is calculated and output.

上記(1)式においてΩは地球自転角速度である。Ｚ角速度推定器３において演算出力されたＺb軸の角速度推定値ωzb’は水平変換器４に入力されるが、水平変換器４においては、前記角速度ωxbおよび角速度ωybの入力と、速度ループ７において更新された姿勢角φおよびθの入力と、該Ｚb軸の角速度推定値ωzb’の入力を受けて、次式により、角速度ωxbおよび角速度ωybを地球水平面に投影した、Ｘ軸の水平角速度ωxLおよびＹ軸の水平角速度ωyLが演算出力される。

In the above equation (1), Ω is the earth rotation angular velocity. The Zb-axis estimated angular velocity value ωzb ′ calculated and output by the Z angular velocity estimator 3 is input to the horizontal converter 4. In the horizontal converter 4, the angular velocity ωxb and the angular velocity ωyb are input. In response to the input of the updated attitude angles φ and θ and the estimated angular velocity value ωzb ′ of the Zb axis, the angular velocity ωxb and the angular velocity ωyb are projected onto the earth horizontal plane by The Y-axis horizontal angular velocity ωyL is calculated and output.

水平変換器４より出力されたＸ軸の水平角速度ωxLおよびＹ軸の水平角速度ωyLと、加速度計５および６より出力される加速度ＡxbおよびＡybは、速度ループ７に入力される。図３は速度ループ７のブロック図であるが、実際には、Ｘ軸水平角速度ωxLと加速度Ａybの入力に対応して、更新された姿勢角φと、角速度推定値ωxL’とを出力する速度ループと、Ｙ軸水平角速度ωyLと加速度Ａxbの入力に対応して、更新された姿勢角θと、Ｙ軸水平角速度推定値ωyL’とを出力する速度ループとは、それぞれ個別に設けられており一体化されてはいないが、図面表示を簡易化するために双方のケースを合わせて記載している。先ず図３を参照して、Ｘ軸水平角速度ωxLと加速度Ａybの入力に対応して、更新された姿勢角φと、角速度推定値ωxL’が生成されて出力される場合について説明する。この動作内容は、更新された姿勢角θと、角速度推定値ωyL’が生成される場合についても全く同様である。 The X-axis horizontal angular velocity ωxL and Y-axis horizontal angular velocity ωyL output from the horizontal converter 4 and the accelerations Axb and Ayb output from the accelerometers 5 and 6 are input to the velocity loop 7. FIG. 3 is a block diagram of the speed loop 7, but actually, the speed at which the updated attitude angle φ and the estimated angular speed value ωxL ′ are output in response to the input of the X-axis horizontal angular speed ωxL and the acceleration Ayb. The loop and the velocity loop that outputs the updated attitude angle θ and the estimated Y-axis horizontal angular velocity value ωyL ′ corresponding to the input of the Y-axis horizontal angular velocity ωyL and the acceleration Axb are individually provided. Although not integrated, both cases are described together to simplify the drawing display. First, with reference to FIG. 3, a case will be described in which the updated attitude angle φ and the estimated angular velocity value ωxL ′ are generated and output in response to the input of the X-axis horizontal angular velocity ωxL and the acceleration Ayb. This operation is exactly the same when the updated posture angle θ and the estimated angular velocity value ωyL ′ are generated.

図３において、水平変換器４より入力されるＸ軸水平角速度ωxLは減算器９に入力され、加速度計５より入力される加速度Ａybは減算器1３に入力される。減算器９において、速度ループの帰還出力により減算された角速度出力は、水平変換器１０において地球水平面に直交する座標系の角速度から、装置を基準とする座標系の角速度に変換されて積分器１１に入力される。積分器１１においては、該角速度が積分処理されて装置のＸb軸の姿勢角φが更新されて出力され、重力加速度カップリング演算器1２に入力されるとともに、帰還出力としてＺ角速度推定器３に送られる。重力加速度カップリング演算器1２においては、この姿勢角φによるＹ軸加速度の重力加速度カップリング成分が演算出力されて減算器１３に入力される。 In FIG. 3, the X-axis horizontal angular velocity ωxL input from the horizontal converter 4 is input to the subtractor 9, and the acceleration Ayb input from the accelerometer 5 is input to the subtractor 13. The angular velocity output subtracted by the feedback output of the velocity loop in the subtractor 9 is converted in the horizontal converter 10 from the angular velocity of the coordinate system orthogonal to the earth horizontal plane to the angular velocity of the coordinate system based on the device, and the integrator 11. Is input. In the integrator 11, the angular velocity is integrated and the attitude angle φ of the Xb axis of the apparatus is updated and output, and is input to the gravitational acceleration coupling calculator 12, and as a feedback output to the Z angular velocity estimator 3. Sent. In the gravitational acceleration coupling calculator 12, the gravitational acceleration coupling component of the Y-axis acceleration based on the posture angle φ is calculated and output and input to the subtractor 13.

減算器１３においては、該重力加速度カップリング成分に対して加速度Ａybによる減算処理が行われて、その差分は水平変換機器1４に入力される。水平変換機器1４においては、該差分に対応して、装置を基準とする座標系から地球水平面に直交する座標系に対する座標変換が行われて、水平変換機器1４からは、地球水平面に直交する座標系に対応する前記差分が出力されて、減算器１５、積分器１６および増幅器１７を含む負帰還ループにより形成される積分回路に入力される。 In the subtractor 13, the gravitational acceleration coupling component is subtracted by the acceleration Ayb, and the difference is input to the horizontal conversion device 14. In the horizontal conversion device 14, coordinate conversion is performed from the coordinate system based on the apparatus to the coordinate system orthogonal to the earth horizontal plane in accordance with the difference, and the horizontal conversion device 14 receives coordinates orthogonal to the earth horizontal plane. The difference corresponding to the system is output and input to an integration circuit formed by a negative feedback loop including a subtractor 15, an integrator 16 and an amplifier 17.

本来、方位計測系に誤差が存在しない場合に該差分が存在するということは、実際に装置が加速している状態にあることを示しており、この差分を前記積分回路により積分することにより、積分器１６からは装置の速度が出力される。しかし、本方位計においては、方位の計測中には移動してはならないという条件があるために、本来は速度が０となる筈である。依って、前記差分を積分して得られた速度は、Ｘ軸水平角速度ωxL自体の誤差に起因するものと考えて、該速度を増幅器１８および１９、積分器２０および加算器２１を含む帰還経路を経由して減算器９に帰還する負帰還ループを用いることにより、Ｘ軸の角速度推定値ωxL’の値を更新することとする。これにより、積分器２０からは、新たに更新された角速度推定値ωxL’が出力されるとともに，積分器１１からは、前述のように、更新された姿勢角φが出力されてＺ角速度推定器３に送られる。このことは、速度ループ７に、Ｙ軸水平角速度ωyLと加速度Ａxbが入力される場合についても同様であり、減算器１３による差分を積分して得られた速度は、Ｙ軸水平角速度ωyL自体の誤差に起因するものと考えて、該速度を上記の帰還経路を経由して減算器９に帰還する負帰還ループを用いることにより、更新されたＹ軸の角速度推定値ωyL’が積分器２０より出力されるとともに、積分器１１からは更新された姿勢角θが出力されてＺ角速度推定器３に送られる。 Originally, when there is no error in the azimuth measuring system, the presence of the difference indicates that the apparatus is actually accelerating, and by integrating this difference by the integration circuit, The integrator 16 outputs the device speed. However, in this compass, since there is a condition that it should not move during the measurement of the azimuth, the speed should be essentially zero. Therefore, the speed obtained by integrating the difference is considered to be caused by an error of the X-axis horizontal angular speed ωxL itself, and the speed is fed back to the feedback path including the amplifiers 18 and 19, the integrator 20 and the adder 21. The value of the estimated X-axis angular velocity ωxL ′ is updated by using a negative feedback loop that feeds back to the subtractor 9 via. Thus, the integrator 20 outputs a newly updated angular velocity estimation value ωxL ′, and the integrator 11 outputs the updated attitude angle φ and outputs the Z angular velocity estimator as described above. Sent to 3. The same applies to the case where the Y-axis horizontal angular velocity ωyL and the acceleration Axb are input to the velocity loop 7, and the velocity obtained by integrating the difference by the subtractor 13 is that of the Y-axis horizontal angular velocity ωyL itself. By considering a negative feedback loop that feeds back the speed to the subtractor 9 via the feedback path, which is considered to be caused by an error, the updated angular velocity estimated value ωyL ′ of the Y axis is obtained from the integrator 20. The updated attitude angle θ is output from the integrator 11 and sent to the Z angular velocity estimator 3.

図１のブロック図を参照して明らかなように、速度ループ７より出力される更新された姿勢角φおよびθは、共にＺ角速度推定器３と水平変換器４に入力されており、Ｚ角速度推定器３においては、これらの姿勢角φおよびθと、角速度ωxbおよびωybと、緯度λの入力を受けて、Ｚ軸の角速度推定値ωzb’が新たに更新出力されて水平変換器４に入力される。 As apparent from the block diagram of FIG. 1, the updated attitude angles φ and θ output from the speed loop 7 are both input to the Z angular velocity estimator 3 and the horizontal converter 4, and the Z angular velocity is In the estimator 3, the attitude angles φ and θ, the angular velocities ωxb and ωyb, and the latitude λ are received and the Z-axis angular velocity estimated value ωzb ′ is newly updated and input to the horizontal converter 4. Is done.

水平変換器４においては、更新された姿勢角φおよびθと、更新された角速度推定値ωzb’と、角速度ωxbおよびωybの入力を受けて、新たに更新されたＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLが生成されて速度ループ７に入力される。速度ループ７においては、更新された水平角速度ωxLおよびωyLと、加速度ＡxbおよびＡybの入力を受けて、前述のように、負帰還ループを介して、角速度推定値ωxL’およびωyL’と、姿勢角φおよびθが、それぞれ改めて更新されて出力される。 In the horizontal converter 4, the updated posture angles φ and θ, the updated angular velocity estimation value ωzb ′, and the angular velocities ωxb and ωyb are received, and the newly updated X-axis horizontal angular velocity ωxL and Y-axis are received. A horizontal angular velocity ωyL is generated and input to the velocity loop 7. In the speed loop 7, the updated horizontal angular velocities ωxL and ωyL and the accelerations Axb and Ayb are input, and as described above, the angular velocity estimation values ωxL ′ and ωyL ′ and the attitude angle are passed through the negative feedback loop. φ and θ are updated again and output, respectively.

このことより、速度ループ７において、積分器１１より出力される姿勢角φおよびθを帰還始動入力として、Ｚ角速度推定器３、水平変換器４および速度ループ７内の負帰還ループを統合する総合帰還ループが形成されていることが明らかである。この総合帰還ループを介して、速度ループ７より出力される角速度推定値ωxL’およびωyL’に対する更新処理が繰り返して行われ、最終的に該総合帰還ループが収束する時点においては、帰還経路に含まれる積分器２０からは、Ｘ軸の角速度推定値ωxL’とＹ軸の角速度推定値ωyL’が、それぞれ最終の角速度推定値として出力され、積分器１１からは、それぞれＸb軸の姿勢角φとＹb軸の姿勢角θが最終値として出力される。 Thus, in the speed loop 7, the Z angular velocity estimator 3, the horizontal converter 4, and the negative feedback loop in the velocity loop 7 are integrated using the attitude angles φ and θ output from the integrator 11 as feedback start inputs. It is clear that a feedback loop is formed. The update process for the angular velocity estimated values ωxL ′ and ωyL ′ output from the speed loop 7 is repeatedly performed through this total feedback loop, and is finally included in the feedback path when the total feedback loop converges. The X-axis angular velocity estimated value ωxL ′ and the Y-axis angular velocity estimated value ωyL ′ are output as final angular velocity estimated values, respectively, and the integrator 11 outputs the Xb-axis attitude angle φ and The attitude angle θ of the Yb axis is output as the final value.

速度ループ７より出力される角速度推定値ωxL’およびωyL’は、総合帰還ループが収束するまでの過程において、逐次方位演算器８に入力されて方位角ψの演算が行われ、且つその演算値が逐次更新されているが、最終の角速度推定値ωxL’およびωyL’の値を受けて、方位演算器８においては、地球水平面における該角速度推定値ωxL’とωyL’の比、およびその極性を考慮して、次式に示される逆正接関数を用いて方位角ψが演算出力される。

The estimated angular velocity values ωxL ′ and ωyL ′ output from the velocity loop 7 are sequentially input to the azimuth calculator 8 to calculate the azimuth angle ψ in the process until the total feedback loop converges, and the calculated value Are successively updated. In response to the final angular velocity estimated values ωxL ′ and ωyL ′, the azimuth calculator 8 determines the ratio between the angular velocity estimated values ωxL ′ and ωyL ′ in the earth horizontal plane and the polarity thereof. In consideration, the azimuth angle ψ is calculated and output using an arctangent function represented by the following equation.

なお速度ループ７においては、例えばＸ軸の角速度推定値ωxL’を求める場合に、方位計測中に姿勢角が変化することがあっても、姿勢角の変化に対応して求められた重力加速度カップリング成分と、Ｙb軸の加速度Ａybとが相殺されるために、その姿勢角変化による影響を受けることなく角速度推定値ωxL’の値を求めることができる。このことはＹ軸の角速度推定値ωyL’を求める場合においても全く同様であり、姿勢角の変化に対応して求められた重力加速度カップリング成分と、Ｘb軸の加速度Ａxbとが相殺されるために、その姿勢角変化による影響を受けることなく角速度推定値ωyL’の値を求めることができる。 In the velocity loop 7, for example, when the estimated angular velocity value ωxL ′ of the X-axis is obtained, even if the posture angle changes during the azimuth measurement, the gravitational acceleration cup obtained corresponding to the change in the posture angle is used. Since the ring component and the Yb-axis acceleration Ayb cancel each other, the value of the estimated angular velocity ωxL ′ can be obtained without being affected by the change in the posture angle. This is exactly the same in the case of obtaining the estimated angular velocity value ωyL ′ of the Y axis, because the gravitational acceleration coupling component obtained corresponding to the change in the attitude angle and the acceleration Axb of the Xb axis cancel each other. In addition, the estimated angular velocity value ωyL ′ can be obtained without being affected by the change in the posture angle.

また図３において、負帰還ループの帰還経路に含まれる増幅器１７，１８および１９の各増幅器としては、角速度推定値ωxL’および角速度推定値ωyL’の値を速やかに収束させるために、時間とともに利得が変化する可変利得増幅器が用いられる。固定利得でも差し支えない。また装置が外乱による振動状態にある状態においても、加速度ＡxbおよびＡybの入力に対応して積分器が設けられており、１回積分値、２回積分値等を角速度補正値としているために、高周波数領域における振動等の外乱による速度誤差は殆ど生じることがない。これにより姿勢角の変動および振動の影響を受けずに正確に方位角を計測することができる。 In FIG. 3, each of the amplifiers 17, 18 and 19 included in the feedback path of the negative feedback loop has a gain over time in order to quickly converge the angular velocity estimated value ωxL ′ and the angular velocity estimated value ωyL ′. A variable gain amplifier is used that varies. A fixed gain can be used. Even when the apparatus is in a vibration state due to disturbance, an integrator is provided corresponding to the inputs of accelerations Axb and Ayb, and the one-time integration value, the two-time integration value, etc. are used as the angular velocity correction value. There is almost no speed error due to disturbances such as vibration in the high frequency region. As a result, the azimuth angle can be accurately measured without being affected by fluctuations in posture angle and vibration.

また本発明においては、Ｚ角速度推定器３を設けることにより、姿勢角φおよびθと、角速度ωxbおよびωybと、緯度λを用いてＺb軸の角速度推定値ωzb’を演算出力する点に一つの特徴があり、このＺb軸の角速度推定値ωzb’を用いることにより、水平変換器４を介してＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLを抽出することが可能となる。このことにより、Ｚb軸にジャイロを配置することは不要となり、ジャイロが２軸のみに配置される状態においても高精度の方位を計測することが可能となる。またＺb軸のジャイロを不要とすることにより、方位計の価格を抑制することが可能になるとともに、さらには、構成上可動部を含まないために、方位計としての耐環境性ならびに信頼性が向上される。 Further, in the present invention, by providing the Z angular velocity estimator 3, one of the points that the Zb axis angular velocity estimated value ωzb ′ is calculated and output using the attitude angles φ and θ, the angular velocities ωxb and ωyb, and the latitude λ. There is a feature, and by using the estimated angular velocity value ωzb ′ of the Zb axis, the X-axis horizontal angular velocity ωxL and the Y-axis horizontal angular velocity ωyL can be extracted via the horizontal converter 4. Accordingly, it is not necessary to arrange the gyro on the Zb axis, and it is possible to measure a highly accurate azimuth even when the gyro is arranged on only two axes. In addition, by eliminating the Zb axis gyro, it becomes possible to reduce the price of the compass and, furthermore, because it does not include a movable part in the configuration, it has environmental resistance and reliability as a compass. Be improved.

本発明の実施形態の機能ブロック図である。It is a functional block diagram of an embodiment of the present invention. 装置の基準となる座標系に設定されるジャイロと加速度計の向きを示す模式図である。It is a schematic diagram which shows the direction of the gyro and accelerometer set to the coordinate system used as the reference | standard of an apparatus. 前記実施形態に含まれる速度ループのブロック図である。It is a block diagram of the speed loop included in the embodiment. 前記実施形態の構成を示す概念図である。It is a conceptual diagram which shows the structure of the said embodiment. 従来例（１）の機能ブロック図である。It is a functional block diagram of a prior art example (1). 従来例（２）の機能ブロック図である。It is a functional block diagram of a prior art example (2). 従来例（３）の機能ブロック図である。It is a functional block diagram of a prior art example (3). 従来例（４）の機能ブロック図である。It is a functional block diagram of a prior art example (4).

Explanation of symbols

１，２，２５，２６，３４，４４，４５，５４〜５６，６１〜６３ジャイロ
３Ｚ角速度推定器
４，１０，１４，６７水平変換器
５，６，２７，２８，３５，３６，４２，４３，５１〜５３，６４〜６６加速度計
７，６８速度ループ
８，６９方位演算器
９，１３，１５減算器
１１，１６，２０積分器
１２重力加速度カップリング演算器
１７〜１９増幅器
２１加算器
２２センサ部
２３ジャイロコントロール
２４ＣＰＵ&Ｉ/Ｆ
２９ジャイロ制御
３０高電圧電源
３１ＣＰＵ
３２加速度デジタイザ
３３シリアルインタフェース
３７，４７座標変換手段
３８ジャイロ読取補償手段
３９緯度推定手段
４０切替器
４１機首角度計算手段
４６重力加速度鉛直成分検出手段
４８地球自転角速度鉛直成分検出手段
４９方位角演算手段
５０姿勢角演算手段
５７第１の演算手段
５８第２の演算手段
５９第３の演算手段
６０距離計 1, 2, 25, 26, 34, 44, 45, 54 to 56, 61 to 63 Gyro 3 Z angular velocity estimator 4, 10, 14, 67 Horizontal converter 5, 6, 27, 28, 35, 36, 42 , 43, 51-53, 64-66 Accelerometer 7, 68 Speed loop 8, 69 Azimuth calculator 9, 13, 15 Subtractor 11, 16, 20 Integrator 12 Gravity acceleration coupling calculator 17-19 Amplifier 21 Addition 22 Sensor unit 23 Gyro control 24 CPU & I / F
29 Gyro Control 30 High Voltage Power Supply 31 CPU
32 Acceleration digitizer 33 Serial interface 37, 47 Coordinate conversion means 38 Gyro reading compensation means 39 Latitude estimation means 40 Switch 41 Nose angle calculation means 46 Gravitational acceleration vertical component detection means 48 Earth rotation angular velocity vertical component detection means 49 Azimuth angle calculation means 50 Attitude angle calculating means 57 First calculating means 58 Second calculating means 59 Third calculating means 60 Distance meter

Claims

In an azimuth meter that measures an azimuth angle ψ by arranging one gyro and one accelerometer on the Xb axis and Yb axis of an orthogonal triaxial coordinate system composed of Xb, Yb, and Zb orthogonal to each other,
Comprising a Z angular velocity estimator, horizontal converter, velocity loop and azimuth calculator,
The Z angular velocity estimator calculates the angular velocity estimated value ωzb ′ of the Zb axis based on the roll angle φ and pitch angle θ received from the velocity loop and the latitude λ input as an initial value,
The horizontal converter receives the angular velocities ωxb and ωyb, the angular velocity estimation value ωzb ′, the roll angle φ and the pitch angle θ, and converts the angular velocities ωxb and ωyb into two horizontal X-axis horizontal axes that are orthogonal to the earth. Convert to angular velocity ωxL and Y-axis horizontal angular velocity ωyL,
The velocity loop receives an input of the Xb-axis acceleration Axb and Yb-axis acceleration Ayb of the accelerometer, the X-axis horizontal angular velocity ωxL and the Y-axis horizontal angular velocity ωyL of the earth horizontal orthogonal two axes, and performs a negative feedback loop. The roll angle φ and the pitch angle θ are updated via the Z angle velocity estimator and the horizontal converter, and the angular velocity estimated value ωxL ′ and the angular velocity estimated value ωyL ′ are generated.
The azimuth calculator receives an angular velocity estimation value ωxL ′ and an angular velocity estimation value ωyL ′ output from the velocity loop, and uses an arctangent function corresponding to a ratio of the angular velocity estimation values ωxL ′ and ωyL ′. An operation for estimating the azimuth angle ψ,
Through an overall feedback loop including the Z angular velocity estimator, a horizontal converter and a velocity loop, the angular velocity estimation value ωxL ′ and the angular velocity estimation value ωyL ′ output from the velocity loop are sequentially updated, and the overall feedback loop An azimuth meter that calculates an azimuth angle ψ in an azimuth calculator using an angular velocity estimated value ωxL ′ and an angular velocity estimated value ωyL ′ that are finally output from the velocity loop at the time of convergence.

In the Z angular velocity estimator, in response to the input of λ, the earth rotation angular velocity Ω, the Xb angular velocity ωxb, the Yb angular velocity ωyb, and the posture angles φ and θ, 2. The azimuth meter according to claim 1, wherein the Zb-axis estimated angular velocity value ωzb ′ is calculated and output.
ωzb '= (ωxb ・ sinθ−ωyb ・ cosθ ・ sinφ−Ω ・ sinλ) / (cosθ ・ cosφ)