JP4938496B2 - 方位計測装置及びその方法 - Google Patents

Description

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

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

真北を正確に計測する真北検知器として、従来より、地球の自転角速度を検知して真北を検知するジャイロコンパスが用いられている。ジャイロコンパスは一般に直交３軸タイプで、大型でかつ高コストである。

そこで、小型化及び低コスト化のために１軸又は２軸タイプがいくつか提案されている。それらの多くは、角速度センサ（ジャイロセンサ）及び加速度センサを回転台の上で回転させるタイプであるが、回転角度が大きく、そのための空間を確保する必要があるため、小型化にも限界がある。また、１軸タイプの多くは水平面を必要とし、取扱いが不便である。水平面を必要としない１軸タイプも提案されているが、方位計測精度は３軸タイプと比較して劣っている。

特開平６−３１４９号公報（特許文献１）に記載の方位計は１軸タイプで、水平な台上でジャイロを一定角速度で回転させる。特開平６−１１３５０号公報（特許文献２）に記載の方位計は１軸タイプで、水平な台上でジャイロを回転させて仮の真北を求める。この仮の真北における角速度計測値と、別に入力した緯度からバイアスを求めて再度真北を求める。特開平１１−１６００７２号公報（特許文献３）に記載の真方位計測装置は水平面を必要としない２軸タイプで、２個の回転台上でジャイロを９０又は１２０度ごとに回転させて計測し、バイアスを除去する。特開平１１−１９０６３３号公報（特許文献４）に記載の方位計は水平面を必要としない１軸タイプで、回転台上でジャイロと加速度センサを３方向に指向させる。ただし、緯度を入力する必要がある。特開２００１−２１５１２１号公報（特許文献５）に記載の方位検出装置は１軸タイプで、水平面上でジャイロを回転させて正弦波状のジャイロ出力から真北を求める。特開２００２−２９６０３７号公報（特許文献６）に記載のジャイロコンパスは３軸タイプで、回転台上でジャイロを同期的に回転させる。特開２００６−４７２９５号公報（特許文献７）は３軸タイプで、２個の回転台上でジャイロを回転させる。米国特許第５６５７５４７号明細書（特許文献８）に記載の鉱井探査用ジャイロ装置は１軸タイプで、ジャイロと加速度センサを回転させる。米国特許第６５２９８３４号明細書（特許文献９）に記載の掘削用ジャイロスコープは１軸又は２軸タイプで、ジャイロを回転させて複数の角度位置で計測し、正弦曲線を当てはめてバイアスを除去する。
特開平６−３１４９号公報 特開平６−１１３５０号公報 特開平１１−１６００７２号公報 特開平１１−１９０６３３号公報 特開２００１−２１５１２１号公報 特開２００２−２９６０３７号公報 特開２００６−４７２９５号公報 米国特許第５６５７５４７号明細書 米国特許第６５２９８３４号明細書

本発明の目的は、より小型化された１軸タイプの方位計測装置を提供することである。

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

本発明による方位計測装置は、ＸＹＺ直交座標系内に設置され、１軸の自転角速度センサと、１軸の重力加速度センサと、回動手段と、揺動手段と、自転角速度計測手段と、重力加速度計測手段と、自転角速度座標変換手段と、方位角算出手段とを備える。自転角速度センサは、互いにβ度で交わるＵ軸、Ｖ軸及びＷ軸周りの自転角速度成分ω_U、ω_V及びω_Wを検出する。重力加速度センサは、Ｕ軸、Ｖ軸及びＷ軸方向の重力加速度成分ｇ_U、ｇ_V及びｇ_Wを検出する。回動手段は、Ｘ軸周りに自転角速度センサ及び重力加速度センサを（６０×ｎ）度（ｎは自然数）回動させて位置決めする。揺動手段は、Ｘ軸と直交する１軸周りに自転角速度センサ及び重力加速度センサを±α度揺動させて位置決めする。自転角速度計測手段は、回動手段及び揺動手段により位置決めされた自転角速度センサにより検出された自転角速度成分ω_U、ω_V及びω_Wを計測する。重力加速度計測手段は、回動手段及び揺動手段により位置決めされた重力加速度センサにより検出された重力加速度成分ｇ_U、ｇ_V及びｇ_Wを計測する。自転角速度座標変換手段は、自転角速度計測手段により計測された自転角速度成分ω_U、ω_V及びω_WをＸＹＺ直交座標系における自転角速度成分ω_X、ω_Y及びω_Zに座標変換する。

重力加速度座標変換手段は、重力加速度計測手段により計測された重力加速度成分ｇ_U、ｇ_V及びｇ_WをＸＹＺ直交座標系における重力加速度成分ｇ_X、ｇ_Y及びｇ_Zに座標変換する。

方位角算出手段は、自転角速度座標変換手段により得られた自転角速度成分ω_X、ω_Y及びω_Zと、重力加速度座標変換手段により得られた重力加速度成分ｇ_X、ｇ_Y及びｇ_Zとに基づいて、次の式（１）により方位角ψを算出する。

