JP2006047295A

JP2006047295A - 方位計測装置

Info

Publication number: JP2006047295A
Application number: JP2005187515A
Authority: JP
Inventors: Futoshi Magosaki; 太孫崎
Original assignee: Sumitomo Precision Products Co Ltd
Current assignee: Sumitomo Precision Products Co Ltd
Priority date: 2004-06-30
Filing date: 2005-06-28
Publication date: 2006-02-16

Abstract

【課題】任意の姿勢で真の方位を正確に計測することができる方位計測装置を提供する。
【解決手段】３軸センサ本体１０は、Ｘ軸周りにおける地球の自転角速度成分ω_xとＸ軸方向における地球の重力加速度成分ｇ_xとを検知する２軸センサユニット１８と、Ｙ軸周りにおける地球の自転角速度成分ω_yとＹ軸方向における地球の重力加速度成分ｇ_zとを検知する１軸センサユニット２０とを備える。センサユニット１８，２０により検知された自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）に基づいて次の式（１）により方位角ψが算出される。

【選択図】なし

Description

本発明は、方位計測装置に関し、さらに詳しくは、３軸ジャイロを利用して真北を検知する真北検知器（North Finder）に関する。

磁北は真北から少しずれているため、磁気コンパスでは真北を正確に計測することはできない。しかしながら、行政地図は真北を基準に作成され、建築基準法も真北を基準に定められている。したがって、土木・建築の分野においては、真北を正確に計測する必要がある。特に地下トンネル工事では鉱脈等の影響で、磁気コンパスは正しく機能しない。

真北を正確に計測する真北検知器として、従来より、地球の自転角速度を検知して真北を検知するジャイロコンパスが用いられている（特許文献１〜１１）。

しかしながら、従来のジャイロコンパスを利用した方位計測装置では、基準面を水平にする必要があるため、運用が制約され、不便である。また、回転テーブル及びそれを駆動するモータが必要であるため、機構が複雑で、信頼性に欠ける。さらに、プラットフォーム方式ジャイロやリングレーザジャイロなど、その多くは高価なジャイロを使用している。

その他、ジャイロを利用しないものとして、ＧＰＳ（Global Positioning System）や磁気コンパスを利用した方位計（特許文献１２〜１４）、カルマンフィルタを利用した姿勢計測（特許文献１５〜１６）などがある。
特開２００１−２１５１２１号公報特開平１１−１９０６３３号公報特開平１１−１６００７２号公報特開平１０−２８１７７３号公報特開平９−１３８１２８号公報特開平７−２９４２５８号公報特開平６−１１３５０号公報特開平６−３１４９号公報米国特許第５４８１９５７号明細書米国特許第５０６０３９２号明細書米国特許第５３６９８８９号明細書米国特許第４２１４４８２号明細書特開２００２−９０１７３号公報特開２００１−９１２５７号公報米国特許第６４１５２２３号明細書特開２００３−７５１７２号公報特開２００２−３２３３２２号公報

本発明の目的は、任意の姿勢で真の方位を正確に計測することができる方位計測装置を提供することである。

課題を解決するための手段及び発明の効果

本発明による方位計測装置は、検知手段と、方位角算出手段とを備える。検知手段は、第１の軸周りの自転角速度成分ω_x及び第１の軸方向の重力加速度成分ｇ_y、第１の軸と直交する第２の軸周りの自転角速度成分ω_y及び第２の軸方向の重力加速度成分ｇ_y、並びに第１及び第２の軸と直交する第３の軸周りの自転角速度成分ω_z及び第３の軸方向の重力加速度成分ｇ_zを検知する。方位角算出手段は、検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（１）により方位角ψを算出する。

この方位計測装置では、検知手段により自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zが検知され、これらに基づいて方位角ψが算出される。したがって、検知手段を任意の姿勢で固定し、真の方位を正確に計測することができ、検知手段を固定するために水平面等の基準が不要である。

好ましくは、方位計測装置はさらに、時間平均値算出手段を備える。時間平均値算出手段は、検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ所定時間内に複数回計測し、それらの平均値を算出する。

この場合、自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zの時間平均値に基づいて方位角ψが算出されるので、算出される方位角ψの誤差が小さくなる。

好ましくは、方位計測装置はさらに、軸方向反転手段と、二方向平均値算出手段とを備える。軸方向反転手段は、第１から第３の軸方向をそれぞれ反転する。二方向平均値算出手段は、軸方向反転手段による軸方向の反転前と反転後とにおいて、検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ計測し、それらの平均値を算出する。

