JP6732926B2 - 方位算出装置、方位算出方法、および方位算出プログラム - Google Patents

Description

本発明は、ＧＮＳＳ信号とジャイロセンサの角速度とを用いて方位を算出する方位算出装置、方位算出方法、および方位算出プログラムに関する。

従来、各種の方位算出装置が考案されている。例えば、特許文献１には、磁気方位と角速度を用いて方位を算出する電子磁気コンパスが記載されている。磁気方位は、地磁気センサで計測される。角速度は、角速度センサで計測される。特許文献１では、磁気方位に含まれる誤差を、角速度を用いて補正している。

また、従来の方位算出装置として、搬送波位相等のＧＮＳＳ信号の観測データを用いたものがある。

特許第４０７０８７９号明細書

ＧＮＳＳ信号の観測データと、ジャイロセンサ（角速度センサ）の角速度とを用いた方位算出装置では、ＧＮＳＳ信号の観測データを用いてジャイロセンサの角速度バイアス誤差を推定している。この方位算出装置は、推定された角速度バイアス誤差を用いて、角速度を補正することによって、方位を高精度に算出している。

しかしながら、このような従来の方位算出装置では、電源投入後等の方位算出の初期において方位を高精度に安定させるまで時間がかかることがある。

この発明は、方位算出の初期から速やかに高精度な方位を算出できる方位算出装置、方位算出方法、および、方位算出プログラムを提供することにある。

この発明の方位算出装置は、ＧＮＳＳ演算部、磁気方位演算部、ジャイロセンサ、誤差推定部、および、統合演算部を備える。ＧＮＳＳ演算部は、ＧＮＳＳ信号の搬送波位相を含むＧＮＳＳ観測データを用いてＧＮＳＳ方位を算出する。磁気方位演算部は、地磁気に基づく磁気方位を算出する。ジャイロセンサは、角速度を検出する。誤差推定部は、磁気方位の変化量またはＧＮＳＳ方位の変化量を用いて、角速度に含まれる角速度バイアス誤差を推定する。統合演算部は、磁気方位またはＧＮＳＳ方位の少なくとも１つ、角速度、および角速度バイアス誤差を用いて、統合方位を算出する。誤差推定部は、ＧＮＳＳ方位の初期化が完了する前の第１期間に、磁気方位の変化量を用いて角速度バイアス誤差の推定を行う。誤差推定部は、ＧＮＳＳ方位の初期化完了後の定常状態である第２期間に、ＧＮＳＳ方位の変化量を用いて角速度バイアス誤差の推定を行う。統合演算部は、第１期間に、磁気方位を用いて統合方位を算出する。統合演算部は、第２期間に、ＧＮＳＳ方位を用いて統合方位を算出する。

この構成では、統合方位が安定して得られていない初期状態において、磁気方位を用いてバイアス誤差が高精度に推定される。これにより、統合方位の算出の初期でも速やかに高精度な方位が得られる。

この発明によれば、統合方位の算出の初期から速やかに高精度な統合方位を算出できる。

第１の実施形態に係る方位算出装置の機能ブロック図 第１の実施形態に係る方位算出装置のＧＮＳＳ方位の初期化完了前の処理を説明する図 第１の実施形態に係る方位算出装置のＧＮＳＳ方位の初期化直後の処理を説明する図 第１の実施形態に係る方位算出装置のＧＮＳＳ方位の初期化後で定常状態の処理を説明する図 第１の実施形態に係る方位算出方法のフローチャート 第２の実施形態に係る方位算出装置の機能ブロック図

本発明の第１の実施形態に係る方位算出装置、方位算出方法、および、方位算出プログラムについて、図を参照して説明する。図１は、第１の実施形態に係る方位算出装置の機能ブロック図である。図２は、第１の実施形態に係る方位算出装置のＧＮＳＳ方位の初期化完了前の処理を説明する図である。図３は、第１の実施形態に係る方位算出装置のＧＮＳＳ方位の初期化直後の処理を説明する図である。図４は、第１の実施形態に係る方位算出装置のＧＮＳＳ方位の初期化後で定常状態の処理を説明する図である。

方位算出装置１０は、ＧＮＳＳ受信部２１，２２、ＧＮＳＳ演算部３０、磁気センサ４０、ジャイロセンサ５１、磁気方位演算部６０、誤差推定部７０、および、統合演算部８０を備える。方位算出装置１０は、船舶等の移動体に設置されている。なお、ＧＮＳＳは、Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍの略語であり、測位システムである。