回動手段が自転角速度センサ及び重力加速度センサを６０度回動させ、かつ、揺動手段が自転角速度センサ及び重力加速度センサを＋α度揺動させたとき、自転角速度センサは＋Ｕ軸周りの自転角速度成分ω _U を検出し、かつ、重力加速度センサは＋Ｕ軸方向の重力加速度成分ｇ _U を検出する。回動手段が自転角速度センサ及び重力加速度センサを１２０度回動させ、かつ、揺動手段が自転角速度センサ及び重力加速度センサを−α度揺動させたとき、自転角速度センサは−Ｖ軸周りの自転角速度成分ω _V を検出し、かつ、重力加速度センサは−Ｖ軸方向の重力加速度成分ｇ _V を検出する。回動手段が自転角速度センサ及び重力加速度センサを１８０度回動させ、かつ、揺動手段が自転角速度センサ及び重力加速度センサを＋α度揺動させたとき、自転角速度センサは＋Ｗ軸周りの自転角速度成分ω _W を検出し、かつ、重力加速度センサは＋Ｗ軸方向の重力加速度成分ｇ _W を検出する。回動手段が自転角速度センサ及び重力加速度センサを２４０度回動させ、かつ、揺動手段が自転角速度センサ及び重力加速度センサを−α度揺動させたとき、自転角速度センサは−Ｕ軸周りの自転角速度成分ω _U を検出し、かつ、重力加速度センサは−Ｕ軸方向の重力加速度成分ｇ _U を検出する。回動手段が自転角速度センサ及び重力加速度センサを３００度回動させ、かつ、揺動手段が自転角速度センサ及び重力加速度センサを＋α度揺動させたとき、自転角速度センサは＋Ｖ軸周りの自転角速度成分ω _V を検出し、かつ、重力加速度センサは＋Ｖ軸方向の重力加速度成分ｇ _V を検出する。回動手段が自転角速度センサ及び重力加速度センサを３６０度回動させ、かつ、揺動手段が自転角速度センサ及び重力加速度センサを−α度揺動させたとき、自転角速度センサは−Ｗ軸周りの自転角速度成分ω _W を検出し、かつ、重力加速度センサは−Ｗ軸方向の重力加速度成分ｇ _W を検出する。

本発明によれば、自転角速度センサ及び重力加速度センサは±α度（たとえばα＝３０〜４０）しか揺動されないので、回転角度が小さく、回転のために大きな空間を必要としない。その結果、１軸タイプの方位計測装置であっても、小型化が可能である。
好ましくは、自転角速度座標変換手段は、自転角速度成分ω _U 、ω _V 及びω _W を次の式（２）により自転角速度成分ω _X 、ω _Y 及びω _Z に座標変換する。

好ましくは、重力加速度座標変換手段は、重力加速度成分ｇ _U 、ｇ _V 及びｇ _W を次の式（３）により重力加速度成分ｇ _X 、ｇ _Y 及びｇ _Z に座標変換する。

好ましくは、α＝３５．２６、β＝９０である。

この場合、自転角速度成分ω_X，ω_Y，ω_Z及び重力加速度成分ｇ_X，ｇ_Y，ｇ_Zの計測誤差が同じになるとともに、方位角ψの計測誤差が方位計測装置の姿勢に依存することなく小さくなる。

好ましくは、回動手段又は揺動手段は、自転角速度センサ及び重力加速度センサをＵ軸の＋及び−方向、Ｖ軸の＋及び−方向並びにＷ軸の＋及び−方向に向ける。

この場合、方位計測装置に特有のバイアスが相殺され、計測誤差が小さくなる。

好ましくは、回動手段又は揺動手段は、自転角速度センサ及び重力加速度センサをＵ軸の＋、−及び＋方向の順に、Ｖ軸の＋、−及び＋方向の順に、並びにＷ軸の＋、−及び＋方向の順に向ける。

この場合、１回目の＋方向の計測時と２回目の＋方向の計測時との間にドリフトが生じても、時間的な計測誤差が小さくなる。

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

［第１の実施の形態］
図１を参照して、本発明の第１の実施の形態による方位計測装置１０は、固定台１２と、固定台１２上に設置されたステッピングモータ１４と、ステッピングモータ１４の回転軸１６に固定されたＬ形金具１８と、Ｌ形金具１８の内面上に設置されたステッピングモータ２０と、ステッピングモータ２０の回転軸２２に固定されたＬ形金具２４と、Ｌ形金具２４の外面上に設置された自転角速度センサ２６及び重力加速度センサ２８とを備える。ここではステッピングモータを用いた例を説明するが、代わりに超音波モータを用いてもよく、モータの種類は特に制限されない。

方位計測装置１０は、ＸＹＺ直交座標系内に設置される。この座標系は、Ｘ，Ｙ，Ｚ軸が互いに９０度をなす直交座標系である。具体的には、Ｘ軸をステッピングモータ１４の回転軸１６上に割り当て、ＸＹ平面を固定台１２と平行に割り当てる。ステッピングモータ１４は、Ｘ軸周りにＬ形金具１８を（６０×ｎ）度（ｎは自然数）回動させて位置決めする。これに伴い、ステッピングモータ２０、Ｌ形金具２４、自転角速度センサ２６及び重力加速度センサ２８がＸ軸周りに（６０×ｎ）度回動する。ステッピングモータ２０は、Ｘ軸と直交する軸（図１に示した状態ではＺ軸）周りにＬ形金具２４を揺動させて位置決めする。

また、上記ＸＹＺ直交座標系に加え、ＵＶＷ座標系を想定する。ＸＹＺ直交座標系とＵＶＷ座標系との関係を図２に示す。図２（ａ）は−Ｘ方向に見たＹＺ平面図を示し、図２（ｂ）は＋Ｙ方向に見たＺＸ平面図（図２（ａ）の右側面図）を示す。Ｕ，Ｖ，Ｗ軸がＹＺ平面となす仰角をαとする。Ｕ，Ｖ，Ｗ軸が互いになす交角をβとする。仰角αの範囲は０〜９０度である。α＝０の場合、＋Ｕ，−Ｕ，＋Ｖ，−Ｖ，＋Ｗ，−Ｗの６軸全てがＹＺ平面内に位置する。また、α＝９０の場合、６軸全てがＸ軸に一致する。