この場合、検知手段の軸方向を反転する前後で計測される、自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zの平均値に基づいて方位角ψが算出されるので、方位計測装置に特有のバイアスが相殺され、算出される方位角ψの誤差が小さくなる。

好ましくは、方位計測装置はさらに、ピッチ角を調節するためのピッチ角調節手段を備える。

この場合、ピッチ角を計測地の緯度とほぼ等しく設定することができ、その結果、算出される方位角ψの誤差が小さくなる。

好ましくは、方位計測装置はさらに、緯度算出手段と、エラー検出手段とを備える。緯度算出手段は、自転角速度Ω、検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（２）により計測値の緯度λを算出する。

エラー検出手段は、緯度算出手段により算出された緯度と既知の緯度との差が所定のしきい値よりも大きいときエラーを通知する。

この場合、計測者は方位計測装置又は計測方法に何らかの不具合があることを知ることができる。

好ましくは、検知手段は、筐体と、第１の角速度センサと、第１の加速度センサと、第２の角速度センサと、第２の加速度センサと、第３の角速度センサと、第３の加速度センサとを含む。第１の角速度センサは、筐体に固定され、自転角速度成分ω_xを検知する。第１の加速度センサは、筐体に固定され、重力加速度成分ｇ_xを検知する。第２の角速度センサは、筐体に固定され、自転角速度成分ω_yを検知する。第２の加速度センサは、筐体に固定され、重力加速度成分ｇ_yを検知する。第３の角速度センサは、筐体に固定され、自転角速度成分ω_zを検知する。第３の加速度センサは、筐体に固定され、重力加速度成分ｇ_zを検知する。

この場合、角速度センサ及び加速度センサは全て筐体に固定されているため、筐体を任意の姿勢で固定すれば、角速度センサ及び加速度センサも全て所定の相対的位置関係で固定される。よって、角速度センサ及び加速度センサの設置が容易である。

本発明による方位計測方法は、第１の軸周りの自転角速度成分ω_x及び第１の軸方向の重力加速度成分ｇ_xを検知するステップと、第１の軸と直交する第２の軸周りの自転角速度成分ω_y及び第２の軸方向の重力加速度成分ｇ_yを検知するステップと、第１及び第２の軸と直交する第３の軸周りの自転角速度成分ω_z及び第３の軸方向の重力加速度成分ｇ_zを検知するステップと、検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（１）により方位角ψを算出するステップとを備える。

この方位計測方法では、自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zが検知され、これらに基づいて方位角ψが算出される。したがって、角速度及び加速度センサを任意の姿勢で固定し、真の方位を正確に計測することができ、角速度及び加速度センサを固定するために水平面等の基準が不要である。

好ましくは、方位計測方法はさらに、検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ所定時間内に複数回計測し、それらの平均値を算出するステップを備える。

好ましくは、方位計測方法はさらに、第１から第３の軸方向をそれぞれ反転するステップと、軸方向の反転前と反転後とにおいて、検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ計測し、それらの平均値を算出するステップとを備える。

この場合、軸方向を反転する前後で計測される、自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zの平均値に基づいて方位角ψが算出されるので、方位計測装置に特有のバイアスが相殺され、算出される方位角ψの誤差が小さくなる。

好ましくは、方位計測方法はさらに、ピッチ角θを計測地の緯度とほぼ等しく設定するステップを備える。

この場合、算出される方位角ψの誤差が小さくなる。

好ましくは、方位計測方法はさらに、自転角速度Ω、検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（２）により計測値の緯度λを算出するステップと、算出された緯度と既知の緯度との差が所定のしきい値よりも大きいときエラーを通知するステップとを備える。

本発明において、平均とは、相加平均（算術平均）、相乗平均、調和平均の他、複数の値を代表する値を算出する全ての方法を含み、たとえば最小二乗法などの統計学的手法も含む。

以下、図面を参照し、本発明の実施の形態を詳しく説明する。図中同一又は相当部分には同一符号を付してその説明は繰り返さない。

［本発明の計測原理］
最初に、本発明による方位計測装置の計測原理を説明する。

図１を参照して、緯度λにおける地球上の任意の位置で接する水平面を想定し、ここに直交座標系を定義する。この直交座標系は、北方向North、東方向East及び鉛直方向Downからなり、以下「ＮＥＤ座標系」と呼ぶ。