ＧＮＳＳ受信部２１は、ＧＮＳＳアンテナＡＮＴ１に接続されている。ＧＮＳＳ受信部２２は、ＧＮＳＳアンテナＡＮＴ２に接続されている。ＧＮＳＳアンテナＡＮＴ１，ＡＮＴ２は、移動体に対して所定の姿勢で固定されている。

ＧＮＳＳアンテナＡＮＴ１は、ＧＮＳＳ衛星ＳＡＴ１からのＧＮＳＳ信号を受信して、ＧＮＳＳ受信部２１に出力する。ＧＮＳＳアンテナＡＮＴ２は、ＧＮＳＳ衛星ＳＡＴ１からのＧＮＳＳ信号を受信して、ＧＮＳＳ受信部２２に出力する。なお、図では、１つのＧＮＳＳ衛星ＳＡＴ１からのＧＮＳＳ信号を受信する状態しか記載していないが、ＧＮＳＳアンテナＡＮＴ１，ＡＮＴ２は、複数のＧＮＳＳ衛星からのＧＮＳＳ信号を受信して、出力している。この際、少なくとも４つ以上のＧＮＳＳ衛星からのＧＮＳＳ信号を受信することが好ましい。

ＧＮＳＳ受信部２１，２２は、ＧＮＳＳ信号を捕捉、追尾して、搬送波位相を含むＧＮＳＳ観測データを生成する。また、ＧＮＳＳ受信部２１，２２は、コード擬似距離と衛星位置とを用いて、ＧＮＳＳ測位位置を算出する。衛星位置は、ＧＮＳＳ信号に重畳された航法メッセージを復調することによって得られる。

ＧＮＳＳ受信部２１，２２は、ＧＮＳＳ観測データとＧＮＳＳ測位位置を、ＧＮＳＳ演算部３０に出力する。ＧＮＳＳ受信部２１は、ＧＮＳＳ測位位置を、磁気方位演算部６０および誤差推定部７０に出力する。

ＧＮＳＳ演算部３０は、複数のＧＮＳＳ観測データとＧＮＳＳ測位位置を用いて、ＧＮＳＳ方位を算出する。例えば、ＧＮＳＳ演算部３０は、ＧＮＳＳ受信部２１，２２のＧＮＳＳ観測データから、ＧＮＳＳアンテナＡＮＴ１，ＡＮＴ２間の搬送波位相差を算出する。ＧＮＳＳ演算部３０は、この搬送波位相差を用いてＧＮＳＳ方位を算出する。このように、搬送波位相差を用いるので、ＧＮＳＳ方位は高精度である。ＧＮＳＳ演算部３０は、ＧＮＳＳ方位およびＧＮＳＳ観測データを、統合演算部８０に出力する。ＧＮＳＳ演算部３０は、ＧＮＳＳ方位を誤差推定部７０に出力する。

磁気センサ４０は、所定の姿勢で移動体に固定されている。磁気センサ４０は、直交三軸の地磁気を検出して磁気方位演算部６０に出力する。

ジャイロセンサ５１は、所定の姿勢で移動体に固定されている。ジャイロセンサ５１は、直交三軸周りの角速度を検出して、誤差推定部７０および統合演算部８０に出力する。

磁気方位演算部６０は、地磁気に基づいて磁気方位を算出する。この際、磁気方位演算部６０は、ＧＮＳＳ測位位置を用いて磁気方位を補正する。このように補正された磁気方位は高精度である。磁気方位演算部６０は、磁気方位を誤差推定部７０および統合演算部８０に出力する。

誤差推定部７０は、磁気方位の変化量、ＧＮＳＳ方位の変化量、ＧＮＳＳ速度の変化量を算出する。磁気方位の変化量は、複数時刻の磁気方位から算出される。ＧＮＳＳ方位の変化量は、複数時刻のＧＮＳＳ方位から算出される。ＧＮＳＳ速度の変化量は、複数時刻のＧＮＳＳ位置から算出される。誤差推定部７０は、磁気方位の変化量またはＧＮＳＳ方位の変化量、およびＧＮＳＳ速度の変化量を用いて、角速度バイアス誤差を推定する。誤差推定部７０は、角速度バイアス誤差を統合演算部８０に出力する。

統合演算部８０は、角速度バイアス誤差を用いて角速度を補正する。統合演算部８０は、磁気方位またはＧＮＳＳ方位と、補正後の角速度とを用いて、統合方位を算出する。このような統合方位を算出することによって、個別に算出された方位のみを用いるよりも、高精度な方位を安定して算出することができる。

このような構成において、方位算出装置１０は、次に示すように、状況に応じて処理を選択的に実行する。

（１）ＧＮＳＳ方位の初期化完了前（第１期間（図２参照））
ＧＮＳＳ方位の算出には、搬送波位相差から、整数値バイアスを決定し、基線ベクトルを算出する必要がある。方位算出装置１０の電源投入直後等の方位算出の初期では、整数値バイアスが決定されておらず、基線ベクトルも算出されていない。ＧＮＳＳ方位の初期化完了とは、この整数値バイアスの決定と基線ベクトルの決定が完了したことを意味する。

このため、ＧＮＳＳ方位の初期化が完了していない第１期間では、高精度なＧＮＳＳ方位は算出されない。したがって、第１期間では、誤差推定部７０は、磁気方位の変化量を用いて角速度バイアス誤差を推定する。磁気方位は高精度であるので、角速度バイアス誤差は、高精度に推定される。

また、統合演算部８０は、磁気方位を初期方位に設定し、統合方位を算出する。この際、磁気方位は高精度であり、角速度は、高精度な角速度バイアス誤差によって補正されている。したがって、統合方位は高精度に算出される。

このように、本実施形態に構成を用いることによって、方位算出の初期に、角速度バイアス誤差を高精度に推定でき、統合方位を高精度に算出できる。また、方位算出装置１０から出力する方位（統合方位）に、精度条件を設定することがある。例えば、統合方位が所定の精度よりも高い時のみに出力するという条件が設定されていることがある。このような場合、本実施形態の構成を用いることによって、精度条件を速く満たすことが可能であり、統合方位算出の開始から統合方位を出力するまでの時間を短縮することができる。

（２）ＧＮＳＳ方位の初期化完了直後（第３期間（図３参照））
ＧＮＳＳ方位の初期化が完了すると、ＧＮＳＳ演算部３０からＧＮＳＳ方位が出力される。この状態では、まだＧＮＳＳ方位の変化量を高精度に得ることが容易でない。したがって、第３期間では、誤差推定部７０は、磁気方位の変化量を用いて角速度バイアス誤差を推定する。第１期間と同様に、磁気方位の変化量は高精度であるので、角速度バイアス誤差は、高精度に推定される。

ＧＮＳＳ方位が算出されているので、統合演算部８０は、ＧＮＳＳ方位、角速度、および角速度バイアス誤差を用いて、統合方位を算出する。ＧＮＳＳ方位および角速度バイアス誤差は高精度であるので、統合方位は高精度に算出される。

（３）ＧＮＳＳ方位の初期化完了後で定常状態（第２期間（図４参照））
第２期間である定常状態では、ＧＮＳＳ方位が連続的に算出される。したがって、ＧＮＳＳ方位の変化量を高精度に算出することができる。したがって、第２期間では、誤差推定部７０は、ＧＮＳＳ方位の変化量を用いて角速度バイアス誤差を推定する。ＧＮＳＳ方位の変化量は高精度であるので、角速度バイアス誤差は、高精度に推定される。

統合演算部８０は、ＧＮＳＳ方位、角速度、および角速度バイアス誤差を用いて、統合方位を算出する。ＧＮＳＳ方位および角速度バイアス誤差は高精度であるので、統合方位は高精度に算出される。

このように、本実施形態の構成を用いることによって、電源投入直後等のＧＮＳＳ方位の初期化が完了していない方位算出の初期に、角速度バイアス誤差を高精度に推定できる。また、方位算出の初期から統合方位を高精度に算出することができる。

また、統合方位の出力に精度の条件が設定されていれば、この精度の条件を素早く満たすことができる。したがって、所定の精度以上の統合方位を、電源投入後に素早く出力することができる。

また、ＧＮＳＳ方位の初期化完了以降は、高精度なＧＮＳＳ方位を、角速度バイアス誤差の推定および統合方位に用いることができるので、初期化完了後も継続的に統合方位を高精度に算出することができる。

なお、上述の説明では、ＧＮＳＳ方位の初期化完了後、統合方位の算出にＧＮＳＳ方位のみを用いる態様を示した。しかしながら、ＧＮＳＳ方位の初期化完了後であっても、ＧＮＳＳ方位と磁気方位を併用してもよい。例えば、ＧＮＳＳ信号を一時的に受信できなくなり、ＧＮＳＳ方位の精度が一時的に劣化するような場合、ＧＮＳＳ方位を磁気方位に置き換えてもよい。

上述の説明では、統合方位の算出の各処理を、複数の機能部で分担して実行する態様を示した。しかしながら、これらの処理をプログラム化して記憶しておき、コンピュータ等の処理装置がこのプログラムを実行してもよい。この場合、処理装置は、図５に示すフローチャートに準じて処理を実行すればよい。図５は、第１の実施形態に係る方位算出方法のフローチャートである。

処理装置は、電源投入後、ＧＮＳＳ観測データの生成および磁気方位の算出を実行する（Ｓ１０１）。具体的には、処理装置は、複数のＧＮＳＳアンテナで受信したＧＮＳＳ信号を捕捉、追尾して、搬送波位相およびコード位相を含むＧＮＳＳ観測データを生成する。処理装置は、このＧＮＳＳ観測データを用いて、整数値バイアス、および基線ベクトルの決定処理を実行する。この処理に並行して、処理装置は、磁気センサからの地磁気を取得して、磁気方位を算出する。この際、処理装置は、ＧＮＳＳ観測データに基づくＧＮＳＳ測位位置を用いて、磁気方位の補正を行う。この処理は、継続的に行われている。

処理装置は、ＧＮＳＳ方位の初期化、すなわち、整数値バイアスの決定および基線ベクトルの算出が完了していない第１期間であれば（Ｓ１０２：ＮＯ）、磁気方位の変化量を算出する（Ｓ１０３）。処理装置は、磁気方位の変化量を用いて、角速度バイアス誤差を推定する（Ｓ１０４）。この際、処理装置は、ＧＮＳＳ測位位置に基づくＧＮＳＳ速度の変化量も用いて、角速度バイアス誤差を推定する。

処理装置は、統合方位の算出開始直後であれば（Ｓ１０５：ＹＥＳ）、磁気方位を統合方位の初期値に設定する（Ｓ１０６）。処理装置は、統合方位の算出開始直後でなければ（Ｓ１０５：ＮＯ）、統合方位の初期値の設定を行わない。

処理装置は、推定された角速度バイアス誤差を用いて、角速度に含まれる誤差を補正する（Ｓ１０７）。処理装置は、誤差補正後の角速度を用いて、統合方位を算出して更新する（Ｓ１０８）。

処理装置は、ＧＮＳＳ方位の初期化が完了していれば（Ｓ１０２：ＹＥＳ）、ＧＮＳＳ方位の初期化完了直後か否かを判定する。

処理装置は、ＧＮＳＳ方位の初期化完了直後の第３期間であれば（Ｓ１１０：ＹＥＳ）、磁気方位の変化量を算出する（Ｓ１１１）。処理装置は、磁気方位の変化量を用いて、角速度バイアス誤差を推定する（Ｓ１１２）。この際、処理装置は、ＧＮＳＳ測位位置に基づくＧＮＳＳ速度の変化量も用いて、角速度バイアス誤差を推定する。

処理装置は、ＧＮＳＳ方位で統合方位を更新する（Ｓ１１３）。この後、処理装置は、推定された角速度バイアス誤差を用いて、角速度に含まれる誤差を補正する（Ｓ１０７）。処理装置は、誤差補正後の角速度を用いて、統合方位を算出して更新する（Ｓ１０８）。

処理装置は、ＧＮＳＳ方位の初期化完了直後でない定常状態の第２期間であれば（Ｓ１１０：ＮＯ）、ＧＮＳＳ方位の変化量を算出する（Ｓ１２１）。処理装置は、ＧＮＳＳ方位の変化量を用いて、角速度バイアス誤差を推定する（Ｓ１２２）。この際、処理装置は、ＧＮＳＳ測位位置に基づくＧＮＳＳ速度の変化量も用いて、角速度バイアス誤差を推定する。この後、処理装置は、推定された角速度バイアス誤差を用いて、角速度に含まれる誤差を補正する（Ｓ１０７）。処理装置は、誤差補正後の角速度を用いて、統合方位を算出して更新する（Ｓ１０８）。

次に、本発明の第２の実施形態に係る方位算出装置について、図を参照して説明する。図６は、第２の実施形態に係る方位算出装置の機能ブロック図である。

図６に示すように、本実施形態の方位算出装置１０Ａは、第１の実施形態に係る方位算出装置１０に対して、加速度センサ５２、加速度姿勢角算出部９０を追加した点、および、磁気方位演算部６０Ａ、誤差推定部７０Ａ、統合演算部８０Ａの処理を部分的に変更した点で異なる。他の構成は、第１の実施形態に係る方位算出装置１０と同じであり、同じ箇所の説明は省略する。

加速度センサ５２は、所定の姿勢で移動体に固定されている。加速度センサ５２は、直交三軸の加速度を検出して、誤差推定部７０Ａ、統合演算部８０Ａ、および、加速度姿勢角算出部９０に出力する。

加速度姿勢角算出部９０は、加速度から姿勢角（加速度姿勢角）を算出する。加速度姿勢角算出部９０は、少なくともロール角、ピッチ角を有する加速度姿勢角を算出する。加速度姿勢角算出部９０は、加速度姿勢角を、磁気方位演算部６０および誤差推定部７０Ａに出力する。

磁気方位演算部６０Ａは、加速度姿勢角を用いて、移動体に対する磁気センサ４０の姿勢に基づく誤差を算出する。磁気方位演算部６０Ａは、この誤差も用いて、磁気方位を補正する。これにより、磁気方位はさらに高精度になる。

誤差推定部７０Ａは、角速度バイアス誤差とともに、加速度バイアス誤差を推定する。角速度バイアス誤差は、誤差推定部７０と同様の方法によって推定される。すなわち、誤差推定部７０Ａは、ＧＮＳＳ方位の初期化完了前または完了直後であれば、磁気方位の変化量から角速度バイアス誤差を推定し、ＧＮＳＳ方位の初期化完了直後を除く初期化完了後であれば、ＧＮＳＳ方位の変化量から角速度バイアス誤差を推定する。加速度バイアス誤差は、ＧＮＳＳ演算部３０によって加速度から直交三軸のＧＮＳＳ速度を算出することによって、このＧＮＳＳ速度の変化量から推定する。このＧＮＳＳ速度の変化量は、角速度バイアス誤差の推定に利用することもできる。

さらに、誤差推定部７０Ａは、角速度バイアス誤差の推定に、加速度姿勢角を用いる。これにより、角速度バイアス誤差は、さらに高精度に推定される。誤差推定部７０Ａは、角速度バイアス誤差と加速度バイアス誤差を統合演算部８０Ａに出力する。

統合演算部８０Ａは、推定された加速度バイアス誤差を用いて、加速度を補正する。統合演算部８０Ａは、加速度姿勢角によって補正された角速度を用いて、統合方位を算出する。これにより、統合方位は、さらに高精度に算出される。

このように、統合方位に角速度と加速度を用いる場合であっても、電源投入直後等の方位算出の初期から高精度な統合方位を算出することができる。また、高精度な統合方位を継続的に算出することができる。

１０：方位算出装置
１０Ａ：方位算出装置
２１，２２：ＧＮＳＳ受信部
３０：ＧＮＳＳ演算部
４０：磁気センサ
５１：ジャイロセンサ
５２：加速度センサ
６０，６０Ａ：磁気方位演算部
７０，７０Ａ：誤差推定部
８０，８０Ａ：統合演算部
９０：加速度姿勢角算出部
ＡＮＴ１，ＡＮＴ２：ＧＮＳＳアンテナ
ＳＡＴ１：ＧＮＳＳ衛星

Claims (9)

1. ＧＮＳＳ信号を受信する複数のＧＮＳＳアンテナと、
   前記ＧＮＳＳ信号の搬送波位相を含むＧＮＳＳ観測データから算出された前記複数のＧＮＳＳアンテナ間の搬送波位相差を用いてＧＮＳＳ方位を算出するＧＮＳＳ演算部と、
   地磁気に基づく磁気方位を算出する磁気方位演算部と、
   角速度を検出するジャイロセンサと、
   前記ＧＮＳＳ方位の初期化が完了する前の第１期間に、前記磁気方位の変化量を用いて前記角速度に含まれる角速度バイアス誤差の推定を行い、
   前記ＧＮＳＳ方位の初期化完了後の定常状態である第２期間に、前記ＧＮＳＳ方位の変化量を用いて前記角速度バイアス誤差の推定を行う誤差推定部と、
   前記第１期間に、前記磁気方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記磁気方位とを用いて統合方位を算出し、
   前記第２期間に、前記ＧＮＳＳ方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記ＧＮＳＳ方位とを用いて統合方位を算出する統合演算部と、
   を備える方位算出装置。