また、＋Ｕ，−Ｕ，＋Ｖ，−Ｖ，＋Ｗ，−Ｗの６軸はＹＺ平面上においては常に６０度ごと離間して配置される。具体的には、＋Ｕ軸はＹＺ平面に垂直に投影されると−Ｚ軸に対して６０度をなす。−Ｕ軸はＹＺ平面に垂直に投影されると＋Ｚ軸に対して６０度をなす。＋Ｖ軸はＹＺ平面に垂直に投影されると−Ｚ軸に対して６０度をなす。−Ｖ軸はＹＺ平面に垂直に投影されると＋Ｚ軸に対して６０度をなす。＋Ｗ軸はＹＺ平面に垂直に投影されると＋Ｚ軸に一致する。−Ｗ軸はＹＺ平面に垂直に投影されると−Ｚ軸に一致する。ここで、仰角αは３０〜４０度が好ましいが、３５．２６度が最も好ましい。また、交角βは９０度が最も好ましい。その理由は後述するが、以下では、α＝３５．２６、β＝９０の場合を説明する。

自転角速度センサ２６は、Ｕ軸、Ｖ軸及びＷ軸周りの自転角速度成分ω_U、ω_V及びω_Wを検出する。重力加速度センサ２８は、Ｕ軸、Ｖ軸及びＷ軸方向の重力加速度成分ｇ_U、ｇ_V及びｇ_Wを検出する。

図３を参照して、方位計測装置１０はさらに、コンピュータ等からなる情報処理装置３０を備える。情報処理装置３０は、モータ制御部３２，３４と、自転角速度計測部３６と、重力加速度計測部３８と、自転角速度座標変換部４０と、重力加速度座標変換部４２と、方位角算出部４４とを備える。

モータ制御部３２は、ステッピングモータ１４を制御する。モータ制御部３４は、ステッピングモータ２０を制御する。自転角速度計測部３６は、自転角速度センサ２６により検出された自転角速度成分ω_U、ω_V及びω_Wを計測する。重力加速度計測部３８は、重力加速度センサ２８により検出された重力加速度成分ｇ_U、ｇ_V及びｇ_Wを計測する。

自転角速度座標変換部４０は、自転角速度計測部３６により計測された自転角速度成分ω_U、ω_V及びω_Wを次の式（４）により自転角速度成分ω_X、ω_Y及びω_Zに座標変換する。

重力加速度座標変換部４２は、重力加速度計測部３８により計測された重力加速度成分ｇ_U、ｇ_V及びｇ_Wを次の式（５）により重力加速度成分ｇ_X、ｇ_Y及びｇ_Zに座標変換する。

方位角算出部４４は、自転角速度座標変換部４０により得られた自転角速度成分ω_X、ω_Y及びω_Zと、重力加速度座標変換部４２により得られた重力加速度成分ｇ_X、ｇ_Y及びｇ_Zとに基づいて、次の式（６）により方位角ψを算出する。

次に図４を参照し、方位計測装置１０の動作を説明する。

モータ制御部３２，３４はステッピングモータ１４，２０を制御することにより、自転角速度センサ２６及び重力加速度センサ２８を「＋Ｕ」→「−Ｕ」→「＋Ｕ」→「＋Ｖ」→「−Ｖ」→「＋Ｖ」→「＋Ｗ」→「−Ｗ」→「＋Ｗ」→「＋Ｕ」の方向へ順に向けていく（Ｓ１〜Ｓ１０）。図５〜１０は、センサ２６，２８が「＋Ｕ」、「−Ｕ」、「＋Ｖ」、「−Ｖ」、「＋Ｗ」及び「−Ｗ」の方向に向けられた状態を示す。各図において、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＸＺ平面図である。

たとえばセンサ２６，２８を＋Ｕ方向（図５）から−Ｕ方向（図６）に転換する場合（Ｓ１→Ｓ２）、ステッピングモータ１４を１８０度回動させ、ステッピングモータ２０を７０．５２度（＝３５．２６度×２）回動させる。また、センサ２６，２８を＋Ｕ方向（図５）から＋Ｖ方向（図７）に方向転換する場合（Ｓ３→Ｓ４）、ステッピングモータ１４のみを１２０度回動させる。

センサ２６，２８が上記６方向のうちいずれかへ向けられた状態において、所定時間（たとえば数秒〜１分）の間、自転角速度計測部３６は、自転角速度センサ２６で検出された自転角速度成分を所定時間（たとえば１〜１０ミリ秒）ごとにサンプリングし、その値を順にメモリに格納していく。一方、重力加速度計測部３８は、重力加速度センサ２８で検出された重力加速度成分を所定時間（たとえば１〜１０ミリ秒）ごとにサンプリングし、その値を順にメモリに格納していく。そして、自転角速度計測部３６は、最小二乗法や算術平均などにより、ＵＶＷ直交座標系における自転角速度成分ω_U，ω_V，ω_Wを算出する。一方、重力加速度計測部３８は、最小二乗法や算術平均などにより、ＵＶＷ直交座標系における重力加速度成分ｇ_U，ｇ_V，ｇ_Wを算出する。

ここでは、ＵＶＷ軸のいずれにおいても、＋及び−の両方向で計測した自転角速度（ω_U，ω_V，ω_W）及び重力加速度（ｇ_U，ｇ_V，ｇ_W）を平均しているので、方位計測装置１０に特有のバイアスが相殺され、計測誤差を小さくすることができる。また、ＵＶＷ軸のいずれにおいても、＋及び−方向の計測後、再び＋方向の計測を行っているので、最初の＋方向の計測時と最後の＋方向の計測時との間に生じるドリフトを考慮に入れ、時間的な計測誤差を小さくすることができる。また、＋Ｕ方向の計測を最初だけでなく最後にもおこなっているので、この間に生じるドリフトも考慮に入れ、時間的な計測誤差をより小さくすることができる。したがって、バイアスが無視できるほど小さい場合は、−Ｕ、−Ｖ及び−Ｗ方向の計測を省略してもよい。また、ドリフトが無視できるほど小さい場合は、２回目の＋Ｕ、＋Ｖ及び＋Ｗ方向の計測や最後の＋Ｕ方向の計測を省略してもよい。

続いて、自転角速度座標変換部４０は、自転角速度計測部３６で算出された自転角速度（ω_U，ω_V，ω_W）をＸＹＺ直交座標系における自転角速度（ω_X，ω_Y，ω_Z）に座標変換する。一方、重力加速度座標変換部４２は、重力加速度計測部３８で算出された重力加速度（ｇ_U，ｇ_V，ｇ_W）をＸＹＺ直交座標系における重力加速度（ｇ_X，ｇ_Y，ｇ_Z）に座標変換する。

最後に、方位角算出部４４は、自転角速度座標変換部４０及び重力加速度座標変換部４２で得られた自転角速度（ω_X，ω_Y，ω_Z）及び重力加速度（ｇ_X，ｇ_Y，ｇ_Z）に基づいて方位角ψを算出する。

以下、仰角αは３０〜４０度が好ましく、さらに３５．２６度が最も好ましく、交角βは９０度が最も好ましい理由について説明する。

Ｕ，Ｖ，Ｗの各軸の先端の位置ｉ_Ｕ，ｉ_Ｖ，ｉ_Ｗは、ＸＹＺ直交座標系において次の式（７）で表される。

Ｕ，Ｖ，Ｗ軸の交角βは、上記の列ベクトル間の内積より次の式（８）で表される。

横軸に仰角α、縦軸に交角βをプロットした結果を図１１に示す。同図から明らかなように、仰角αが０度のとき交角βは１２０度となり、仰角αが９０度のとき交角βは０度となる。

ここで、Ｕ，Ｖ，Ｗ軸周りの自転角速度成分ω_Ｕ，ω_Ｖ，ω_Ｗは、式（７）より、Ｘ，Ｙ，Ｚ軸周りの自転角速度成分をω_Ｘ，ω_Ｙ，ω_Ｚを用いて次の式（９）で表される。

式（９）中、ＡはＸＹＺ直交座標系からＵＶＷ座標系への座標変換行列、｜Ａ｜は座標変換行列Ａの行列式である。

以下、自転角速度ωを代表的に説明するが、重力加速度ｇも同様である。すなわち、Ｘ，Ｙ，Ｚ軸方向の重力加速度成分をｇ_Ｘ，ｇ_Ｙ，ｇ_Ｚとすると、Ｕ，Ｖ，Ｗ軸方向の重力加速度成分ｇ_Ｕ，ｇ_Ｖ，ｇ_Ｗは、式（９）と同様に表される。

α＝０又は９０の場合、｜Ａ｜＝０となり、行列Ａの逆行列は存在しない。それ以外の場合、行列Ａの逆行列Ｃは次の式（１０）で表される。

直交座標系から直交座標系への通常の座標変換行列の場合、逆行列は元の行列の転置行列であるが、この場合は通常の座標変換行列と異なり、逆行列Ｃは行列Ａの転置行列ではない。

式（９）及び（１０）より、Ｘ，Ｙ，Ｚ軸周りの自転角速度成分ω_Ｘ，ω_Ｙ，ω_Ｚは、次の式（１１）で表される。

次に、自転角速度センサ２６及び重力加速度センサ２８の計測誤差について説明する。

式（１１）を微分すると、次の式（１２）が得られる。

式（１２）の左辺の二乗平均の期待値〈δω_Ｘ ²〉，〈δω_Ｙ ²〉，〈δω_Ｚ ²〉は、δω_Ｘ，δω_Ｙ，δω_Ｚが相互に独立であることから、次の式（１３）で表される。

Ｕ，Ｖ，Ｗの３軸で誤差が同じ場合、式（１３）において、〈δω_U ²〉，〈δω_V ²〉，〈δω_W ²〉≡〈δω²〉とおくと、次の式（１４）が得られる。

さらに、式（１４）を無次元化すると、Ｘ，Ｙ，Ｚ軸方向の誤差評価関数ｆ１，ｆ２は次の式（１５）で表される。

式（１５）より、Ｙ軸方向の誤差とＺ軸方向の誤差とは同じである。このことは、図２に示したＹ軸及びＺ軸の対称性からも予想されるとおりである。

横軸に仰角α、縦軸に誤差評価関数ｆ１，ｆ２をプロットした結果を図１２に示す。同図から明らかなように、α＝０の場合、ｆ１＝∞となる。Ｙ軸及びＺ軸方向の計測ができないからである。

ｆ１＝ｆ２となる仰角αは、式（１５）より、次の式（１６）で表される。

式（１６）の関係を図１３に示す。同図から次の式（１７）が導かれる。

したがって、この場合における座標変換行列Ｃは、次の式（１８）で表される。

また、この場合における交角βは、式（８）より、次の式（１９）で表される。

交角βが９０度であるから、この場合におけるＵＶＷ座標系は直交座標系である。

次に、仰角αが３５．２６度からずれた場合、式（１５）の誤差評価関数ｆ１，ｆ２がどの程度ずれるかを次の式（２０）で表す。

横軸に仰角α、縦軸に式（２０）の値をプロットした結果を図１４に示す。同図から明らかなように、仰角αが３０〜４０度の範囲内であれば、Ｕ，Ｖ，Ｗ軸間の差は約２０％以内に収まる。

次に、方位角誤差への影響を説明する。

地球の自転角速度Ω及び重力加速度Ｇは、次の式（２１）で表される。

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

ロール角φ、ピッチ角θ及び方位角ψは、次の式（２２）で表される。

また、方位計測装置１０が設置されている場所の緯度λは、次の式（２３）で表される。

したがって、図３に示した情報処理装置３０はさらに、ロール角算出部、ピッチ角算出部、緯度算出部を備えていてもよい。

式（２１）〜（２３）より、方位角ψの誤差と自転角速度センサ２６の計測誤差との関係は、次の式（２４）で表される。

式（２４）に式（１４）を代入すると、次の式（２５）が得られる。

式（２５）中の方位角誤差評価関数ｆから明らかなように、方位角誤差δψは方位計測装置１０の姿勢（方位角ψ及びピッチ角θ）に応じて変化する。横軸に仰角α、縦軸に方位角誤差評価関数ｆをプロットした結果を図１６及び１７に示す。図１６はθ＝０の場合を示し、図１７はθ＝４５の場合を示す。図１６及び１７から明らかなように、仰角αが３０〜４０度の範囲内であれば、誤差は比較的小さくなる。特にα＝３５．２６の場合、式（２４）より次の式（２６）が得られる。

式（２６）から明らかなように、この場合の方位角誤差δψは、方位計測装置１０の姿勢に影響を受けることなく常に一定となる。

以上より、ＵＶＷ座標系の仰角αは３０〜４０度の範囲が望ましい。このときの交角βは図１１より約１００〜８０度の範囲である。また、仰角αは３５．２６度が最も望ましい。このときの交角βは９０度であり、ＵＶＷ座標系は直交座標系となる。

ＵＶＷ座標系が直交座標系であれば、ＸＹＺ座標系との相対的な位置関係に関わらず、計測誤差は同じになるが、以下、この場合の長手方向（Ｘ軸方向）のエンベロープについて説明する。

仰角をαとすると、Ｕ，Ｖ，Ｗ軸の単位ベクトルの先端位置ｉ_Ｕ，ｉ_Ｖ，ｉ_Ｗは、ＸＹＺ座標系において次の式（２７）で表される。

図１８（ａ），（ｂ）に示すように、Ｘ軸方向のエンベロープをＬとし、ベクトルの回転半径をＲとすると、それらの比Ｌ／Ｒは、Ｕ，Ｖ，Ｗの各軸の正負で６方向を考慮すると、次の式（２８）で表される。

横軸に仰角α、縦軸に式（２８）のＬ／Ｒをプロットした結果を図１９に示す。同図から明らかなように、α＝０の場合、Ｗ軸はＹＺ平面内にあってＺ軸に一致する。α＝９０の場合、Ｗ軸はＹＺ平面に垂直でＸ軸に一致し、Ｕ，Ｗ軸はＹＺ平面内にある。

Ｌ／Ｒが最小となるのは、式（２８）及び図１９より、次の式（２９）のとおりα＝３５．２６のときである。

また、このとき、Ｕ，Ｖ，Ｗ軸の単位ベクトルの先端位置ｉ_Ｕ，ｉ_Ｖ，ｉ_Ｗは、式（２７）より、次の式（３０）で表される。これらは、式（１８）の第１列、第２列及び第３列に相当する。

以上、第１の実施の形態によれば、自転角速度センサ２６及び重力加速度センサ２８が±３５．２６度しか揺動されないので、回転角度が小さく、回転のために大きな空間を必要としない。その結果、この方位計測装置１０は１軸タイプであるが、小型化することができる。また、ＵＶＷ座標系が直交座標系で、仰角αが３５．２６度であるため、ＸＹＺ直交座標系における自転角速度成分ω_X、ω_Y及びω_Zの計測誤差が互いに同じなり、かつ、ＸＹＺ直交座標系における重力加速度成分ｇ_X、ｇ_Y及びｇ_Zの計測誤差が互いに同じになる。また、方位角ψの計測誤差が方位計測装置１０の姿勢に依存することなく小さくなる。また、方位計測装置１０を任意の姿勢で設置すればよいので、水平面等の基準を全く必要とせず、容易に方位を計測することができる。

また、方位角ψだけでなく、方位計測装置１０を設置した場所の緯度も正確に計測することができる。さらに、方位角だけでなく、ロール角φ及びピッチ角θも算出することにより、設置した方位計測装置１０の姿勢も計測することができる。

また、ＵＶＷ軸のいずれにおいても、＋及び−の両方向で計測した自転角速度（ω_U，ω_V，ω_W）及び重力加速度（ｇ_U，ｇ_V，ｇ_W）を平均しているため、方位計測装置１０に特有のバイアスが相殺され、計測誤差を小さくすることができる。