ここで、地球の自転角速度をΩとすると、自転角速度の北方向成分Ω_Ｎ、東方向成分Ω_Ｅ及び鉛直方向成分Ω_Ｄは、それぞれ次の式（３）〜（５）で表される。
Ω_Ｎ＝Ωｃｏｓλ …（３）
Ω_Ｅ＝０ …（４）
Ω_Ｄ＝−Ωｓｉｎλ …（５）

本発明では、図２に示すように、３軸（Ｘ，Ｙ，Ｚ）周りの自転角速度（ω_x，ω_y，ω_z）及び３軸（Ｘ，Ｙ，Ｚ）方向の重力加速度（ｇ_x，ｇ_y，ｇ_z）を検知することができる３軸センサ本体１０を用いる。センサ本体１０は静止ステージ１２上に載置される。静止ステージ１２は水平面である必要はなく、センサ本体１０は任意の姿勢で静止される。

センサ本体１０の姿勢は、図３に示したオイラー角（方位角ψ、ピッチ角θ及びロール角φ）で表される。方位角ψは、図３（ａ）に示すように、鉛直方向Down（又はＺ方向）を向いて時計回りを正とし、北方向NorthとＸ方向（又は東方向EastとＹ方向）のなす角である。ピッチ角θは、図３（ｂ）に示すように、東方向East（又はＹ方向）を向いて時計回りを正とし、北方向NorthとＸ方向（又は鉛直方向DownとＺ方向）のなす角である。ロール角φは、図３（ｃ）に示すように、北方向North（又はＸ方向）を向いて時計回りを正とし、東方向EastとＹ方向（又は鉛直方向DownとＺ方向）のなす角である。

ＮＥＤ座標系からセンサ本体１０のＸＹＺ座標系への座標変換行列Ｃ_ｎｂは次の式（６）で表される。

したがって、地球の自転角速度（ω_x，ω_y，ω_z）は次の式（７）で表される。

また、地球の重力加速度（ｇ_x，ｇ_y，ｇ_z）は次の式（８）で表される。

ここで、ロール角φは次の式（９）で表される。

ｔａｎφ＝ｇ_ｙ／ｇ_ｚ …（９）
また、ピッチ角θは次の式（１０）で表される。

ｓｉｎθ＝−ｇ_ｘ …（１０）
そして、方位角ψは次の式（１１）で表される。

なお、緯度λは次の式（１２）で表される。

以上より、地球の自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を検知すれば、原理的には、方位角ψ、さらには緯度λも算出することができる。

ところで、実際に市販されているジャイロセンサには製品ごとに特性のばらつきがあるため、ほとんどの場合、出力に一定のバイアス（オフセット）が現れ、これにより計測誤差が生じる。

そこで、このバイアスを除去するため、図４に示すように、姿勢を反転させ、反転の前後で２回、自転角速度及び重力加速度を計測する。

まず図４（ａ）に示したステップＩで、センサ本体１０をある姿勢で静止ステージ１２に位置決めし、自転角速度（ω_x,I，ω_y,I，ω_z,I）及び重力加速度（ｇ_x,I，ｇ_y,I，ｇ_z,I）を計測する。

ここで計測される自転角速度（ω_x,I，ω_y,I，ω_z,I）は、これに含まれるバイアスを（ｂ_ωx，ｂ_ωy，ｂ_ωz）と表し、求めるべき真の自転角速度を（ω_x,true，ω_y,true，ω_z,true）と表すと、次の式（１３）で表される。

また、ここで計測される重力加速度（ｇ_x,I，ｇ_y,I，ｇ_z,I）は、これに含まれるバイアスを（ｂ_gx，ｂ_gy，ｂ_gz）と表し、求めるべき真の重力加速度（ｇ_x,true，ｇ_y,true，ｇ_z,true）と表すと、次の式（１４）で表される。

次に図４（ｂ）に示したステップIIで、センサ本体１０をＸＹＺの各軸周りに１８０度回転させ、再び静止ステージ１２に位置決めする。そして、この姿勢で再び自転角速度（ω_x,II，ω_y,II，ω_z,II）及び重力加速度（ｇ_x,II，ｇ_y,II，ｇ_z,II）を計測する。

ここで計測される自転角速度（ω_x,II，ω_y,II，ω_z,II）は、次の式（１５）で表される。

また、ここで計測される重力加速度（ｇ_x,II，ｇ_y,II，ｇ_z,II）は次の式（１６）で表される。

