(57)【要約】      （修正有）【課題】装置の姿勢角の変動ならびに外乱による影響を排除して方位計測精度の向上を図るとともに、ジャイロの配置を低減して低価格の方位計を提供する。【解決手段】３軸（Ｘb、Ｙb、Ｚb）座標系において、ジャイロ１、２のＸb軸角速度出力ωxbとＹb軸角速度出力ωybは、Ｚ角速度推定器３と水平変換器４に入力され、Ｘb軸加速度計５、Yb軸加速度計６の出力Ａxb、Ａybは速度ループ７に入力される。Ｚ軸速度推定器３では、緯度λ、姿勢角φ、θの入力を受けてＺb軸の角速度推定値ωzb’が演算され、水平変換器４に入力される。水平変換器４では姿勢角φ、θを受け、地球水平面に投影されたωxLとωyLが生成され、速度ループ７に入力される。速度ループ７においては、ωxL、ωyLと、Ａxb、Ａybの入力を受けて、角速度推定値ωxL’およびωyL’が出力されて方位演算器８に入力され、これらのωxL’およびωyL’の最終更新値を用いて、方位角ψが演算出力される。【選択図】図１
2005283385.tif 000002
【特許請求の範囲】
【請求項１】
互いに直交するＸ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’とを用いて、方位演算器において方位角ψを演算することを特徴とする方位計。
【請求項２】
前記Ｚ角速度推定器において、初期値として入力されるλと、地球自転角速度Ωと、Ｘb軸の角速度ωxbおよびＹb軸の角速度ωybと、姿勢角φおよびθとの入力を受けて、次式を用いてＺb軸の角速度推定値ωzb’を演算出力することを特徴とする請求項1記載の方位計。
ωzb’＝(ωxb・sinθ−ωyb・cosθ・sinφ−Ω・sinλ)/(cosθ・cosφ)  
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は方位計に関し、特に車両、穴掘用掘進機などの装置に搭載され、ジャイロおよび加速度計を用いて、該車両または他の装置等の進行方位角を計測する方位計に関する。
【背景技術】
【０００２】
従来、ジャイロおよび加速度計を用いて方位角を計測する方位計としては、特許文献１（特公平７−１５３８４）に、ジャイロコンパシング装置として実現された北探知システムが開示されている。このシステムにおける、ジャイロと加速度計より方位角を計測する主要機能ブロック図を図５に示す。図５に示されるように、Ｘ軸またはＹ軸の何れか一方の１軸に配置された１個のジャイロ３４と、Ｘ軸およびＹ軸の２軸に配置された加速度計３５および３６と、座標変換手段３７と、ジャイロ読取補償手段３８と、緯度推定手段３９と、切替器４０と、機首角度計算手段４１とを備えて構成されている。
【０００３】
また特許文献２（特開平６−１１３４９）の方位計には、方位測定の時間短縮ならびに測定悪条件に対応できる方位計が開示されている。この従来例における、ジャイロと加速度計より方位角を計測する主要機能ブロック図は、図６に示されるように、Ｘ軸およびＹ軸に配置されたジャイロ４２および４３と、Ｘ軸およびＹ軸に配置された加速度計４４および４５と、重力加速度鉛直成分検出手段４６と、座標変換手段４７と、地球自転角速度鉛直成分検出手段４８と、方位角演算手段４９と、姿勢角演算手段５０とを備えて構成される。
【０００４】
更に、特許文献３（特開平５−１８７８７３）には、あらゆる方向の被測定孔の傾斜角および方位角を、同時に、且つセンサ部を停止させることなく短時間にて計測することを目的とした、孔曲がり計測装置および孔曲がり計測方法が開示されている、この従来例における、ジャイロと加速度計より方位角を計測する主要機能ブロック図は、図７に示されるように、Ｘ軸、Ｙ軸およびＺ軸に配置された加速度計５１、５２および５３と、Ｘ軸、Ｙ軸およびＺ軸に配置されたにジャイロ５４、５５および５６と、第１の演算手段５７と、第２の演算手段５８と、第３の演算手段５９と、距離計６０とを備えて構成される。
【０００５】
他の例として、非特許文献１[日本航空宇宙工業会発行の技術書「スキューリダンダント・イナーシャルシステムの研究」（発行日：１９８５年３月），ISSN0910-1500]においては、ジャイロと加速度計より方位角を計測する主要機能ブロック図として、図８に示されるように、Ｘ軸、Ｙ軸およびＺ軸に配置されたジャイロ６１、６２および６３と、Ｘ軸、Ｙ軸およびＺ軸に配置された加速度計６４、６５および６６と、水平変換器６７と、速度ループ６８と、方位演算器６９とを備えたブロック図が示されている。
【０００６】
【特許文献１】特公平７−１５３８４
【特許文献２】特開平６−１１３４９
【特許文献３】特開平５−１８７８７３
【非特許文献１】日本航空宇宙工業会発行の技術書「スキューリダンダント・イナーシャルシステムの研究」（発行日：１９８５年３月）， ISSN0910-1500
【発明の開示】
【発明が解決しようとする課題】
【０００７】
上述した特許文献１の方位計においては、リングレーザージャイロを２軸分設けているために、ランダムドリフトの影響を抑制する時間が1軸分に比して２倍の時間を必要とし、また入力軸を２方向に指向させるための可動部を要するために、耐環境性/信頼性に乏しいという欠点があり、加速度計出力の微分値については、特に高周波領域における振動入力等の外乱による影響を大きく受けやすく、このために角速度補正の誤差が生じ易いという欠点も存在している。また、特許文献２の方位計においては、方位計に動揺が入力されると、ベクトル値が15度/Ｈよりも大きくなるような場合には、方位計算に誤差が生じるという問題があり、特許文献３の孔曲がり計測装置および孔曲がり計測方法においては、３軸の動きに対しては誤差を持たずに孔曲がりの計測を行うことは可能であるが、ジャイロおよび加速度計をそれぞれ３個づつ設けることを必要とし、コストアップとなり高価格になるという欠点がある。更に非特許文献１においても同様に、３軸の動きに対して誤差を持たずに方位角を計測することはできるが、ジャイロと加速度計をそれぞれ３個づつ設けることが必要となり、コストアップとなって高価格になるという欠点がある。
【０００８】
本発明の目的は、上記の従来例の課題を解決して可動部分を除去して耐環境性/信頼性の向上を図り、装置の姿勢角変化による影響ならびに２軸に対する高周波数領域等における振動の外乱による影響を排除して、精度高い方位計測を可能にするとともに、地球自転の鉛直成分算定に際しては、ベクトル値が15度/Ｈよりも大きくなるような状態においても計算誤差を生じることなく、且つジャイロと加速度計をそれぞれ２軸のみに配置することにより、価格の低減を実現することのできる方位計を提供することにある。
【課題を解決するための手段】
【０００９】
前述の課題を解決するために本発明は次の手段を提供する。
【００１０】
（１）互いに直交するＸ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’とを用いて、方位演算器において方位角ψを演算することを特徴とする方位計。
【００１１】
（２）前記Ｚ角速度推定器において、初期値として入力されるλと、地球自転角速度Ωと、Ｘb軸の角速度ωxbおよびＹb軸の角速度ωybと、姿勢角φおよびθとの入力を受けて、次式を用いてＺb軸の角速度推定値ωzb’を演算出力することを特徴とする請求項1記載の方位計。
ω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’の最終推定値を用いて方位演算を行うことにより、姿勢角変化および振動の影響を受けることなく正確に方位角が計測される。
【発明の効果】
【００１３】
以上要するに、本発明には、本発明の特徴とするＺ角速度推定器により抽出されるＺb軸角速度推定値と、Ｘb軸角速度およびＹb軸角速度と、速度ループより出力される姿勢角を入力として、水平変換器より出力される水平面直交Ｘ、Ｙ２軸の角速度推定値を速度ループに入力し、該Ｚ角速度推定器、水平変換器および速度ループを含む総合帰還ループを介して、該速度ループにおいて生成されるＸ軸角速度推定値とＹ軸角速度推定値を用いて方位角を演算出力することにより、姿勢角変化および外乱の振動の影響を受けることなく正確に方位角が計測することができるという効果がある。
【００１４】
また、本発明には、Ｚ角速度推定器によるＺb軸の角速度推定値の抽出により、Ｚb軸に対するジャイロの配置を不要とし、これにより２軸のジャイロのみにて高精度の方位を計測することが可能になるとともに、方位計の価格を低減することができるという効果があり、さらに方位計の構成に可動部分を含まないために、耐環境性/信頼性の向上を図ることができるという効果がある。
【発明を実施するための最良の形態】
【００１５】
次に、本発明の実施形態について説明する。図１は本実施形態のジャイロと加速度計より方位角を計測する主要機能ブロック図である。図1に示されるように、本実施形態は、方位計測の基準として装置に設定された直交３軸座標系のＸb軸に配置されるジャイロ１および加速度計５と、Ｙb軸に配置されるジャイロ２および加速度計６と、Ｚ軸角速度推定器３と、水平変換器４と、速度ループ７と、方位演算器８とを備えて構成される。上記の速度ループ７は、図３に示されるように、減算器９，１３および１５と、水平変換器１０および１４と、積分器１１，１６および２０と、重力加速度カップリング演算器１２と、増幅器１７〜１９と、加算器２1とを備えて構成される。なお図３においては、Ｘb軸とＹb軸の各軸に対応する速度ループ７が併せて示されているが、実際には、Ｘb軸とＹb軸の各軸に対応して速度ループ７が求められる。
【００１６】
上記の図1および図３を参照して、本実施形態の方位計測の動作原理に関わる機能動作を説明する前に、図２および図４を参照して、本発明の方位計の構成概要・装置設置条件に関わる信頼性ならびに耐環境性の優位性について説明する。本装置は車両または他の装置に搭載されて、該車両または装置の方位角を計測する装置であり、図４に示されるように、Ｘb軸のジャイロ２５、Ｙb軸のジャイロ２６、Ｘb軸の加速度計２７およびＹb軸の加速度計２８を含むセンサ部２２と、ジャイロ制御２９および高電圧電源３０を含むジャイロコントロール２３と、ＣＰＵ３１、加速度デジタイザ３２およびシリアルインタフェース３３を含むＣＰＵ&I/F２４とを備えて構成される。ジャイロ２５およびジャイロ２６としては、バイアス再現性が高いＲＬＧ（リングレーザージャイロ）などの高精度ジャイロが使用される。加速度計２７および加速度計２８は、姿勢角再現性がある加速度計、傾斜計等が使用される。ジャイロ２５および２６からの角速度は、ジャイロコントロール２３を介してＣＰＵ３１に入力され、加速度計２７および２８からの加速度は加速度デジタイザ３２を介してＣＰＵ３１に入力されて、ＣＰＵ３１において演算処理が行われる。この場合に装置に対するコマンド内容および計測地点における緯度情報等は、予めシリアルインタフェース３３を介してＣＰＵ３１に与えられており、この演算処理結果による装置の方位角および姿勢角は、シリアルインタフェース３３を介して外部に出力される。
【００１７】
次に図２(a) に示されるのは、Ｘb軸がロール軸であり、Ｙb軸がピッチ軸である場合について、装置の側面より見た装置前方向と上方向に対応して配置されるジャイロ２５と加速度計２７の設定向きと、ジャイロ２６と加速度計２８の設定向きとを示す模式図であり、図２(b)は、装置の上部より見た装置前方向、装置右方向および装置左方向に対応して配置されるジャイロ２５と加速度計２７の設定向きと、ジャイロ２６と加速度計２８の設定向きとを示す模式図である。図２においては、各ジャイロおよび各加速度計には、対応する軸のＸbおよびＹbが付記されている。これらのジャイロおよび加速度計に対応する入力軸は、通常は直方体型として形成されるケースの底面に平行になるように設定されており、Ｘb軸のジャイロ２５と加速度計２７の入力軸は、前方（計測する方位方向）に向いて一致するように設定されている。またＹb軸のジャイロ２６と加速度計２８は、ジャイロ２５または加速度計２７に対して直交する方向（Ｘb軸方向を前方とする場合には、Ｙb軸は右方）を入力軸として設定される。このように、本装置の構成に関しては、回転機構を要する可動部を含むことがなく、装置運用上の信頼性ならびに耐環境性に優れているのみならず、該可動部における疲労症状等に対応する定期的なメンテナンス作業を、全く不要にすることができるという利点がある。
【００１８】
以下においては、図１および図３を参照して、本実施形態の動作原理に関わる機能動作について説明する。図１において、Ｘb軸のジャイロ１より検出出力される角速度ωxbとＹb軸のジャイロ２より検出出力される角速度ωybは、Ｚ角速度推定器３と水平変換器４に入力される。また、Ｘb軸の加速度計５より検出出力される加速度ＡxbとＹb軸の加速度計６より検出出力される加速度Ａybは、直接速度ループ７に入力される。Ｚ角速度推定器３においては、方位計測中には方位が不変であるとする条件を基にして、前記角速度ωxbおよび角速度ωybの入力と、速度ループ７より出力される更新された姿勢角φおよびθの入力を受けて、予め初期値として与えられている計測地点の緯度λを含む次式による演算処理が行われて、Ｘb軸、Ｙb軸の直交２軸に対応するＺb軸の角速度推定値ωzb’が演算出力される。
【数１】
2005283385.tif 000003
【００１９】
上記(1)式においてΩは地球自転角速度である。Ｚ角速度推定器３において演算出力されたＺb軸の角速度推定値ωzb’は水平変換器４に入力されるが、水平変換器４においては、前記角速度ωxbおよび角速度ωybの入力と、速度ループ７において更新された姿勢角φおよびθの入力と、該Ｚb軸の角速度推定値ωzb’の入力を受けて、次式により、角速度ωxbおよび角速度ωybを地球水平面に投影した、Ｘ軸の水平角速度ωxLおよびＹ軸の水平角速度ωyLが演算出力される。
【数２】
2005283385.tif 000004
【００２０】
水平変換器４より出力されたＸ軸の水平角速度ωxLおよびＹ軸の水平角速度ωyLと、加速度計５および６より出力される加速度ＡxbおよびＡybは、速度ループ７に入力される。図３は速度ループ７のブロック図であるが、実際には、Ｘ軸水平角速度ωxLと加速度Ａybの入力に対応して、更新された姿勢角φと、角速度推定値ωxL’とを出力する速度ループと、Ｙ軸水平角速度ωyLと加速度Ａxbの入力に対応して、更新された姿勢角θと、Ｙ軸水平角速度推定値ωyL’とを出力する速度ループとは、それぞれ個別に設けられており一体化されてはいないが、図面表示を簡易化するために双方のケースを合わせて記載している。先ず図３を参照して、Ｘ軸水平角速度ωxLと加速度Ａybの入力に対応して、更新された姿勢角φと、角速度推定値ωxL’が生成されて出力される場合について説明する。この動作内容は、更新された姿勢角θと、角速度推定値ωyL’が生成される場合についても全く同様である。
【００２１】
図３において、水平変換器４より入力されるＸ軸水平角速度ωxLは減算器９に入力され、加速度計５より入力される加速度Ａybは減算器1３に入力される。減算器９において、速度ループの帰還出力により減算された角速度出力は、水平変換器１０において地球水平面に直交する座標系の角速度から、装置を基準とする座標系の角速度に変換されて積分器１１に入力される。積分器１１においては、該角速度が積分処理されて装置のＸb軸の姿勢角φが更新されて出力され、重力加速度カップリング演算器1２に入力されるとともに、帰還出力としてＺ角速度推定器３に送られる。重力加速度カップリング演算器1２においては、この姿勢角φによるＹ軸加速度の重力加速度カップリング成分が演算出力されて減算器１３に入力される。
【００２２】
減算器１３においては、該重力加速度カップリング成分に対して加速度Ａybによる減算処理が行われて、その差分は水平変換機器1４に入力される。水平変換機器1４においては、該差分に対応して、装置を基準とする座標系から地球水平面に直交する座標系に対する座標変換が行われて、水平変換機器1４からは、地球水平面に直交する座標系に対応する前記差分が出力されて、減算器１５、積分器１６および増幅器１７を含む負帰還ループにより形成される積分回路に入力される。
【００２３】
本来、方位計測系に誤差が存在しない場合に該差分が存在するということは、実際に装置が加速している状態にあることを示しており、この差分を前記積分回路により積分することにより、積分器１６からは装置の速度が出力される。しかし、本方位計においては、方位の計測中には移動してはならないという条件があるために、本来は速度が０となる筈である。依って、前記差分を積分して得られた速度は、Ｘ軸水平角速度ωxL自体の誤差に起因するものと考えて、該速度を増幅器１８および１９、積分器２０および加算器２１を含む帰還経路を経由して減算器９に帰還する負帰還ループを用いることにより、Ｘ軸の角速度推定値ωxL’の値を更新することとする。これにより、積分器２０からは、新たに更新された角速度推定値ωxL’が出力されるとともに，積分器１１からは、前述のように、更新された姿勢角φが出力されてＺ角速度推定器３に送られる。このことは、速度ループ７に、Ｙ軸水平角速度ωyLと加速度Ａxbが入力される場合についても同様であり、減算器１３による差分を積分して得られた速度は、Ｙ軸水平角速度ωyL自体の誤差に起因するものと考えて、該速度を上記の帰還経路を経由して減算器９に帰還する負帰還ループを用いることにより、更新されたＹ軸の角速度推定値ωyL’が積分器２０より出力されるとともに、積分器１１からは更新された姿勢角θが出力されてＺ角速度推定器３に送られる。
【００２４】
図１のブロック図を参照して明らかなように、速度ループ７より出力される更新された姿勢角φおよびθは、共にＺ角速度推定器３と水平変換器４に入力されており、Ｚ角速度推定器３においては、これらの姿勢角φおよびθと、角速度ωxbおよびωybと、緯度λの入力を受けて、Ｚ軸の角速度推定値ωzb’が新たに更新出力されて水平変換器４に入力される。
【００２５】
水平変換器４においては、更新された姿勢角φおよびθと、更新された角速度推定値ωzb’と、角速度ωxbおよびωybの入力を受けて、新たに更新されたＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLが生成されて速度ループ７に入力される。速度ループ７においては、更新された水平角速度ωxLおよびωyLと、加速度ＡxbおよびＡybの入力を受けて、前述のように、負帰還ループを介して、角速度推定値ωxL’およびωyL’と、姿勢角φおよびθが、それぞれ改めて更新されて出力される。
【００２６】
このことより、速度ループ７において、積分器１１より出力される姿勢角φおよびθを帰還始動入力として、Ｚ角速度推定器３、水平変換器４および速度ループ７内の負帰還ループを統合する総合帰還ループが形成されていることが明らかである。この総合帰還ループを介して、速度ループ７より出力される角速度推定値ωxL’およびωyL’に対する更新処理が繰り返して行われ、最終的に該総合帰還ループが収束する時点においては、帰還経路に含まれる積分器２０からは、Ｘ軸の角速度推定値ωxL’とＹ軸の角速度推定値ωyL’が、それぞれ最終の角速度推定値として出力され、積分器１１からは、それぞれＸb軸の姿勢角φとＹb軸の姿勢角θが最終値として出力される。
【００２７】
速度ループ７より出力される角速度推定値ωxL’およびωyL’は、総合帰還ループが収束するまでの過程において、逐次方位演算器８に入力されて方位角ψの演算が行われ、且つその演算値が逐次更新されているが、最終の角速度推定値ωxL’およびωyL’の値を受けて、方位演算器８においては、地球水平面における該角速度推定値ωxL’とωyL’の比、およびその極性を考慮して、次式に示される逆正接関数を用いて方位角ψが演算出力される。
【数３】
2005283385.tif 000005
【００２８】
なお速度ループ７においては、例えばＸ軸の角速度推定値ωxL’を求める場合に、方位計測中に姿勢角が変化することがあっても、姿勢角の変化に対応して求められた重力加速度カップリング成分と、Ｙb軸の加速度Ａybとが相殺されるために、その姿勢角変化による影響を受けることなく角速度推定値ωxL’の値を求めることができる。このことはＹ軸の角速度推定値ωyL’を求める場合においても全く同様であり、姿勢角の変化に対応して求められた重力加速度カップリング成分と、Ｘb軸の加速度Ａxbとが相殺されるために、その姿勢角変化による影響を受けることなく角速度推定値ωyL’の値を求めることができる。
【００２９】
また図３において、負帰還ループの帰還経路に含まれる増幅器１７，１８および１９の各増幅器としては、角速度推定値ωxL’および角速度推定値ωyL’の値を速やかに収束させるために、時間とともに利得が変化する可変利得増幅器が用いられる。固定利得でも差し支えない。また装置が外乱による振動状態にある状態においても、加速度ＡxbおよびＡybの入力に対応して積分器が設けられており、１回積分値、２回積分値等を角速度補正値としているために、高周波数領域における振動等の外乱による速度誤差は殆ど生じることがない。これにより姿勢角の変動および振動の影響を受けずに正確に方位角を計測することができる。
【００３０】
また本発明においては、Ｚ角速度推定器３を設けることにより、姿勢角φおよびθと、角速度ωxbおよびωybと、緯度λを用いてＺb軸の角速度推定値ωzb’を演算出力する点に一つの特徴があり、このＺb軸の角速度推定値ωzb’を用いることにより、水平変換器４を介してＸ軸水平角速度ωxLおよびＹ軸水平角速度ωyLを抽出することが可能となる。このことにより、Ｚb軸にジャイロを配置することは不要となり、ジャイロが２軸のみに配置される状態においても高精度の方位を計測することが可能となる。またＺb軸のジャイロを不要とすることにより、方位計の価格を抑制することが可能になるとともに、さらには、構成上可動部を含まないために、方位計としての耐環境性ならびに信頼性が向上される。
【図面の簡単な説明】
【００３１】
【図１】本発明の実施形態の機能ブロック図である。
【図２】装置の基準となる座標系に設定されるジャイロと加速度計の向きを示す模式図である。
【図３】前記実施形態に含まれる速度ループのブロック図である。
【図４】前記実施形態の構成を示す概念図である。
【図５】従来例（１）の機能ブロック図である。
【図６】従来例（２）の機能ブロック図である。
【図７】従来例（３）の機能ブロック図である。
【図８】従来例（４）の機能ブロック図である。
【符号の説明】
【００３２】
１，２，２５，２６，３４，４４，４５，５４〜５６，６１〜６３    ジャイロ
３    Ｚ角速度推定器
４，１０，１４，６７    水平変換器
５，６，２７，２８，３５，３６，４２，４３，５１〜５３，６４〜６６    加速度計
７，６８    速度ループ
８，６９    方位演算器
９，１３，１５    減算器
１１，１６，２０    積分器
１２    重力加速度カップリング演算器
１７〜１９    増幅器
２１    加算器
２２    センサ部
２３    ジャイロコントロール
２４    ＣＰＵ&Ｉ/Ｆ
２９    ジャイロ制御
３０    高電圧電源
３１    ＣＰＵ
３２    加速度デジタイザ
３３    シリアルインタフェース
３７，４７    座標変換手段
３８    ジャイロ読取補償手段
３９    緯度推定手段
４０    切替器
４１    機首角度計算手段
４６    重力加速度鉛直成分検出手段
４８    地球自転角速度鉛直成分検出手段
４９    方位角演算手段
５０    姿勢角演算手段
５７    第１の演算手段
５８    第２の演算手段
５９    第３の演算手段
６０    距離計
【図１】
2005283385.tif 000006
【図２】
2005283385.tif 000007
【図３】
2005283385.tif 000008
【図４】
2005283385.tif 000009
【図５】
2005283385.tif 000010
【図６】
2005283385.tif 000011
【図７】
2005283385.tif 000012
【図８】
2005283385.tif 000013