なお、上記実施の形態では、ステッピングモータ１４を１８０度回動させ、ステッピングモータ２０を７０．５２度回動させることにより、センサ２６，２８を＋Ｕ方向（図５）から−Ｕ方向（図６）に方向転換しているが、ステッピングモータ２０のみを１８０度回動させることにより、センサ２６，２８を＋Ｕ方向（図５）から−Ｕ方向（図２０）に転換するようにしてもよい。センサ２６，２８を＋Ｖ方向（図７）から−Ｖ方向（図２１）に転換する場合、及び＋Ｗ方向（図９）から−Ｖ方向（図２２）に転換する場合も同様である。この場合、１回の動作で方向転換を行うため、２回の動作で方向転換を行う上記の場合と比べると、動作範囲のエンベロープは大きくなるが、動作シーケンスは簡単になる。

また、Ｕ，Ｖ，Ｗ軸方向の計測順序は特に限定されない。たとえば＋Ｕ方向の次に−Ｖ方向の計測を行う場合は、ステッピングモータ１４を６０度回動させ、かつ、ステッピングモータ２０を７０．５２度回動させればよい。また、ステッピングモータ１４，２０はどちらを先に回動させてもよく、また、両方を同時に回動させてもよい。

［第２の実施の形態］
図２３を参照して、本発明の第２の実施の形態による方位計測装置５０においては、上記第１の実施の形態と異なり、ステッピングモータ２０の回転軸２２がステッピングモータ１４の回転軸１６と同じＸ軸方向に向けられる。この方位計測装置５０はさらに、傘歯車５２及び５４を備える。傘歯車５２は、ステッピングモータ２０の回転軸２２に固定される。傘歯車５４は、Ｌ形金具１８の外面に回動自在に支持される。傘歯車５２及び５４は９０度をなし、互いに歯合される。Ｌ形金具２４は、傘歯車５４に固定される。したがって、ステッピングモータ２０が傘歯車５２を回動させると、傘歯車５４及びＬ形金具２４が回動される。たとえば傘歯車５２及び５４の歯数比が１：１の場合、ステッピングモータ２０が傘歯車５２を左右に３５．２６度ずつ回動させると、センサ２６及び２８は左右に３５．２６度ずつ揺動する。

［第３の実施の形態］
図２４を参照して、本発明の第３の実施の形態による方位計測装置６０においては、上記第１及び第２の実施の形態と異なり、ステッピングモータ１４がＬ形の固定台６２上に設置される。ステッピングモータ１４の回転軸１６はＺ軸方向に向けられる。この方位計測装置６０はさらに、傘歯車６４及び６６を備える。傘歯車６４は、ステッピングモータ１４の回転軸１６に固定される。傘歯車６６は、Ｌ形の固定台６２の内面に回動自在に支持される。傘歯車６４及び６６は９０度をなし、互いに歯合される。Ｌ形金具６８は、Ｌ形の固定台６２の外面に回動自在に支持され、かつ、傘歯車６６の回転軸（図示せず）に固定される。ステッピングモータ２０は、Ｌ形金具６８の内面上に設置される。したがって、ステッピングモータ１４が傘歯車６４を回動させると、傘歯車６６及びＬ形金具６８が回動される。

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

本発明の第１の実施の形態による方位計測装置の外観構成を示す斜視図である。 図１に示した方位計測装置を設置するＸＹＺ座標系と自転角速度及び重力加速度を計測するＵＶＷ座標系との間の位置関係を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１に示した方位計測装置の構成を示す機能ブロック図である。 図１に示した方位計測装置の動作を示す図である。 図１に示した方位計測装置が自転角速度センサ及び重力加速度センサを＋Ｕ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１に示した方位計測装置が自転角速度センサ及び重力加速度センサを−Ｕ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１に示した方位計測装置が自転角速度センサ及び重力加速度センサを＋Ｖ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１に示した方位計測装置が自転角速度センサ及び重力加速度センサを−Ｖ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１に示した方位計測装置が自転角速度センサ及び重力加速度センサを＋Ｗ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１に示した方位計測装置が自転角速度センサ及び重力加速度センサを−Ｗ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図２に示したＸＹＺ及びＵＶＷ座標系における仰角α及び交角βの関係を示すグラフである。 Ｘ軸方向の誤差評価関数ｆ１及びＹ，Ｚ軸方向の誤差評価関数ｆ２と仰角αとの関係を示すグラフである。 α＝３５．２６度の直角三角形を示す図である。 ｆ２／ｆ１−１と仰角αとの関係を示すグラフである。 図１に示した方位計測装置の姿勢（オイラー角）を表す図であり、（ａ）は方位角ψ、（ｂ）はピッチ角θ、（ｃ）はロール角φである。 θ＝０度の場合における方位角誤差評価関数ｆと仰角αとの関係を示すグラフである。 θ＝４５度の場合における方位角誤差評価関数ｆと仰角αとの関係を示すグラフである。 （ａ）は従来の方位計測装置が自転角速度センサ及び重力加速度センサを回動させた場合におけるエンベロープを示し、（ｂ）は図１に示した方位計測装置の場合におけるエンベロープを示す。 図１８に示したＬ／Ｒと仰角αとの関係を示すグラフである。 図６と異なる方法で自転角速度センサ及び重力加速度センサを−Ｕ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図８と異なる方法で自転角速度センサ及び重力加速度センサを−Ｖ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 図１０と異なる方法で自転角速度センサ及び重力加速度センサを−Ｗ方向に向けた状態を示し、（ａ）は−Ｘ方向に見たＹＺ平面図であり、（ｂ）は＋Ｙ方向に見たＺＸ平面図である。 本発明の第２の実施の形態による方位計測装置の外観構成を示す斜視図である。 本発明の第３の実施の形態による方位計測装置の外観構成を示す斜視図である。

符号の説明