よって、真の自転角速度（ω_x,true，ω_y,true，ω_z,true）は次の式（１７）で求められる。

また、真の重力加速度（ｇ_x,true，ｇ_y,true，ｇ_z,true）は次の式（１８）で求められる。

以上より、姿勢を反転させ、反転の前後で自転角速度及び重力加速度を計測し、それらの平均を算出すれば、バイアスは相殺され、真の自転角速度（ω_x,true，ω_y,true，ω_z,true）及び真の重力加速度（ｇ_x,true，ｇ_y,true，ｇ_z,true）を算出することができる。

［第１の実施の形態］
（１）方位計測装置の構成
図５を参照して、本発明の第１の実施の形態による方位計測装置は、自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を検知するための３軸センサ本体１０と、検知された自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）に基づいて方位角ψを算出するためのコンピュータ１４とを備える。

図６を参照して、３軸センサ本体１０は、概ね立方体の筐体１６と、筐体１６の上面に回動自在に取り付けられた２軸センサユニット１８と、筐体１６の側面に回動自在に取り付けられた１軸センサユニット２０とを備える。筐体１６は、下面から両側に延びる概ね長方形の鍔２２（一方の鍔は図示されていない）を有する。各鍔２２には２つの孔２４が形成されている。これらの孔２４にボルト（図示せず）等を通し、静止ステージ１２に螺合することにより、センサ本体１０を固定する。

図７を参照して、２軸センサユニット１８は、その内部に、Ｘ軸角速度センサ２６と、Ｘ軸加速度センサ２７と、Ｙ軸角速度センサ２８と、Ｙ軸加速度センサ２９とを備える。Ｘ軸角速度センサ２６は、Ｘ軸周りにおける地球の自転角速度成分ω_xを検知する。Ｘ軸加速度センサ２７は、Ｘ軸方向における地球の重力加速度成分ｇ_xを検知する。Ｙ軸角速度センサ２８は、Ｘ軸と直交するＹ軸周りにおける地球の自転角速度成分ω_yを検知する。Ｙ軸加速度センサ２９は、Ｙ軸方向における地球の重力加速度成分ｇ_zを検知する。

図８を参照して、１軸センサユニット２０は、その内部に、Ｚ軸角速度センサ３０と、Ｚ軸加速度センサ３１とを備える。Ｚ軸角速度センサ３０は、Ｘ軸及びＹ軸と直交するＺ軸周りにおける地球の自転角速度成分ω_zを検知する。Ｚ軸加速度センサ３１は、Ｚ軸方向における地球の重力加速度成分ｇ_zを検知する。

角速度センサ２６，２８，３０としては、廉価でかつ小型のセンサ本体１０を実現するために、たとえばＭＥＭＳ（Micro Electro Mechanical System）技術で製造された振動ジャイロが用いられる。

図９を参照して、センサ本体１０は、その筐体１６内に、２軸センサユニット１８を回動させるためのＸＹモータ３２と、１軸センサユニット２０を回動させるためのＺモータ３４とを備える。これらのモータ３２，３４としては、たとえばステッピングモータが用いられる。

再び図５を参照して、コンピュータ１４は、計測部３６と、メモリ３８と、時間平均値算出部４０と、二方向平均値算出部４２と、方位角算出部４４と、緯度算出部４３と、エラー検出部４５と、ディスプレイ４６と、ＸＹモータドライバ４８と、Ｚモータドライバ５０とを備える。ここで、計測部３６、時間平均値算出部４０、二方向平均値算出部４２、方位角算出部４４、緯度算出部４３、及びエラー検出部４５は、コンピュータプログラム（ソフトウェア）により実現される。

計測部３６は、センサ本体１０で検知された自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を所定時間（たとえば１〜１０ミリ秒）ごとに計測し、順次メモリ３８に保存する。

所定時間Ｔ内にｎ個の自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）が計測された場合、時間平均値算出部４０は次の式（１９）によりそれらの平均値を算出し、メモリ３８に保存する。式（１９）中、オーババーは平均値を表す。Ｘ_ｎはｎ回目に計測されたデータを表す。

