方位測定装置
【課題】比較的低精度（低価格）のジャイロを用いて実用的なストラップダウン方式の方位測定装置を実現する。
【解決手段】ストラップダウン方式の方位測定装置であって、一定間隔を隔てて配置された２つに受信アンテナを備え、各受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段をさらに備え、演算装置は、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定する。
【解決手段】ストラップダウン方式の方位測定装置であって、一定間隔を隔てて配置された２つに受信アンテナを備え、各受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段をさらに備え、演算装置は、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ストラップダウン型の方位測定装置に関する。
【背景技術】
【０００２】
一般にジャイロコンパスと言われる方位測定装置には、ジンバル機構を用いることにより慣性センサを移動体に対して可動自在に設置するタイプと、ジンバル機構を用いることなく慣性センサを移動体に固定設置するタイプとがある。前者は、例えばジンバル式プラットフォーム型と言われ、後者はストラップダウン方式と言われている。後者のストラップダウン方式の方位測定装置では、慣性センサが移動体に固定設置されている、つまり慣性センサの検出量は、移動体固有の直交座標系（移動体固有直交座標系）に関するものであり、移動体の位置（局地）を緯度、経度及び高度で示す航行座標系（局地水平座標系）に関するものではないので、両座標系間での座標変換が必要であり、よって高性能な演算装置を必要とする。
【０００３】
このようなストラップダウン方式の方位測定装置では、例えば下記特許文献１に記載されているように、自らの測定速度や測定位置等の測定データと、移動体に別途設けられた電磁ログやＧＰＳ受信機等の観測機器で観測された基準速度や基準位置等の観測データとをカルマンフィルタで演算処理することにより誤差量を推定し、この誤差量に基づいて測定データの演算過程で誤差補正を行うことにより精度補償を行っている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−２７０１７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、ストラップダウン方式の方位測定装置に利用される慣性センサは、ジャイロと加速度センサであり、方位測定装置の測定精度を支配する部品である。また、ストラップダウン方式の方位測定装置に利用されるジャイロは、移動体に固定設置する関係で、主に光学式ジャイロが使用される。しかしながら、高精度の光学式ジャイロは価格が数百万円と比較的高価なので、現状ではストラップダウン方式の方位測定装置を比較的低価格な民間船舶用の方位測定装置として実現することは極めて困難である。また、種々のジャイロコンパスは、一般的に移動体が高緯度に行く程、方位角の検出誤差が大きくなるという欠点がある。
【０００６】
本発明は、比較的低精度（低価格）のジャイロを用いて実用的なストラップダウン方式の方位測定装置を実現することを目的とするものである。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、本発明では、第１の解決手段として、移動体に固定設置され、移動体直交座標系の各座標軸周りの角速度及び各座標軸方向の加速度を検出する慣性センサと、移動体の移動体直交座標系における速度を基準速度として測定する速度計と、移動体の航行座標系における位置を基準位置として測定する測位装置と、各座標軸周りの角速度に基づいて移動体直交座標系と航行座標系とに関する座標変換行列を時系列的に順次演算し、該座標変換行列を用いて各座標軸周りの角速度から航行座標系における移動体の姿勢及び方位角を時系列的に順次演算すると共に各座標軸方向の加速度から航行座標系における移動体の速度及び位置を測定速度及び測定位置として時系列的に順次演算し、かつ、基準速度と測定速度との差分速度及び基準位置と測定位置との差分位置に基づいて少なくとも移動体の姿勢及び方位角の修正量を推定して移動体の姿勢及び方位角の演算に反映させる演算装置とを備えるストラップダウン方式の方位測定装置であって、一定間隔を隔てて配置された２つに受信アンテナを備え、各受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段をさらに備え、演算装置は、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定する、という手段を採用する。
【０００８】
第２の解決手段として、上記第１の手段において、演算装置は、クオータニオンを用いることにより座標変換行列の初期値を求める、という手段を採用する。
【０００９】
第３の解決手段として、上記第１または第２の手段において、基準方位角測定手段は、位置が既知の基準局が発信する電波を利用することによりＧＰＳ（Grobal Positioning System）に基づいて測定した各受信アンテナの位置を補正し、当該補正位置に基づいて基準方位角を算出するＧＰＳ（Grobal Positioning System）受信機である、という手段を採用する。
【発明の効果】
【００１０】
本発明によれば、一定間隔を隔てて配置された２つに受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段を備え、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定するので、比較的低精度（低価格）のジャイロを用いて実用的なストラップダウン方式の方位測定装置を実現することが可能である。
【図面の簡単な説明】
【００１１】
【図１】本発明の一実施形態に係る方位測定装置の機能構成を示すブロック図である。
【図２】本発明の一実施形態に係る方位測定装置の要部構成を示す模式図である。
【発明を実施するための形態】
【００１２】
以下、図面を参照して、本発明の一実施形態について説明する。
本実施形態に係る方位測定装置は、図１に示すように、ジャイロ１、加速度センサ２、電磁ログ３、第１ＧＰＳ受信機４、第２ＧＰＳ受信機５及び演算装置６から構成されている。また演算装置６は、基準方位角演算部６ａ、ハイブリッド演算部６ｂ、座標変換部６ｃ、純慣性航法演算部６ｄ及びジャイロ出力部６ｅから構成されている。なお、第１ＧＰＳ受信機４、第２ＧＰＳ受信機５及び基準方位角演算部６ａは、基準方位角測定手段を構成する。
【００１３】
このような方位測定装置は、移動体の一種である船舶、例えば艦艇程に高精度な測定精度が要求されない民間船舶に搭載されて姿勢、方位角、速度及び位置を測定するものである。また、この方位測定装置は、ジャイロ１及び加速度センサ２、つまり船舶に作用する慣性力を検出する慣性センサが船舶（移動体）の任意位置（動揺中心近傍が好ましい。）に固定設置されたストラップダウン方式の方位測定装置である。
【００１４】
なお、図１では、本発明の本質に直接関係しないので省略しているが、ジャイロ１、加速度センサ２、電磁ログ３、第１ＧＰＳ受信機４及び第２ＧＰＳ受信機５は、例えばＲＳ-４２２等の汎用通信規格に準拠したインタフェース回路によって演算装置６と相互接続されている。
【００１５】
ジャイロ１は、船舶の任意位置（動揺中心近傍が好ましい。）に設けられており、船舶の運動に伴う直交座標軸（Ｘ軸、Ｙ軸、Ｚ軸）回りの角速度を検出して座標変換部６ｃに出力する。これらＸ軸、Ｙ軸、Ｚ軸からなる直交座標系は、船舶に固有に設定された船体固有座標系であり、例えばＸ軸及は船体のロール軸（船首尾方向）、Ｙ軸は船体のピッチ軸（船幅方向）、Ｚ軸はヨー軸（船体上下方向）である。すなわち、ジャイロ１は、上記Ｘ軸、Ｙ軸、Ｚ軸にそれぞれ対応する３つのジャイロ（Ｘ軸ジャイロ、Ｙ軸ジャイロ、Ｚ軸ジャイロ）から構成されており、Ｘ軸（ロール軸）、Ｙ軸（ピッチ軸）、Ｚ軸（ヨー軸）周りの各加速度つまりロール軸角速度、ピッチ軸角速度及びヨー軸角速度を検出する。
【００１６】
加速度センサ２は、船舶の任意位置（動揺中心近傍が好ましい。）に設けられており、船舶の運動に伴う直交座標軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の加速度を検出して座標変換部６ｃに出力する。すなわち、この加速度センサ２は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向の加速度をそれぞれ検出する３つの加速度センサ（Ｘ軸加速度センサ、Ｙ軸加速度センサ、Ｚ軸加速度センサ）から構成されている。
【００１７】
電磁ログ３は、船舶の船底部に水中に露出する状態で設けられており、水に対する船舶の速度（対水速度）を検出し、当該対水速度を基準速度としてハイブリッド演算部６ｂに出力する。すなわち、この基準速度は、船舶固有の直交座標系、つまり船首尾方向をロール軸とし、当該ロール軸に直交する水平方向をピッチ軸とし、またロール軸に直交する鉛直方向をヨー軸とする船体直交座標系（移動体直交座標系）における速度である。
【００１８】
第１ＧＰＳ受信機４は、ＧＰＳ（Grobal Positioning System）に準拠した測位装置であり、図２に示すように、船舶の船尾中央に第１受信アンテナ４ａが設置されている。この第１ＧＰＳ受信機４は、第１受信アンテナ４ａが受信した測位用電波に基づいて、航行座標系（局地水平座標系）における第１受信アンテナ４ａの位置（緯度及び経度）を第１位置Ｐ1として測定して基準方位角演算部６ａに出力する。
【００１９】
第２ＧＰＳ受信機５は、上記第１ＧＰＳ受信機４と同様にＧＰＳに準拠した測位装置であり、図２に示すように、船舶の船首中央に第２受信アンテナ５ａが設置されている。この第２ＧＰＳ受信機５は、第２受信アンテナ５ａが受信した測位用電波に基づいて、航行座標系における第２受信アンテナ５ａの位置（緯度及び経度）を第２位置Ｐ2として測定して基準方位角演算部６ａに出力する。
【００２０】
ここで、ＧＰＳは、位置（緯度及び経度）が既知の基準局が発信する電波（ＦＭ波）を測位用電波の１つとし、また複数のＧＰＳ衛星が発信するＧＰＳ波を他の測位用電波として移動体の位置（緯度及び経度）を測定する測位システムとして広く知られている。すなわち、ＧＰＳは、複数のＧＰＳ波から得られた移動体の位置（緯度及び経度）を基準局の位置に基づいて補正することにより高精度化する測位システムである。また、上記航行座標系は、周知のように地球固有の座標系であり、緯度、経度及び高度によって地球に対する移動体の位置を示すものである。
【００２１】
演算装置６は、所定の演算プログラムに基づいて動作するコンピュータであり、上記角速度、加速度、基準速度、第１位置及び第２位置に上記演算プログラムに基づく演算処理（測位処理）を施すことにより船舶の姿勢、方位角、速度及び位置を演算し、これら姿勢、方位角、速度及び位置をジャイロ出力として外部に出力する。
【００２２】
基準方位角演算部６ａは、第１位置Ｐ1及び第の位置Ｐ2に基づいて基準方位角ΨＲを求め、この基準方位角ΨＲを座標変換部６ｃに出力する。すなわち、図２に示すように、第１位置Ｐ1と第２位置Ｐ2とを結ぶ方向（ロール軸方向）は、船舶の進行方向つまり方位角を示すものである。基準方位角演算部６ａは、このような第１位置Ｐ1と第２位置Ｐ2とに基づいて第２受信アンテナ５ａから第１受信アンテナ４ａを臨む方向、つまり航行座標系における船舶の進行方向を基準方位角ΨＲとして求める。
【００２３】
このような基準方位角ΨＲは、以下に説明するように、船舶の緯度による誤差を含まない高精度な方位角である。すなわち、図２に示すように、航行座標系における第１位置Ｐ1の座標を（ｘ1，ｙ1）とし、同じく第２位置Ｐ2の座標を（ｘ2，ｙ2）とすると、基準方位角ΨＲは緯度ＬＡＴを変数として含む式（１）によって表わされる。また、各軸方向の距離Ｌｘ，Ｌｙは式（２）、（３）によって表わされる。
【００２４】
【数１】
【００２５】
【数２】
【００２６】
【数３】
【００２７】
そして、これら式（２）、（３）を式（１）に代入すると、基準方位角ΨＲは、式（４）のように表わされる。この式（４）は、緯度ＬＡＴを変数として含むものではなく、各軸方向の距離Ｌｘ，Ｌｙのみを変数とする形態の式なので、よって基準方位角ΨＲは船舶の緯度ＬＡＴによる誤差を含まない高精度な方位角である。
【００２８】
【数４】
【００２９】
また、基準方位角演算部６ａは、第１の位置及び第２の位置に基づいて船舶の位置（緯度及び経度）を基準位置として求め、この基準位置をハイブリッド演算部６ｂに出力する。この基準位置は、例えば船舶の重心位置を示すものである。
【００３０】
ハイブリッド演算部６ｂは、純慣性航法演算部６ｄから入力される船舶の速度及び位置、基準方位角演算部６ａから入力される基準位置及び電磁ログ３から入力される基準速度とに基づいて、慣性センサであるジャイロ１及び加速度センサ２の検出誤差並びに方位誤差、姿勢誤差、速度誤差及び位置誤差を推定するカルマンフィルタである。
【００３１】
すなわち、ハイブリッド演算部６ｂは、純慣性航法演算部６ｄから入力される船舶の速度と電磁ログ３から入力される基準速度との差分（速度誤差）及び純慣性航法演算部６ｄから入力される船舶の位置と基準方位角演算部６ａから入力される基準位置との差分（位置誤差）とを観測データとし、ジャイロ１及び加速度センサ２の検出誤差並びに方位誤差、姿勢誤差、速度誤差及び位置誤差の推定値を演算し、この推定値を座標変換部６ｃ及び純慣性航法演算部６ｄに誤差推定値として出力する。なお、ハイブリッド演算部６ｂにおける誤差推定値の演算アルゴリズムについては、例えば特開平6-265367号公報、特開平7-270176号公報、また特開平8-21740号公報等に詳細が記載されているので、ここでは詳細な説明を省略する。
【００３２】
座標変換部６ｃは、上述したＸ軸（ロール軸：船首尾方向）、Ｙ軸（ピッチ軸：船幅方向）、Ｚ軸（ヨー軸：船体上下方向）からなる船体固有座標系と緯度、Ｎ軸（南北方向）、Ｅ軸（東西方向）及びＺ軸（上下方向）からなる地球固有の航行座標系との座標変換行列を一定の時間インターバルで演算する。詳細については後述するが、座標変換部６ｃは、ジャイロ１から入力されるロール軸角速度、ピッチ軸角速度及びヨー軸角速度に基づいて座標変換行列を求めるが、この際にハイブリッド演算部６ｂから入力される誤差推定値（ジャイロ１の検出誤差に関する誤差推定値）に基づいてジャイロ１の検出値を補正する。
【００３３】
また、座標変換部６ｃは、上記座標変換行列を用いることにより加速度センサ２から入力されたＸ軸、Ｙ軸及びＺ軸方向の加速度を航行座標系に座標変換した加速度（変換加速度）を純慣性航法演算部６ｄに順次出力する一方、上記座標変換行列及びジャイロ１から入力されたＸ軸、Ｙ軸及びＺ軸回りの角速度に基づいて航行座標系における船舶の姿勢（ロール角φ及びピッチ角θ）及び方位角ψを演算してジャイロ出力部６ｅに順次出力する。
【００３４】
また、この座標変換部６ｃは、初期アライメント演算をも行うものである。すなわち、座標変換部６ｃは、一定の時間インターバル毎に上記座標変換行列の演算処理を船舶の航行時における通常処理として演算する一方、本方位測定装置の起動時（つまり通常は船舶の出航前（ただし航行中でも可能））における起動処理として上記座標変換行列の初期値を初期アライメント演算によって求める。この初期アライメント演算は、本方位測定装置の最も特徴的な処理であり、その詳細については後述するが、基準方位角演算部６ａから入力される基準方位角ΨＲを用いることにより、ジャイロ及び加速度センサのみによるものよりも高精度である。
【００３５】
純慣性航法演算部６ｄは、このような座標変換部６ｃから入力される変換加速度及びハイブリッド演算部６ｂから入力される誤差推定値に基づいて、航行座標系における船舶の速度及び位置を演算してハイブリッド演算部６ｂに及びジャイロ出力部６ｅに出力する。すなわち、この純慣性航法演算部６ｄは、変換加速度に含まれる誤差を誤差推定値で補正し、これによって得られた補正加速度を順次積分することによって船舶の速度ｖｎ，ｖｅ，ｖｄを求め、また当該速度ｖｎ，ｖｅ，ｖｄを順次積分することによって船舶の位置（緯度ＬＡＴ，経度ＬＯＮ）を求める。
【００３６】
ジャイロ出力部６ｅは、座標変換部６ｃから入力される船舶の姿勢（ロール角φ，ピッチ角θ）及び方位角ψ（ヨー角）並びに純慣性航法演算部６ｄから入力される船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）をジャイロ出力として外部に出力する。すなわち、このジャイロ出力部６ｅは、船舶の姿勢（ロール角φ，ピッチ角θ）及び方位角ψ（ヨー角）並びに船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）に関する信号を外部器機との通信規格（例えばＲＳ-４２２等の汎用通信規格）に準じた信号形式に変換して外部に出力する一種の信号変換装置である。
【００３７】
続いて、このように構成された方位測定装置の時系列的な動作について詳しく説明する。
【００３８】
本方位測定装置が起動すると、本方位測定装置は起動処理を最初に実行する。すなわち、座標変換部６ｃは、ジャイロ１から入力されるロール軸角速度、ピッチ軸角速度及びヨー軸角速度並びに上記基準方位角ΨＲを用いることにより、以下のように座標変換行列の初期値を演算する。
【００３９】
周知のように、ある任意の三次元座標系ｂから他の三次元座標系ｎへの座標変換行列Ｃｂｎは、三行三列の行列として表される。いま、三次元座標系ｂをＸ軸（ロール軸）、Ｙ軸（ピッチ軸）及びＺ軸（ヨー軸）からなる船体固有座標系、三次元座標系ｎを航行座標系とすると、座標変換行列Ｃｂｎは、下式（５）のように表される。また、航行座標系（三次元座標系ｎ）における船舶のロール角φ、ピッチ角θ及び方位角ψ（ヨー角）は、この式（５）から式（６）のように表される。なお、上述した座標変換行列を示す記号「Ｃｂｎ」は、下付き添え字「ｂ」及び上付き添え字「ｎ」が横方向において位置がずれているが、以下の数式では下付き添え字「ｂ」及び上付き添え字「ｎ」を横方向の同一位置に表記している。
【００４０】
【数５】
【００４１】
【数６】
【００４２】
ここで、ｑ1，ｑ2，ｑ3，ｑ4は、クオータニオンｑの各要素である。このクオータニオンｑは、「ハミルトンの四元数」として一般に知られるものであり、コンピュータを用いた座標変換処理（数値計算）に広く利用されている数学的概念である。このようなクオータニオンｑは、航行座標系（局地水平座標系）に対する船体固有座標系の相対回転角速度ベクトルの各要素Ｐ，Ｑ，Ｒからなる対称行列Ψを媒介として時間変化率との間に下式（７）の関係がある。
【００４３】
【数７】
【００４４】
ここで、本方位測定装置の起動時つまり船舶が停泊している状態では、航行座標系におけるロール角φ及びピッチ角θは「０（ゼロ）」を初期値とすることができ、また航行座標系における方位角ψ（ヨー角）については、基準方位角演算部６ａが第１位置Ｐ1及び第の位置Ｐ2に基づいて演算する基準方位角ΨＲを初期値とすることができる。すなわち、ロール角φ、ピッチ角θ及び方位角ψ（ヨー角）の各初期値をφ０，θ０，ψ０とすると、座標変換行列Ｃｂｎにおける各要素の初期値は式（８）のように表される。なお、船舶が航行中に本方位測定装置を起動した時点であっても、船舶の揺動に起因する誤差分は徐々に補正されるので、上述した初期値を用いて初期アライメント処理を行うことができる。
【００４５】
【数８】
【００４６】
そして、クオータニオンｑの各要素ｑ1，ｑ2，ｑ3，ｑ4の初期値、つまり時刻ｔ＝０における各要素ｑ1，ｑ2，ｑ3，ｑ4の値は、上記座標変換行列Ｃｂｎにおける各要素の初期値及び式（５）から式（９）のように表される。この式（９）において、式（８）に基づいて初期値ｑ4（０）を最初に求めることによって、残りの初期値ｑ1（０），ｑ2（０），ｑ3（０）を求めることができる。すなわち、このようにしてクオータニオンｑの各要素ｑ1，ｑ2，ｑ3，ｑ4の初期値ｑ1（０），ｑ2（０），ｑ3（０），ｑ4（０）が演算されることにより、座標変換行列Ｃｂｎの初期値が特定される。
【００４７】
【数９】
【００４８】
座標変換部６ｃは、このようにして座標変換行列Ｃｂｎの初期値を特定すると、当該初期値並びにロール角φ、ピッチ角θ及び方位角ψ（ヨー角）を用いることにより、航行座標系における船舶の姿勢（ロール角φｎ及びピッチ角θｎ）及び方位角ψｎを演算してジャイロ出力部６ｅに出力する。また、座標変換部６ｃは、上記座標変換行列Ｃｂｎの初期値を用いて加速度センサ２から入力されたＸ軸方向、Ｙ軸方向、Ｚ軸方向の加速度を航行座標系における加速度（変換加速度）に座標変換して純慣性航法演算部６ｄに出力する。
【００４９】
そして、純慣性航法演算部６ｄは、このような変換加速度に基づいて航行座標系における船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）を求めてジャイロ出力部６ｅに出力する。本方位測定装置の起動時には、船舶が停止中あるいは航行中に拘らずハイブリッド演算部６ｂから座標変換部６ｃ及び純慣性航法演算部６ｄに出力される誤差推定値は誤差の大きいものであるが、上記初期値を用いて初期アライメント処理を複数回繰り返すことにより、上記誤差推定値に含まれる誤差は徐々に小さくなり、この結果、ロール角φ、ピッチ角θ、方位角ψ（ヨー角）、速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）も誤差の小さなものとなる。
【００５０】
このような起動処理が終了すると、座標変換部６ｃは、５０Ｈｚあるいは１００Ｈｚの周期で座標変換行列Ｃｂｎを更新する。そして、この更新後の座標変換行列Ｃｂｎを用いて上述したと同様に船舶の姿勢（ロール角φｎ及びピッチ角θｎ）及び方位角ψｎを演算してジャイロ出力部６ｅに出力すると共に、更新後の座標変換行列Ｃｂｎを用いて演算した変換加速度を純慣性航法演算部６ｄに出力する。そして、純慣性航法演算部６ｄは、このような変換加速度に基づいて航行座標系における船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）を演算してジャイロ出力部６ｅに出力する。
【００５１】
上記座標変換部６ｃは、上記対称行列Ψの各要素Ｐ，Ｑ，Ｒを下式（１０）によって求める。また、この式（１０）の第２式における右辺第２項は、座標変換行列Ｃｂｎを用いて下式（１１）のように表わされる。
【００５２】
【数１０】
【００５３】
【数１１】
【００５４】
なお、この式（１１）において、角速度Ωは約15.0410687°/hrとして与えられるものであり、またＬは緯度、ｌ（Ｌの小文字）は経度、ｈは高度、さらにＲＬは地球の緯度方向における曲率半径、Ｒｌは地球の経度方向における曲率半径である。上記緯度Ｌの変化率及び経度ｌの変化率は下式（１２）によって与えられる。なお、この式（１２）において、ｈ（ｉ）＝０、またＲＬ，Ｒｌ＝「ｉの取り得る範囲（１〜４）の中の一定値」と近似することができる。
【００５５】
【数１２】
【００５６】
このようにして対称行列Ψの各要素Ｐ，Ｑ，Ｒは、ｉ毎に一定のタイムインターバルで更新される値として求められる。また、上記座標変換部６ｃは、クオータニオンｑの時間変化率ベクトルの各要素を下式（１３）によって求め、このクオータニオンｑの時間変化率ベクトルを上式（７）に代入することにより、一定タイムインターバル後のクオータニオンｑを求める。そして、上記座標変換部６ｃは、この一定タイムインターバル後のクオータニオンｑと式（５）とに基づいて更新後の座標変換行列Ｃｂｎを求める。
【００５７】
【数１３】
【００５８】
ここで、一定タイムインターバル後のクオータニオンｑを求めるに際して、座標変換部６ｃは、下式（１４）に基づいて直交補正された要素ｑ10，ｑ20，ｑ30，ｑ40を要素ｑ1，ｑ2，ｑ3，ｑ4として用いることにより、より正確な更新後の座標変換行列Ｃｂｎを求める。
【００５９】
【数１４】
【００６０】
このような直交補正によって得られた要素ｑ1，ｑ2，ｑ3，ｑ4からなる更新クオータニオンｑを用いることにより座標変換行列Ｃｂｎの各要素は下式（１５）のように表される。そして、上記座標変換部６ｃは、この下式（１５）と上式（６）とによって更新後の座標変換行列Ｃｂｎを用いた新たな船舶の姿勢（ロール角φｎ及びピッチ角θｎ）及び方位角ψｎを求める。
【００６１】
【数１５】
【００６２】
このような本方位測定装置によれば、ロール角φ及びピッチ角θの初期値を「０（ゼロ）」とし、また方位角ψ（ヨー角）の初期値を基準方位角ΨＲとすることにより、座標変換行列Ｃｂｎにおける各要素の初期値を高精度に特定することができる。また、本方位測定装置によれば、方位角中の初期値を基準方位角ΨＲによって一定周期（例えば１秒周期）にリセットする（置き換える）ので、高精度の方位角等を継続的に出力することが可能である。
【００６３】
すなわち、従来の方位測定装置では基準方位角ΨＲを求める手段を備えていなかったので、座標変換行列Ｃｂｎの初期値の精度が低かったが、本方位測定装置では、第１ＧＰＳ受信機４、第２ＧＰＳ受信機５及び基準方位角演算部６ａからなる基準方位角測定手段を備えており、起動時及びその後の一定周期毎に方位角中の初期値を基準方位角測定手段から得られる基準方位角ΨＲに置き換えるので、座標変換行列Ｃｂｎに関する初期値を高精度に特定することが可能であり、よって実航行時の方位誤差を低減することが可能である。
【００６４】
なお、本発明は、上記実施形態に限定されるものではなく、例えば以下のような変形例が考えられる。
（１）上記実施形態では、クオータニオンｑを用いて座標変換行列Ｃｂｎを求めたが、本発明はこれに限定されない。例えば、上述した特開平6-265367号公報、特開平7-270176号公報、また特開平8-21740号公報等では、座標変換行列Ｃｂｎとその時間変化率を示す行列とを航行座標系に対する船体固有座標系の相対回転角速度ベクトルの歪対称行列によって関係付ける計算方法を採用しており、このような計算方法を本発明に適用することも可能である。
【００６５】
（２）上記実施形態では、第１位置が示す第１受信アンテナ４ａの位置と第２位置が示す第２受信アンテナ５ａの位置とを結ぶ直線が船舶の基準方位角となるように、第１受信アンテナ４ａを船舶の船首中央に、かつ、第２受信アンテナ５ａを船舶の船尾中央に設けたが、本発明はこれに限定されない。例えば、船舶の形状と第１受信アンテナ４ａの位置及び第２受信アンテナ５ａの位置との関係を示すアンテナ配置データを基準方位角演算部６ａに記憶させ、このアンテナ配置データ、第１受信アンテナ４ａの位置及び第２受信アンテナ５ａの位置から基準方位角ΨＲを求めるようにしてもよい。
【００６６】
（３）上記実施形態では、基準方位角測定手段を第１ＧＰＳ受信機４及び第２ＧＰＳ受信機５を用いて構成したが、本発明はこれに限定されない。すなわち、基準方位角測定手段は、ＧＰＳに基づく測位システムに限定されず、ＧＲＯＮＡＳＳ（Global Navigation Satellite System）あるいはＧａｌｉｌｅｏ（Galileo Positioning System）等、他の測位システムであってもよい。
【符号の説明】
【００６７】
１…ジャイロ、２…加速度センサ、３…電磁ログ、４…第１ＧＰＳ受信機、５…第２ＧＰＳ受信機、６…演算装置、６ａ…基準方位角演算部、６ｂ…ハイブリッド演算部、６ｃ…座標変換部、６ｄ…純慣性航法演算部、６ｅ…ジャイロ出力部
【技術分野】
【０００１】
本発明は、ストラップダウン型の方位測定装置に関する。
【背景技術】
【０００２】
一般にジャイロコンパスと言われる方位測定装置には、ジンバル機構を用いることにより慣性センサを移動体に対して可動自在に設置するタイプと、ジンバル機構を用いることなく慣性センサを移動体に固定設置するタイプとがある。前者は、例えばジンバル式プラットフォーム型と言われ、後者はストラップダウン方式と言われている。後者のストラップダウン方式の方位測定装置では、慣性センサが移動体に固定設置されている、つまり慣性センサの検出量は、移動体固有の直交座標系（移動体固有直交座標系）に関するものであり、移動体の位置（局地）を緯度、経度及び高度で示す航行座標系（局地水平座標系）に関するものではないので、両座標系間での座標変換が必要であり、よって高性能な演算装置を必要とする。
【０００３】
このようなストラップダウン方式の方位測定装置では、例えば下記特許文献１に記載されているように、自らの測定速度や測定位置等の測定データと、移動体に別途設けられた電磁ログやＧＰＳ受信機等の観測機器で観測された基準速度や基準位置等の観測データとをカルマンフィルタで演算処理することにより誤差量を推定し、この誤差量に基づいて測定データの演算過程で誤差補正を行うことにより精度補償を行っている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平７−２７０１７６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、ストラップダウン方式の方位測定装置に利用される慣性センサは、ジャイロと加速度センサであり、方位測定装置の測定精度を支配する部品である。また、ストラップダウン方式の方位測定装置に利用されるジャイロは、移動体に固定設置する関係で、主に光学式ジャイロが使用される。しかしながら、高精度の光学式ジャイロは価格が数百万円と比較的高価なので、現状ではストラップダウン方式の方位測定装置を比較的低価格な民間船舶用の方位測定装置として実現することは極めて困難である。また、種々のジャイロコンパスは、一般的に移動体が高緯度に行く程、方位角の検出誤差が大きくなるという欠点がある。
【０００６】
本発明は、比較的低精度（低価格）のジャイロを用いて実用的なストラップダウン方式の方位測定装置を実現することを目的とするものである。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、本発明では、第１の解決手段として、移動体に固定設置され、移動体直交座標系の各座標軸周りの角速度及び各座標軸方向の加速度を検出する慣性センサと、移動体の移動体直交座標系における速度を基準速度として測定する速度計と、移動体の航行座標系における位置を基準位置として測定する測位装置と、各座標軸周りの角速度に基づいて移動体直交座標系と航行座標系とに関する座標変換行列を時系列的に順次演算し、該座標変換行列を用いて各座標軸周りの角速度から航行座標系における移動体の姿勢及び方位角を時系列的に順次演算すると共に各座標軸方向の加速度から航行座標系における移動体の速度及び位置を測定速度及び測定位置として時系列的に順次演算し、かつ、基準速度と測定速度との差分速度及び基準位置と測定位置との差分位置に基づいて少なくとも移動体の姿勢及び方位角の修正量を推定して移動体の姿勢及び方位角の演算に反映させる演算装置とを備えるストラップダウン方式の方位測定装置であって、一定間隔を隔てて配置された２つに受信アンテナを備え、各受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段をさらに備え、演算装置は、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定する、という手段を採用する。
【０００８】
第２の解決手段として、上記第１の手段において、演算装置は、クオータニオンを用いることにより座標変換行列の初期値を求める、という手段を採用する。
【０００９】
第３の解決手段として、上記第１または第２の手段において、基準方位角測定手段は、位置が既知の基準局が発信する電波を利用することによりＧＰＳ（Grobal Positioning System）に基づいて測定した各受信アンテナの位置を補正し、当該補正位置に基づいて基準方位角を算出するＧＰＳ（Grobal Positioning System）受信機である、という手段を採用する。
【発明の効果】
【００１０】
本発明によれば、一定間隔を隔てて配置された２つに受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段を備え、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定するので、比較的低精度（低価格）のジャイロを用いて実用的なストラップダウン方式の方位測定装置を実現することが可能である。
【図面の簡単な説明】
【００１１】
【図１】本発明の一実施形態に係る方位測定装置の機能構成を示すブロック図である。
【図２】本発明の一実施形態に係る方位測定装置の要部構成を示す模式図である。
【発明を実施するための形態】
【００１２】
以下、図面を参照して、本発明の一実施形態について説明する。
本実施形態に係る方位測定装置は、図１に示すように、ジャイロ１、加速度センサ２、電磁ログ３、第１ＧＰＳ受信機４、第２ＧＰＳ受信機５及び演算装置６から構成されている。また演算装置６は、基準方位角演算部６ａ、ハイブリッド演算部６ｂ、座標変換部６ｃ、純慣性航法演算部６ｄ及びジャイロ出力部６ｅから構成されている。なお、第１ＧＰＳ受信機４、第２ＧＰＳ受信機５及び基準方位角演算部６ａは、基準方位角測定手段を構成する。
【００１３】
このような方位測定装置は、移動体の一種である船舶、例えば艦艇程に高精度な測定精度が要求されない民間船舶に搭載されて姿勢、方位角、速度及び位置を測定するものである。また、この方位測定装置は、ジャイロ１及び加速度センサ２、つまり船舶に作用する慣性力を検出する慣性センサが船舶（移動体）の任意位置（動揺中心近傍が好ましい。）に固定設置されたストラップダウン方式の方位測定装置である。
【００１４】
なお、図１では、本発明の本質に直接関係しないので省略しているが、ジャイロ１、加速度センサ２、電磁ログ３、第１ＧＰＳ受信機４及び第２ＧＰＳ受信機５は、例えばＲＳ-４２２等の汎用通信規格に準拠したインタフェース回路によって演算装置６と相互接続されている。
【００１５】
ジャイロ１は、船舶の任意位置（動揺中心近傍が好ましい。）に設けられており、船舶の運動に伴う直交座標軸（Ｘ軸、Ｙ軸、Ｚ軸）回りの角速度を検出して座標変換部６ｃに出力する。これらＸ軸、Ｙ軸、Ｚ軸からなる直交座標系は、船舶に固有に設定された船体固有座標系であり、例えばＸ軸及は船体のロール軸（船首尾方向）、Ｙ軸は船体のピッチ軸（船幅方向）、Ｚ軸はヨー軸（船体上下方向）である。すなわち、ジャイロ１は、上記Ｘ軸、Ｙ軸、Ｚ軸にそれぞれ対応する３つのジャイロ（Ｘ軸ジャイロ、Ｙ軸ジャイロ、Ｚ軸ジャイロ）から構成されており、Ｘ軸（ロール軸）、Ｙ軸（ピッチ軸）、Ｚ軸（ヨー軸）周りの各加速度つまりロール軸角速度、ピッチ軸角速度及びヨー軸角速度を検出する。
【００１６】
加速度センサ２は、船舶の任意位置（動揺中心近傍が好ましい。）に設けられており、船舶の運動に伴う直交座標軸（Ｘ軸、Ｙ軸、Ｚ軸）方向の加速度を検出して座標変換部６ｃに出力する。すなわち、この加速度センサ２は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向の加速度をそれぞれ検出する３つの加速度センサ（Ｘ軸加速度センサ、Ｙ軸加速度センサ、Ｚ軸加速度センサ）から構成されている。
【００１７】
電磁ログ３は、船舶の船底部に水中に露出する状態で設けられており、水に対する船舶の速度（対水速度）を検出し、当該対水速度を基準速度としてハイブリッド演算部６ｂに出力する。すなわち、この基準速度は、船舶固有の直交座標系、つまり船首尾方向をロール軸とし、当該ロール軸に直交する水平方向をピッチ軸とし、またロール軸に直交する鉛直方向をヨー軸とする船体直交座標系（移動体直交座標系）における速度である。
【００１８】
第１ＧＰＳ受信機４は、ＧＰＳ（Grobal Positioning System）に準拠した測位装置であり、図２に示すように、船舶の船尾中央に第１受信アンテナ４ａが設置されている。この第１ＧＰＳ受信機４は、第１受信アンテナ４ａが受信した測位用電波に基づいて、航行座標系（局地水平座標系）における第１受信アンテナ４ａの位置（緯度及び経度）を第１位置Ｐ1として測定して基準方位角演算部６ａに出力する。
【００１９】
第２ＧＰＳ受信機５は、上記第１ＧＰＳ受信機４と同様にＧＰＳに準拠した測位装置であり、図２に示すように、船舶の船首中央に第２受信アンテナ５ａが設置されている。この第２ＧＰＳ受信機５は、第２受信アンテナ５ａが受信した測位用電波に基づいて、航行座標系における第２受信アンテナ５ａの位置（緯度及び経度）を第２位置Ｐ2として測定して基準方位角演算部６ａに出力する。
【００２０】
ここで、ＧＰＳは、位置（緯度及び経度）が既知の基準局が発信する電波（ＦＭ波）を測位用電波の１つとし、また複数のＧＰＳ衛星が発信するＧＰＳ波を他の測位用電波として移動体の位置（緯度及び経度）を測定する測位システムとして広く知られている。すなわち、ＧＰＳは、複数のＧＰＳ波から得られた移動体の位置（緯度及び経度）を基準局の位置に基づいて補正することにより高精度化する測位システムである。また、上記航行座標系は、周知のように地球固有の座標系であり、緯度、経度及び高度によって地球に対する移動体の位置を示すものである。
【００２１】
演算装置６は、所定の演算プログラムに基づいて動作するコンピュータであり、上記角速度、加速度、基準速度、第１位置及び第２位置に上記演算プログラムに基づく演算処理（測位処理）を施すことにより船舶の姿勢、方位角、速度及び位置を演算し、これら姿勢、方位角、速度及び位置をジャイロ出力として外部に出力する。
【００２２】
基準方位角演算部６ａは、第１位置Ｐ1及び第の位置Ｐ2に基づいて基準方位角ΨＲを求め、この基準方位角ΨＲを座標変換部６ｃに出力する。すなわち、図２に示すように、第１位置Ｐ1と第２位置Ｐ2とを結ぶ方向（ロール軸方向）は、船舶の進行方向つまり方位角を示すものである。基準方位角演算部６ａは、このような第１位置Ｐ1と第２位置Ｐ2とに基づいて第２受信アンテナ５ａから第１受信アンテナ４ａを臨む方向、つまり航行座標系における船舶の進行方向を基準方位角ΨＲとして求める。
【００２３】
このような基準方位角ΨＲは、以下に説明するように、船舶の緯度による誤差を含まない高精度な方位角である。すなわち、図２に示すように、航行座標系における第１位置Ｐ1の座標を（ｘ1，ｙ1）とし、同じく第２位置Ｐ2の座標を（ｘ2，ｙ2）とすると、基準方位角ΨＲは緯度ＬＡＴを変数として含む式（１）によって表わされる。また、各軸方向の距離Ｌｘ，Ｌｙは式（２）、（３）によって表わされる。
【００２４】
【数１】
【００２５】
【数２】
【００２６】
【数３】
【００２７】
そして、これら式（２）、（３）を式（１）に代入すると、基準方位角ΨＲは、式（４）のように表わされる。この式（４）は、緯度ＬＡＴを変数として含むものではなく、各軸方向の距離Ｌｘ，Ｌｙのみを変数とする形態の式なので、よって基準方位角ΨＲは船舶の緯度ＬＡＴによる誤差を含まない高精度な方位角である。
【００２８】
【数４】
【００２９】
また、基準方位角演算部６ａは、第１の位置及び第２の位置に基づいて船舶の位置（緯度及び経度）を基準位置として求め、この基準位置をハイブリッド演算部６ｂに出力する。この基準位置は、例えば船舶の重心位置を示すものである。
【００３０】
ハイブリッド演算部６ｂは、純慣性航法演算部６ｄから入力される船舶の速度及び位置、基準方位角演算部６ａから入力される基準位置及び電磁ログ３から入力される基準速度とに基づいて、慣性センサであるジャイロ１及び加速度センサ２の検出誤差並びに方位誤差、姿勢誤差、速度誤差及び位置誤差を推定するカルマンフィルタである。
【００３１】
すなわち、ハイブリッド演算部６ｂは、純慣性航法演算部６ｄから入力される船舶の速度と電磁ログ３から入力される基準速度との差分（速度誤差）及び純慣性航法演算部６ｄから入力される船舶の位置と基準方位角演算部６ａから入力される基準位置との差分（位置誤差）とを観測データとし、ジャイロ１及び加速度センサ２の検出誤差並びに方位誤差、姿勢誤差、速度誤差及び位置誤差の推定値を演算し、この推定値を座標変換部６ｃ及び純慣性航法演算部６ｄに誤差推定値として出力する。なお、ハイブリッド演算部６ｂにおける誤差推定値の演算アルゴリズムについては、例えば特開平6-265367号公報、特開平7-270176号公報、また特開平8-21740号公報等に詳細が記載されているので、ここでは詳細な説明を省略する。
【００３２】
座標変換部６ｃは、上述したＸ軸（ロール軸：船首尾方向）、Ｙ軸（ピッチ軸：船幅方向）、Ｚ軸（ヨー軸：船体上下方向）からなる船体固有座標系と緯度、Ｎ軸（南北方向）、Ｅ軸（東西方向）及びＺ軸（上下方向）からなる地球固有の航行座標系との座標変換行列を一定の時間インターバルで演算する。詳細については後述するが、座標変換部６ｃは、ジャイロ１から入力されるロール軸角速度、ピッチ軸角速度及びヨー軸角速度に基づいて座標変換行列を求めるが、この際にハイブリッド演算部６ｂから入力される誤差推定値（ジャイロ１の検出誤差に関する誤差推定値）に基づいてジャイロ１の検出値を補正する。
【００３３】
また、座標変換部６ｃは、上記座標変換行列を用いることにより加速度センサ２から入力されたＸ軸、Ｙ軸及びＺ軸方向の加速度を航行座標系に座標変換した加速度（変換加速度）を純慣性航法演算部６ｄに順次出力する一方、上記座標変換行列及びジャイロ１から入力されたＸ軸、Ｙ軸及びＺ軸回りの角速度に基づいて航行座標系における船舶の姿勢（ロール角φ及びピッチ角θ）及び方位角ψを演算してジャイロ出力部６ｅに順次出力する。
【００３４】
また、この座標変換部６ｃは、初期アライメント演算をも行うものである。すなわち、座標変換部６ｃは、一定の時間インターバル毎に上記座標変換行列の演算処理を船舶の航行時における通常処理として演算する一方、本方位測定装置の起動時（つまり通常は船舶の出航前（ただし航行中でも可能））における起動処理として上記座標変換行列の初期値を初期アライメント演算によって求める。この初期アライメント演算は、本方位測定装置の最も特徴的な処理であり、その詳細については後述するが、基準方位角演算部６ａから入力される基準方位角ΨＲを用いることにより、ジャイロ及び加速度センサのみによるものよりも高精度である。
【００３５】
純慣性航法演算部６ｄは、このような座標変換部６ｃから入力される変換加速度及びハイブリッド演算部６ｂから入力される誤差推定値に基づいて、航行座標系における船舶の速度及び位置を演算してハイブリッド演算部６ｂに及びジャイロ出力部６ｅに出力する。すなわち、この純慣性航法演算部６ｄは、変換加速度に含まれる誤差を誤差推定値で補正し、これによって得られた補正加速度を順次積分することによって船舶の速度ｖｎ，ｖｅ，ｖｄを求め、また当該速度ｖｎ，ｖｅ，ｖｄを順次積分することによって船舶の位置（緯度ＬＡＴ，経度ＬＯＮ）を求める。
【００３６】
ジャイロ出力部６ｅは、座標変換部６ｃから入力される船舶の姿勢（ロール角φ，ピッチ角θ）及び方位角ψ（ヨー角）並びに純慣性航法演算部６ｄから入力される船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）をジャイロ出力として外部に出力する。すなわち、このジャイロ出力部６ｅは、船舶の姿勢（ロール角φ，ピッチ角θ）及び方位角ψ（ヨー角）並びに船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）に関する信号を外部器機との通信規格（例えばＲＳ-４２２等の汎用通信規格）に準じた信号形式に変換して外部に出力する一種の信号変換装置である。
【００３７】
続いて、このように構成された方位測定装置の時系列的な動作について詳しく説明する。
【００３８】
本方位測定装置が起動すると、本方位測定装置は起動処理を最初に実行する。すなわち、座標変換部６ｃは、ジャイロ１から入力されるロール軸角速度、ピッチ軸角速度及びヨー軸角速度並びに上記基準方位角ΨＲを用いることにより、以下のように座標変換行列の初期値を演算する。
【００３９】
周知のように、ある任意の三次元座標系ｂから他の三次元座標系ｎへの座標変換行列Ｃｂｎは、三行三列の行列として表される。いま、三次元座標系ｂをＸ軸（ロール軸）、Ｙ軸（ピッチ軸）及びＺ軸（ヨー軸）からなる船体固有座標系、三次元座標系ｎを航行座標系とすると、座標変換行列Ｃｂｎは、下式（５）のように表される。また、航行座標系（三次元座標系ｎ）における船舶のロール角φ、ピッチ角θ及び方位角ψ（ヨー角）は、この式（５）から式（６）のように表される。なお、上述した座標変換行列を示す記号「Ｃｂｎ」は、下付き添え字「ｂ」及び上付き添え字「ｎ」が横方向において位置がずれているが、以下の数式では下付き添え字「ｂ」及び上付き添え字「ｎ」を横方向の同一位置に表記している。
【００４０】
【数５】
【００４１】
【数６】
【００４２】
ここで、ｑ1，ｑ2，ｑ3，ｑ4は、クオータニオンｑの各要素である。このクオータニオンｑは、「ハミルトンの四元数」として一般に知られるものであり、コンピュータを用いた座標変換処理（数値計算）に広く利用されている数学的概念である。このようなクオータニオンｑは、航行座標系（局地水平座標系）に対する船体固有座標系の相対回転角速度ベクトルの各要素Ｐ，Ｑ，Ｒからなる対称行列Ψを媒介として時間変化率との間に下式（７）の関係がある。
【００４３】
【数７】
【００４４】
ここで、本方位測定装置の起動時つまり船舶が停泊している状態では、航行座標系におけるロール角φ及びピッチ角θは「０（ゼロ）」を初期値とすることができ、また航行座標系における方位角ψ（ヨー角）については、基準方位角演算部６ａが第１位置Ｐ1及び第の位置Ｐ2に基づいて演算する基準方位角ΨＲを初期値とすることができる。すなわち、ロール角φ、ピッチ角θ及び方位角ψ（ヨー角）の各初期値をφ０，θ０，ψ０とすると、座標変換行列Ｃｂｎにおける各要素の初期値は式（８）のように表される。なお、船舶が航行中に本方位測定装置を起動した時点であっても、船舶の揺動に起因する誤差分は徐々に補正されるので、上述した初期値を用いて初期アライメント処理を行うことができる。
【００４５】
【数８】
【００４６】
そして、クオータニオンｑの各要素ｑ1，ｑ2，ｑ3，ｑ4の初期値、つまり時刻ｔ＝０における各要素ｑ1，ｑ2，ｑ3，ｑ4の値は、上記座標変換行列Ｃｂｎにおける各要素の初期値及び式（５）から式（９）のように表される。この式（９）において、式（８）に基づいて初期値ｑ4（０）を最初に求めることによって、残りの初期値ｑ1（０），ｑ2（０），ｑ3（０）を求めることができる。すなわち、このようにしてクオータニオンｑの各要素ｑ1，ｑ2，ｑ3，ｑ4の初期値ｑ1（０），ｑ2（０），ｑ3（０），ｑ4（０）が演算されることにより、座標変換行列Ｃｂｎの初期値が特定される。
【００４７】
【数９】
【００４８】
座標変換部６ｃは、このようにして座標変換行列Ｃｂｎの初期値を特定すると、当該初期値並びにロール角φ、ピッチ角θ及び方位角ψ（ヨー角）を用いることにより、航行座標系における船舶の姿勢（ロール角φｎ及びピッチ角θｎ）及び方位角ψｎを演算してジャイロ出力部６ｅに出力する。また、座標変換部６ｃは、上記座標変換行列Ｃｂｎの初期値を用いて加速度センサ２から入力されたＸ軸方向、Ｙ軸方向、Ｚ軸方向の加速度を航行座標系における加速度（変換加速度）に座標変換して純慣性航法演算部６ｄに出力する。
【００４９】
そして、純慣性航法演算部６ｄは、このような変換加速度に基づいて航行座標系における船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）を求めてジャイロ出力部６ｅに出力する。本方位測定装置の起動時には、船舶が停止中あるいは航行中に拘らずハイブリッド演算部６ｂから座標変換部６ｃ及び純慣性航法演算部６ｄに出力される誤差推定値は誤差の大きいものであるが、上記初期値を用いて初期アライメント処理を複数回繰り返すことにより、上記誤差推定値に含まれる誤差は徐々に小さくなり、この結果、ロール角φ、ピッチ角θ、方位角ψ（ヨー角）、速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）も誤差の小さなものとなる。
【００５０】
このような起動処理が終了すると、座標変換部６ｃは、５０Ｈｚあるいは１００Ｈｚの周期で座標変換行列Ｃｂｎを更新する。そして、この更新後の座標変換行列Ｃｂｎを用いて上述したと同様に船舶の姿勢（ロール角φｎ及びピッチ角θｎ）及び方位角ψｎを演算してジャイロ出力部６ｅに出力すると共に、更新後の座標変換行列Ｃｂｎを用いて演算した変換加速度を純慣性航法演算部６ｄに出力する。そして、純慣性航法演算部６ｄは、このような変換加速度に基づいて航行座標系における船舶の速度ｖｎ，ｖｅ，ｖｄ及び位置（緯度ＬＡＴ，経度ＬＯＮ）を演算してジャイロ出力部６ｅに出力する。
【００５１】
上記座標変換部６ｃは、上記対称行列Ψの各要素Ｐ，Ｑ，Ｒを下式（１０）によって求める。また、この式（１０）の第２式における右辺第２項は、座標変換行列Ｃｂｎを用いて下式（１１）のように表わされる。
【００５２】
【数１０】
【００５３】
【数１１】
【００５４】
なお、この式（１１）において、角速度Ωは約15.0410687°/hrとして与えられるものであり、またＬは緯度、ｌ（Ｌの小文字）は経度、ｈは高度、さらにＲＬは地球の緯度方向における曲率半径、Ｒｌは地球の経度方向における曲率半径である。上記緯度Ｌの変化率及び経度ｌの変化率は下式（１２）によって与えられる。なお、この式（１２）において、ｈ（ｉ）＝０、またＲＬ，Ｒｌ＝「ｉの取り得る範囲（１〜４）の中の一定値」と近似することができる。
【００５５】
【数１２】
【００５６】
このようにして対称行列Ψの各要素Ｐ，Ｑ，Ｒは、ｉ毎に一定のタイムインターバルで更新される値として求められる。また、上記座標変換部６ｃは、クオータニオンｑの時間変化率ベクトルの各要素を下式（１３）によって求め、このクオータニオンｑの時間変化率ベクトルを上式（７）に代入することにより、一定タイムインターバル後のクオータニオンｑを求める。そして、上記座標変換部６ｃは、この一定タイムインターバル後のクオータニオンｑと式（５）とに基づいて更新後の座標変換行列Ｃｂｎを求める。
【００５７】
【数１３】
【００５８】
ここで、一定タイムインターバル後のクオータニオンｑを求めるに際して、座標変換部６ｃは、下式（１４）に基づいて直交補正された要素ｑ10，ｑ20，ｑ30，ｑ40を要素ｑ1，ｑ2，ｑ3，ｑ4として用いることにより、より正確な更新後の座標変換行列Ｃｂｎを求める。
【００５９】
【数１４】
【００６０】
このような直交補正によって得られた要素ｑ1，ｑ2，ｑ3，ｑ4からなる更新クオータニオンｑを用いることにより座標変換行列Ｃｂｎの各要素は下式（１５）のように表される。そして、上記座標変換部６ｃは、この下式（１５）と上式（６）とによって更新後の座標変換行列Ｃｂｎを用いた新たな船舶の姿勢（ロール角φｎ及びピッチ角θｎ）及び方位角ψｎを求める。
【００６１】
【数１５】
【００６２】
このような本方位測定装置によれば、ロール角φ及びピッチ角θの初期値を「０（ゼロ）」とし、また方位角ψ（ヨー角）の初期値を基準方位角ΨＲとすることにより、座標変換行列Ｃｂｎにおける各要素の初期値を高精度に特定することができる。また、本方位測定装置によれば、方位角中の初期値を基準方位角ΨＲによって一定周期（例えば１秒周期）にリセットする（置き換える）ので、高精度の方位角等を継続的に出力することが可能である。
【００６３】
すなわち、従来の方位測定装置では基準方位角ΨＲを求める手段を備えていなかったので、座標変換行列Ｃｂｎの初期値の精度が低かったが、本方位測定装置では、第１ＧＰＳ受信機４、第２ＧＰＳ受信機５及び基準方位角演算部６ａからなる基準方位角測定手段を備えており、起動時及びその後の一定周期毎に方位角中の初期値を基準方位角測定手段から得られる基準方位角ΨＲに置き換えるので、座標変換行列Ｃｂｎに関する初期値を高精度に特定することが可能であり、よって実航行時の方位誤差を低減することが可能である。
【００６４】
なお、本発明は、上記実施形態に限定されるものではなく、例えば以下のような変形例が考えられる。
（１）上記実施形態では、クオータニオンｑを用いて座標変換行列Ｃｂｎを求めたが、本発明はこれに限定されない。例えば、上述した特開平6-265367号公報、特開平7-270176号公報、また特開平8-21740号公報等では、座標変換行列Ｃｂｎとその時間変化率を示す行列とを航行座標系に対する船体固有座標系の相対回転角速度ベクトルの歪対称行列によって関係付ける計算方法を採用しており、このような計算方法を本発明に適用することも可能である。
【００６５】
（２）上記実施形態では、第１位置が示す第１受信アンテナ４ａの位置と第２位置が示す第２受信アンテナ５ａの位置とを結ぶ直線が船舶の基準方位角となるように、第１受信アンテナ４ａを船舶の船首中央に、かつ、第２受信アンテナ５ａを船舶の船尾中央に設けたが、本発明はこれに限定されない。例えば、船舶の形状と第１受信アンテナ４ａの位置及び第２受信アンテナ５ａの位置との関係を示すアンテナ配置データを基準方位角演算部６ａに記憶させ、このアンテナ配置データ、第１受信アンテナ４ａの位置及び第２受信アンテナ５ａの位置から基準方位角ΨＲを求めるようにしてもよい。
【００６６】
（３）上記実施形態では、基準方位角測定手段を第１ＧＰＳ受信機４及び第２ＧＰＳ受信機５を用いて構成したが、本発明はこれに限定されない。すなわち、基準方位角測定手段は、ＧＰＳに基づく測位システムに限定されず、ＧＲＯＮＡＳＳ（Global Navigation Satellite System）あるいはＧａｌｉｌｅｏ（Galileo Positioning System）等、他の測位システムであってもよい。
【符号の説明】
【００６７】
１…ジャイロ、２…加速度センサ、３…電磁ログ、４…第１ＧＰＳ受信機、５…第２ＧＰＳ受信機、６…演算装置、６ａ…基準方位角演算部、６ｂ…ハイブリッド演算部、６ｃ…座標変換部、６ｄ…純慣性航法演算部、６ｅ…ジャイロ出力部
【特許請求の範囲】
【請求項１】
移動体に固定設置され、移動体直交座標系の各座標軸周りの角速度及び各座標軸方向の加速度を検出する慣性センサと、
移動体の移動体直交座標系における速度を基準速度として測定する速度計と、
移動体の航行座標系における位置を基準位置として測定する測位装置と、
各座標軸周りの角速度に基づいて移動体直交座標系と航行座標系とに関する座標変換行列を時系列的に順次演算し、該座標変換行列を用いて各座標軸周りの角速度から航行座標系における移動体の姿勢及び方位角を時系列的に順次演算すると共に各座標軸方向の加速度から航行座標系における移動体の速度及び位置を測定速度及び測定位置として時系列的に順次演算し、かつ、基準速度と測定速度との差分速度及び基準位置と測定位置との差分位置に基づいて少なくとも移動体の姿勢及び方位角の修正量を推定して移動体の姿勢及び方位角の演算に反映させる演算装置とを備えるストラップダウン方式の方位測定装置であって、
一定間隔を隔てて配置された２つに受信アンテナを備え、各受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段をさらに備え、
演算装置は、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定することを特徴とする方位測定装置。
【請求項２】
演算装置は、クオータニオンを用いることにより座標変換行列の初期値を求めることを特徴とする請求項１記載の方位測定装置。
【請求項３】
基準方位角測定手段は、位置が既知の基準局が発信する電波を利用することによりＧＰＳ（Grobal Positioning System）に基づいて測定した各受信アンテナの位置を補正し、当該補正位置に基づいて基準方位角を算出するＤＧＰＳ（Differential Grobal Positioning System）受信機であることを特徴とする請求項１または２記載の方位測定装置。
【請求項１】
移動体に固定設置され、移動体直交座標系の各座標軸周りの角速度及び各座標軸方向の加速度を検出する慣性センサと、
移動体の移動体直交座標系における速度を基準速度として測定する速度計と、
移動体の航行座標系における位置を基準位置として測定する測位装置と、
各座標軸周りの角速度に基づいて移動体直交座標系と航行座標系とに関する座標変換行列を時系列的に順次演算し、該座標変換行列を用いて各座標軸周りの角速度から航行座標系における移動体の姿勢及び方位角を時系列的に順次演算すると共に各座標軸方向の加速度から航行座標系における移動体の速度及び位置を測定速度及び測定位置として時系列的に順次演算し、かつ、基準速度と測定速度との差分速度及び基準位置と測定位置との差分位置に基づいて少なくとも移動体の姿勢及び方位角の修正量を推定して移動体の姿勢及び方位角の演算に反映させる演算装置とを備えるストラップダウン方式の方位測定装置であって、
一定間隔を隔てて配置された２つに受信アンテナを備え、各受信アンテナで受信した測位用電波に基づいて各受信アンテナの位置を演算し、当該各受信アンテナの位置に基づいて移動体の基準方位角を測定する基準方位角測定手段をさらに備え、
演算装置は、該基準方位角測定手段から得られる基準方位角を用いて座標変換行列の初期値を決定することを特徴とする方位測定装置。
【請求項２】
演算装置は、クオータニオンを用いることにより座標変換行列の初期値を求めることを特徴とする請求項１記載の方位測定装置。
【請求項３】
基準方位角測定手段は、位置が既知の基準局が発信する電波を利用することによりＧＰＳ（Grobal Positioning System）に基づいて測定した各受信アンテナの位置を補正し、当該補正位置に基づいて基準方位角を算出するＤＧＰＳ（Differential Grobal Positioning System）受信機であることを特徴とする請求項１または２記載の方位測定装置。
【図１】
【図２】
【図２】
【公開番号】特開２０１２−２０２７４９（Ｐ２０１２−２０２７４９Ａ）
【公開日】平成２４年１０月２２日（２０１２．１０．２２）
【国際特許分類】
【出願番号】特願２０１１−６５８６３（Ｐ２０１１−６５８６３）
【出願日】平成２３年３月２４日（２０１１．３．２４）
【出願人】（０００２３２３５７）横河電子機器株式会社 (109)
【Ｆターム（参考）】
【公開日】平成２４年１０月２２日（２０１２．１０．２２）
【国際特許分類】
【出願日】平成２３年３月２４日（２０１１．３．２４）
【出願人】（０００２３２３５７）横河電子機器株式会社 (109)
【Ｆターム（参考）】
[ Back to top ]