１０，５０，６０ 方位計測装置
１４，２０ ステッピングモータ
２６ 自転角速度センサ
２８ 重力加速度センサ
３２，３４ モータ制御部
３６ 自転角速度計測部
３８ 重力加速度計測部
４０ 自転角速度座標変換部
４２ 重力加速度座標変換部
４４ 方位角算出部

Claims (8)

1. ＸＹＺ直交座標系内に設置された方位計測装置であって、
   互いにβ度で交わるＵ軸、Ｖ軸及びＷ軸周りの自転角速度成分ω_U、ω_V及びω_Wを検出する１軸の自転角速度センサと、
   Ｕ軸、Ｖ軸及びＷ軸方向の重力加速度成分ｇ_U、ｇ_V及びｇ_Wを検出する１軸の重力加速度センサと、
   Ｘ軸周りに前記自転角速度センサ及び前記重力加速度センサを（６０×ｎ）度（ｎは自然数）回動させて位置決めする回動手段と、
   前記Ｘ軸と直交する１軸周りに前記自転角速度センサ及び前記重力加速度センサを±α度揺動させて位置決めする揺動手段とを備え、
   前記回動手段が前記自転角速度センサ及び前記重力加速度センサを６０度回動させ、かつ、前記揺動手段が前記自転角速度センサ及び前記重力加速度センサを＋α度揺動させたとき、前記自転角速度センサは＋Ｕ軸周りの自転角速度成分ω_Uを検出し、かつ、前記重力加速度センサは＋Ｕ軸方向の重力加速度成分ｇ_Uを検出し、
   前記回動手段が前記自転角速度センサ及び前記重力加速度センサを１２０度回動させ、かつ、前記揺動手段が前記自転角速度センサ及び前記重力加速度センサを−α度揺動させたとき、前記自転角速度センサは−Ｖ軸周りの自転角速度成分ω_Vを検出し、かつ、前記重力加速度センサは−Ｖ軸方向の重力加速度成分ｇ_Vを検出し、
   前記回動手段が前記自転角速度センサ及び前記重力加速度センサを１８０度回動させ、かつ、前記揺動手段が前記自転角速度センサ及び前記重力加速度センサを＋α度揺動させたとき、前記自転角速度センサは＋Ｗ軸周りの自転角速度成分ω_Wを検出し、かつ、前記重力加速度センサは＋Ｗ軸方向の重力加速度成分ｇ_Wを検出し、
   前記回動手段が前記自転角速度センサ及び前記重力加速度センサを２４０度回動させ、かつ、前記揺動手段が前記自転角速度センサ及び前記重力加速度センサを−α度揺動させたとき、前記自転角速度センサは−Ｕ軸周りの自転角速度成分ω_Uを検出し、かつ、前記重力加速度センサは−Ｕ軸方向の重力加速度成分ｇ_Uを検出し、
   前記回動手段が前記自転角速度センサ及び前記重力加速度センサを３００度回動させ、かつ、前記揺動手段が前記自転角速度センサ及び前記重力加速度センサを＋α度揺動させたとき、前記自転角速度センサは＋Ｖ軸周りの自転角速度成分ω_Vを検出し、かつ、前記重力加速度センサは＋Ｖ軸方向の重力加速度成分ｇ_Vを検出し、
   前記回動手段が前記自転角速度センサ及び前記重力加速度センサを３６０度回動させ、かつ、前記揺動手段が前記自転角速度センサ及び前記重力加速度センサを−α度揺動させたとき、前記自転角速度センサは−Ｗ軸周りの自転角速度成分ω_Wを検出し、かつ、前記重力加速度センサは−Ｗ軸方向の重力加速度成分ｇ_Wを検出し、
   前記方位計測装置はさらに、
   前記回動手段及び前記揺動手段により位置決めされた自転角速度センサにより検出された自転角速度成分ω_U、ω_V及びω_Wを計測する自転角速度計測手段と、
   前記回動手段及び前記揺動手段により位置決めされた重力加速度センサにより検出された重力加速度成分ｇ_U、ｇ_V及びｇ_Wを計測する重力加速度計測手段と、
   前記自転角速度計測手段により計測された自転角速度成分ω_U、ω_V及びω_WをＸＹＺ直交座標系における自転角速度成分ω_X、ω_Y及びω_Zに座標変換する自転角速度座標変換手段と、
   前記重力加速度計測手段により計測された重力加速度成分ｇ_U、ｇ_V及びｇ_WをＸＹＺ直交座標系における重力加速度成分ｇ_X、ｇ_Y及びｇ_Zに座標変換する重力加速度座標変換手段と、
   前記自転角速度座標変換手段により得られた自転角速度成分ω_X、ω_Y及びω_Zと、前記重力加速度座標変換手段により得られた重力加速度成分ｇ_X、ｇ_Y及びｇ_Zとに基づいて、次の式（１）により方位角ψを算出する方位角算出手段とを備えたことを特徴とする方位計測装置。
   

   
2. 請求項１に記載の方位計測装置であって、
   前記自転角速度座標変換手段は、前記自転角速度成分ω_U、ω_V及びω_Wを次の式（２）により前記自転角速度成分ω_X、ω_Y及びω_Zに座標変換し、
   

   

   前記重力加速度座標変換手段は、前記重力加速度成分ｇ_U、ｇ_V及びｇ_Wを次の式（３）により前記重力加速度成分ｇ_X、ｇ_Y及びｇ_Zに座標変換する、ことを特徴とする方位計測装置。
   

   
3. 請求項１又は２に記載の方位計測装置であって、
   α＝３０〜４０であることを特徴とする方位計測装置。
4. 請求項１〜３のいずれか１項に記載の方位計測装置であって、
   α＝３５．２６、β＝９０であることを特徴とする方位計測装置。