ＸＹモータドライバ４８は、所定の指令に従ってＸＹモータ３２を駆動し、２軸センサユニット１８を図１０に示すようにＺ軸周りに１８０度回動させる。Ｚモータドライバ５０は、所定の指令に従ってＺモータ３４を駆動し、１軸センサユニット２０を図１０に示すようにＸ軸周りに１８０度回動させる。これによりセンサ本体１０の姿勢を反転し、図７及び図８に示したＸ軸角速度センサ２６、Ｘ軸加速度センサ２７、Ｙ軸角速度センサ２８、Ｙ軸加速度センサ２９、Ｚ軸角速度センサ３０、及びＺ軸加速度センサ３１の軸方向をそれぞれ反転する。

二方向平均値算出部４２は、モータ３２，３４によるセンサ本体１０の姿勢の反転前と反転後とにおいて、時間平均値算出部４０により算出された自転角速度及び重力加速度の平均値を算出する。方位角算出部４４及び緯度算出部４３は、二方向平均値算出部４２により算出された自転角速度及び重力加速度に基づいて、次の式（２０）により方位角ψ及び緯度λを算出し、これらをディスプレイ４６に表示する。

エラー検出部４５は、緯度算出部４３により算出された緯度λと既知の緯度との差が所定のしきい値よりも大きいとき、その旨をエラーとしてディスプレイ４６に表示する。

（２）方位計測方法
次に、上述した方位計測装置を用いて真北を計測する方法を説明する。

まず計測者はセンサ本体１０を任意の姿勢で静止ステージ１２上に固定する。掘削機等の移動体にセンサ本体１０を固定する場合は移動体を停止させる。

続いて、センサ本体１０は自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を検知する。コンピュータ１４はこれらを取り込み、図１１に示した手順に従って処理を実行する。

計測部３６は検知された自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を所定時間ごとに計測し（Ｓ１）、順次メモリ３８に保存する。コンピュータ１４は、所定時間Ｔが経過するまでこの計測を繰り返す（Ｓ２）。

所定時間Ｔ内にｎ個の自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）が計測された場合、時間平均値算出部４０は上記式（１９）によりそれらの平均値を算出し（Ｓ３）、メモリ３８に保存する。

続いて、コンピュータ１４は、センサ本体１０が反転される前か反転された後かを判断する（Ｓ４）。

反転される前の場合、コンピュータ１４はセンサ本体１０の姿勢を反転する（Ｓ５）。具体的には、ＸＹモータドライバ４８は、所定の指令に従ってＸＹモータ３２を駆動し、２軸センサユニット１８を図１０に示すようにＺ軸周りに１８０度回動させる。一方、Ｚモータドライバ５０は、所定の指令に従ってＺモータ３４を駆動し、１軸センサユニット２０を図１０に示すようにＸ軸周りに１８０度回動させる。これによりセンサ本体１０の姿勢を反転し、図７及び図８に示したＸ軸角速度センサ２６、Ｘ軸加速度センサ２７、Ｙ軸角速度センサ２８、Ｙ軸加速度センサ２９、Ｚ軸角速度センサ３０、及びＺ軸加速度センサ３１の軸方向をそれぞれ反転する。

反転後、計測部３６は、再び所定時間Ｔが経過するまで自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を計測し（Ｓ１，Ｓ２）、時間平均値算出部４０はそれらの平均値を算出する（Ｓ３）。

一方、反転された後の場合、二方向平均値算出部４２は、センサ本体１０の反転前と反転後とにおいて、時間平均値算出部４０により算出された自転角速度及び重力加速度の平均値を算出する（Ｓ６）。

続いて、方位角算出部４４は、二方向平均値算出部４２により算出された自転角速度及び重力加速度に基づいて、上記式（２０）により方位角ψ及び緯度λを算出する（Ｓ７）。

続いて、緯度が既知の場合（Ｓ８でＹＥＳ）、エラー検出部４５は、観測者の操作に応じて既知の緯度（推定値）を入力し（Ｓ９）、ステップＳ７で算出された緯度と入力された緯度との差を誤差として算出する（Ｓ１０）。続いて、エラー検出部４５は、算出された緯度の誤差が次の式（２１）で表されるしきい値ＴＨ_λよりも小さいか否かを判断する（Ｓ１１）。

式（２１）中、ｋは所定の係数、Ωは自転角速度、σ_ωは自転角速度の誤差（標準偏差）、σ_gは重力加速度の誤差（標準偏差）である。
一般に、緯度λの標準偏差σ_λは次の式（２２）で表される。

ピッチ角θが概ねゼロであれば、ｃｏｓθ≒１、ｓｉｎ^２ψ＋ｃｏｓ^２ψ＝１であるから、標準偏差σ_λは次の式（２３）で表される。

