JP6425377B2 - 全地球航法衛星システム用フィルタ - Google Patents

Description

本発明は、全地球航法衛星システム（ＧＮＳＳ：Global Navigation Satellite System）用フィルタに関する。

船体制御において、ＧＮＳＳは自船位置を知るため、必須センサになっている。特に航路制御や位置制御では、位置信号を参照するため、その不具合は制御性能に影響を与える。ＧＮＳＳの不具合の要因は、偶然的なものと必然的なものに分けられる。偶発的なものは、ＧＮＳＳ自体の問題から生じ、信号を利用できない状態である。また、必然的なものは、不具合の発生を検出でき、信号を利用できる状態である。以下では、特に、後者を対象とし、制御性能に与える影響を低減することについて説明する。

ＧＮＳＳの位置信号は、常時バラツキ、分解能（量子化）によるランダム性外乱成分をもつが、長期的には位置信号の平均値が真値付近にあるとみなせる。また、ＧＮＳＳを利用した制御システムは、通常、入力信号に含まれる外乱成分をフィルタする機能をもつ。しかしながら、外乱の抑制効果を制御システムに初めから依存させることは適切でない。システム設計はプラントモデリング、航海センサ及び制御システムを統合して捉えた上で、各々を設計することが重要である。すなわちＧＮＳＳの外乱成分は、制御システムに入力する前に抑制すべきである。制御システムに入力する前に外乱成分を除去した上で、制御システムは、閉ループ安定性及び外乱除去性を設計することにより、制御性能の向上が期待できる。よって、ＧＮＳＳからの信号のみを用いて、位置信号に含まれる外乱成分を低減することが望まれる。

ＧＮＳＳの外乱成分は、位置飛び（ＰＪ：Position Jump）や量子化誤差が知られている。位置飛びはマルチバスの影響、受信衛星数の変化、ＧＮＳＳから信号処理演算装置への受信変化あるいはその逆などの場合に生じ、短い発生時間及び飛びの大きさにランダム性をもつ。しかしステップ状の位置飛びが長期間持続すると、外乱でなく信号として扱われてしまう。その対策としては、例えば特許文献１を参照すると、更新時間ごとに位置変化の絶対値がしきい値を越えた場合、位置出力を推定位置に置換することが行われていた。推定位置は速度と方位から求めたもので、段階的に位置信号に追従させていた。一方、量子化誤差は、例えば、分解能０．０１ａｒｃ ｍｉｎｕｔｅであれば、１ＮＭ÷１００≒１８．５ｍ以下の位置変化を出力できないことによる外乱成分である。

特開２０１３−１２２４０６号公報

以上のことから、つぎの課題が挙げられる。
（１）現在の位置はＧＮＳＳからの位置を利用するため、ＧＮＳＳのランダム誤差を含み、位置飛びの誤検出を含む。
（２）位置飛び変化を分割して追従させる対策や量子化誤差は制御システムにステップ入力として作用するため、絶えず過渡現象を誘起させる。

したがって本発明は、上記課題に鑑みてなされたものであり、本発明の目的は、外乱成分を抑制することができるとともに、外乱成分による過渡誤差を抑制することの可能な、全地球航法衛星システム用フィルタを提供することである。

かかる目的を達成するために、本発明は２つの機能を有する。第１機能はＧＮＳＳ信号から真値の位置信号を推定する機能であり、第２機能は位置飛びに滑らかに追従する参照位置信号を生成する機能である。

すなわち、本発明の全地球航法衛星システム用フィルタは、参照位置に基づいて、オフセット速度成分を加えた推定方程式によって推定位置を算出する推定位置算出部（第１機能）と、ＧＮＳＳ信号に基づく現在の測定位置と前回の参照位置との差の絶対値が所定の距離閾値以下である場合に現在の測定位置を前記参照位置とし、前記差の絶対値が前記距離閾値より大きい場合に前記測定位置に位置飛びが発生したと判定して前回の参照位置に基づく前記参照位置を生成する位置飛び修正部（第２機能）とを備える。そして、前記位置飛び修正部は、位置飛びの発生期間が所定の期間閾値以下である場合に前回の参照位置を参照位置とし、位置飛びの発生期間が前記期間閾値より大きい場合に前記距離閾値により前回の参照位置を修正して前記距離閾値単位で前記測定位置に追従する参照位置とすることを特徴とする。

前記推定方程式としては、以下の式で表される方程式とすることが可能である。ここで、＾：推定値、＾ｘ，＾ｙ：それぞれ北向き、西向き推定位置成分であり、＾ｕ_o，＾ｖ_o：それぞれ北向き、西向きオフセット速度成分の推定値であり、ｕ_x，ｖ_y：それぞれ北向き、西向き速度成分であり、ｋ₁，ｋ₂：推定ゲインであり、ｘ_g，ｙ_g：ＧＮＳＳのそれぞれ北向き、西向き位置成分であり、（ｘ_g−＾ｘ），（ｙ_g−＾ｙ）：それぞれ北向き、西向き位置誤差成分である。

かかる２つの機能を有する本発明によれば、まず第１機能により、ＧＮＳＳ信号のみを利用するので、単独で構成できる。また、推定位置を用いることにより、制御システムの外乱除去性への要求が低下し、その分閉ループ系の性能向上が期待できる。このようにして、外乱成分を抑制することができる。さらに第２機能により、位置飛びの特性に応じた保持時間、追従速度に設定できるため、適用範囲がＧＮＳＳ環境で広く、また、外乱成分による過渡誤差を抑制することが可能である。

本発明によれば、以下のような効果が得られる。
（１）ＧＮＳＳ信号のみを利用するので、単独で構成できる。
（２）推定位置を用いることにより、制御システムの外乱除去性への要求が低下し、その分閉ループ系の性能向上が期待できる。
（３）位置飛びの特性に応じた保持時間、追従速度に設定できるため、適用範囲がＧＮＳＳ環境で広い。

ＧＮＳＳフィルタが設置される航路制御系の一例を示すブロック図であり、（ａ）は航路制御系１０を示し、（ｂ）は（ａ）の制御器１２の内部に配置されたＧＮＳＳフィルタ１６を示す。 参照位置の生成アルゴリズムを示す図である。 ＧＮＳＳフィルタにおける参照位置と推定位置との関係を示すブロック図である。 実船試験データからの方位、速度及び速度成分を示す図である。 位置飛びなしの場合で、推定位置の効果を示す図である。 図５のｘの部分を拡大した図である。 位置飛びありの場合で、推定位置の効果を示す図である。 図７のｘの部分を拡大した図である。

以下に添付図面を参照しながら、本発明の好適な実施形態について詳細に説明する。

（１）位置信号の推定（第１機能）
本実施形態にかかるＧＮＳＳフィルタ１６は、図１（ａ）に例示する航路制御系１０において、制御器１２の内部に配置される。航路制御系１０は、大まかに、計画方位・計画航路が入力される制御器１２と、制御部１２からの指令舵角が入力される操舵機１４と、操舵機１４の舵角に従う船体２０と、ＧＮＳＳを含み、船体２０の変化を検出して速度、方位、位置の信号を制御器１２に入力する航海センサ類２２と、を備える。

ＧＮＳＳフィルタ１６は、図１（ｂ）に示したように、ＧＮＳＳ信号から確からしい位置信号を推定する。位置信号は検出時外乱成分を含むが、その長期間の平均値はほぼ真値と扱える。ＧＮＳＳ信号はＮＥＤ座標（Ｘ軸を北方向、Ｙ軸を西方向、Ｚ軸を重力方向にとる局地水平座標）において、緯度経度の位置、対地の方位ＣＯＧ（Course Over Ground）と速度ＳＯＧ（speed Over Ground）をもつ。速度成分はＣＯＧ、ＳＯＧから、以下の数式（１）のようになる。

ここで、ｕ_x，ｖ_y：それぞれ北向き、西向き速度成分、ｕ_g，ψ_g：ＧＮＳＳのそれぞれ速度、方位である。

推定位置は、従来、以下の数式（２）から求めていた。

ここで＾：推定値、＾ｘ，＾ｙ：それぞれ北向き、西向き推定位置成分である。しかしながらＧＮＳＳで得られる速度と方位、及び位置はそれぞれ異なる方法によって計算される。このため、上記従来の方法では、ＧＮＳＳの位置と上記式を積分した位置とは等価になる保証がない。

そこで本実施形態では、推定位置を上記式でない別の方法で求めることについて説明する。本実施形態では、オフセット速度成分を加えた推定方程式から推定位置を求める。かかる推定方程式は、例えば以下の数式（３）で表される。

ここで＾ｕ_o，＾ｖ_o：それぞれ北向き、西向きオフセット速度成分の推定値、ｋ₁，ｋ₂：推定ゲイン、ｘ_g，ｙ_g：ＧＮＳＳのそれぞれ北向き、西向き位置成分、（ｘ_g−＾ｘ），（ｙ_g−＾ｙ）：それぞれ北向き、西向き位置誤差成分（緯度経度値でない）である。

数式（３）はｘ系［＾ｘ，＾ｕ_o］とｙ系［＾ｙ，＾ｖ_o］とが同形であるので、以下では、ｘ系に関してのみ説明する。ｘ系の特性多項式は、数式（３）をラプラス変換すると以下の数式（４）のようになる。

ここでｓ：ラプラス演算子である。

一方、期待する特性多項式は、以下の数式（５）と定める。

ここでω_e1，ω_e2：推定角周波数である。これより推定ゲインは数式（４）、数式（５）の両者の係数を等しくすると、以下の数式（６）になる。

ここで、ＣＯＧに含まれる動揺成分の影響について説明する。ＣＯＧ、ＳＯＧは外乱成分をもつ。ＣＯＧは特に船体の動揺成分を含む。その誤差が上述した第１機能による推定位置に与える影響を説明する。方位を以下の数式（７）とおく。

ここでψ_t：真方位、ψ_w：動揺成分。簡単化のためψ_t＝０及び、ψ_w＝ａ_wｓｉｎω_wｔ、（ここで、ａ_w：微小の振幅、ω_w：周波数、ｔ：時間）とし、これらを代入した上式を数式（１）に代入すると、以下の数式（８）になる。

ｖ_yを積分すると、以下の数式（９）になる。

ここでＣ：積分定数である。上式の係数は、以下の数式（１０）になる。

ここでｐｅｒｉｏｄ_w：動揺周期である。数値例として、ｕ_g＝２０ｋｎ、ａ_w＝２ｄｅｇ、ｐｅｒｉｏｄ_w＝１０ｓｅｃのとき係数は０．５７ｍになり、ｐｅｒｉｏｄ_w＝２０ｓｅｃのとき係数は１．１４ｍになる。よって係数はＧＮＳＳの高分解能０．００１ａｒｃ ｍｉｎｕｔｅ＝１ＮＭ÷１０００≒１．８５ｍ以下になるので、動揺成分による影響は実用上無視できることがわかる。

以上、本実施形態にかかる位置信号の推定（第１機能）について説明した。かかる方法によれば、ＧＮＳＳ信号のみを利用するので、単独で構成できる。また、推定位置を用いることにより、制御システムの外乱除去性への要求が低下し、その分閉ループ系の性能向上が期待できる。

（２）位置飛び修正（第２機能）
位置飛び修正は、「（１）位置信号の推定」で推定した推定位置を用いる。本実施形態では、推定位置は、位置飛びの発生期間が短いときＧＮＳＳ位置に追従せず、その発生期間が長いときＧＮＳＳ位置に追従するようにする。これを実現するため、ＧＮＳＳ位置から参照位置を生成し、参照位置に推定位置を追従させる。

ｘ_gの参照位置の生成アルゴリズムを図２に示す。ｙ_gの参照位置も同様であるので、説明を省略する。

図２に示したアルゴリズムについて説明する。まず関数と変数を定義する（図２の第３行〜第４行）。ｄｉｓｔＴｈｒ：しきい値、ｉＨｏｌｄ：保持回数、ｉ：カウント数、ｘ_r0：前回の参照位置、ｘ_r：参照位置であり、ｄｉｓｔＴｈｒ：しきい値は、以下の数式（１１）である。

ここで、￣Ｓｏｇ：フィルタしたＳＯＧ（時定数１０秒２段ローパスフィルタ）、ｓｔ：サンプル時間［ｓｅｃ］、ｓｃａＤｉｓｔＳｏｇ＝１．１：スケールファクタ、ＮＭ＝１８５２ｍ、ｒｅｓｏ：分解能で０．０１’なら１８．５２ｍ、０．００１’なら１．８５２ｍとなる。

そして、位置飛びを検出するために変数ｘｒＤｅｌ＝ｘｇ−ｘｒ０を定義する（図２の第６行）。そして、ｘｒＤｅｌの絶対値が、しきい値ｄｉｓｔＴｈｒ以下であれば、位置飛びを検出していないと判断し、ｘｒＤｅｌの絶対値が、しきい値ｄｉｓｔＴｈｒ以上であれば、位置飛びを検出したと判断する（図２の第８行〜第２３行）。

参照位置は、位置飛びを検出したら、前回の参照位置を保持回数だけ保持し、その後ＧＮＳＳ位置にしきい値ごとに追従する（図２の第１２行〜第２２行）。保持状態では（３）式の（ｘ_g−＾ｘ）＝０になるため、推定機能は働かない。なお、位置飛びはＧＮＳＳ位置と前回の参照位置との差異を検出する。

図２のアルゴリズムを用いた参照位置と推定位置の関係を図３に示す。ｘとｙとは同形なので、図３では、ｘに関してのみ説明する。

図３に示したように、ＧＮＳＳの北向き成分ｘ_gと、しきい値ｄｉｓｔＴｈｒ及び保持回数ｉＨｏｌｄが与えられて、参照位置ｘ_ｒが得られる。この参照位置ｘ_ｒに、北向き速度成分ｕ_ｘが与えられて、北向き推測位置成分＾ｘが得られる。

以下に、本実施形態の機能をシミュレーションによって検証する。図４は実船試験データからの方位、速度及び速度成分を示す。ＣＯＧは振幅約５度、周期約１０秒の動揺成分をもち、積分係数は（１０）式より約０．６ｍになる。ＧＮＳＳの分解能は０．０１’（約１８．５ｍ）なので、動揺外乱の影響は無視できる。

図５及び図６は位置飛びなしの場合で、推定位置の効果を示す。この場合、参照位置（reference position）はＧＮＳＳ位置（normal position）と一致する。図６は、図５のｘの部分を拡大したものである。

図６より、従来の推定位置である速度を積分した位置（velocity-integrated position）は時刻１５０秒の旋回後ＧＮＳＳ位置と誤差を生じるが、推定位置（estimated position）は誤差を生じなく追従している。その効果は推定速度オフセットがプラス側に対応して、位置誤差を収斂させる。ω_e1＝ω_e2＝０.０３ｒａｄ／ｓｅｃを用いる。上述した第１機能による推定位置における動揺成分の拡大は、速度積分位置と比較すると、実用上無視できる範囲内にある。

また位置誤差の平均値を（表１）に比較する。平均は０から３６０秒で求める。推定位置の誤差は速度積分した位置のそれと比べてω_e1＝ω_e2＝０.０３ｒａｄ／ｓｅｃのとき１０分１以下に減少する。ω_e1＝ω_e2＝０.０５ｒａｄ／ｓｅｃ の場合はω_e1＝ω_e2＝０.０３ｒａｄ／ｓｅｃより減少するが、位置飛びが加わったとき過渡現象が大きくなるため採用しない。

