JP3884742B2 - Compass - Google Patents
Compass Download PDFInfo
- Publication number
- JP3884742B2 JP3884742B2 JP2004098835A JP2004098835A JP3884742B2 JP 3884742 B2 JP3884742 B2 JP 3884742B2 JP 2004098835 A JP2004098835 A JP 2004098835A JP 2004098835 A JP2004098835 A JP 2004098835A JP 3884742 B2 JP3884742 B2 JP 3884742B2
- Authority
- JP
- Japan
- Prior art keywords
- angular velocity
- axis
- horizontal
- velocity
- loop
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
- 230000001133 acceleration Effects 0.000 claims description 34
- 230000004044 response Effects 0.000 claims description 8
- 238000010586 diagram Methods 0.000 description 18
- 238000005259 measurement Methods 0.000 description 12
- 238000006243 chemical reaction Methods 0.000 description 7
- 230000008878 coupling Effects 0.000 description 7
- 238000010168 coupling process Methods 0.000 description 7
- 238000005859 coupling reaction Methods 0.000 description 7
- 230000008859 change Effects 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 6
- 238000005452 bending Methods 0.000 description 5
- 238000001514 detection method Methods 0.000 description 5
- 230000010354 integration Effects 0.000 description 4
- 238000000034 method Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 1
- 238000005553 drilling Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 206010016256 fatigue Diseases 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 208000024891 symptom Diseases 0.000 description 1
Images
Landscapes
- Gyroscopes (AREA)
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.
従来、ジャイロおよび加速度計を用いて方位角を計測する方位計としては、特許文献1(特公平7−15384)に、ジャイロコンパシング装置として実現された北探知システムが開示されている。このシステムにおける、ジャイロと加速度計より方位角を計測する主要機能ブロック図を図5に示す。図5に示されるように、X軸またはY軸の何れか一方の1軸に配置された1個のジャイロ34と、X軸およびY軸の2軸に配置された加速度計35および36と、座標変換手段37と、ジャイロ読取補償手段38と、緯度推定手段39と、切替器40と、機首角度計算手段41とを備えて構成されている。
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
また特許文献2(特開平6−11349)の方位計には、方位測定の時間短縮ならびに測定悪条件に対応できる方位計が開示されている。この従来例における、ジャイロと加速度計より方位角を計測する主要機能ブロック図は、図6に示されるように、X軸およびY軸に配置されたジャイロ42および43と、X軸およびY軸に配置された加速度計44および45と、重力加速度鉛直成分検出手段46と、座標変換手段47と、地球自転角速度鉛直成分検出手段48と、方位角演算手段49と、姿勢角演算手段50とを備えて構成される。
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
更に、特許文献3(特開平5−187873)には、あらゆる方向の被測定孔の傾斜角および方位角を、同時に、且つセンサ部を停止させることなく短時間にて計測することを目的とした、孔曲がり計測装置および孔曲がり計測方法が開示されている、この従来例における、ジャイロと加速度計より方位角を計測する主要機能ブロック図は、図7に示されるように、X軸、Y軸およびZ軸に配置された加速度計51、52および53と、X軸、Y軸およびZ軸に配置されたにジャイロ54、55および56と、第1の演算手段57と、第2の演算手段58と、第3の演算手段59と、距離計60とを備えて構成される。
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
他の例として、非特許文献1[日本航空宇宙工業会発行の技術書「スキューリダンダント・イナーシャルシステムの研究」(発行日:1985年3月),ISSN0910-1500]においては、ジャイロと加速度計より方位角を計測する主要機能ブロック図として、図8に示されるように、X軸、Y軸およびZ軸に配置されたジャイロ61、62および63と、X軸、Y軸およびZ軸に配置された加速度計64、65および66と、水平変換器67と、速度ループ68と、方位演算器69とを備えたブロック図が示されている。
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,
上述した特許文献1の方位計においては、リングレーザージャイロを2軸分設けているために、ランダムドリフトの影響を抑制する時間が1軸分に比して2倍の時間を必要とし、また入力軸を2方向に指向させるための可動部を要するために、耐環境性/信頼性に乏しいという欠点があり、加速度計出力の微分値については、特に高周波領域における振動入力等の外乱による影響を大きく受けやすく、このために角速度補正の誤差が生じ易いという欠点も存在している。また、特許文献2の方位計においては、方位計に動揺が入力されると、ベクトル値が15度/Hよりも大きくなるような場合には、方位計算に誤差が生じるという問題があり、特許文献3の孔曲がり計測装置および孔曲がり計測方法においては、3軸の動きに対しては誤差を持たずに孔曲がりの計測を行うことは可能であるが、ジャイロおよび加速度計をそれぞれ3個づつ設けることを必要とし、コストアップとなり高価格になるという欠点がある。更に非特許文献1においても同様に、3軸の動きに対して誤差を持たずに方位角を計測することはできるが、ジャイロと加速度計をそれぞれ3個づつ設けることが必要となり、コストアップとなって高価格になるという欠点がある。
In the azimuth meter of
本発明の目的は、上記の従来例の課題を解決して可動部分を除去して耐環境性/信頼性の向上を図り、装置の姿勢角変化による影響ならびに2軸に対する高周波数領域等における振動の外乱による影響を排除して、精度高い方位計測を可能にするとともに、地球自転の鉛直成分算定に際しては、ベクトル値が15度/Hよりも大きくなるような状態においても計算誤差を生じることなく、且つジャイロと加速度計をそれぞれ2軸のみに配置することにより、価格の低減を実現することのできる方位計を提供することにある。 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.
(1)互いに直交するXb,Yb及びZbでなる直交3軸座標系のXb軸およびYb軸にジャイロ及び加速度計をそれぞれ1個ずつ配置し、方位角ψを計測する方位計において、
Z角速度推定器、水平変換器、速度ループ及び方位演算器を備えてなり、
前記Z角速度推定器は、前記速度ループから受けるロール角φおよびピッチ角θ、並びに 初期値として入力されている緯度 λに基づき、前記Zb軸の角速度推定値ωzb’を演算し、
前記水平変換器は、前記角速度ωxbおよびωybと、前記角速度推定値ωzb’と、ロール角φおよびピッチ角θの入力を受けて、該角速度ωxbおよびωybを、地球水平直交2軸のX軸水平角速度ωxLおよびY軸水平角速度ωyLに変換し、
前記速度ループは、前記加速度計のXb軸の加速度AxbおよびYb軸の加速度Aybと、前記地球水平直交2軸のX軸水平角速度ωxLおよびY軸水平角速度ωyLの入力を受けて、負帰還ループを介して前記ロール角φおよびピッチ角θの更新を行い、前記Z角速度推定器ならびに水平変換器に帰還するとともに、角速度推定値ωxL’および角速度推定値ωyL’を生成し、
前記方位演算器は、前記速度ループより出力される角速度推定値ωxL’および角速度推定値ωyL’の入力を受けて、これら角速度推定値ωxL’およびωyL’の比に対応する逆正接関数を用いて方位角ψを推定する演算をし、
前記Z角速度推定器、水平変換器および速度ループを含む総合帰還ループを介して、前記速度ループより出力される角速度推定値ω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.
(2)前記Z角速度推定器において、初期値として入力されるλと、地球自転角速度Ωと、Xb軸の角速度ωxbおよびYb軸の角速度ωybと、姿勢角φおよびθとの入力を受けて、次式を用いてZb軸の角速度推定値ω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
ωzb '= (ωxb ・ sinθ−ωyb ・ cosθ ・ sinφ−Ω ・ sinλ) / (cosθ ・ cosφ)
上記構成の手段によれば、ジャイロと加速度計を含むセンサの構成がストラップダウンであり、Z角速度推定器を介して、Xb軸の角速度ωxb、Yb軸の角速度ωyb、姿勢角φ、θおよび緯度λ等によりZ軸角速度推定値ωzb’が得られ、これらのωxb、ωyb、姿勢角φ、θおよびZ軸角速度推定値ωzb’の入力を受けて、水平変換器からは水平面に投影されたX軸水平角速度ωxLおよびY軸水平角速度ωyLが生成される。このX軸水平角速度ωxLおよびY軸水平角速度ωyLと、X軸加速度AxbおよびYb軸加速度Aybの入力を受けて、負帰還ループを含む速度ループからは、角速度推定値ω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.
以上要するに、本発明には、本発明の特徴とするZ角速度推定器により抽出されるZb軸角速度推定値と、Xb軸角速度およびYb軸角速度と、速度ループより出力される姿勢角を入力として、水平変換器より出力される水平面直交X、Y2軸の角速度推定値を速度ループに入力し、該Z角速度推定器、水平変換器および速度ループを含む総合帰還ループを介して、該速度ループにおいて生成されるX軸角速度推定値とY軸角速度推定値を用いて方位角を演算出力することにより、姿勢角変化および外乱の振動の影響を受けることなく正確に方位角が計測することができるという効果がある。 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.
また、本発明には、Z角速度推定器によるZb軸の角速度推定値の抽出により、Zb軸に対するジャイロの配置を不要とし、これにより2軸のジャイロのみにて高精度の方位を計測することが可能になるとともに、方位計の価格を低減することができるという効果があり、さらに方位計の構成に可動部分を含まないために、耐環境性/信頼性の向上を図ることができるという効果がある。 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は本実施形態のジャイロと加速度計より方位角を計測する主要機能ブロック図である。図1に示されるように、本実施形態は、方位計測の基準として装置に設定された直交3軸座標系のXb軸に配置されるジャイロ1および加速度計5と、Yb軸に配置されるジャイロ2および加速度計6と、Z軸角速度推定器3と、水平変換器4と、速度ループ7と、方位演算器8とを備えて構成される。上記の速度ループ7は、図3に示されるように、減算器9,13および15と、水平変換器10および14と、積分器11,16および20と、重力加速度カップリング演算器12と、増幅器17〜19と、加算器21とを備えて構成される。なお図3においては、Xb軸とYb軸の各軸に対応する速度ループ7が併せて示されているが、実際には、Xb軸とYb軸の各軸に対応して速度ループ7が求められる。
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
上記の図1および図3を参照して、本実施形態の方位計測の動作原理に関わる機能動作を説明する前に、図2および図4を参照して、本発明の方位計の構成概要・装置設置条件に関わる信頼性ならびに耐環境性の優位性について説明する。本装置は車両または他の装置に搭載されて、該車両または装置の方位角を計測する装置であり、図4に示されるように、Xb軸のジャイロ25、Yb軸のジャイロ26、Xb軸の加速度計27およびYb軸の加速度計28を含むセンサ部22と、ジャイロ制御29および高電圧電源30を含むジャイロコントロール23と、CPU31、加速度デジタイザ32およびシリアルインタフェース33を含むCPU&I/F24とを備えて構成される。ジャイロ25およびジャイロ26としては、バイアス再現性が高いRLG(リングレーザージャイロ)などの高精度ジャイロが使用される。加速度計27および加速度計28は、姿勢角再現性がある加速度計、傾斜計等が使用される。ジャイロ25および26からの角速度は、ジャイロコントロール23を介してCPU31に入力され、加速度計27および28からの加速度は加速度デジタイザ32を介してCPU31に入力されて、CPU31において演算処理が行われる。この場合に装置に対するコマンド内容および計測地点における緯度情報等は、予めシリアルインタフェース33を介してCPU31に与えられており、この演算処理結果による装置の方位角および姿勢角は、シリアルインタフェース33を介して外部に出力される。
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
次に図2(a) に示されるのは、Xb軸がロール軸であり、Yb軸がピッチ軸である場合について、装置の側面より見た装置前方向と上方向に対応して配置されるジャイロ25と加速度計27の設定向きと、ジャイロ26と加速度計28の設定向きとを示す模式図であり、図2(b)は、装置の上部より見た装置前方向、装置右方向および装置左方向に対応して配置されるジャイロ25と加速度計27の設定向きと、ジャイロ26と加速度計28の設定向きとを示す模式図である。図2においては、各ジャイロおよび各加速度計には、対応する軸のXbおよびYbが付記されている。これらのジャイロおよび加速度計に対応する入力軸は、通常は直方体型として形成されるケースの底面に平行になるように設定されており、Xb軸のジャイロ25と加速度計27の入力軸は、前方(計測する方位方向)に向いて一致するように設定されている。またYb軸のジャイロ26と加速度計28は、ジャイロ25または加速度計27に対して直交する方向(Xb軸方向を前方とする場合には、Yb軸は右方)を入力軸として設定される。このように、本装置の構成に関しては、回転機構を要する可動部を含むことがなく、装置運用上の信頼性ならびに耐環境性に優れているのみならず、該可動部における疲労症状等に対応する定期的なメンテナンス作業を、全く不要にすることができるという利点がある。
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
以下においては、図1および図3を参照して、本実施形態の動作原理に関わる機能動作について説明する。図1において、Xb軸のジャイロ1より検出出力される角速度ωxbとYb軸のジャイロ2より検出出力される角速度ωybは、Z角速度推定器3と水平変換器4に入力される。また、Xb軸の加速度計5より検出出力される加速度AxbとYb軸の加速度計6より検出出力される加速度Aybは、直接速度ループ7に入力される。Z角速度推定器3においては、方位計測中には方位が不変であるとする条件を基にして、前記角速度ωxbおよび角速度ωybの入力と、速度ループ7より出力される更新された姿勢角φおよびθの入力を受けて、予め初期値として与えられている計測地点の緯度λを含む次式による演算処理が行われて、Xb軸、Yb軸の直交2軸に対応するZb軸の角速度推定値ωzb’が演算出力される。
上記(1)式においてΩは地球自転角速度である。Z角速度推定器3において演算出力されたZb軸の角速度推定値ωzb’は水平変換器4に入力されるが、水平変換器4においては、前記角速度ωxbおよび角速度ωybの入力と、速度ループ7において更新された姿勢角φおよびθの入力と、該Zb軸の角速度推定値ωzb’の入力を受けて、次式により、角速度ωxbおよび角速度ωybを地球水平面に投影した、X軸の水平角速度ωxLおよびY軸の水平角速度ωyLが演算出力される。
水平変換器4より出力されたX軸の水平角速度ωxLおよびY軸の水平角速度ωyLと、加速度計5および6より出力される加速度AxbおよびAybは、速度ループ7に入力される。図3は速度ループ7のブロック図であるが、実際には、X軸水平角速度ωxLと加速度Aybの入力に対応して、更新された姿勢角φと、角速度推定値ωxL’とを出力する速度ループと、Y軸水平角速度ωyLと加速度Axbの入力に対応して、更新された姿勢角θと、Y軸水平角速度推定値ωyL’とを出力する速度ループとは、それぞれ個別に設けられており一体化されてはいないが、図面表示を簡易化するために双方のケースを合わせて記載している。先ず図3を参照して、X軸水平角速度ωxLと加速度Aybの入力に対応して、更新された姿勢角φと、角速度推定値ωxL’が生成されて出力される場合について説明する。この動作内容は、更新された姿勢角θと、角速度推定値ωyL’が生成される場合についても全く同様である。
The X-axis horizontal angular velocity ωxL and Y-axis horizontal angular velocity ωyL output from the
図3において、水平変換器4より入力されるX軸水平角速度ωxLは減算器9に入力され、加速度計5より入力される加速度Aybは減算器13に入力される。減算器9において、速度ループの帰還出力により減算された角速度出力は、水平変換器10において地球水平面に直交する座標系の角速度から、装置を基準とする座標系の角速度に変換されて積分器11に入力される。積分器11においては、該角速度が積分処理されて装置のXb軸の姿勢角φが更新されて出力され、重力加速度カップリング演算器12に入力されるとともに、帰還出力としてZ角速度推定器3に送られる。重力加速度カップリング演算器12においては、この姿勢角φによるY軸加速度の重力加速度カップリング成分が演算出力されて減算器13に入力される。
In FIG. 3, the X-axis horizontal angular velocity ωxL input from the
減算器13においては、該重力加速度カップリング成分に対して加速度Aybによる減算処理が行われて、その差分は水平変換機器14に入力される。水平変換機器14においては、該差分に対応して、装置を基準とする座標系から地球水平面に直交する座標系に対する座標変換が行われて、水平変換機器14からは、地球水平面に直交する座標系に対応する前記差分が出力されて、減算器15、積分器16および増幅器17を含む負帰還ループにより形成される積分回路に入力される。
In the
本来、方位計測系に誤差が存在しない場合に該差分が存在するということは、実際に装置が加速している状態にあることを示しており、この差分を前記積分回路により積分することにより、積分器16からは装置の速度が出力される。しかし、本方位計においては、方位の計測中には移動してはならないという条件があるために、本来は速度が0となる筈である。依って、前記差分を積分して得られた速度は、X軸水平角速度ωxL自体の誤差に起因するものと考えて、該速度を増幅器18および19、積分器20および加算器21を含む帰還経路を経由して減算器9に帰還する負帰還ループを用いることにより、X軸の角速度推定値ωxL’の値を更新することとする。これにより、積分器20からは、新たに更新された角速度推定値ωxL’が出力されるとともに,積分器11からは、前述のように、更新された姿勢角φが出力されてZ角速度推定器3に送られる。このことは、速度ループ7に、Y軸水平角速度ωyLと加速度Axbが入力される場合についても同様であり、減算器13による差分を積分して得られた速度は、Y軸水平角速度ωyL自体の誤差に起因するものと考えて、該速度を上記の帰還経路を経由して減算器9に帰還する負帰還ループを用いることにより、更新されたY軸の角速度推定値ωyL’が積分器20より出力されるとともに、積分器11からは更新された姿勢角θが出力されてZ角速度推定器3に送られる。
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
図1のブロック図を参照して明らかなように、速度ループ7より出力される更新された姿勢角φおよびθは、共にZ角速度推定器3と水平変換器4に入力されており、Z角速度推定器3においては、これらの姿勢角φおよびθと、角速度ωxbおよびωybと、緯度λの入力を受けて、Z軸の角速度推定値ωzb’が新たに更新出力されて水平変換器4に入力される。
As apparent from the block diagram of FIG. 1, the updated attitude angles φ and θ output from the
水平変換器4においては、更新された姿勢角φおよびθと、更新された角速度推定値ωzb’と、角速度ωxbおよびωybの入力を受けて、新たに更新されたX軸水平角速度ωxLおよびY軸水平角速度ωyLが生成されて速度ループ7に入力される。速度ループ7においては、更新された水平角速度ωxLおよびωyLと、加速度AxbおよびAybの入力を受けて、前述のように、負帰還ループを介して、角速度推定値ωxL’およびωyL’と、姿勢角φおよびθが、それぞれ改めて更新されて出力される。
In the
このことより、速度ループ7において、積分器11より出力される姿勢角φおよびθを帰還始動入力として、Z角速度推定器3、水平変換器4および速度ループ7内の負帰還ループを統合する総合帰還ループが形成されていることが明らかである。この総合帰還ループを介して、速度ループ7より出力される角速度推定値ωxL’およびωyL’に対する更新処理が繰り返して行われ、最終的に該総合帰還ループが収束する時点においては、帰還経路に含まれる積分器20からは、X軸の角速度推定値ωxL’とY軸の角速度推定値ωyL’が、それぞれ最終の角速度推定値として出力され、積分器11からは、それぞれXb軸の姿勢角φとYb軸の姿勢角θが最終値として出力される。
Thus, in the
速度ループ7より出力される角速度推定値ωxL’およびωyL’は、総合帰還ループが収束するまでの過程において、逐次方位演算器8に入力されて方位角ψの演算が行われ、且つその演算値が逐次更新されているが、最終の角速度推定値ωxL’およびωyL’の値を受けて、方位演算器8においては、地球水平面における該角速度推定値ωxL’とωyL’の比、およびその極性を考慮して、次式に示される逆正接関数を用いて方位角ψが演算出力される。
なお速度ループ7においては、例えばX軸の角速度推定値ωxL’を求める場合に、方位計測中に姿勢角が変化することがあっても、姿勢角の変化に対応して求められた重力加速度カップリング成分と、Yb軸の加速度Aybとが相殺されるために、その姿勢角変化による影響を受けることなく角速度推定値ωxL’の値を求めることができる。このことはY軸の角速度推定値ωyL’を求める場合においても全く同様であり、姿勢角の変化に対応して求められた重力加速度カップリング成分と、Xb軸の加速度Axbとが相殺されるために、その姿勢角変化による影響を受けることなく角速度推定値ωyL’の値を求めることができる。
In the
また図3において、負帰還ループの帰還経路に含まれる増幅器17,18および19の各増幅器としては、角速度推定値ωxL’および角速度推定値ωyL’の値を速やかに収束させるために、時間とともに利得が変化する可変利得増幅器が用いられる。固定利得でも差し支えない。また装置が外乱による振動状態にある状態においても、加速度AxbおよびAybの入力に対応して積分器が設けられており、1回積分値、2回積分値等を角速度補正値としているために、高周波数領域における振動等の外乱による速度誤差は殆ど生じることがない。これにより姿勢角の変動および振動の影響を受けずに正確に方位角を計測することができる。
In FIG. 3, each of the
また本発明においては、Z角速度推定器3を設けることにより、姿勢角φおよびθと、角速度ωxbおよびωybと、緯度λを用いてZb軸の角速度推定値ωzb’を演算出力する点に一つの特徴があり、このZb軸の角速度推定値ωzb’を用いることにより、水平変換器4を介してX軸水平角速度ωxLおよびY軸水平角速度ωyLを抽出することが可能となる。このことにより、Zb軸にジャイロを配置することは不要となり、ジャイロが2軸のみに配置される状態においても高精度の方位を計測することが可能となる。またZb軸のジャイロを不要とすることにより、方位計の価格を抑制することが可能になるとともに、さらには、構成上可動部を含まないために、方位計としての耐環境性ならびに信頼性が向上される。
Further, in the present invention, by providing the Z
1,2,25,26,34,44,45,54〜56,61〜63 ジャイロ
3 Z角速度推定器
4,10,14,67 水平変換器
5,6,27,28,35,36,42,43,51〜53,64〜66 加速度計
7,68 速度ループ
8,69 方位演算器
9,13,15 減算器
11,16,20 積分器
12 重力加速度カップリング演算器
17〜19 増幅器
21 加算器
22 センサ部
23 ジャイロコントロール
24 CPU&I/F
29 ジャイロ制御
30 高電圧電源
31 CPU
32 加速度デジタイザ
33 シリアルインタフェース
37,47 座標変換手段
38 ジャイロ読取補償手段
39 緯度推定手段
40 切替器
41 機首角度計算手段
46 重力加速度鉛直成分検出手段
48 地球自転角速度鉛直成分検出手段
49 方位角演算手段
50 姿勢角演算手段
57 第1の演算手段
58 第2の演算手段
59 第3の演算手段
60 距離計
1, 2, 25, 26, 34, 44, 45, 54 to 56, 61 to 63 Gyro 3 Z
29
32
Claims (2)
Z角速度推定器、水平変換器、速度ループ及び方位演算器を備えてなり、
前記Z角速度推定器は、前記速度ループから受けるロール角φおよびピッチ角θ、並びに 初期値として入力されている緯度 λに基づき、前記Zb軸の角速度推定値ωzb’を演算し、
前記水平変換器は、前記角速度ωxbおよびωybと、前記角速度推定値ωzb’と、ロール角φおよびピッチ角θの入力を受けて、該角速度ωxbおよびωybを、地球水平直交2軸のX軸水平角速度ωxLおよびY軸水平角速度ωyLに変換し、
前記速度ループは、前記加速度計のXb軸の加速度AxbおよびYb軸の加速度Aybと、前記地球水平直交2軸のX軸水平角速度ωxLおよびY軸水平角速度ωyLの入力を受けて、負帰還ループを介して前記ロール角φおよびピッチ角θの更新を行い、前記Z角速度推定器ならびに水平変換器に帰還するとともに、角速度推定値ωxL’および角速度推定値ωyL’を生成し、
前記方位演算器は、前記速度ループより出力される角速度推定値ωxL’および角速度推定値ωyL’の入力を受けて、これら角速度推定値ωxL’およびωyL’の比に対応する逆正接関数を用いて前記方位角ψを推定する演算をし、
前記Z角速度推定器、水平変換器および速度ループを含む総合帰還ループを介して、前記速度ループより出力される角速度推定値ωxL’と角速度推定値ωyL’とを逐次更新し、該総合帰還ループの収束時に最終的に該速度ループより出力される角速度推定値ωxL’と角速度推定値ωyL’とを用いて、方位演算器において方位角ψを演算することを特徴とする方位計。 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.
ωzb’=(ωxb・sinθ−ωyb・cosθ・sinφ−Ω・sinλ)/(cosθ・cosφ) 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φ)
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2004098835A JP3884742B2 (en) | 2004-03-30 | 2004-03-30 | Compass |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2004098835A JP3884742B2 (en) | 2004-03-30 | 2004-03-30 | Compass |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2005283385A JP2005283385A (en) | 2005-10-13 |
JP3884742B2 true JP3884742B2 (en) | 2007-02-21 |
Family
ID=35181932
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2004098835A Expired - Lifetime JP3884742B2 (en) | 2004-03-30 | 2004-03-30 | Compass |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3884742B2 (en) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114322970B (en) * | 2021-11-30 | 2024-01-12 | 湖南航天机电设备与特种材料研究所 | Dual gyroscope north-seeking method, system and storage medium |
-
2004
- 2004-03-30 JP JP2004098835A patent/JP3884742B2/en not_active Expired - Lifetime
Also Published As
Publication number | Publication date |
---|---|
JP2005283385A (en) | 2005-10-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR100651549B1 (en) | Method and apparatus for measuring speed of land vehicle | |
JP5602070B2 (en) | POSITIONING DEVICE, POSITIONING METHOD OF POSITIONING DEVICE, AND POSITIONING PROGRAM | |
JP2009002735A (en) | Angular velocity detector | |
JP2012173190A (en) | Positioning system and positioning method | |
JP2005283586A (en) | Error correction of inertia navigation system | |
JP2008002992A (en) | Device and method for detecting attitude angle | |
JP7025215B2 (en) | Positioning system and positioning method | |
CN101617199A (en) | A kind of method that is used for the drift of compensated position measuring equipment | |
CN103712622A (en) | Gyroscopic drift estimation compensation method and device based on rotation of inertial measurement unit | |
JP4447791B2 (en) | Aircraft attitude determination apparatus having a gyrometer and an accelerometer | |
JP2000321070A (en) | Strap-down inertial navigation device | |
CN108871323B (en) | High-precision navigation method of low-cost inertial sensor in locomotive environment | |
CN108592917A (en) | A kind of Kalman filtering Attitude estimation method based on misalignment | |
CN116429095A (en) | Method for measuring muzzle vibration during advancing based on combination of main inertial navigation and sub inertial navigation | |
CN110779553A (en) | Calibration method for magnetometer data | |
CN111141285B (en) | Aviation gravity measuring device | |
JP2015004593A (en) | Navigation device | |
JP3884742B2 (en) | Compass | |
CN115523919A (en) | Nine-axis attitude calculation method based on gyro drift optimization | |
JP3425689B2 (en) | Inertial device | |
JP2023101310A (en) | Method for calculating calibration value | |
JP2003139536A (en) | Declinometer and azimuth measuring method | |
JPS62106385A (en) | Method of calculating roll angle and pitch angle of vehicle and doppler inertial loop therefor | |
JP6632727B2 (en) | Angle measuring device | |
JP2010271209A (en) | Portable information apparatus, method and program for correcting offset error of acceleration sensor |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20060929 |
|
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: 20061114 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20061117 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 Ref document number: 3884742 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: 20091124 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101124 Year of fee payment: 4 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101124 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101124 Year of fee payment: 4 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111124 Year of fee payment: 5 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20111124 Year of fee payment: 5 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121124 Year of fee payment: 6 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121124 Year of fee payment: 6 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20121124 Year of fee payment: 6 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131124 Year of fee payment: 7 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |