(57)【要約】【課題】外乱成分を抑制することができるとともに、外乱成分による過渡誤差を抑制することの可能な全地球航法衛星システム用フィルタを提供する。【解決手段】ＧＮＳＳ信号から、オフセット速度成分を加えた推定方程式によって推定位置を算出する推定位置算出部と、推定位置の位置飛びの発生期間に応じて、推定位置を、ＧＮＳＳ信号から生成される参照位置に追従させる位置飛び修正部と、を備え、位置飛び修正部は、推定位置の位置飛びの発生期間が所定のしきい値よりも短いときには推定位置を参照位置に追従させず、推定位置の位置飛びの発生期間が所定のしきい値よりも長いときには推定位置を参照位置に追従させる。【選択図】図１
2015094626.tif 000002
【特許請求の範囲】
【請求項１】
ＧＮＳＳ信号から、オフセット速度成分を加えた推定方程式によって推定位置を算出する推定位置算出部と、
前記推定位置の位置飛びの発生期間に応じて、前記推定位置を、前記ＧＮＳＳ信号から生成される参照位置に追従させる位置飛び修正部と、
を備え、
前記位置飛び修正部は、
前記推定位置の位置飛びの発生期間が所定のしきい値よりも短いときには前記推定位置を前記参照位置に追従させず、前記推定位置の位置飛びの発生期間が所定のしきい値よりも長いときには前記推定位置を前記参照位置に追従させることを特徴とする、全地球航法衛星システム用フィルタ。
【請求項２】
前記推定方程式は、以下の式で表されることを特徴とする、請求項１に記載の全地球航法衛星システム用フィルタ。
【数１】
2015094626.tif 000016
ここで、＾：推定値、＾ｘ，＾ｙ：それぞれ北向き、西向き推定位置成分であり、＾ｕo，＾ｖo：それぞれ北向き、西向きオフセット速度成分の推定値であり、ｕx，ｖy：それぞれ北向き、西向き速度成分であり、ｋ1，ｋ2：推定ゲインであり、ｘg，ｙg：ＧＮＳＳのそれぞれ北向き、西向き位置成分であり、（ｘg−＾ｘ），（ｙg−＾ｙ）：それぞれ北向き、西向き位置誤差成分である。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、全地球航法衛星システム（ＧＮＳＳ：Global Navigation Satellite System）用フィルタに関する。
【背景技術】
【０００２】
船体制御において、ＧＮＳＳは自船位置を知るため、必須センサになっている。特に航路制御や位置制御では、位置信号を参照するため、その不具合は制御性能に影響を与える。ＧＮＳＳの不具合の要因は、偶然的なものと必然的なものに分けられる。偶発的なものは、ＧＮＳＳ自体の問題から生じ、信号を利用できない状態である。また、必然的なものは、不具合の発生を検出でき、信号を利用できる状態である。以下では、特に、後者を対象とし、制御性能に与える影響を低減することについて説明する。
【０００３】
ＧＮＳＳの位置信号は、常時バラツキ、分解能（量子化）によるランダム性外乱成分をもつが、長期的には位置信号の平均値が真値付近にあるとみなせる。また、ＧＮＳＳを利用した制御システムは、通常、入力信号に含まれる外乱成分をフィルタする機能をもつ。しかしながら、外乱の抑制効果を制御システムに初めから依存させることは適切でない。システム設計はプラントモデリング、航海センサ及び制御システムを統合して捉えた上で、各々を設計することが重要である。すなわちＧＮＳＳの外乱成分は、制御システムに入力する前に抑制すべきである。制御システムに入力する前に外乱成分を除去した上で、制御システムは、閉ループ安定性及び外乱除去性を設計することにより、制御性能の向上が期待できる。よって、ＧＮＳＳからの信号のみを用いて、位置信号に含まれる外乱成分を低減することが望まれる。
【０００４】
ＧＮＳＳの外乱成分は、位置飛び（ＰＪ：Position Jump）や量子化誤差が知られている。位置飛びはマルチバスの影響、受信衛星数の変化、ＧＮＳＳから信号処理演算装置への受信変化あるいはその逆などの場合に生じ、短い発生時間及び飛びの大きさにランダム性をもつ。しかしステップ状の位置飛びが長期間持続すると、外乱でなく信号として扱われてしまう。その対策としては、例えば特許文献１を参照すると、更新時間ごとに位置変化の絶対値がしきい値を越えた場合、位置出力を推定位置に置換することが行われていた。推定位置は速度と方位から求めたもので、段階的に位置信号に追従させていた。一方、量子化誤差は、例えば、分解能０．０１ａｒｃ  ｍｉｎｕｔｅであれば、１ＮＭ÷１００≒１８．５ｍ以下の位置変化を出力できないことによる外乱成分である。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１３−１２２４０６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
以上のことから、つぎの課題が挙げられる。
（１）現在の位置はＧＮＳＳからの位置を利用するため、ＧＮＳＳのランダム誤差を含み、位置飛びの誤検出を含む。
（２）位置飛び変化を分割して追従させる対策や量子化誤差は制御システムにステップ入力として作用するため、絶えず過渡現象を誘起させる。
【０００７】
したがって本発明は、上記課題に鑑みてなされたものであり、本発明の目的は、外乱成分を抑制することができるとともに、外乱成分による過渡誤差を抑制することの可能な、全地球航法衛星システム用フィルタを提供することである。
【課題を解決するための手段】
【０００８】
かかる目的を達成するために、本発明は２つの機能を有する。第１機能はＧＮＳＳ信号から真値の位置信号を推定する機能であり、第２機能は位置飛びに滑らかに追従する参照位置信号を生成する機能である。
【０００９】
すなわち、本発明の全地球航法衛星システム用フィルタは、ＧＮＳＳ信号から、オフセット速度成分を加えた推定方程式によって推定位置を算出する推定位置算出部（第１機能）と、前記推定位置の位置飛びの発生期間に応じて、前記推定位置を、前記ＧＮＳＳ信号から生成される参照位置に追従させる位置飛び修正部（第２機能）とを備える。そして、前記位置飛び修正部は、前記推定位置の位置飛びの発生期間が所定のしきい値よりも短いときには前記推定位置を前記参照位置に追従させず、前記推定位置の位置飛びの発生期間が所定のしきい値よりも長いときには前記推定位置を前記参照位置に追従させることを特徴とする。
【００１０】
前記推定方程式としては、以下の式で表される方程式とすることが可能である。ここで、＾：推定値、＾ｘ，＾ｙ：それぞれ北向き、西向き推定位置成分であり、＾ｕo，＾ｖo：それぞれ北向き、西向きオフセット速度成分の推定値であり、ｕx，ｖy：それぞれ北向き、西向き速度成分であり、ｋ1，ｋ2：推定ゲインであり、ｘg，ｙg：ＧＮＳＳのそれぞれ北向き、西向き位置成分であり、（ｘg−＾ｘ），（ｙg−＾ｙ）：それぞれ北向き、西向き位置誤差成分である。
【００１１】
【数１】
2015094626.tif 000003
【００１２】
かかる２つの機能を有する本発明によれば、まず第１機能により、ＧＮＳＳ信号のみを利用するので、単独で構成できる。また、推定位置を用いることにより、制御システムの外乱除去性への要求が低下し、その分閉ループ系の性能向上が期待できる。このようにして、外乱成分を抑制することができる。さらに第２機能により、位置飛びの特性に応じた保持時間、追従速度に設定できるため、適用範囲がＧＮＳＳ環境で広く、また、外乱成分による過渡誤差を抑制することが可能である。
【発明の効果】
【００１３】
本発明によれば、以下のような効果が得られる。
（１）ＧＮＳＳ信号のみを利用するので、単独で構成できる。
（２）推定位置を用いることにより、制御システムの外乱除去性への要求が低下し、その分閉ループ系の性能向上が期待できる。
（３）位置飛びの特性に応じた保持時間、追従速度に設定できるため、適用範囲がＧＮＳＳ環境で広い。
【図面の簡単な説明】
【００１４】
【図１】ＧＮＳＳフィルタが設置される航路制御系の一例を示すブロック図であり、（ａ）は航路制御系１０を示し、（ｂ）は（ａ）の制御器１２の内部に配置されたＧＮＳＳフィルタ１６を示す。
【図２】参照位置の生成アルゴリズムを示す図である。
【図３】ＧＮＳＳフィルタにおける参照位置と推定位置との関係を示すブロック図である。
【図４】実船試験データからの方位、速度及び速度成分を示す図である。
【図５】位置飛びなしの場合で、推定位置の効果を示す図である。
【図６】図５のｘの部分を拡大した図である。
【図７】位置飛びありの場合で、推定位置の効果を示す図である。
【図８】図７のｘの部分を拡大した図である。
【発明を実施するための形態】
【００１５】
以下に添付図面を参照しながら、本発明の好適な実施形態について詳細に説明する。
【００１６】
（１）位置信号の推定（第１機能）
本実施形態にかかるＧＮＳＳフィルタ１６は、図１（ａ）に例示する航路制御系１０において、制御器１２の内部に配置される。航路制御系１０は、大まかに、計画方位・計画航路が入力される制御器１２と、制御部１２からの指令舵角が入力される操舵機１４と、操舵機１４の舵角に従う船体２０と、ＧＮＳＳを含み、船体２０の変化を検出して速度、方位、位置の信号を制御器１２に入力する航海センサ類２２と、を備える。
【００１７】
ＧＮＳＳフィルタ１６は、図１（ｂ）に示したように、ＧＮＳＳ信号から確からしい位置信号を推定する。位置信号は検出時外乱成分を含むが、その長期間の平均値はほぼ真値と扱える。ＧＮＳＳ信号はＮＥＤ座標（Ｘ軸を北方向、Ｙ軸を西方向、Ｚ軸を重力方向にとる局地水平座標）において、緯度経度の位置、対地の方位ＣＯＧ（Course Over Ground）と速度ＳＯＧ（speed Over Ground）をもつ。速度成分はＣＯＧ、ＳＯＧから、以下の数式（１）のようになる。
【００１８】
【数２】
2015094626.tif 000004
【００１９】
ここで、ｕx，ｖy：それぞれ北向き、西向き速度成分、ｕg，ψg：ＧＮＳＳのそれぞれ速度、方位である。
【００２０】
推定位置は、従来、以下の数式（２）から求めていた。
【００２１】
【数３】
2015094626.tif 000005
【００２２】
ここで＾：推定値、＾ｘ，＾ｙ：それぞれ北向き、西向き推定位置成分である。しかしながらＧＮＳＳで得られる速度と方位、及び位置はそれぞれ異なる方法によって計算される。このため、上記従来の方法では、ＧＮＳＳの位置と上記式を積分した位置とは等価になる保証がない。
【００２３】
そこで本実施形態では、推定位置を上記式でない別の方法で求めることについて説明する。本実施形態では、オフセット速度成分を加えた推定方程式から推定位置を求める。かかる推定方程式は、例えば以下の数式（３）で表される。
【００２４】
【数４】
2015094626.tif 000006
【００２５】
ここで＾ｕo，＾ｖo：それぞれ北向き、西向きオフセット速度成分の推定値、ｋ1，ｋ2：推定ゲイン、ｘg，ｙg：ＧＮＳＳのそれぞれ北向き、西向き位置成分、（ｘg−＾ｘ），（ｙg−＾ｙ）：それぞれ北向き、西向き位置誤差成分（緯度経度値でない）である。
【００２６】
数式（３）はｘ系［＾ｘ，＾ｕo］とｙ系［＾ｙ，＾ｖo］とが同形であるので、以下では、ｘ系に関してのみ説明する。ｘ系の特性多項式は、以下の数式（４）のようになる。
【００２７】
【数５】
2015094626.tif 000007
【００２８】
ここでｓ：ラプラス演算子である。
【００２９】
一方、期待する特性多項式は、以下の数式（５）と定める。
【００３０】
【数６】
2015094626.tif 000008
【００３１】
ここでωe1，ωe2：推定角周波数である。これより推定ゲインは数式（４）、数式（５）の両者の係数を等しくすると、以下の数式（６）になる。
【００３２】
【数７】
2015094626.tif 000009
【００３３】
ここで、ＣＯＧに含まれる動揺成分の影響について説明する。ＣＯＧ、ＳＯＧは外乱成分をもつ。ＣＯＧは特に船体の動揺成分を含む。その誤差が推定位置に与える影響を説明する。方位を以下の数式（７）とおく。
【００３４】
【数８】
2015094626.tif 000010
【００３５】
ここでψt：真方位、ψw：動揺成分。簡単化のためψt＝０及び、ψw＝ａwｓｉｎωwｔ、（ここで、ａw：微小の振幅、ωw：周波数、ｔ：時間）とし、これらを代入した上式を数式（１）に代入すると、以下の数式（８）になる。
【００３６】
【数９】
2015094626.tif 000011
【００３７】
ｖyを積分すると、以下の数式（９）になる。
【００３８】
【数１０】
2015094626.tif 000012
【００３９】
ここでＣ：積分定数である。上式の係数は、以下の数式（１０）になる。
【００４０】
【数１１】
2015094626.tif 000013
【００４１】
ここでｐｅｒｉｏｄw：動揺周期である。数値例として、ｕg＝２０ｋｎ、ａw＝２ｄｅｇ、ｐｅｒｉｏｄw＝１０ｓｅｃのとき係数は０．５７ｍになり、ｐｅｒｉｏｄw＝２０ｓｅｃのとき係数は１．１４ｍになる。よって係数はＧＮＳＳの高分解能０．００１ａｒｃ  ｍｉｎｕｔｅ＝１ＮＭ÷１０００≒１．８５ｍ以下になるので、動揺成分による影響は実用上無視できることがわかる。
【００４２】
以上、本実施形態にかかる位置信号の推定（第１機能）について説明した。かかる方法によれば、ＧＮＳＳ信号のみを利用するので、単独で構成できる。また、推定位置を用いることにより、制御システムの外乱除去性への要求が低下し、その分閉ループ系の性能向上が期待できる。
【００４３】
（２）位置飛び修正（第２機能）
位置飛び修正は、「（１）位置信号の推定」で推定した推定位置を用いる。本実施形態では、推定位置は、位置飛びの発生期間が短いときＧＮＳＳ位置に追従せず、その発生期間が長いときＧＮＳＳ位置に追従するようにする。これを実現するため、ＧＮＳＳ位置から参照位置を生成し、参照位置に推定位置を追従させる。
【００４４】
ｘgの参照位置の生成アルゴリズムを図２に示す。ｙgの参照位置も同様であるので、説明を省略する。
【００４５】
図２に示したアルゴリズムについて説明する。まず関数と変数を定義する（図２の第３行〜第４行）。ｄｉｓｔＴｈｒ：しきい値、ｉＨｏｌｄ：保持回数、ｉ：カウント数、ｘr0：前回の参照位置、ｘr：参照位置であり、ｄｉｓｔＴｈｒ：しきい値は、以下の数式（１１）である。
【００４６】
【数１２】
2015094626.tif 000014
【００４７】
ここで、￣Ｓｏｇ：フィルタしたＳＯＧ（時定数１０秒２段ローパスフィルタ）、ｓｔ：サンプル時間［ｓｅｃ］、ｓｃａＤｉｓｔＳｏｇ＝１．１：スケールファクタ、ＮＭ＝１８５２ｍ、ｒｅｓｏ：分解能で０．０１’なら１８．５２ｍ、０．００１’なら１．８５２ｍとなる。
【００４８】
そして、位置飛びを検出するために変数ｘｒＤｅｌ＝ｘｇ−ｘｒ０を定義する（図２の第６行）。そして、ｘｒＤｅｌの絶対値が、しきい値ｄｉｓｔＴｈｒ以下であれば、位置飛びを検出していないと判断し、ｘｒＤｅｌの絶対値が、しきい値ｄｉｓｔＴｈｒ以上であれば、位置飛びを検出したと判断する（図２の第８行〜第２３行）。
【００４９】
参照位置は、位置飛びを検出したら、前回の参照位置を保持回数だけ保持し、その後ＧＮＳＳ位置にしきい値ごとに追従する（図２の第１２行〜第２２行）。保持状態では（３）式の（ｘg−＾ｘ）＝０になるため、推定機能は働かない。なお、位置飛びはＧＮＳＳ位置と前回の参照位置との差異を検出する。
【００５０】
図２のアルゴリズムを用いた参照位置と推定位置の関係を図３に示す。ｘとｙとは同形なので、図３では、ｘに関してのみ説明する。
【００５１】
図３に示したように、ＧＮＳＳの北向き成分ｘgと、しきい値ｄｉｓｔＴｈｒ及び保持回数ｉＨｏｌｄが与えられて、参照位置ｘｒが得られる。この参照位置ｘｒに、北向き速度成分ｕｘが与えられて、北向き推測位置成分＾ｘが得られる。
【００５２】
以下に、本実施形態の機能をシミュレーションによって検証する。図４は実船試験データからの方位、速度及び速度成分を示す。ＣＯＧは振幅約５度、周期約１０秒の動揺成分をもち、積分係数は（１０）式より約０．６ｍになる。ＧＮＳＳの分解能は０．０１’（約１８．５ｍ）なので、動揺外乱の影響は無視できる。
【００５３】
図５及び図６は位置飛びなしの場合で、推定位置の効果を示す。この場合、参照位置（reference position）はＧＮＳＳ位置（normal position）と一致する。図６は、図５のｘの部分を拡大したものである。
【００５４】
図６より、速度を積分した位置（velocity-integrated position）は時刻１５０秒の旋回後ＧＮＳＳ位置と誤差を生じるが、推定位置（estimated position）は誤差を生じなく追従している。その効果は推定速度オフセットがプラス側に対応して、位置誤差を収斂させる。ωe1＝ωe2＝０.０３ｒａｄ／ｓｅｃを用いる。推定位置における動揺成分の拡大は、速度積分位置と比較すると、実用上無視できる範囲内にある。
【００５５】
また位置誤差の平均値を（表１）に比較する。平均は０から３６０秒で求める。推定位置の誤差は速度積分した位置のそれと比べてωe1＝ωe2＝０.０３ｒａｄ／ｓｅｃのとき１０分１以下に減少する。ωe1＝ωe2＝０.０５ｒａｄ／ｓｅｃ  の場合はωe1＝ωe2＝０.０３ｒａｄ／ｓｅｃより減少するが、位置飛びが加わったとき過渡現象が大きくなるため採用しない。
【００５６】
【表１】
2015094626.tif 000015
【００５７】
図７、図８は位置飛びありの場合で、位置飛び修正の効果を示す。図８は、図７のｘの部分を拡大したものである。位置飛びは、発生時刻２００秒（持続時間２０秒）、２５０秒（持続時間２秒）、３００秒（持続時間１秒）、大きさ１００ｍ（位置ｘ）、３００ｍ（位置ｙ）で発生する。ｄｉｓｔＴｈｒ＝９．３ｍ、ｉＨｏｌｄ＝２を用いる。
【００５８】
図８より、ＧＮＳＳ位置（Ａ）を用いた場合、推定位置は位置飛びに応答し過渡現象を生じる。一方、参照位置（Ｂ）を用いた場合、推定位置は位置飛びがｉＨｏｌｄ以下のとき応答せず、位置飛びがｉＨｏｌｄより大きいとき参照位置がランプ状態になるので、過渡現象を抑制する。
【００５９】
以上、本実施形態にかかる位置飛び修正（第２機能）について説明した。かかる方法によれば、位置飛び特性に応じた保持時間、追従速度に設定できるため、適用範囲がＧＮＳＳ環境で広く、また、外乱成分による過渡誤差を抑制することが可能である。
【００６０】
＜本実施形態の効果＞
以上説明したように、本実施形態によれば、外乱成分を抑制することができるとともに、外乱成分による過渡誤差を抑制することが可能である。本実施形態では、シミュレーションによってその効果を検証し、その有効性を確認することができた。
【００６１】
以上、添付図面を参照しながら本発明の好適な実施形態について説明したが、本発明はかかる例に限定されないことは言うまでもない。当業者であれば、特許請求の範囲に記載された範疇内において、各種の変更例又は修正例に想到し得ることは明らかであり、それらについても当然に本発明の技術的範囲に属するものと了解される。
【００６２】
例えば、上記実施形態では、オフセット速度成分を加えた推定方程式として、数式（３）を例に挙げて説明したが、本発明はこれに限定されない。オフセット速度成分を考慮した推定方程式であれば、どのような数式でもよく、パラメータや係数なども任意のものを採用することができる。
【産業上の利用可能性】
【００６３】
本発明は、全地球航法衛星システム（ＧＮＳＳ：Global Navigation Satellite System）用フィルタに利用可能である。
【符号の説明】
【００６４】
１０    航路制御系
１２    制御器
１４    操舵機
１６    ＧＮＳＳフィルタ
２０    船体
２２    航海センサ類
【図１】
2015094626.tif 000017
【図２】
2015094626.tif 000018
【図３】
2015094626.tif 000019
【図４】
2015094626.tif 000020
【図５】
2015094626.tif 000021
【図６】
2015094626.tif 000022
【図７】
2015094626.tif 000023
【図８】
2015094626.tif 000024