この標準偏差σ_λに所定の係数ｋを掛けた値を上記しきい値ＴＨ_λとする。正規分布では３σを外れる確率は両側でわずか０．２６％であるから、係数ｋはたとえば３、又はこれよりも少し大きい値に設定すればよい。

判断の結果、算出した緯度の誤差が所定のしきい値σ_λよりも大きい場合、エラー検出部４５はエラーをディスプレイ４６に表示する（Ｓ１２）。したがって、計測者は方位計測装置又は計測方法に何らかの不具合があることを知ることができる。一方、算出した緯度の誤差が所定のしきい値σ_λよりも小さい場合、ディスプレイ４６はステップＳ７で算出した方位角ψ及び緯度λを表示する（Ｓ１３）。

（３）第１の実施の形態の効果
以上のように第１の実施の形態によれば、３軸センサ本体１０を用いて自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を検知し、これに基づいて上記式（２０）により方位角ψを算出しているため、真北、その他の方位を正確に計測することができる。３軸センサ本体１０を任意の姿勢で固定すればよいので、水平面等の基準を全く必要とせず、容易に方位を計測することができる。

また、方位角ψだけでなく、緯度λも算出しているため、センサ本体１０を固定した位置（緯度）も正確に計測することができる。さらに、方位角ψだけでなく、ロール角φ及びピッチ角θも算出しているため、固定したセンサ本体１０の姿勢も計測することができる。

また、センサ本体１０を反転し、反転の前後で計測した自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を平均しているため、センサ本体１０に特有のバイアスが相殺され、計測誤差を小さくすることができる。

図１２は、データの計測回数ｎと計測誤差との関係を示すシミュレーション結果である。この結果から明らかなように、計測回数ｎが増加すると、方位角ψ、ロール角φ、ピッチ角θ、及び緯度λの誤差は小さくなり、徐々にゼロに収束する。

［第２の実施の形態］
計測される方位角ψの誤差（分散）Ｐ_ψは、ロール角φの誤差（分散）をＰ_φとし、ピッチ角θの誤差（分散）をＰ_θとし、自転角速度の誤差（分散）をＰ_ωとすると、次の式（２４）で表される。

θ≒λ、ψ≒０とすると、式（２４）の第２項及び第３項は消え、方位角ψの誤差Ｐ_ψは次の式（２５）で表される。

したがって、ピッチ角θが緯度λと概ね等しく、かつ方位角ψが概ねゼロになるように、センサ本体を固定すれば、方位角ψの誤差Ｐ_ψを最小にすることができる。

図１３を参照して、本発明の第２の実施の形態におけるセンサ本体６０は、台座６２と、台座６２からほぼ垂直に立ち上がる２枚の支持板６４（一方は図示せず）とを備える。センサ本体６０の筐体６６は、Ｙ軸周りに回動自在に支持板６４に取り付けられる。筐体６６の側面にはラインマーク６８が刻まれ、支持板６４にはピッチ角θを計測するための目盛７０が刻まれている。

このセンサ本体６０を用いて真北を計測する場合、Ｘ方向を概ね真北（磁北や天体等から推測される北）に向け、かつ図１４に示すように筐体６６を回動させてピッチ角θが概ね緯度λと等しくなるように固定する。以降の計測方法は上記第１の実施の形態と同じである。

この第２の実施の形態によれば、ピッチ角θを自在に調節できるように筐体６６が支持板６４に回動自在に取り付けられているため、ピッチ角θを概ね緯度λと等しくしかつＸ方向を概ね真北に向けた状態で方位角ψを計測すれば、その誤差Ｐ_ψを最小にすることができる。

［その他の実施の形態］
上記実施の形態ではセンサユニット１８，２０をモータ３２，３４で自動的に回動しているが、モータ３２，３４を設けず、センサユニット１８，２０を手動で回動するようにしてもよい。また、センサ本体の形状及び構造は図示されたものに限定されることなく、たとえば筐体の内部にＸＹＺの３方向を向いた３つの角速度センサ及び３つの加速度センサを設けたものでもよい。

また、上記実施の形態では時間平均値算出部４０及び二方向平均値算出部４２は相加平均値（算術平均値）を算出しているが、相乗平均値、調和平均値などを算出してもよく、要するに、複数の値を代表する値を算出すればよい。

また、上記［本発明の計測原理］の欄ではセンサ本体１０の姿勢を１回だけ反転させ、反転前後の姿勢で１回ずつ、合計２回、自転角速度及び重力加速度を計測すると説明したが、言うまでもなく、計測回数はこれに限定されない。