図７、図８は位置飛びありの場合で、位置飛び修正の効果を示す。図８は、図７のｘの部分を拡大したものである。位置飛びは、発生時刻２００秒（持続時間２０秒）、２５０秒（持続時間２秒）、３００秒（持続時間１秒）、大きさ１００ｍ（位置ｘ）、３００ｍ（位置ｙ）で発生する。ｄｉｓｔＴｈｒ＝９．３ｍ、ｉＨｏｌｄ＝２を用いる。

図８より、ＧＮＳＳ位置（Ａ）を用いた場合、推定位置は位置飛びに応答し過渡現象を生じる。一方、参照位置（Ｂ）を用いた場合、推定位置は位置飛びがｉＨｏｌｄ以下のとき応答せず、位置飛びがｉＨｏｌｄより大きいとき参照位置がランプ状態になるので、過渡現象を抑制する。

以上、本実施形態にかかる位置飛び修正（第２機能）について説明した。かかる方法によれば、位置飛び特性に応じた保持時間、追従速度に設定できるため、適用範囲がＧＮＳＳ環境で広く、また、外乱成分による過渡誤差を抑制することが可能である。

＜本実施形態の効果＞
以上説明したように、本実施形態によれば、外乱成分を抑制することができるとともに、外乱成分による過渡誤差を抑制することが可能である。本実施形態では、シミュレーションによってその効果を検証し、その有効性を確認することができた。

以上、添付図面を参照しながら本発明の好適な実施形態について説明したが、本発明はかかる例に限定されないことは言うまでもない。当業者であれば、特許請求の範囲に記載された範疇内において、各種の変更例又は修正例に想到し得ることは明らかであり、それらについても当然に本発明の技術的範囲に属するものと了解される。

例えば、上記実施形態では、オフセット速度成分を加えた推定方程式として、数式（３）を例に挙げて説明したが、本発明はこれに限定されない。オフセット速度成分を考慮した推定方程式であれば、どのような数式でもよく、パラメータや係数なども任意のものを採用することができる。

本発明は、全地球航法衛星システム（ＧＮＳＳ：Global Navigation Satellite System）用フィルタに利用可能である。

１０ 航路制御系
１２ 制御器
１４ 操舵機
１６ ＧＮＳＳフィルタ
２０ 船体
２２ 航海センサ類

Claims (1)

1. ＧＮＳＳ信号に基づく現在の測定位置と前回の参照位置との差の絶対値が所定の距離閾値以下である場合に前記現在の測定位置を参照位置とし、前記差の絶対値が前記距離閾値より大きい場合に前記現在の測定位置に位置飛びが発生したと判定して前記前回の参照位置に基づく参照位置を生成する位置飛び修正部と、
   前記参照位置に基づいて、オフセット速度成分を加えた推定方程式によって推定位置を算出する推定位置算出部とを備え、
   前記位置飛び修正部は、位置飛びの発生期間が所定の期間閾値以下である場合に前記前回の参照位置を参照位置とし、位置飛びの発生期間が前記期間閾値より大きい場合に前記距離閾値により前記前回の参照位置を修正して前記距離閾値単位で前記測定位置に追従する参照位置とし、
   前記推定位置算出部は、添字＾を推定値、＾ｘ，＾ｙをそれぞれ、前記参照位置の北向き位置成分に北向き速度成分を与えた北向き推定位置成分、前記参照位置の西向き位置成分に西向き速度成分を与えた西向き推定位置成分とし、＾ｕ_o，＾ｖ_oをそれぞれ北向き、西向きオフセット速度成分の推定値、ｕ_x，ｖ_yをそれぞれＧＮＳＳの速度及び方位に基づく北向き、西向き速度成分、ｋ₁，ｋ₂を推定ゲイン、ｘ_g，ｙ_gをＧＮＳＳのそれぞれ北向き、西向き位置成分、（ｘ_g−＾ｘ），（ｙ_g−＾ｙ）をそれぞれ北向き、西向き位置誤差成分として、
   

   

   の式で表される前記推定方程式によって前記推定位置を算出することを特徴とする、全地球航法衛星システム用フィルタ。

Images