5. ＸＹＺ直交座標系内に設置され、互いにβ度で交わるＵ軸、Ｖ軸及びＷ軸周りの自転角速度成分ω_U、ω_V及びω_Wを検出する１軸の自転角速度センサと、Ｕ軸、Ｖ軸及びＷ軸方向の重力加速度成分ｇ_U、ｇ_V及びｇ_Wを検出する１軸の重力加速度センサとを用いた方位計測方法であって、
   Ｘ軸周りに前記自転角速度センサ及び前記重力加速度センサを（６０×ｎ）度（ｎは自然数）回動させて位置決めする回動ステップと、
   前記Ｘ軸と直交する１軸周りに前記自転角速度センサ及び前記重力加速度センサを±α度揺動させて位置決めする揺動ステップとを備え、
   前記回動ステップが前記自転角速度センサ及び前記重力加速度センサを６０度回動させ、かつ、前記揺動ステップが前記自転角速度センサ及び前記重力加速度センサを＋α度揺動させたとき、前記自転角速度センサは＋Ｕ軸周りの自転角速度成分ω_Uを検出し、かつ、前記重力加速度センサは＋Ｕ軸方向の重力加速度成分ｇ_Uを検出し、
   前記回動ステップが前記自転角速度センサ及び前記重力加速度センサを１２０度回動させ、かつ、前記揺動ステップが前記自転角速度センサ及び前記重力加速度センサを−α度揺動させたとき、前記自転角速度センサは−Ｖ軸周りの自転角速度成分ω_Vを検出し、かつ、前記重力加速度センサは−Ｖ軸方向の重力加速度成分ｇ_Vを検出し、
   前記回動ステップが前記自転角速度センサ及び前記重力加速度センサを１８０度回動させ、かつ、前記揺動ステップが前記自転角速度センサ及び前記重力加速度センサを＋α度揺動させたとき、前記自転角速度センサは＋Ｗ軸周りの自転角速度成分ω_Wを検出し、かつ、前記重力加速度センサは＋Ｗ軸方向の重力加速度成分ｇ_Wを検出し、
   前記回動ステップが前記自転角速度センサ及び前記重力加速度センサを２４０度回動させ、かつ、前記揺動ステップが前記自転角速度センサ及び前記重力加速度センサを−α度揺動させたとき、前記自転角速度センサは−Ｕ軸周りの自転角速度成分ω_Uを検出し、かつ、前記重力加速度センサは−Ｕ軸方向の重力加速度成分ｇ_Uを検出し、
   前記回動ステップが前記自転角速度センサ及び前記重力加速度センサを３００度回動させ、かつ、前記揺動ステップが前記自転角速度センサ及び前記重力加速度センサを＋α度揺動させたとき、前記自転角速度センサは＋Ｖ軸周りの自転角速度成分ω_Vを検出し、かつ、前記重力加速度センサは＋Ｖ軸方向の重力加速度成分ｇ_Vを検出し、
   前記回動ステップが前記自転角速度センサ及び前記重力加速度センサを３６０度回動させ、かつ、前記揺動ステップが前記自転角速度センサ及び前記重力加速度センサを−α度揺動させたとき、前記自転角速度センサは−Ｗ軸周りの自転角速度成分ω_Wを検出し、かつ、前記重力加速度センサは−Ｗ軸方向の重力加速度成分ｇ_Wを検出し、
   前記方位計測方法はさらに、
   前記回動ステップ及び前記揺動ステップにより位置決めされた自転角速度センサにより検出された自転角速度成分ω_U、ω_V及びω_Wを計測する自転角速度計測ステップと、
   前記回動ステップ及び前記揺動ステップにより位置決めされた重力加速度センサにより検出された重力加速度成分ｇ_U、ｇ_V及びｇ_Wを計測する重力加速度計測ステップと、
   前記自転角速度計測ステップにより計測された自転角速度成分ω_U、ω_V及びω_WをＸＹＺ直交座標系における自転角速度成分ω_X、ω_Y及びω_Zに座標変換する自転角速度座標変換ステップと、
   前記重力加速度計測ステップにより計測された重力加速度成分ｇ_U、ｇ_V及びｇ_WをＸＹＺ直交座標系における重力加速度成分ｇ_X、ｇ_Y及びｇ_Zに座標変換する重力加速度座標変換ステップと、
   前記自転角速度座標変換ステップにより得られた自転角速度成分ω_X、ω_Y及びω_Zと、前記重力加速度座標変換ステップにより得られた重力加速度成分ｇ_X、ｇ_Y及びｇ_Zとに基づいて、次の式（１）により方位角ψを算出する方位角算出ステップとを備えたことを特徴とする方位計測方法。
   

   
6. 請求項５に記載の方位計測方法であって、
   前記自転角速度座標変換ステップは、前記自転角速度成分ω_U、ω_V及びω_Wを次の式（２）により前記自転角速度成分ω_X、ω_Y及びω_Zに座標変換し、
   

   

   前記重力加速度座標変換ステップは、前記重力加速度成分ｇ_U、ｇ_V及びｇ_Wを次の式（３）により前記重力加速度成分ｇ_X、ｇ_Y及びｇ_Zに座標変換する、ことを特徴とする方位計測方法。
   

   
7. 請求項５又は６に記載の方位計測方法であって、
   α＝３０〜４０であることを特徴とする方位計測方法。
8. 請求項５〜７のいずれか１項に記載の方位計測方法であって、
   α＝３５．２６、β＝９０であることを特徴とする方位計測方法。

Images