たとえば、センサ本体１０の姿勢を２回反転させ、反転前の姿勢で２回、反転後の姿勢で１回、合計３回、自転角速度及び重力加速度を計測してもよい。具体的には、最初に図４（ａ）に示したステップＩで自転角速度及び重力加速度を計測し、次にセンサ本体１０の姿勢を反転させ、図４（ｂ）に示したステップIIで自転角速度及び重力加速度を計測し、再びセンサ本体１０の姿勢を反転させて反転前の姿勢に戻し、図４（ａ）に示したステップＩで自転角速度及び重力加速度を計測する。そして、ステップＩで２回計測した自転角速度及び重力加速度の平均を反転前の姿勢における自転角速度（ω_x,I，ω_y,I，ω_z,I）及び重力加速度（ｇ_x,I，ｇ_y,I，ｇ_z,I）とすればよい。

この例は時間平均値を考慮しないで説明したが、上記実施の形態と同様に、所定時間Ｔ内に複数の自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を計測し、それらの平均値を算出してもよい。

また、時間平均値を算出するために、所定時間Ｔをｍ個の所定時間ｔに分割し、所定時間ｔ内に複数の自転角速度（ω_x，ω_y，ω_z）及び重力加速度（ｇ_x，ｇ_y，ｇ_z）を計測し、それらの平均値をｍ個算出し、さらに、そのｍ個の平均値の平均値を算出してもよい。

図１５は、自転角速度ω_xの計測結果の一例を示す。たとえば５分の所定時間Ｔを５個の所定時間ｔに分割し、計測のためのサンプリング周波数を１ＫＨｚとすると、１分の所定時間ｔ内に６万個の自転角速度ω_xを計測し、それらの平均値を算出する。その結果、５分の所定時間Ｔ内に５個の平均自転角速度が得られる。このような計測を反転前の姿勢で１回、反転後の姿勢で１回行った後、再び姿勢を反転して反転前の姿勢に戻してもう１回行う。そして、段差変数Ｄを有するステップ関数ＳＴＥＰを最小二乗法により決定し、これにより自転角速度ω_x,I及びω_x,IIの平均値を算出してもよい。その他の自転角速度及び重力加速度も同様である。

以上、本発明の実施の形態を説明したが、上述した実施の形態は本発明を実施するための例示に過ぎない。よって、本発明は上述した実施の形態に限定されることなく、その趣旨を逸脱しない範囲内で上述した実施の形態を適宜変形して実施することが可能である。

本発明による方位計測装置は、真北検知器等のジャイロコンパスに利用可能である。

ＮＥＤ座標系における地球の自転角速度を表す図である。本発明におけるセンサ本体を載置するためのＸＹＺ座標系を表す図である。センサ本体のオイラー角を表す図であり、（ａ）は方位角、（ｂ）はピッチ角、（ｃ）はロール角である。センサ本体の姿勢を反転する様子を示す斜視図であり、（ａ）は反転前、（ｂ）は反転後である。本発明の第１の実施の形態による方位計測装置の全体構成を示す機能ブロック図である。図５中のセンサ本体の外観構成を示す斜視図である。図６中の２軸センサユニットの内部構成を示す断面図である。図６中の１軸センサユニットの内部構成を示す断面図である。図６中の筐体の内部構成を示す断面図である。図６に示したセンサ本体の姿勢を反転した状態を示す斜視図である。図５中のコンピュータの動作を示すフロー図である。図５中のコンピュータによる所定時間内における自転角速度及び重力加速度の計測回数と方位角等の計測誤差との関係についてシミュレーション結果を示す図である。本発明の第２の実施の形態によるセンサ本体の外観構成を示す斜視図である。図１３に示したセンサ本体のピッチ角を緯度に合わせた状態を示す斜視図である。本発明の他の実施の形態による自転角速度の計測結果の一例を示す図である。

符号の説明

１０，６０センサ本体
１２静止ステージ
１４コンピュータ
１６，６６筐体
１８，２０センサユニット
２６，２８，３０角速度センサ
２７，２９，３１加速度センサ
３２，３４モータ
３６計測部
４０時間平均値算出部
４２二方向平均値算出部
４３緯度算出部
４４方位角算出部
４５エラー検出部
４８，５０モータドライバ
６８ラインマーク
７０目盛
ｇ_x，ｇ_y，ｇ_z 重力加速度成分
ω_x，ω_y，ω_z 自転角速度成分
Ω 自転角速度
λ 緯度
θ ピッチ角
φ ロール角
ψ 方位角

