(57)【要約】  真北の検出精度を向上することができる方位検出装置を提供する。方位検出装置１において、姿勢変更装置１００は、角速度センサ２６を検出軸周りに回転させ、検出軸を所定の測定方向と所定の測定方向と反対の方向とに向けるように角速度センサ２６の姿勢を変更する。制御装置３０は、角速度センサ２６が所定の測定方向周りの角速度の検出を開始する前に、角速度センサ２６が検出軸周りを第１回転方向に回転するように姿勢変更装置１００を制御する。制御装置３０は、角速度センサ２６が反対の方向周りの角速度の検出を開始する前に、角速度センサ２６が検出軸周りを第１回転方向と反対の第２回転方向に回転し、角速度センサ２６が検出軸周りをさらに第１回転方向に回転するように姿勢変更装置１００を制御する。
2014156288.tif 000002
【特許請求の範囲】
【請求項１】
角速度センサと、
前記角速度センサを検出軸周りに回転させ、前記検出軸を所定の測定方向と前記所定の測定方向と反対の方向とに向けるように前記角速度センサの姿勢を変更する姿勢変更装置と、
前記姿勢変更装置を制御する制御装置と、
を備え、
前記制御装置は、
前記角速度センサが前記所定の測定方向周りの角速度の検出を開始する前に、前記角速度センサが前記検出軸周りを第１回転方向に回転するように前記姿勢変更装置を制御する第１制御部と、
前記角速度センサが前記反対の方向周りの角速度の検出を開始する前に、前記角速度センサが前記検出軸周りを前記第１回転方向と反対の第２回転方向に回転し、前記角速度センサが前記検出軸周りをさらに前記第１回転方向に回転するように前記姿勢変更装置を制御する第２制御部とを備える、方位検出装置。
【請求項２】
請求項１に記載の方位検出装置であって、
前記制御装置は、前記角速度センサにより検出された前記所定の測定方向周りの角速度と、前記角速度センサにより検出された前記反対の方向周りの角速度との差分を計算して、前記所定の測定方向に沿った軸周りの自転角速度を取得する、方位検出装置。
【請求項３】
請求項１又は２に記載の方位検出装置であって、
前記姿勢変更装置は、ＸＹＺ直交座標系内に設置され、
前記姿勢変更装置は、
Ｘ軸方向に延びたシャフトと、
前記シャフトに対してα度傾斜した傾斜軸周りを回転可能であり、前記シャフトと連結され、ｘｙｚ直交動座標系を有し、球体状表面と、前記球体状表面のｘ軸周りに形成された軌道部とを備える回転部材と、
前記制御装置により制御され、前記シャフトをＸ軸周りに回転する駆動源と、
前記ＸＹＺ直交座標系に固定され、前記軌道部と接触するガイド部材とを備え、
前記軌道部は、前記シャフトの回転により前記ガイド部材と接触しながら回転するときに、前記回転部材のｙ軸を、Ｘ軸周りの６０度おきに互いに交わるＵ軸、Ｖ軸及びＷ軸のプラス及びマイナス方向に順次向ける形状を有し、ｘ軸のプラス方向に凸状の４つの山部と、マイナス方向に凸状の４つの谷部とを有するジグザグ状であり、
前記角速度センサは、前記検出軸がｙ軸を向くように前記回転部材に取り付けられ、
所定の測定方向及び反対の方向は、Ｕ軸、Ｖ軸及びＷ軸のいずれか１つに一致する、方位検出装置。
【請求項４】
請求項３に記載の方位検出装置であって、
前記制御装置は、前記角速度センサがＵ軸、Ｖ軸及びＷ軸のいずれか１つの軸方向周りの角速度を検出する場合、前記検出軸が前記いずれか１つの軸のプラス方向、マイナス方向、プラス方向の順に向けられるように、前記姿勢変更装置を制御し、
前記角速度センサが前記いずれか１つの軸方向周りの角速度を検出する際に、前記第２制御部により制御される回数が、前記第１制御部により制御される回数よりも少ない、方位検出装置。
【請求項５】
請求項３又は４に記載の方位検出装置であって、
前記検出軸がＵ軸、Ｖ軸及びＷ軸のいずれか１つの軸のプラス方向に向けられる場合、前記検出軸の符号がプラスであり、前記検出軸がＵ軸、Ｖ軸及びＷ軸のいずれかの１つ軸のマイナス方向に向けられる場合、前記検出軸の符号がマイナスであり、
前記シャフトが前記方位検出装置の正面から見て時計回りに回転する場合、前記シャフトの回転方向の符号がプラスであり、前記シャフトが反時計回りに回転する場合、前記シャフトの回転方向の符号がマイナスであり、
前記検出軸が前記いずれか１つの軸のプラス方向、マイナス方向に向けられる場合、前記姿勢変更装置が前記第１制御部により制御されるか否かが、前記検出軸の符号と前記回転方向の符号との積に基づいて決定される、方位検出装置。
【請求項６】
検出軸周りの角速度を検出する角速度センサと、前記角速度センサを前記検出軸周りに回転させ、前記検出軸を所定の測定方向と前記所定の測定方向と反対の方向に向けるように前記角速度センサの姿勢を変更する姿勢変更装置とを備える方位検出装置に搭載されるコンピュータに、
前記角速度センサが前記所定の測定方向周りの角速度の検出を開始する前に、前記角速度センサが検出軸周りを第１回転方向に回転するように前記姿勢変更装置を制御するステップと、
前記角速度センサが前記反対の方向周りの角速度の検出を開始する前に、前記角速度センサが前記検出軸周りを前記第１回転方向と反対の第２回転方向に回転し、前記角速度センサが前記検出軸周りをさらに前記第１回転方向に回転するように前記姿勢変更装置を制御するステップとを実行させるための制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、方位検出装置に関し、さらに詳しくは、角速度センサを用いて真北を検出する方位検出装置に関する。
【背景技術】
【０００２】
磁北は真北から少しずれているため、磁気コンパスでは真北を計測することはできない。しかしながら、行政地図は真北を基準に作成され、建築基準法も真北を基準に定められている。したがって、土木・建築の分野においては、真北を正確に計測する必要がある。特に地下トンネル工事では鉱脈等の影響で、磁気コンパスは正しく機能しない。【０００３】
真北を正確に計測する真北検知器として、従来より、地球の自転角速度を検知して真北を検知するジャイロコンパスが知られている。ジャイロコンパスは一般的に直交３軸タイプである。これらのジャイロコンパスは、大型でかつ高コストである。【０００４】
そこで、小型化及び低コスト化のために、１軸又は２軸タイプのジャイロコンパスが提案されている。しかし、これらのジャイロコンパスの多くは、ジャイロセンサ及び加速度センサを回転台の上で回転させるタイプである。しかしながら、これらのジャイロコンパスは、回転角度が大きく、大きな空間を確保する必要があるため、小型化に限界がある。また、１軸タイプのジャイロコンパスの多くは水平面を必要とし、取り扱いが不便である。また、水平面を必要としない１軸タイプのジャイロコンパスも提案されている。しかしながら、これらのジャイロコンパスは、方位計測精度が３軸タイプと比較して劣っている。【０００５】
本願出願人は、特開２０１２−１１２６７９号公報にて１軸タイプでかつ水平面を必要としない６方向指向装置を提案した。この６方向指向装置は、ＸＹＺ直交座標系に加え、ＵＶＷ直交座標系を想定する。＋Ｕ、−Ｕ、＋Ｖ、−Ｖ、＋Ｗ、−Ｗの６方向は、ＹＺ平面に投影されると６０度おきに離間して配置される。【０００６】
また、この６方向指向装置は、シャフトと、駆動源と、回転部材と、ガイド部材とを備える。シャフトは、Ｘ軸方向に延びる。駆動源は、シャフトをＸ軸周りに回転させる。回転部材は、シャフトに対してα度傾斜した傾斜軸周りを回転可能であり、シャフトと連結される。回転部材は、球体状表面と、球体状表面のｘ軸周りに形成された軌道部とを備える。ガイド部材は、ＸＹＺ直交座標系に固定され、軌道部と接触する。また、この６方向指向装置では、回転部材に固定されたｘｙｚ座標系を想定する。ｘｙｚ座標系は、回転部材の回転に応じて、ＸＹＺ座標系内を回転する。軌道部は、回転部材がシャフトの回転によりガイド部材と接触しながら回転するとき、回転部材のｙ軸を＋Ｕ、−Ｕ、＋Ｖ、−Ｖ、＋Ｗ、−Ｗの各方向に向ける形状を有する。【０００７】
回転部材の内部には、角速度センサと加速度センサとが配置される。角速度センサは、ｙ軸方向周りの角速度を検出する。加速度センサは、ｙ軸方向周りの重力加速度を検出する。６方向指向装置は、回転部材のｙ軸を＋Ｕ、−Ｕ、＋Ｖ、−Ｖ、＋Ｗ、−Ｗの向きに順次指向させることにより、Ｕ軸、Ｖ軸、Ｗ軸周りの自転角速度及び重力加速度を計測する。方位角ψが、計測された自転角速度及び重力加速度に基づいて算出される。【０００８】
角速度センサが、検出軸周りの回転が発生していないにもかかわらず、０でない角速度（オフセット）を出力することが知られている。オフセットは、Ｕ軸、Ｖ軸、Ｗ軸周りの各々の自転角速度の計測誤差の原因となる。そこで、６方向指向装置は、例えば、ｙ軸が＋Ｖ方向に向いたときに計測される自転角速度と、−Ｖ方向に向いたときに計測される自転角速度との差分を計算する。これにより、＋Ｖ軸及び−Ｖ軸の各々の自転角速度に含まれるオフセットが相殺される。
【発明の開示】
【０００９】
上述の６方向指向装置において、角速度センサの時定数に伴う誤差が発生し、時定数に伴う誤差を除去できないという問題がある。以下、具体的に説明する。
【００１０】
回転部材がシャフトの回転に伴ってガイド部材と接触しながら回転するときに、回転部材は、傾斜軸周りだけでなく、ｙ軸周りにも回転する。すなわち、回転部材のｙ軸を＋Ｕ、−Ｕ、＋Ｖ、−Ｖ、＋Ｗ、−Ｗの各方向に移動させる間、角速度センサは、ｙ軸周りを回転しながら移動する。角速度センサは、ｙ軸の移動中に、ｙ軸周りの回転に伴う角速度を検出し続ける。ｙ軸の移動中に、角速度センサは、数度／秒（数万度／時間）レベルの角速度を出力する。
【００１１】
そして、ｙ軸が＋Ｕ、−Ｕ、＋Ｖ、−Ｖ、＋Ｗ、−Ｗの各方向に向いて停止した後に、６方向指向装置は、各方向周りの自転角速度を計測する。計測される自転角速度のレベルは、１５度／時間程度である。一方、ｙ軸が移動するときの角速度センサの出力は、数万度／時間レベルである。従って、角速度センサの出力レベルは、ｙ軸が＋Ｕ軸等の方向に向いて停止する前後に急激に変化する。したがって、角速度センサの出力は、ｙ軸が停止してから計測が終了するまでの間、移動時の数万度／時間レベルの角速度出力による時定数の影響を受けて、継続的に増加（あるいは減少)する。したがって、計測される自転角速度は、時定数に伴う誤差を含む。
【００１２】
ｙ軸を停止させる直前における回転部材のｙ軸周りの回転方向は、シャフトの回転位置に応じて変化する。例えば、ｙ軸を＋Ｖ方向に向ける場合と、ｙ軸を−Ｖ方向に向ける場合において、ｙ軸の停止直前におけるｙ軸周りの回転方向は、逆となる場合がある。この結果、ｙ軸を＋Ｖ方向に向けたときの時定数に伴う誤差の特性と、ｙ軸を−Ｖ方向に向けたときの時定数に伴う誤差の特性とが、逆になる。上述のように、オフセットを相殺するために、＋Ｖ方向の自転角速度と−Ｖ方向の自転角速度との差分値が計算されるが、この計算の際に、時定数に伴う誤差は、相殺されず、加算される。Ｖ軸周りの自転角速度が時定数に伴う誤差を含むため、６方向指向装置は、真北を正確に検知することができない。
【００１３】
本発明の目的は、真北の検出精度を向上することができる方位検出装置を提供することである。
【００１４】
本発明に係る方位検出装置は、角速度センサと、姿勢変更装置と、制御装置とを備える。姿勢変更装置は、角速度センサを検出軸周りに回転させ、検出軸を所定の測定方向と所定の測定方向と反対の方向とに向けるように角速度センサの姿勢を変更する。制御装置は、姿勢変更装置を制御する。制御装置は、第１制御部と、第２制御部とを備える。第１制御部は、角速度センサが所定の測定方向周りの角速度の検出を開始する前に、角速度センサが前記検出軸周りを第１回転方向に回転するように姿勢変更装置を制御する。第２制御部は、角速度センサが反対の方向周りの角速度の検出を開始する前に、角速度センサが前記検出軸周りを前記第１回転方向と反対の第２回転方向に回転し、角速度センサが検出軸周りをさらに前記第１回転方向に回転するように姿勢変更装置を制御する。
【００１５】
本発明によれば、所定の測定方向周りの角速度と、反対の方向周りの角速度との各々に含まれる時定数に伴う誤差の特性を一致させることができる。したがって、真北の検出精度を向上させることができる。
【００１６】
好ましくは、制御装置は、角速度センサにより検出された所定の測定方向周りの角速度と、角速度センサにより検出された反対の方向周りの角速度との差分を計算して、所定の測定方向に沿った軸周りの自転角速度を取得する。
【００１７】
これにより、所定の測定方向に沿った軸周りの自転角速度において、時定数に伴う誤差が相殺されるため、真北の検出精度を向上させることができる。
【００１８】
好ましくは、姿勢変更装置は、ＸＹＺ直交座標系内に設置される。姿勢変更装置は、シャフトと、回転部材と、駆動源と、ガイド部材とを備える。シャフトは、Ｘ軸方向に延びる。回転部材は、シャフトに対してα度傾斜した傾斜軸周りを回転可能であり、シャフトと連結される。回転部材は、ｘｙｚ直交動座標系を有する。回転部材は、球体状表面と、球体状表面のｘ軸周りに形成された軌道部とを備える。駆動源は、制御装置により制御され、シャフトをＸ軸周りに回転する。ガイド部材は、ＸＹＺ直交座標系に固定され、軌道部と接触する。軌道部は、シャフトの回転によりガイド部材と接触しながら回転するときに、回転部材のｙ軸を、Ｘ軸周りの６０度おきに互いに交わるＵ軸、Ｖ軸及びＷ軸のプラス及びマイナス方向に順次向ける形状を有し、ｘ軸のプラス方向に凸状の４つの山部と、マイナス方向に凸状の４つの谷部とを有するジグザグ状である。角速度センサは、検出軸がｙ軸を向くように回転部材に取り付けられる。所定の測定方向及び反対の方向は、Ｕ軸、Ｖ軸及びＷ軸のいずれか１つに一致する。
【００１９】
これにより、１軸タイプの方位検出装置において、真北の検出精度を向上することができる。
【００２０】
好ましくは、制御部は、角速度センサがＵ軸、Ｖ軸及びＷ軸のいずれか１つの軸方向周りの角速度を検出する場合、検出軸がいずれか１つの軸のプラス方向、マイナス方向、プラス方向の順に向けられるように、姿勢変更装置を制御する。角速度センサがいずれか１つの軸方向周りの角速度を検出する際に、第２制御部により制御される回数が、第１制御部により制御される回数よりも少ない。
【００２１】
これにより、姿勢変更装置が第２制御部により制御される回数を抑制できるため、真北を検出するまでの時間を短縮することができる。
【００２２】
好ましくは、検出軸がＵ軸、Ｖ軸及びＷ軸のいずれか１つの軸のプラス方向に向けられる場合、検出軸の符号がプラスであり、検出軸がＵ軸、Ｖ軸及びＷ軸のいずれかの１つ軸のマイナス方向に向けられる場合、検出軸の符号がマイナスである。シャフトが方位検出装置の正面から見て時計回りに回転する場合、シャフトの回転方向の符号がプラスである。シャフトが反時計回りに回転する場合、シャフトの回転方向の符号がマイナスである。検出軸がいずれか１つの軸のプラス方向、マイナス方向に向けられる場合、姿勢変更装置が第１制御部により制御されるか否かが、検出軸の符号と回転方向の符号との積に基づいて決定される。
【００２３】
これにより、姿勢変更装置が第２制御部により制御される基準を容易に決定することができる。
【００２４】
本発明に係る制御プログラムは、本発明に係る方位検出装置に用いられる。
【図面の簡単な説明】
【００２５】
【図１】図１は、本実施の形態における方位検出装置の外観図である。
【図２】図２は、図１中の方位検出装置内の構成を示す斜視図である。
【図３】図３は、図２中の方位検出装置のうち、傾斜クランク機構とその近傍の側面図である。
【図４】図４は、図３中の回転部材の斜視図である。
【図５】図５は、図３中の回転部材の正面図である。
【図６】図６は、図３中の回転部材の平面図である。
【図７】図７は、図３中の回転部材の側面図である。
【図８】図８は、図３中の回転部材の背面図である。
【図９】図９は、図２中のガイド部材の斜視図である。
【図１０】図１０は、回転部材と各ガイド部材とのＸＹＺ直交座標系での配置関係を示す図である。
【図１１】図１１は、ＸＹＺ直交座標系とＵＶＷ座標系との関係を示す図である。
【図１２】図１２は、ＸＹＺ直交座標系とＵＶＷ座標系との関係を示す他の図である。
【図１３】図１３は、＋Ｙ方向に見た場合の、ＸＹＺ直交座標系とｘｙｚ直交座標系との関係を示す模式図である。
【図１４】図１４は、回転部材が回転開始位置に配置された場合の回転部材の正面図である。
【図１５】図１５は、回転開始位置における回転部材を−Ｚ方向から＋Ｚ方向に見た図である。
【図１６】図１６は、図１４において、図１５の位置からＸ軸周りに時計回りに６０度回転した方向から見た図である。
【図１７】図１７は、図３中のシャフトの回転位置と、図１３中の＋ｙ軸の指向方向との関係を示す表である。
【図１８】図１８は、図３中のシャフトの回転位置と、図１３中の＋ｙ軸周りの回転方向との関係を示す表である。
【図１９】図１９は、図１中の方位検出装置が備える制御装置の構成を示す機能ブロック図である。
【図２０】図２０は、図１９中の回転位置制御テーブルの一例を示す図である。
【図２１】図２１は、図４中の角速度センサの角速度出力の時間変化の一例を示すグラフである。
【図２２】図２２は、図４中の角速度センサの角速度出力の時間変化の他の例を示す図である。
【図２３】図２３は、図３中のシャフトの回転方向と、図１１中のＵ軸、Ｖ軸及びＷ軸の符号との関係を示す表である。
【図２４】図２４は、図１１中のＵ軸及びＶ軸周りの自転角速度の計測結果を示すグラフである。
【図２５】図２５は、図１９中の制御装置のフローチャートである。
【図２６】図２６は、図２５中の通常回転制御処理のフローチャートである。
【図２７】図２７は、図２５中の過剰回転制御処理のフローチャートである。
【発明を実施するための形態】
【００２６】
以下、図面を参照し、本発明の実施の形態を詳しく説明する。図中同一又は相当部分には同一符号を付してその説明は繰り返さない。
【００２７】
［方位検出装置の構成］
図１は本実施の形態における方位検出装置１の外観図である。方位検出装置１は、筒状の筐体１０を備える。
【００２８】
図２は、図１中の筐体１０内の構成を示す斜視図である。図２では、筐体１０の一部を１点鎖線で示す。図２を参照して、方位検出装置１はさらに、姿勢変更装置１００を備える。姿勢変更装置１００は、筐体１０に収納され、駆動源３と、傾斜クランク機構２と、複数のガイド部材４とを備える。駆動源３と複数のガイド部材４は、筐体１０に固定される。
【００２９】
駆動源３は、筐体１０の後部に配置される。駆動源３は駆動軸３１を有し、駆動軸３１を駆動軸３１の中心軸周りに回転する。駆動源３はたとえば、モータである。モータの種類は特に限定されない。モータはたとえば、ステッピングモータや超音波モータ等である。
【００３０】
図２に示すとおり、方位検出装置１は、ＸＹＺ直交座標系内に設置される。より具体的には、筐体１０はＸＹＺ直交座標系に固定される。ＸＹＺ直交座標系は、Ｘ，Ｙ，Ｚ軸が互いに９０度をなす直交座標系である。筐体１０は、Ｘ軸を駆動源３の駆動軸３１上に割り当てる。本例では、＋Ｘ方向を筐体１０の前方側と定義し、−Ｘ方向を筐体１０の後方側と定義する。また、本例では、Ｚ軸を筐体１０の鉛直方向に割り当てる。さらに、Ｙ軸を筐体１０の水平方向に割り当てる。本例では、＋Ｚ方向を筐体１０の下方側と定義し、−Ｚ方向を筐体１０の上方側と定義する。本例ではさらに、＋Ｙ方向を筐体１０の正面から見て左側と定義し、−Ｙ方向を筐体１０の正面から見て右側と定義する。駆動源３及びガイド部材４は、筐体１０に固定される。したがって、駆動源３及びガイド部材４は、ＸＹＺ直交座標系に固定される。
【００３１】
傾斜クランク機構２は、筐体１０の中央部に配置される。傾斜クランク機構２は、駆動源３の前方に配置される。
【００３２】
［傾斜クランク機構２の構成］
図３は、図２中の方位検出装置１のうち、傾斜クランク機構２とその近傍の側面図である。図３では、筐体１０の一部を１点鎖線で示す。図３を参照して、傾斜クランク機構２は、シャフト２０と、回転部材２１とを備える。
【００３３】
シャフト２０は、駆動源３により回転する。回転部材２１は、シャフト２０に対して傾斜した傾斜軸２２０周りを回転可能にシャフト２０に連結される。シャフト２０が回転するとき、回転部材２１は、Ｘ軸に対して上下に揺動しながら、傾斜軸２２０周りを回転する。傾斜軸２２０は、Ｘ軸と傾斜角α度で交差する。
【００３４】
シャフト２０は、棒状の本体２０１と、アーム部材２０２とを備える。本体２０１はＸ軸上に配置される。本体２０１の後端は、駆動軸３１に結合される。したがって、駆動源３の駆動により、本体２０１は、本体２０１の中心軸周りに回転する。本体２０１の先端には、アーム部材２０２が配置される。
【００３５】
アーム部材２０２は、Ｘ軸に対して交差する方向に延びる。本例では、アーム部材２０２は、Ｘ軸に対してほぼ直交して延び、上端部が方位検出装置１の前方に曲がっている。アーム部材２０２の後端は本体２０１に取り付けられる。アーム部材２０２の上端部は、回転部材２１と連結される。
【００３６】
［回転部材２１の構成］
回転部材２１は、シャフト２０の前方に配置され、アーム部材２０２を介してシャフト２０と連結される。回転部材２１は、本体２１０と、傾斜シャフト２１１とを備える。本体２１０は球状であり、前方が開口する収納室２１２を有する。傾斜シャフト２１１は、回転部材２１の表面に立てて配置される。傾斜シャフト２１１はさらに、傾斜軸２２０上に配置される。傾斜シャフト２１１の上端部は、アーム部材２０２のボス内に回転可能に配置される。これにより、回転部材２１は、シャフト２０の本体２０１に対してα度傾斜した傾斜軸２２０の周りを回転可能に取り付けられる。
【００３７】
図４は回転部材２１の斜視図である。図４を参照して、回転部材２１はさらに、複数の基板２２と、角速度センサ２６と、加速度センサ２８と、位置センサ２５とを備える。複数の基板２２は、収納室２１２に収納される。複数の基板２２は、図４において上下方向に隙間を空けて配置される。角速度センサ２６は、最上段の基板２２上に取り付けられる。加速度センサ２８は、中段の基板２２上に取り付けられる。角速度センサ２６、加速度センサ２８の配置場所は特に限定されず、どの基板２２上に配置されてもよい。
【００３８】
位置センサ２５は、最下段の基板２２上に配置される。位置センサ２５は光発信器及び受光器を備える。図３を参照して、筐体１０の下方には、反射板２９が配置される。方位検出装置１を正面から見たとき、反射板２９は、Ｚ軸と重複する。位置センサ２５内の光発信器は、光を回転部材２１の外部に向かって出力する。駆動源３により回転部材２１が所定の位置に移動（回転）すると、光発信器から出射された光が反射板２９で反射する。位置センサ２５内の受光器が反射光を受けたとき、位置センサ２５は検知信号を後述する制御装置３０に出力する。以上の動作により、方位検出装置１は、位置センサ２５により、回転部材２１の回転開始位置を認識できる。
【００３９】
図４に戻って、回転部材２１はさらに、ｘｙｚ直交座標系を有する。ｘｙｚ直交座標系は、ＸＹＺ直交座標系と異なり、回転部材２１に固定される。図４に示すとおり、本例では、＋ｘ方向は、回転部材２１の前方に向く。＋ｙ方向は、角速度センサ２６の検出軸及び加速度センサ２８の検出軸と一致する。
【００４０】
後述するように、＋ｙ方向は、回転部材２１の回転に応じて、Ｕ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に指向する。したがって、角速度センサ２６は、＋ｙ方向の指向方向周りの角速度を検出する。加速度センサ２８は、＋ｙ方向の指向方向の重力加速度を検出する。
【００４１】
上述のとおり、ｘｙｚ直交座標系は、回転部材２１に固定される。一方、ＸＹＺ直交座標系は、筐体１０に固定される。そのため、ＸＹＺ直交座標系に対して回転部材２１が移動（回転）すれば、ＸＹＺ座標系に対してｘｙｚ直交座標系も移動（回転）する。
【００４２】
図５は、回転部材２１の正面図である。図６は、回転部材２１の平面図である。図７は、回転部材２１の側面図である。図８は、回転部材２１の背面図である。図５〜図８を参照して、本体２１０は球状表面ＳＳを有する。球状表面ＳＳには、軌道部ＯＰが形成される。本例では、軌道部ＯＰは球状表面ＳＳに形成される溝である。軌道部ＯＰは、ｘ軸周りに形成され、＋ｘ方向に凸を有する複数の山部Ｍ１〜Ｍ４と、−ｘ方向に凸を有する複数の谷部Ｖ１〜Ｖ４とを有する。要するに、本例では、軌道部ＯＰは、ｘ軸周りに延びたジグザグ状の溝である。図４〜図７を参照して、収納室２１２の開口は矩形状であり、その各角の配置位置は、４つの谷部Ｖ１〜Ｖ４の配置位置に対応する。これにより、軌道部ＯＰと重複することなく、収納室２１２の容量を大きく形成できる。
【００４３】
図２及び図３を参照して、複数のガイド部材４（４Ａ〜４Ｃ）は、筐体１０に固定される。各ガイド部材は、同一のＹＺ平面上に配置される。図９は、ガイド部材４の斜視図である。図９を参照して、ガイド部材４は、台座４０と、ボール４１とを備える。ボール４１は、台座４０の下端に回転可能に取り付けられる。
【００４４】
図１０は、回転部材２１と各ガイド部材４とのＸＹＺ直交座標系での配置関係を示す図である。図１０を参照して、本例では、３つのガイド部材４Ａ〜４ＣがＸＹＺ直交座標系に固定される。各ガイド部材４Ａ〜４Ｃは、Ｘ軸周りに等間隔に配置される。換言すれば、各ガイド部材４Ａ〜４Ｃは、Ｘ軸周りにおいて、１２０度おきに配置される。本例では、ガイド部材４Ａは、Ｚ軸上（＋Ｚ方向側）に配置される。ガイド部材４Ｂは、方位検出装置１を正面から見たとき、ガイド部材４ＡからＸ軸周りを反時計回りに１２０度回転した位置に配置され、ガイド部材４Ｃは、２４０度回転した位置に配置される。
【００４５】
各ガイド部材４Ａ〜４Ｃのボール４１は、溝状の軌道部ＯＰ内に挿入され、軌道部ＯＰと接触する。回転部材２１は、ガイド部材４Ａ〜４Ｃにより、筐体１０に回動可能に支持される。回転部材２１が回転するとき、ボール４１は、軌道部ＯＰ上を移動する。したがって、回転部材２１の回転の仕方は、軌道部ＯＰの形状に依存する。
【００４６】
［ＵＶＷ座標系とＸＹＺ直交座標系との関係］
上述のとおり、方位検出装置１はＸＹＺ直交座標系内に設置される。本実施の形態ではさらに、ＵＶＷ座標系を想定する。ＸＹＺ直交座標系とＵＶＷ直交座標系との関係を図１１及び図１２に示す。図１１は＋Ｘ方向から−Ｘ方向に見たＹＺ平面図を示し、図１２は−Ｙ方向から＋Ｙ方向に見たＺＸ平面図（図１１の右側面図）を示す。Ｕ、Ｖ、Ｗ軸がＹＺ平面となす仰角をα度とする。つまり、シャフト２０に対する傾斜軸２２０の傾斜角αは、この仰角と一致する。さらに、Ｕ，Ｖ，Ｗ軸が互いになす交角をβとする。
【００４７】
＋Ｕ，−Ｕ，＋Ｖ，−Ｖ，＋Ｗ，−Ｗの６軸はＹＺ平面上においては常に６０度ごとに離間して配置される。具体的には、図１１を参照して、＋Ｕ軸はＹＺ平面に垂直に投影されると−Ｚ軸と一致する。−Ｕ軸はＹＺ平面に垂直に投影されると＋Ｚ軸と一致する。−Ｗ軸は、ＹＺ平面に垂直に投影されると−Ｚ軸に対して６０度をなす。＋Ｗ軸はＹＺ平面に垂直に投影されると＋Ｚ軸に対して６０度をなす。＋Ｖ軸はＹＺ平面に垂直に投影されると＋Ｚ軸に対して６０度をなす。−Ｖ軸はＹＺ平面に垂直に投影されると−Ｚ軸に対して６０度をなす。図１１中の＋Ｕ，−Ｕ，＋Ｖ，−Ｖ，＋Ｗ，−Ｗの６軸のうち、点線の軸は、図１１の紙面よりも後方に延びることを示し、実線の軸は、図１１の紙面よりも手前に延びることを示す。仰角αは３０〜４０度が好ましいが、最も好ましい仰角αは、α＝ｔａｎ−１（１／√２）＝３５．２６度である。また、交角βは９０度が最も好ましい。以下では、α＝３５．２６度、β＝９０度の場合を説明する。
【００４８】
また、以降の説明では、ｘｙｚ直交座標系において、＋ｘ側のｘ軸を「＋ｘ軸」、−ｘ軸側のｘ軸を「−ｘ軸」という。他の軸についてもｘ軸の場合と同様に定義する。また、他の座標系（ＸＹＺ直交座標系及びＵＶＷ座標系）においても、ｘ軸の場合と同様に定義する。
【００４９】
また、ＸＹＺ直交座標系、ｘｙｚ直交座標系及びＵＶＷ直交座標系の原点は、いずれも、回転部材２１の本体２１０の中心点である。
【００５０】
［方位検出装置１の動作］
方位検出装置１は、ＵＶＷ直交座標系の各軸周りの自転角速度ωＵ、ωＶ及びωＷと、各軸方向の重力加速度ｇＵ、ｇＶ及びｇＷとを計測する。次に、方位検出装置１は、計測された自転角速度ωＵ、ωＶ、ωＷ及び重力加速度ｇＵ、ｇＶ、ｇＷをＸＹＺ直交座標系における自転角速度ωＸ、ωＹ、ωＺ及び重力加速度ｇＸ、ｇＹ、ｇＺに座標変換する。次に、方位検出装置１は、自転角速度ωＸ、ωＹ、ωＺ及び重力加速度ｇＸ、ｇＹ、ｇＺに基づいて方位角ψを算出する。方位検出装置１では、角速度センサ２６及び加速度センサ２８はＹＺ平面に対して±α度しか揺動されない。そのため、回転角度が小さく、従来の方位測定装置よりも大きな空間を必要としない。
【００５１】
ＵＶＷ直交座標系の自転角速度ωＵ、ωＶ及びωＷと、重力加速度ｇＵ、ｇＶ及びｇＷとを計測する場合、回転部材２１の回転により、角速度センサ２６及び加速度センサ２８の検出軸の方向（本例では＋ｙ軸）を、Ｕ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に指向させる必要がある。
【００５２】
軌道部ＯＰは、シャフト２０の回転により回転部材２１が複数のガイド部材４Ａ〜４Ｃと接触しながら回転するとき、回転部材２１の＋ｙ軸を、Ｕ軸、Ｖ軸びＷ軸のプラス及びマイナス方向に順次向ける形状を有する。
【００５３】
［回転部材２１が回転したときの＋ｙ軸の軌跡］
初めに、回転部材２１が回転したときの＋ｙ軸の軌跡を説明する。
【００５４】
初めに、筐体１０に固定されたＸＹＺ直交座標系と、回転部材２１に固定されたｘｙｚ直交座標系との関係を説明する。図１３は、＋Ｙ方向に見た場合の、ＸＹＺ直交座標系とｘｙｚ直交座標系との関係を示す模式図である。図１３を参照して、ｘ軸は、傾斜軸２２０と一致する。したがって、ｘ軸とＸ軸とがなす角はα度である。ｙ軸は、回転部材２１が図１３に示す位置に配置される場合、ＸＺ平面上に配置される。図１２及び図１３を参照して、回転部材２１が図１３に示す位置に配置される場合、＋ｙ軸は、＋Ｕ軸と一致する。以降の説明では、図１３に示す位置を回転部材２１の回転開始位置と定義する。なお、位置センサ２５は、回転部材２１が図１３の位置に移動したとき、検知信号を制御装置３０（図１９参照）に出力する。
【００５５】
図１４は、回転部材２１が回転開始位置に配置された場合の回転部材２１の正面図、つまり、＋Ｘ方向から−Ｘ方向に見た図である。図１５は、回転開始位置における回転部材２１を−Ｚ方向から＋Ｚ方向に見た図である。図１６は、図１４において、図１５の位置からＸ軸周りに時計回りに６０度回転した方向から見た図である。
【００５６】
図１４〜図１６では、＋ｙ軸を棒状に示す。図１４〜図１６を参照して、回転開始位置において、上述のとおり、＋ｙ軸は、＋Ｕ軸と一致する。
【００５７】
上述の通り、ＵＶＷ直交座標系の自転角速度ωＵ、ωＶ及びωＷと、重力加速度ｇＵ、ｇＶ及びｇＷとを計測する場合、方位検出装置１は、回転部材２１の回転により、角速度センサ２６及び加速度センサ２８の検出軸に一致する＋ｙ軸を、Ｕ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向を指向させる必要がある。
【００５８】
図１４〜図１６に示す例では、方位検出装置１は、＋ｙ軸を、＋Ｕ軸→−Ｗ軸→＋Ｖ軸→−Ｕ軸→＋Ｗ軸→−Ｖ軸（→＋Ｕ軸）の順に向けていく。このとき、＋ｙ軸は、軌跡Ｒ１→軌跡Ｒ２→軌跡Ｒ３→軌跡Ｒ４→軌跡Ｒ５→軌跡Ｒ６（→軌跡Ｒ１）の順に移動する。なお、＋ｙ軸がＵ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に指向する順序は、上記の順序に限定されない。後述するように、上記の順序と異なる順序で、＋ｙ軸をＵ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に指向させてもよい。
【００５９】
図１７は、＋ｙ軸の指向方向と、シャフト２０の回転位置と、＋ｙ軸のＸ軸周りの回転量との関係を示す表である。図１７を参照して、シャフト２０の回転位置は、回転開始位置を基準にした場合に、−Ｘ方向に見てシャフト２０がＸ軸周りを反時計回りに回転するときの回転角を示す。回転部材２１が回転開始位置にいる場合のシャフト２０の回転位置は、０度である。
【００６０】
シャフト２０が０〜１４４０度回転することにより、＋ｙ軸は、＋Ｕ軸→−Ｗ軸→＋Ｖ軸→−Ｕ軸→＋Ｗ軸→−Ｖ軸（→＋Ｕ軸）の順に指向する。例えば、シャフト２０が、−Ｘ方向に見て反時計周りに０度から２４０度回転することにより、＋ｙ軸は、＋Ｕ軸から−Ｗ軸へ移動する（図１４〜図１６参照）。このとき、＋ｙ軸は、Ｘ軸周りを６０度回転する。同様に、＋ｙ軸が、軌跡Ｒ２、軌跡Ｒ３、軌跡Ｒ４、軌跡Ｒ５及び軌跡Ｒ６を移動する場合、シャフト２０は、２４０度回転し、＋ｙ軸は、Ｘ軸周りを６０度回転する。
【００６１】
［回転部材２１の＋ｙ軸周りの回転］
＋ｙ軸がＵ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に向けて移動する間、回転部材２１は、＋ｙ軸周りを回転する。
【００６２】
最初に、回転部材２１が＋ｙ軸周りを回転する理由を説明する。回転部材２１は、シャフト２０の回転に応じて、傾斜軸２２０（ｘ軸）周りを回転する。回転部材２１のｘ軸周りの回転に伴って、ガイド部材４Ａ〜４Ｃは、軌道部ＯＰ上を移動する。軌道部ＯＰが、＋ｘ軸周りに延びるジグザグ状の溝であるため、回転部材２１は、ガイド部材４Ａ〜４Ｃが軌道部ＯＰ上を移動することに伴って＋ｙ軸周りに回転する。＋ｙ軸周りの回転とは、ｙ軸を中心軸に回転する回転運動を意味する。したがって、＋ｙ軸がＵ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に移動する間、角速度センサ２６は、＋ｙ軸周りの回転に伴う角速度を出力する。
【００６３】
＋ｙ軸周りの回転方向は、シャフト２０の回転位置に応じて変化する。図１８は、シャフト２０の回転位置と、＋ｙ軸周りの回転方向との対応関係を示す表である。図１８を参照して、＋ｙ軸周りの回転方向「ＣＷ」は、−ｙ方向に見て時計回りの方向を示し、「ＣＣＷ」は、−ｙ軸方向に見て反時計回りの方向を示す。方向転換点は、＋ｙ軸周りの回転方向が変化するときのシャフト２０の回転位置である。継続回転量は、＋ｙ軸周りの回転方向が次に変化するまでに必要なシャフト２０の回転量である。
【００６４】
例えば、＋ｙ軸が＋Ｕ軸から−Ｗ軸へ移動する場合（軌跡Ｒ１を移動する場合）、回転部材２１は、シャフト２０の回転に伴って、＋ｙ軸周りを反時計回りに回転する。シャフト２０の回転位置が回転開始位置から方向転換点（１２０度）に達するまで、回転部材２１は、＋ｙ軸を反時計回りに回転し続ける。そして、シャフト２０の回転位置が１２０度より大きくなったときに、＋ｙ軸周りの回転が反時計回りから時計回りに変化する。回転部材２１は、＋ｙ軸が−Ｗ軸方向に指向するまで、＋ｙ軸周りを時計回りに回転し続ける。
【００６５】
さらに、＋ｙ軸が−Ｗ軸から＋Ｖ軸に移動する場合（軌跡Ｒ２を移動する場合）、シャフト２０の回転位置が方向転換点（３６０度）に達するまで、回転部材２１は、＋ｙ軸周りを時計回りに回転し続ける。シャフト２０の回転位置が、方向転換点（３６０度）より大きくなったとき、＋ｙ軸周りの回転が時計回りから反時計回りに変化する。回転部材２１は、＋ｙ軸が＋Ｖ軸方向に指向するまで、＋ｙ軸周りを反時計回りに回転し続ける。
【００６６】
＋ｙ軸が軌跡Ｒ３、軌跡Ｒ４、軌跡Ｒ５及び軌跡Ｒ６に沿って移動する場合も同様に、＋ｙ軸周りの回転方向は、シャフト２０の回転位置が方向転換点に達するたびに変化する。つまり、＋ｙ軸周りの回転方向は、シャフト２０が２４０度回転するたびに変化する。
【００６７】
なお、シャフト２０が−Ｘ方向に見て時計回りに回転する場合、＋ｙ軸周りの回転方向は、図１８とは逆になる。例えば、シャフト２０を時計回りに回転させることにより、＋ｙ軸を−Ｗ軸から＋Ｕ軸へ移動させる場合、＋ｙ軸は、最初に反時計回り方向（ＣＣＷ）に回転する。その後、回転位置が方向転換点（１２０度）よりも小さくなったときに、＋ｙ軸周りの回転は、時計回り方向（ＣＷ）に変化する。
【００６８】
＋ｙ軸周りの回転方向が変化するシャフト２０の回転位置（方向転換点）は、１２０、３６０、６００、８４０、１０８０及び１３２０度である。シャフト２０の回転位置が方向転換点に達したときに、ガイド部材４Ａ〜４Ｃのいずれかが、軌道部ＯＰが有する山部Ｍ１〜Ｍ４及び谷部Ｖ１〜Ｖ４のうち、＋ｙ軸と球状表面ＳＳとの交点ＰＩ（図６参照）の直近に位置する山部Ｍ１を通過する。このとき、回転部材２１に対するガイド部材４Ａ〜４Ｃの移動方向が逆転するため、＋ｙ軸周りの回転方向が変化する。つまり、角速度センサの検出軸（＋ｙ軸）が所定の測定方向（例えば、＋Ｕ軸）から他の測定方向（例えば、−Ｖ軸）に移動する間に、角速度センサの検出軸（＋ｙ軸）周りの回転方向が変化することになる。
【００６９】
［制御装置３０の構成］
図１９を参照して、方位検出装置１はさらに、コンピュータ等からなる制御装置３０を備える。制御装置３０は、姿勢変更装置１００を制御する。制御装置３０と回転部材２１内の角速度センサ２６、加速度センサ２８及び位置センサ２５とは、有線又は無線で接続されている。
【００７０】
制御装置３０は、駆動源制御部３２と、回転位置制御テーブル３３と、自転角速度計測部３６と、重力加速度計測部３８と、自転角速度座標変換部４２と、重力加速度座標変換部４６と、方位角算出部４４とを備える。
【００７１】
駆動源制御部３２は、シャフト２０の回転量が設定された回転位置制御テーブル３３に基づいて、駆動源３を制御する。回転位置制御テーブル３３は、図示しない記憶装置に格納される。
【００７２】
自転角速度計測部３６は、角速度センサ２６により所定時間内に検出された角速度を、自転角速度ω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とに基づいて、方位角ψを算出する。
【００７５】
［回転位置制御テーブル３３]
図２０を参照して、回転位置制御テーブル３３には、＋ｙ軸をＵ軸、Ｖ軸及びＷ軸の各プラス及びマイナス方向に向ける順序が設定されている。＋ｙ軸は、図１６に示す順序と異なり、回転開始位置の＋Ｕ軸からスタートして、−Ｕ軸、＋Ｕ軸、＋Ｖ軸、−Ｖ軸、＋Ｖ軸、＋Ｗ軸、−Ｗ軸、＋Ｗ軸、＋Ｕ軸の順に指向する。＋ｙ軸が＋Ｕ軸に一致しているときのシャフトの位置が、シャフト２０の回転開始位置（回転角＝０度）となる。
【００７６】
自転角速度ωＵは、＋Ｕ軸の自転角速度と−Ｕ軸の自転角速度との差分を計算することにより得られる。自転角速度ωＶ及び自転角速度ωＷも、同様にして得られる。これにより、角速度センサ２６に固有のバイアスが相殺される。
【００７７】
＋Ｕ軸の自転角速度の計測が、−Ｕ軸の自転角速度の計測の前後に２回行われる理由を説明する。角速度センサ２６の角速度出力は、角速度センサ２６に固有のオフセットを含む。オフセットは、温度などの動作条件の違いによって変化するが、ドリフトによっても変化する。ドリフトとは、オフセットが時間の経過に応じて増加（又は減少）する現象を指す。方位検出装置１は、−Ｕ軸の計測の前後に＋Ｕ軸の計測を２回行うことにより、ドリフトに伴うオフセットの変化率を取得する。ドリフトは、取得した変化率に基づいて、計測された角速度から除去される。＋Ｖ軸、＋Ｗ軸が２回計測される理由も同様である。
【００７８】
また、最初に＋Ｕ軸の自転角速度を計測し、最後に＋Ｕ軸の自転角速度を計測する理由も、ドリフトを除去するためである。計測の最後に、回転開始位置（＋Ｕ軸）の自転角速度成分を計測することで、方位角ψを得るために必要な一連の計測期間に生じるオフセットの変化率を取得することができる。
【００７９】
回転位置制御テーブル３３において、シャフト２０の回転位置は、図１７に示す回転位置と同様である。つまり、回転位置は、＋ｙ軸を回転開始位置から各軸方向に指向させるために必要なシャフト２０の回転角である。
【００８０】
回転量は、＋ｙ軸を現在の軸方向から次の軸方向に指向させる際のシャフト２０の回転角である。回転量の符号は、シャフト２０の回転方向を示し、符号「＋」が、−Ｘ方向に見て反時計回りの方向であり、符号「−」が−Ｘ方向に見て時計回りの方向である。例えば、−Ｕ軸の回転量は、＋７２０度に設定されている。したがって、制御装置３０は、＋ｙ軸を＋Ｕ軸から−Ｕ軸に移動させる場合、シャフト２０を−Ｘ方向に見て反時計回りに７２０度回転させる。
【００８１】
制御フラグは、制御装置３０が通常回転制御及び過剰回転制御のどちらでシャフト２０を回転させるかを示す。制御フラグが０の場合、通常回転制御の実行を示し、制御フラグが１の場合、過剰回転制御の実行を示す。通常回転制御及び過剰回転制御の詳細については、後述する。
【００８２】
［制御装置３０の基本的な動作］
＋ｙ軸を各軸方向に向ける際の制御装置３０の基本的な動作を説明する。
【００８３】
最初に、通常回転制御を実行する制御装置３０の動作を、＋ｙ軸を＋Ｕ軸から−Ｕ軸へ移動させる場合を例に説明する。以下、特に説明のない限り、重力加速度の計測の説明については省略するが、重力加速度は、自転角速度と並行して計測される。
【００８４】
図２０を参照して、＋Ｕ軸の１回目の自転角速度の計測が終了した後に、制御装置３０は、−Ｕ軸の制御フラグが０に設定されているため、通常回転制御を実行する。通常回転制御とは、回転位置制御テーブル３３に設定された回転量だけシャフト２０を回転させる処理である。制御装置３０は、−Ｕ軸に設定された回転量（＋７２０度）だけシャフト２０を回転させる。この結果、＋ｙ軸は、−Ｗ軸、＋Ｖ軸を通過して（図１４参照）、−Ｕ軸に到達する。
【００８５】
次に、過剰回転制御を実行する制御装置３０の動作を、＋ｙ軸が＋Ｕ軸から＋Ｖ軸へ移動する場合を例に説明する。
【００８６】
＋Ｕ軸の自転角速度の２回目の計測が終了した後に、制御装置３０は、次の計測対象である＋Ｖ軸の制御フラグが１に設定されているため、過剰回転制御を実行して＋ｙ軸を＋Ｕ軸から＋Ｖ軸へ移動させる。
【００８７】
回転位置制御テーブル３３には、＋Ｕ軸から＋Ｖ軸へ移動する際の回転量として、＋４８０度が設定されている。制御装置３０は、予め設定された過剰量（例えば、４０度）を＋４８０度に加算し、＋５２０度を算出する。制御装置３０は、シャフト２０を−Ｘ方向に見て反時計回りの方向に５２０度回転させる（過剰回転）。＋ｙ軸は、−Ｗ軸を通過し、さらに、＋Ｖ軸を一旦通過する（図１４参照）。つまり、＋ｙ軸は、過剰回転により、軌跡Ｒ３の途中（＋Ｖ軸と−Ｕ軸との間）まで移動する。
【００８８】
制御装置３０は、＋５２０度回転させたシャフト２０を、さらに過剰量だけ逆回転させる。具体的には、制御装置３０は、シャフト２０を−Ｘ方向に見て時計回りに４０度回転させる。＋Ｖ軸を一旦通過した＋ｙ軸は、逆回転により、軌跡Ｒ３を逆にたどって＋Ｖ軸に到達する。
【００８９】
次に、過剰回転制御を実行する理由を説明する。
【００９０】
例えば、＋ｙ軸が＋Ｕ軸、＋Ｖ軸、−Ｖ軸の順に移動する際に通常回転制御のみが用いられる場合、制御装置３０は、自転角速度ωＶを正確に計測することができない。
【００９１】
図２１は、＋Ｕ軸から＋Ｖ軸への＋ｙ軸の移動及び＋Ｖ軸から−Ｖ軸への＋ｙ軸の移動の際に通常回転制御が実行された場合における、角速度センサ２６により検出される角速度の時間変化を示すグラフである。
【００９２】
図１８、図２０及び図２１を参照して、通常回転制御により＋ｙ軸が＋Ｕ軸から＋Ｖ軸へ移動する場合、制御装置３０は、シャフト２０を＋４８０度回転させる。＋ｙ軸は、シャフト２０の回転に伴って、＋ｙ軸周りを回転しながら＋Ｖ軸に到達する。角速度センサ２６は、＋ｙ軸の移動中にも、ｙ軸周りの回転に伴う角速度(以下、「移動角速度」)を出力する。
【００９３】
＋ｙ軸は、時刻ｔ１１に＋Ｖ軸に到達する。＋Ｖ軸に到達する直前の＋ｙ軸周りの回転方向は、反時計回りであるため、角速度センサ２６は、時刻ｔ１１以前にマイナスの角速度を出力する。＋ｙ軸が＋Ｖ軸で停止した時刻ｔ１１以降、角速度出力は、増加を続ける。この理由は、角速度センサ２６の時定数の影響により、移動角速度がｙ軸の停止後でも角速度出力に含まれるためである。
【００９４】
＋ｙ軸が＋Ｖ軸で停止した直後の期間（時刻ｔ１１〜ｔ１２）の角速度出力は、時定数の影響により大きく変動して安定しない。時定数の影響を抑制するために、制御装置３０は、所定期間（時刻ｔ１２〜ｔ１３）内での角速度センサ２６の角速度出力を、＋Ｖ軸の自転角速度として計測する。しかし、所定期間においても時定数の影響が残っているため、＋Ｖ軸の自転角速度は、僅かながら増加を続ける。つまり、＋Ｖ軸の自転角速度は、時間とともに増加する特性を有する時定数に伴う誤差を含む。
【００９５】
その後、シャフト２０が＋７２０度回転することにより、＋ｙ軸は、＋Ｖ軸から−Ｖ軸へ移動する（時刻ｔ１３〜ｔ１４）。＋ｙ軸が−Ｖ軸に到達する直前に、＋ｙ軸が＋ｙ軸周りを時計回りに回転しているため、角速度センサ２６は、時刻ｔ１４の直前にプラスの角速度を出力する。＋ｙ軸が−Ｖ軸で停止した時刻ｔ１４以降の角速度出力は、時定数の影響により減少を続ける。したがって、時刻ｔ１５〜ｔ１６に計測される−Ｖ軸の自転角速度は、時間とともに減少する特性を有する時定数に伴う誤差を含む。−Ｖ軸の自転角速度に含まれる時定数に伴う誤差の特性は、＋Ｖ軸の自転角速度に含まれる時定数に伴う誤差の特性と逆である。
【００９６】
＋Ｖ軸の自転角速度ωＶ＋及び−Ｖ軸の自転角速度ωＶ−は、以下のように表される。
【００９７】
ωＶ＋＝ωＶ＋Ｔｒｕｅ＋Ｖ＋δ１  ・・・（１）
ωＶ−＝ωＶ−Ｔｒｕｅ＋Ｖ＋δ２  ・・・（２）
【００９８】
上記式において、ωＶ＋Ｔｒｕｅは、＋Ｖ軸の上述した誤差を含まない、真の自転角速度であり、ωＶ−Ｔｒｕｅは、−Ｖ軸の真の自転角速度である。ωＶ＋ＴｒｕｅとωＶ−Ｔｒｕｅは絶対値が等しく、符号が異なる値である。Ｖは、オフセットである。δ１は、＋Ｖ軸の自転角速度ωＶ＋に含まれる時定数に伴う誤差である。δ２は、自転角速度ωＶ−に含まれる時定数に伴う誤差である。自転角速度ωＶは、式（１）から式（２）を減算することにより得られる。オフセットは、上記の減算により相殺される。しかし、時定数に伴う誤差δ１の特性が、時定数に伴う誤差δ２の特性と逆であるため、上記の減算により、時定数に伴う誤差δ２は、時定数に伴う誤差δ１と相殺されず、逆に、時定数に伴う誤差δ１に加算される。このように、＋Ｕ軸から＋Ｖ軸への移動及び＋Ｖ軸から−Ｖ軸への移動に通常回転制御を用いた場合、時定数に伴う誤差が拡大される。制御装置３０は、自転角速度ωＶを正確に計測することができず、真北を正確に検出することができない。
【００９９】
そこで、制御装置３０は、自転角速度ωＶに含まれる時定数に伴う誤差を小さくするために、＋ｙ軸を＋Ｕ軸から＋Ｖ軸に移動させる際に、過剰回転制御を実行する。なお、＋ｙ軸が＋Ｖ軸から−Ｖ軸に移動する際には、通常回転制御が実行される。
【０１００】
図２２は、過剰回転制御が＋Ｕ軸から＋Ｖ軸への移動に用いられ、通常回転制御が＋Ｖ軸から−Ｖ軸への移動に用いられた場合における、角速度センサ２６の角速度出力の時間変化を示すグラフである。
【０１０１】
制御装置３０は、＋７２０度に過剰量（４０度）を加算した回転量でシャフト２０を回転させる（過剰回転）。＋ｙ軸は、過剰回転に伴って、＋Ｖ軸を一旦通過し、軌跡Ｒ３の途中で停止する。＋ｙ軸が軌跡Ｒ３の途中で停止したとき、＋ｙ軸周りの回転方向は、反時計回りである。
【０１０２】
そして、制御装置３０は、シャフト２０を４０度逆回転させて、＋ｙ軸を＋Ｖ軸に移動させる。シャフト２０を−Ｘ方向に見て時計回りに回転させた場合、＋ｙ軸周りの回転方向は、図１８に示す回転方向と逆になる。したがって、＋ｙ軸が＋Ｖ軸に到達する直前の＋ｙ軸周りの回転方向は、時計回りとなる。
【０１０３】
この場合、角速度センサ２６は、時刻ｔ２１以前にプラスの移動角速度を出力する。＋ｙ軸が＋Ｖ軸で停止した時刻ｔ２１以降、角速度出力は、減少を続ける。この結果、＋Ｖ軸の自転角速度に含まれる時定数に伴う誤差δ１は、時間とともに減少する特性を有する。
【０１０４】
＋ｙ軸は、通常回転制御により、＋Ｖ軸から−Ｖ軸まで移動する。＋ｙ軸が−Ｖ軸に到達するまで（時刻ｔ２５まで）の角速度出力の時間変化、＋Ｖ軸で停止しているときの角速度出力の時間変化（時刻ｔ２５以降）は、図２１に示す時刻ｔ１４以降の時間変化と同様である。つまり、−Ｖ軸の自転角速度に含まれる時定数に伴う誤差δ２は、時間とともに減少する特性を有する。
【０１０５】
過剰回転制御により、＋Ｖ軸の自転角速度に含まれる時定数に伴う誤差δ１は、−Ｖ軸の自転角速度に含まれる時定数に伴う誤差δ２と同様の特性を有する。したがって、自転角速度ωＶ（＋Ｖ方向及び−Ｖ方向に沿った軸方向の自転角速度）を得るために、式（１）から式（２）を減算した場合、時定数に伴う誤差δ１は、時定数に伴う誤差δ２と相殺される。自転角速度ωＶにおいて、時定数に伴う誤差の影響を抑えることができるため、真北の検出精度を向上させることができる。
【０１０６】
次に、＋ｙ軸の移動が過剰回転制御の対象となる基準について説明する。
【０１０７】
図２３は、シャフト２０の回転方向と、Ｕ軸、Ｖ軸、Ｗ軸の符号との関係を示す表である。図２３を参照して、Ａは、Ｕ軸、Ｖ軸、Ｗ軸の符号を示す。＋Ｕ軸、＋Ｖ軸及び＋Ｗの符号は、プラスである。−Ｕ軸、−Ｖ軸又は−Ｗ軸の符号は、マイナスである。
【０１０８】
Ｂは、回転位置制御テーブル３３に基づいてシャフト２０を回転させるときのシャフト２０の回転方向を示す。シャフト２０が−Ｘ方向に見て時計回りに回転する場合、Ｂは、マイナスに設定され、シャフト２０が−Ｘ方向に見て反時計回りに回転する場合、Ｂは、プラスに設定される。
【０１０９】
Ａ×Ｂは、Ｕ軸、Ｖ軸、Ｗ軸の符号（Ａ）と、シャフト２０の回転方向の符号（Ｂ）との積である。以下、Ａ×Ｂを便宜的に「符号積」と呼ぶ。符号積は、＋ｙ軸が各軸で停止する直前の＋ｙ軸周りの回転方向に相当する。符号積がプラスである場合、各軸で停止する直前のｙ軸周りの回転方向は、反時計回りである。符号積がマイナスである場合、各軸で停止する直前のｙ軸周りの回転方向は、時計回りである。
【０１１０】
例えば、＋ｙ軸が＋Ｖ軸から−Ｖ軸に移動する場合、＋ｙ軸が−Ｖ軸で停止する直前の＋ｙ軸の回転方向は、時計回りであり、図１８に示す＋ｙ軸周りの回転方向に一致する。
【０１１１】
＋ｙ軸の移動が、通常回転制御の対象となるか、過剰回転制御の対象となるかは、符号積に基づいて決定される。具体的には、過剰回転制御では、シャフト２０は、回転位置制御テーブル３３で設定された回転量よりも過剰に回転した後に逆回転するため、過剰回転制御に要する時間は、通常回転制御に要する時間よりも長い。したがって、制御装置３０は、ある軸の自転角速度を計測する場合、通常回転制御の実行回数が過剰回転制御の実行回数よりも多くなるように、過剰回転制御の対象を決定すればよい。
【０１１２】
例えば、Ｖ軸の場合、符号積Ａ×Ｂは、プラスよりもマイナスの方が多い。したがって、制御装置３０は、＋Ｖ軸から−Ｖ軸への移動、及び−Ｖ軸から＋Ｖ軸への移動の際に通常回転制御を実行し、＋Ｕ軸から＋Ｖ軸への移動の際に過剰回転制御を実行する。過剰回転制御の実行回数を抑制することにより、各軸の自転角速度の計測時間を短縮できる。
【０１１３】
Ｕ軸の場合、符号積Ａ×Ｂは、マイナスで揃っており、Ｗ軸の場合、符号積Ａ×Ｂは、プラスで揃っている。したがって、自転角速度ωＵ、及び自転角速度ωＷの計測の際には、制御装置３０は、通常回転制御のみを実行すればよい。
【０１１４】
あるいは、各軸の符号積がすべて一致するように、過剰回転制御の対象を決定してもよい。図２３に示す符号積Ａ×Ｂにおいて、プラスの数は、マイナスの数よりも少ない。したがって、符号積Ａ×Ｂがマイナスに設定された＋Ｕ軸から−Ｕ軸への移動、−Ｕ軸から＋Ｕ軸への移動、＋Ｖ軸から−Ｖ軸への移動及び−Ｖ軸から＋Ｖ軸への移動の際に、過剰回転制御を実行してもよい。
【０１１５】
このように、制御装置３０は、角速度センサ２６が所定の測定方向周りの角速度の計測を開始する前に、通常回転制御により、角速度センサ２６が検出軸（＋ｙ軸）周りを第１回転方向に回転するように姿勢変更装置１００を制御する。制御装置３０は、角速度センサ２６が所定の測定方向と反対の方向周りの角速度の計測を開始する前に、過剰回転制御により、角速度センサ２６が検出軸（＋ｙ軸）周りを第１回転方向と反対の第２回転方向に回転し、角速度センサ２６が検出軸（＋ｙ軸）周りをさらに第１回転方向に回転するように姿勢変更装置１００を制御する。これにより、角速度センサからの出力に含まれる時定数の影響による誤差を抑制することができる。
【０１１６】
図２４は、通常回転制御のみを用いた場合と、通常回転制御に加えて過剰回転制御を併用した場合とにおける自転角速度の計測結果を示すグラフである。図２４のグラフは、図２０に示す回転位置制御テーブル３３で指定された順序にしたがって、各軸の自転角速度を計測した結果を示す。
【０１１７】
過剰回転制御を併用した場合の自転角速度が、通常回転制御のみを用いた場合の自転角速度よりも全体的に小さくなっている。この理由は、時定数に伴う誤差が、通常回転制御のみを用いた場合には相殺されず、自転角速度に上乗せされているのに対して、過剰回転制御を用いた場合では相殺されているためである。過剰回転制御を用いることにより、自転角速度の計測精度を向上できることが分かる。
【０１１８】
また、過剰回転制御を用いた場合の自転角速度のばらつきが、通常回転制御のみを用いた場合の自転角速度のばらつきよりも小さい。自転角速度のばらつきは、時定数に伴う誤差により生じる。時定数に伴う誤差が、過剰回転制御により相殺されることにより、自転角速度のばらつきが抑制されていることが分かる。
【０１１９】
［シャフト２０の回転制御］
図２５は、制御装置３０のフローチャートである。図２５を参照して、制御装置３０は、＋ｙ軸の現在位置と、回転位置制御テーブル３３とに基づいて、＋ｙ軸の次の指向方向を特定する（ステップＳ１）。
【０１２０】
制御装置３０は、＋ｙ軸を次の軸方向に移動させる際に、過剰回転制御を実行するか否かを判断する（ステップＳ２）。具体的には、制御装置３０は、回転位置制御テーブル３３を参照して、次の軸方向に対応する制御フラグが０に設定されている場合（ステップＳ２においてＮｏ）、通常回転制御を実行する（ステップＳ３）。例えば、図２０を参照して、次の軸方向が−Ｕ軸である場合、制御装置３０は、−Ｕ軸の制御フラグが０に設定されているため、通常回転制御を実行する。
【０１２１】
一方、次の軸方向の制御フラグが「１」に設定されている場合（ステップＳ２においてＹｅｓ）、制御装置３０は、過剰回転制御を実行する（ステップＳ４）。例えば、図２０を参照して、＋ｙ軸の現在位置が＋Ｕ軸であり、次の停止位置が＋Ｖ軸である場合、制御装置３０は、＋Ｖ軸の制御フラグが「１」に設定されているため、過剰回転制御を実行する。
【０１２２】
図２６は、通常回転制御（ステップＳ３）のフローチャートである。図２６を参照して、制御装置３０は、次の軸方向に対応する回転量を回転位置制御テーブル３３から取得し（ステップＳ３１）、取得した回転量をシャフト２０の回転量として設定する（ステップＳ３２）。制御装置３０は、取得した回転量に応じたパルス信号を駆動源３に出力する（ステップＳ３３）。シャフト２０の回転量が設定された回転量に達した場合（ステップＳ３４においてＹｅｓ）、制御装置３０は、通常回転制御を終了する。
【０１２３】
図２７は、過剰回転制御（ステップＳ４）のフローチャートである。図２７を参照して、制御装置３０は、次の停止位置に対応する回転量を回転位置制御テーブル３３から取得する（ステップＳ４１）。駆動源制御部３２は、予め設定された過剰量（４０度）を取得した回転量に加算することにより、シャフト２０の回転量を過剰に設定する（ステップＳ４２）。取得した回転量の符号がプラスの場合、過剰回転量が取得した回転量に加算されることにより、シャフト２０の回転量が設定される。取得した回転量の符号がマイナスの場合、過剰回転量が取得した回転量から減算されることにより、シャフト２０の回転量が過剰に設定される。
【０１２４】
制御装置３０は、設定された回転量に応じたパルス信号を駆動源３に出力する（ステップＳ４３）。これにより、＋ｙ軸が次の軸方向を一旦通過する。
【０１２５】
シャフト２０の回転移動量が過剰に設定された回転量に達した場合（ステップＳ４４においてＹｅｓ）、制御装置３０は、過剰量をシャフト２０の回転量として設定する（ステップＳ４５）。回転方向は、ステップＳ４１で取得した回転量の符号に基づいて、逆方向に設定される。制御装置３０は、設定された回転量に応じたパルス信号を駆動源３に出力する（ステップＳ４６）。シャフト２０の回転量が設定された回転量となるまで（ステップＳ４６においてＹｅｓ）、パルス信号を駆動源３に出力する。これにより、＋ｙ軸が、指定された次の軸方向で停止する。
【０１２６】
以上説明したように、方位検出装置１は、通常回転制御又は過剰回転制御を実行する。これにより、＋ｙ軸がＵ軸、Ｖ軸、Ｗ軸の各プラス及びマイナス方向に到達する直前のｙ軸周りの回転方向を一致させることができる。Ｕ軸、Ｖ軸、Ｗ軸の自転角速度を計算する際に時定数に伴う誤差を相殺することができるため、方位角ψの検出精度を向上することができる。
【０１２７】
上記実施の形態では、１軸タイプの方位検出装置１が通常回転制御及び過剰回転制御を実行する例を説明したが、これに限られない。２軸又は３軸タイプの方位検出装置が、通常回転制御及び過剰回転制御を実行してもよい。
【０１２８】
例えば、Ｘ軸用の角速度センサと、Ｙ軸用の角速度センサと、Ｚ軸用の角速度センサとを備える３軸タイプの方位検出装置を考える。３軸タイプの方位検出装置は、Ｘ軸用の角速度センサをＹ軸周りに回転させることにより、＋Ｘ軸の自転角速度と−Ｘ軸の自転角速度とを計測する。このとき、Ｙ軸用の角速度センサは、Ｙ軸周りを回転する。したがって、＋Ｘ軸、−Ｘ軸、＋Ｙ軸、−Ｙ軸の計測順序によっては、Ｙ軸用の角速度センサの回転方向が、＋Ｙ軸の自転角速度を計測する直前と、−Ｙ軸の自転角速度を計測する直前とで、逆になる場合がある。
【０１２９】
このような場合、３軸タイプの方位検出装置は、＋Ｙ軸の自転角速度の計測を開始する前に、過剰回転制御を実行する。これにより、Ｙ軸用の自転角速度センサの回転方向を、＋Ｙ軸の自転角速度を計測する直前と−Ｙ軸の自転角速度を計測する直前とで揃えることができるため、Ｙ軸方向の自転角速度の計測精度を向上させることが可能となる。
【０１３０】
上記実施の形態において、制御装置３０の各機能ブロックは、ＬＳＩなどの半導体装置により個別に１チップ化されても良いし、一部又は全部を含むように１チップ化されても良い。また、集積回路化の手法はＬＳＩに限るものではなく、専用回路又は汎用プロセサで実現してもよい。ＬＳＩ製造後に、プログラムすることが可能なＦＰＧＡ（Ｆｉｅｌｄ  Ｐｒｏｇｒａｍｍａｂｌｅ  Ｇａｔｅ  Ａｒｒａｙ）や、ＬＳＩ内部の回路セルの接続や設定を再構成可能なリコンフィギュラブル・プロセッサを利用しても良い。
【０１３１】
また、上記の各機能ブロックの処理の一部または全部は、プログラムにより実現されるものであってもよい。そして、上記各実施形態の各機能ブロックの処理の一部または全部は、コンピュータにおいて、中央演算装置（ＣＰＵ）により行われる。また、それぞれの処理を行うためのプログラムは、ハードディスク、ＲＯＭなどの記憶装置に格納されており、ＲＯＭにおいて、あるいはＲＡＭに読み出されて実行される。また、これらのプログラムは、ＣＤ、ＤＶＤなどの記録媒体に記憶され提供されてもよい。
【０１３２】
また、上記実施形態の各処理をハードウェアにより実現してもよいし、ソフトウェア（ＯＳ（オペレーティングシステム）、ミドルウェア、あるいは、所定のライブラリとともに実現される場合を含む。） により実現してもよい。さらに、ソフトウェアおよびハードウェアの混在処理により実現しても良い。なお、上記実施形態に係る制御装置３０をハードウェアにより実現する場合、各処理を行うためのタイミング調整を行う必要があるのは言うまでもない。上記実施形態においては、説明便宜のため、実際のハードウェア設計で生じる各種信号のタイミング調整の詳細については省略している。
【０１３３】
以上、本発明の実施の形態を説明したが、上述した実施の形態は本発明を実施するための例示に過ぎない。よって、本発明は上述した実施の形態に限定されることなく、その趣旨を逸脱しない範囲内で上述した実施の形態を適宜変形して実施することが可能である。
【図１】
2014156288.tif 000003
【図２】
2014156288.tif 000004
【図３】
2014156288.tif 000005
【図４】
2014156288.tif 000006
【図５】
2014156288.tif 000007
【図６】
2014156288.tif 000008
【図７】
2014156288.tif 000009
【図８】
2014156288.tif 000010
【図９】
2014156288.tif 000011
【図１０】
2014156288.tif 000012
【図１１】
2014156288.tif 000013
【図１２】
2014156288.tif 000014
【図１３】
2014156288.tif 000015
【図１４】
2014156288.tif 000016
【図１５】
2014156288.tif 000017
【図１６】
2014156288.tif 000018
【図１７】
2014156288.tif 000019
【図１８】
2014156288.tif 000020
【図１９】
2014156288.tif 000021
【図２０】
2014156288.tif 000022
【図２１】
2014156288.tif 000023
【図２２】
2014156288.tif 000024
【図２３】
2014156288.tif 000025
【図２４】
2014156288.tif 000026
【図２５】
2014156288.tif 000027
【図２６】
2014156288.tif 000028
【図２７】
2014156288.tif 000029
【国際調査報告】
2014156288.tif 000030
2014156288.tif 000031
2014156288.tif 000032
2014156288.tif 000033