2. 請求項１に記載の方位算出装置であって、
   前記誤差推定部は、
   前記初期化の完了直後である第３期間に、前記磁気方位の変化量を用いて前記角速度バイアス誤差を推定し、
   前記統合演算部は、
   前記第３期間に、前記磁気方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記ＧＮＳＳ方位とを用いて前記統合方位を算出する、
   方位算出装置。
3. 請求項１または請求項２に記載の方位算出装置であって、
   前記ＧＮＳＳ信号から前記ＧＮＳＳ観測データを算出するとともに、ＧＮＳＳ測位位置を算出するＧＮＳＳ受信部をさらに備え、
   前記磁気方位演算部は、
   前記ＧＮＳＳ測位位置を用いて前記磁気方位を算出する、
   方位算出装置。
4. 請求項１乃至請求項３のいずれか１項に記載の方位算出装置であって、
   前記統合演算部は、
   前記第２期間に、前記ＧＮＳＳ方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記ＧＮＳＳ方位と前記磁気方位とを併用して、前記統合方位を算出する、
   方位算出装置。
5. 請求項１乃至請求項４のいずれか１項に記載の方位算出装置であって、
   加速度を検出する加速度センサをさらに備え、
   前記磁気方位演算部は、
   前記加速度から得られる姿勢角を用いて、前記磁気方位を算出し、
   前記誤差推定部は、
   前記ＧＮＳＳ速度の変化量を用いて加速度バイアス誤差の推定を行い、
   前記統合演算部は、
   前記加速度バイアス誤差によって補正された加速度と、前記加速度から得られる姿勢角によって補正された角速度とを用いて、前記統合方位を算出する、
   方位算出装置。
6. 複数のＧＮＳＳアンテナでＧＮＳＳ信号をそれぞれ受信し、
   前記ＧＮＳＳ信号の搬送波位相を含むＧＮＳＳ観測データから算出された前記複数のＧＮＳＳアンテナ間の搬送波位相差を用いてＧＮＳＳ方位を算出し、
   地磁気に基づく磁気方位を算出し、
   ジャイロセンサを用いて角速度を検出し、
   前記ＧＮＳＳ方位の初期化が完了する前の第１期間に、前記磁気方位の変化量を用いて前記角速度に含まれる前記角速度バイアス誤差の推定し、
   前記ＧＮＳＳ方位の初期化完了後の定常状態である第２期間に、前記ＧＮＳＳ方位の変化量を用いて前記角速度バイアス誤差の推定し、
   前記第１期間に、前記磁気方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記磁気方位とを用いて統合方位を算出し、
   前記第２期間に、前記ＧＮＳＳ方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記ＧＮＳＳ方位とを用いて統合方位を算出する、
   方位算出方法。
7. 請求項６に記載の方位算出方法であって、
   前記初期化の完了直後である第３期間に、前記磁気方位の変化量を用いて前記角速度バイアス誤差を推定し、
   前記第３期間に、前記ＧＮＳＳ方位で前記統合方位を更新する、
   方位算出方法。
8. 複数のＧＮＳＳアンテナでそれぞれ受信したＧＮＳＳ信号とジャイロセンサで検出された角速度を用いて統合方位を算出する処理をコンピュータに実行させる方位算出プログラムであって、
   前記コンピュータは、
   前記ＧＮＳＳ信号の搬送波位相を含むＧＮＳＳ観測データから算出された前記複数のＧＮＳＳアンテナ間の搬送波位相差を用いてＧＮＳＳ方位を算出し、
   地磁気に基づいて磁気方位を算出し、
   前記ＧＮＳＳ方位の初期化が完了する前の第１期間に、前記磁気方位の変化量を用いて前記角速度に含まれる前記角速度バイアス誤差の推定し、
   前記ＧＮＳＳ方位の初期化完了後の定常状態である第２期間に、前記ＧＮＳＳ方位の変化量を用いて前記角速度バイアス誤差の推定し、
   前記第１期間に、前記磁気方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記磁気方位とを用いて前記統合方位を算出し、
   前記第２期間に、前記ＧＮＳＳ方位の変化量を用いて推定された前記角速度バイアス誤差により補正された前記角速度と前記ＧＮＳＳ方位とを用い前記統合方位を算出する、
   方位算出プログラム。
9. 請求項８に記載の方位算出プログラムであって、
   前記コンピュータは、
   前記初期化の完了直後である第３期間に、前記磁気方位の変化量を用いて前記角速度バイアス誤差を推定し、
   前記第３期間に、前記ＧＮＳＳ方位で前記統合方位を更新する、
   方位算出プログラム。

Images