Claims

第１の軸周りの自転角速度成分ω_x及び前記第１の軸方向の重力加速度成分ｇ_x、前記第１の軸と直交する第２の軸周りの自転角速度成分ω_y及び前記第２の軸方向の重力加速度成分ｇ_y、並びに前記第１及び第２の軸と直交する第３の軸周りの自転角速度成分ω_z及び前記第３の軸方向の重力加速度成分ｇ_zを検知する検知手段と、
前記検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（１）により方位角ψを算出する方位角算出手段とを備えたことを特徴とする方位計測装置。
請求項１に記載の方位計測装置であってさらに、
前記検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ所定時間内に複数回計測し、それらの平均値を算出する時間平均値算出手段を備えたことを特徴とする方位計測装置。
請求項１又は請求項２に記載の方位計測装置であってさらに、
前記第１から第３の軸方向をそれぞれ反転する軸方向反転手段と、
前記軸方向反転手段による軸方向の反転前と反転後とにおいて、前記検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ計測し、それらの平均値を算出する二方向平均値算出手段とを備えたことを特徴とする方位計測装置。
請求項１〜請求項３のいずれか１項に記載の方位計測装置であってさらに、
ピッチ角θを調節するためのピッチ角調節手段を備えたことを特徴とする方位計測装置。
請求項１〜請求項４のいずれか１項に記載の方位計測装置であってさらに、
自転角速度Ω、前記検知手段により検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（２）により計測値の緯度λを算出する緯度算出手段と、

前記緯度算出手段により算出された緯度と既知の緯度との差が所定のしきい値よりも大きいときエラーを通知するエラー検出手段とを備えたことを特徴とする方位計測装置。
請求項１〜請求項５のいずれか１項に記載の方位計測装置であって、
前記検知手段は、
筐体と、
前記筐体に固定され、前記自転角速度成分ω_xを検知する第１の角速度センサと、
前記筐体に固定され、前記重力加速度成分ｇ_xを検知する第１の加速度センサと、
前記筐体に固定され、前記自転角速度成分ω_yを検知する第２の角速度センサと、
前記筐体に固定され、前記重力加速度成分ｇ_yを検知する第２の加速度センサと、
前記筐体に固定され、前記自転角速度成分ω_zを検知する第３の角速度センサと、
前記筐体に固定され、前記重力加速度成分ｇ_zを検知する第３の加速度センサとを含むことを特徴とする方位計測装置。
第１の軸周りの自転角速度成分ω_xを検知するステップと、
前記第１の軸方向の重力加速度成分ｇ_xを検知するステップと、
前記第１の軸と直交する第２の軸周りの自転角速度成分ω_yを検知するステップと、
前記第２の軸方向の重力加速度成分ｇ_yを検知するステップと、
前記第１及び第２の軸と直交する第３の軸周りの自転角速度成分ω_zを検知するステップと、
前記第３の軸方向の重力加速度成分ｇ_zを検知するステップと、
前記検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（１）により方位角ψを算出するステップとを備えたことを特徴とする方位計測方法。
請求項７に記載の方位計測方法であってさらに、
前記検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ所定時間内に複数回計測し、それらの平均値を算出するステップを備えたことを特徴とする方位計測方法。
請求項７又は請求項８に記載の方位計測方法であってさらに、
前記第１から第３の軸方向をそれぞれ反転するステップと、
前記軸方向の反転前と反転後とにおいて、前記検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zをそれぞれ計測し、それらの平均値を算出するステップとを備えたことを特徴とする方位計測方法。
請求項７〜請求項９のいずれか１項に記載の方位計測方法であってさらに、
ピッチ角θを計測地の緯度とほぼ等しく、かつ前記第１の軸方向を概ね真北に設定するステップを備えたことを特徴とする方位計測方法。
請求項７〜請求項１０のいずれか１項に記載の方位計測方法であってさらに、
自転角速度Ω、前記検知された自転角速度成分ω_x、ω_y及びω_z、並びに重力加速度成分ｇ_x、ｇ_y及びｇ_zに基づいて、次の式（２）により計測値の緯度λを算出するステップと、

前記算出された緯度と既知の緯度との差が所定のしきい値よりも大きいときエラーを通知するステップとを備えたことを特徴とする方位計測方法。