JPWO2019003758A1

JPWO2019003758A1 - 航跡予測装置、航跡予測方法、および、航跡予測プログラム

Info

Publication number: JPWO2019003758A1
Application number: JP2019526709A
Authority: JP
Inventors: 中村　拓; 拓中村; 戸田　裕行; 裕行戸田; 奈緒美藤澤; 明大肥野
Original assignee: Furuno Electric Co Ltd
Current assignee: Furuno Electric Co Ltd
Priority date: 2017-06-30
Filing date: 2018-05-29
Publication date: 2020-04-09
Anticipated expiration: 2038-05-29
Also published as: CN110832342A; EP3647817A4; EP3647817A1; JP6808835B2; WO2019003758A1; US20200200539A1; US11255675B2

Abstract

【課題】予測位置を高精度に算出する。【解決手段】航跡予測装置１０は、角速度算出部３０、水平対地速度算出部７０、および、予測位置算出部８０を備える。角速度算出部３０は、移動体の角速度を計測または算出する。水平対地速度算出部７０は、移動体の姿勢角、対地進路、および、対地船速から水平対地速度を算出する。予測位置算出部８０は、現在時刻から予測先時刻までの時間と水平対地速度とを用い、角速度が回動検出閾値を超えていれば角速度の積分演算を用いて予測位置を算出する。【選択図】図１

Description

本発明は、移動体の移動位置を予測する航跡予測装置、航跡予測方法、および、航跡予測プログラムに関する。

従来、予測航跡を表示する機能を有する航法装置が各種考案され、実用化されている。従来の航法装置は、現在時刻から予測先時刻迄の時間に、現在の速度を乗じることで距離を算出する。従来の航法装置は、この算出した距離を現在位置に加算することで、予測位置を算出する。そして、従来の航法装置は、この予測位置の算出を順次繰り返すことによって、予測航跡を算出する。

このような航法装置として、特許文献１に記載の航法装置（船舶用表示装置）は、さらに、加速度の二重積分を用いて、予測位置を算出している。

特許第０４２８６３６号明細書

しかしながら、特許文献１に記載の航法装置では、時間の２乗の１／２乗値に、加速度を乗じた値を用いるため、加速度の観測誤差等によるノイズ成分が増大してしまう。これにより、予測位置および予測航跡の誤差が大きくなってしまう。

したがって、本発明の目的は、予測位置を高精度に算出する航跡予測装置、航跡予測方法、および、航跡予測プログラムを提供することにある。

この発明の航跡予測装置は、水平対地速度算出部、角速度算出部、および、予測位置算出部を備える。水平対地速度算出部は、移動体の姿勢角、対地進路、および、対地船速から水平対地速度を算出する。角速度算出部は、移動体の角速度を計測または算出する。予測位置算出部は、現在時刻から予測先時刻までの時間と水平対地速度とを用い、角速度が回動検出閾値を超えていれば角速度の積分演算を用いて予測位置を算出する。

この構成では、予測位置は、移動体が回転運動を行っている時には、取得された角速度の積分値を用いて算出される。

この発明によれば、予測位置を高精度に算出できる。

本発明の第１の実施形態に係る航跡予測装置のブロックである。本発明の第１の実施形態の予測位置の算出概念を説明するための図である。本実施形態の航跡予測装置と比較例の航跡予測装置との航跡予測結果をシミュレーションした図である。本実施形態の航跡予測装置と比較例の航跡予測装置との実航跡に対する予測航跡の標準偏差を示した図である。本発明の実施形態に係る航跡予測のフローチャートである。本発明の実施形態に係る予測位置の算出のフローチャートである。本発明の第２の実施形態に係る航跡予測装置のブロックである。

本発明の第１の実施形態に係る航跡予測装置、航跡予測方法、および、航跡予測プログラムについて、図を参照して説明する。なお、以下では、移動体として船舶を用いる態様を示すが、他の水上、水中移動体、陸上移動体、または、空中移動体にも、本願発明の構成は適用できる。

図１は、本発明の第１の実施形態に係る航跡予測装置のブロックである。図２は、本発明の第１の実施形態の予測位置の算出概念を説明するための図である。

図１に示すように、航跡予測装置１０は、現在位置算出部２０、角速度算出部３０、姿勢角算出部４０、対地進路算出部５０、対地船速算出部６０、水平対地速度算出部７０、および、予測位置算出部８０を備える。

現在位置算出部２０は、航跡予測装置１０が装備される移動体の現在位置Ｐを算出する。現在位置Ｐ（０）は、緯度成分Ｐｌａｔ（０）と経度成分Ｐｌｏｎ（０）とを有する。現在位置算出部２０は、例えば、測位信号のコード擬似距離等を用いて、ＥＣＥＦ直交座標系の現在位置Ｐｅｃ（０）を算出する。現在位置算出部２０は、船体に装着された少なくとも１個のアンテナで受信した測位信号を用いて、現在位置Ｐｅｃ（０）を算出する。現在位置算出部２０は、ＥＣＥＦ直交座標系とＥＮＵ座標系との座標変換行列を用いて、ＥＣＥＦ直交座標系の現在位置Ｐｅｃ（０）を、ＥＮＵ座標系の現在位置Ｐに変換し、現在位置Ｐの緯度成分Ｐｌａｔ（０）と経度成分Ｐｌｏｎ（０）とを算出する。現在位置算出部２０は、現在位置Ｐ（０）を、予測位置算出部８０に出力する。

角速度算出部３０は、船体の角速度ω（ｔ）を算出する。角速度算出部３０は、一例として、慣性センサであるジャイロセンサ等であり、角速度ω（ｔ）を計測して出力する。角速度算出部３０は、また別の一例として、測位信号の搬送波位相差を用いて角速度ωを算出する。この場合、角速度算出部３０は、移動体の異なる位置に配置された少なくとも２個のアンテナで受信した搬送波位相の差を用いて、角速度ω（ｔ）を算出する。角速度算出部３０は、角速度ω（ｔ）を、予測位置算出部８０に出力する。搬送波位相を用いることによって、簡素な構成で、高精度な角速度が得られる。

姿勢角算出部４０は、船体の姿勢角を算出する。姿勢角は、通常、ピッチ角φ（ｔ）、ロール角θ（ｔ）、および、ヨー角ψ（ｔ）で構成されている。姿勢角算出部４０は、少なくともヨー角ψ（ｔ）を算出する。姿勢角算出部４０は、角速度算出部３０と、慣性センサの計測値または搬送波位相差を用いて、少なくともヨー角ψ（ｔ）を含む姿勢角を算出する。なお、姿勢角算出部４０は、角速度算出部３０で算出された角速度からヨー角ψ（ｔ）を算出してもよい。搬送波位相を用いることによって、簡素な構成で、高精度な姿勢角が得られる。

対地進路算出部５０は、船体の姿勢角等を用いて、対地進路ＣＯＧ（ｔ）を算出する。対地進路算出部５０は、対地進路ＣＯＧ（ｔ）を、水平対地速度算出部７０に出力する。

対地船速算出部６０は、ドップラソナーの出力等を用いて、対地船速ＳＯＧ（ｔ）を算出する。対地船速算出部６０は、対地船速ＳＯＧ（ｔ）を、水平対地速度算出部７０に出力する。

水平対地速度算出部７０は、次に示す方法を用いて、船体座標系の水平対地速度Ｖｂ（ｔ）を算出する。水平対地速度Ｖｂ（ｔ）は、ベクトル量であり、ｘ方向成分Ｖｂｘ（ｔ）と、ｙ方向成分Ｖｂｙ（ｔ）とから構成されている。ｘ方向とは、図２に示すように船舶１００の船首方向に平行な成分であり、船尾から船首に向く方向を正方向とする。ｙ方向とは、船首方向（ｘ方向）に直交し、左舷から右舷に向く方向を正の方向とする。

図２に示すように、水平対地速度Ｖｂ（ｔ）のｘ方向成分Ｖｂｘ（ｔ）とｙ方向成分Ｖｂｙ（ｔ）とは、偏角β（ｔ）として、次式から得られる。

Ｖｂｘ（ｔ）＝ＳＯＧ（ｔ）・ｃｏｓβ（ｔ） −（式１）
Ｖｂｙ（ｔ）＝ＳＯＧ（ｔ）・ｓｉｎβ（ｔ） −（式２）

ここで、偏角βは、対地進路ＣＯＧとヨー角ψから次式で得られる。

β（ｔ）＝ＣＯＧ（ｔ）−ψ（ｔ） −（式３）

水平対地速度算出部７０は、次式を用いて、ＥＮＵ座標系の水平対地速度Ｖｎ（ｔ）を算出する。

図２に示すように、ＥＮＵ座標系（ＮＥＤ座標系）の水平対地速度Ｖｎ（ｔ）と船体座標系の水平対地速度Ｖｂ（ｔ）とは、ヨー角ψ（ｔ）を２個の座標系の成す角とする関係にある。したがって、ＥＮＵ座標系（ＮＥＤ座標系）の水平対地速度Ｖｎ（ｔ）のノース方向成分ＶｎＮ（ｔ）とイースト方向成分ＶｎＥ（ｔ）とは、船体座標系の水平対地速度Ｖｂ（ｔ）のｘ方向成分Ｖｂｘ（ｔ）とｙ方向成分Ｖｂｙ（ｔ）、および、ヨー角ψ（ｔ）から算出される。

ＶｎＮ（ｔ）＝Ｖｂｘ（ｔ）・ｃｏｓψ（ｔ）−Ｖｂｙ（ｔ）・ｓｉｎψ（ｔ）
−（式４）
ＶｎＥ（ｔ）＝Ｖｂｘ（ｔ）・ｓｉｎψ（ｔ）＋Ｖｂｙ（ｔ）・ｃｏｓψ（ｔ）
−（式５）

水平対地速度算出部７０は、ＥＮＵ座標系の水平対地速度Ｖｎ（ｔ）＝（ＶｎＮ（ｔ）、ＶｎＥ（ｔ））を、予測位置算出部８０に出力する。

予測位置算出部８０は、次の方法を用いて、予測時刻τの予測位置Ｐ（τ）を算出する。

時刻ｔにおいて、予測位置Ｐ（ｔ）＝（Ｐｌａｔ（ｔ）、Ｐｌｏｎ（ｔ））と、水平対地速度Ｖｎ（ｔ）＝（ＶｎＮ（ｔ）、ＶｎＥ（ｔ））との間には、以下の関係が成り立つ。

ｄＰｌａｔ（ｔ）／ｄｔ＝ＶｎＮ（ｔ） −（式６）
ｄＰｌｏｎ（ｔ）／ｄｔ＝ＶｎＥ（ｔ） −（式７）

また、予測時刻のヨー角ψ（τ）は、初期時刻のヨー角ψ（０）と、初期時刻ｔ＝０から予測時刻ｔ＝τまでの角速度ω（ｔ）の積分演算とを用いて得られる。船舶１００が定常旋回すると想定すると、初期時刻ｔ＝０から予測時刻ｔ＝τまで、ω（ｔ）は一定値ωとなる。したがって、予測時刻のヨー角ψ（τ）は次式で得られる。

ψ（τ）＝ψ（０）＋ω・τ −（式８）

また、予測位置算出部８０は、角速度ωから、船舶１００が並進運動中か回転運動中かを判断する。予測位置算出部８０は、それぞれの場合に応じて、Ｐ（τ）＝（Ｐｌａｔ（τ）、Ｐｌｏｎ（τ））を算出する。

予測位置算出部８０は、回転運動中、すなわち、ヨー角ψ（τ）が、回動検出閾値を超えていれば、予測位置（式６）、（式７）の積分演算を用いて、予測位置Ｐ（τ）＝（Ｐｌａｔ（τ）、Ｐｌｏｎ（τ））を算出する。

Ｐｌａｔ（τ）＝Ｐｌａｔ（０）＋（積分演算（ＶｎＮ））
＝Ｐｌａｔ（０）＋（ＶｎＮ（０）・ｓｉｎωτ）／ω−（ＶｎＥ（０）・（１−ｃｏｓωτ））／ω −（式９）
Ｐｌｏｎ（τ）＝Ｐｌｏｎ（０）＋（積分演算（ＶｎＥ））
＝Ｐｌｏｎ（０）＋（ＶｎＮ（０）・（１−ｃｏｓωτ））／ω−（ＶｎＥ（０）・ｓｉｎωτ）／ω −（式１０）

すなわち、予測位置算出部８０は、船舶１００が回転運動中であれば、回頭方向の加速度である角速度の積分値を含む演算を用いて、予測位置Ｐ（τ）を算出する。

一方、予測位置算出部８０は、並進運動中、すなわち、ヨー角ψ（τ）が、回動検出閾値以下であれば、加速度（角速度）を用いない次の演算を用いて、予測位置Ｐ（τ）を算出する。

Ｐｌａｔ（τ）＝Ｐｌａｔ（０）＋ＶｎＮ（０）・τ −（式１１）
Ｐｌｏｎ（τ）＝Ｐｌｏｎ（０）＋ＶｎＥ（０）・τ −（式１２）

予測位置算出部８０は、算出した予測位置Ｐ（τ）＝（Ｐｌａｔ（τ）、Ｐｌｏｎ（τ））を出力する。また、予測位置算出部８０は、この予測位置Ｐ（τ）の算出を、継続的に実行していき、複数の時刻τでの予測位置Ｐ（τ）を算出する。そして、予測位置算出部８０は、これら複数の時刻τでの予測位置Ｐ（τ）を繋ぐことで、予測航跡を得ることができる。

このような構成および処理を用いることによって、航跡予測装置１０は、船舶１００の挙動に応じて、予測位置Ｐ（τ）の算出への不必要な加速度項の利用を抑制できる。これにより、当該加速度項を用いることによる誤差の増加は抑制される。したがって、航跡予測装置１０は、予測位置Ｐ（τ）を高精度に算出できる。さらに、速度センサ、角速度センサ等の慣性センサの船体での位置情報（重心位置からの船体座標）があれば船体の姿勢角から船体の重心位置に補正し、予測位置Ｐ（τ）をより高精度に算出できる。

図３は、本実施形態の航跡予測装置と比較例の航跡予測装置との航跡予測結果をシミュレーションした図である。図３（Ａ）は、本願の構成を用いた予測航跡を示し、図３（Ｂ）は、速度と速度の変化量とを常時用いた予測航跡を示し、図３（Ｃ）は、速度のみを用いた予測航跡を示し、図３（Ｄ）は、速度と加速度を常時用いた予測航跡を示す。図３（Ａ）、図３（Ｂ）、図３（Ｃ）、図３（Ｄ）において、点線は実航跡を示し、実線は予測航跡を示す。

図３（Ａ）、図３（Ｂ）、図３（Ｃ）、図３（Ｄ）に示すように、本実施形態に航跡予測装置１０の構成を用いることで、実航跡と予測航跡との差を小さくできる。

図４は、本実施形態の航跡予測装置と比較例の航跡予測装置との実航跡に対する予測航跡の標準偏差を示した図である。図４（Ａ）は、緯度における予測航跡の標準偏差を示し、図４（Ｂ）は、経度における予測航跡の標準偏差を示す。図４（Ａ）、図４（Ｂ）において、実線は、本願の予測航跡の標準偏差であり、点線は、速度のみを用いた予測航跡の標準偏差であり、一点鎖線は、速度と速度の変化量とを常時用いた予測航跡の標準偏差であり、二点鎖線は、速度と加速度を常時用いた予測航跡の標準偏差である。

図４（Ａ）、図４（Ｂ）、図４（Ｃ）、図４（Ｄ）に示すように、本実施形態に航跡予測装置１０の構成を用いることで、予測航跡の標準偏差を小さくできる。すなわち、実航跡に対する予測航跡の誤差を小さくできる。

なお、上述の説明では、航跡予測装置１０で実行する処理を複数の機能部によって実現する態様を示した。しかしながら、上述の複数の処理をプログラム化して記憶媒体に記憶しておき、コンピュータ等の演算処理器で、当該プログラムを読み出して実行してもよい。この場合、演算処理器は、図５、図６に示すフローチャートに応じた処理を実行すればよい。

図５は、本発明の実施形態に係る航跡予測のフローチャートである。図６は、本発明の実施形態に係る予測位置の算出のフローチャートである。なお、各処理の具体的な実現方法は、上述の各機能部の処理を同じであるので、説明は省略する。

図５に示すように、演算処理器は、現在位置Ｐ（０）を算出する（Ｓ１１）。演算処理器は、角速度ω（ｔ）を算出する。演算処理器は、姿勢角のヨー角ψ（ｔ）、対地進路ＣＯＧ（ｔ）、および、対地船速ＳＯＧ（ｔ）を用いて、水平対地速度Ｖｎを算出する（Ｓ１３）。演算処理器は、現在位置Ｐ（０）、角速度ω、および、予測時刻τの水平対地速度Ｖｎ（τ）を用いて、予測位置Ｐ（τ）を算出する。

より具体的には、図６に示すように、演算処理器は、角速度ωが略０である、すなわち、回動検出閾値以下であることを検出すると（Ｓ４１：ＹＥＳ）、加速度を用いずに、予測位置Ｐ（τ）を算出する（Ｓ４２）。演算処理器は、角速度ωが０と大きく異なっている、すなわち、回動検出閾値を超えていることを検出すると（Ｓ４１：ＮＯ）、回頭方向の加速度である角速度を利用した積分演算で、予測位置Ｐ（τ）を算出する（Ｓ４３）。

次に、第２の実施形態に係る航跡予測装置、航跡予測方法、および、航跡予測プログラムについて、図を参照して説明する。図７は、本発明の第２の実施形態に係る航跡予測装置のブロックである。

図７に示すように、本発明の第２の実施形態に係る航跡予測装置１０Ａは、第１の実施形態に係る航跡予測装置１０に対して、表示器９０を追加した点において異なる。航跡予測装置１０Ａの他の構成は、航跡予測装置１０と同様であり、同様の箇所の説明は省略する。

予測位置算出部８０は、算出した予測位置を表示器９０に出力する。表示器９０は、この予測位置を表示する。また、予測位置算出部８０は、予測航跡を算出した場合、当該予測航跡を表示器９０に出力する。表示器９０は、予測航跡を表示する。

このような構成とすることによって，予測位置や予測航跡を、オペレータが容易に視認できる。

なお、上述の航跡予測装置１０、１０Ａに対して、姿勢角算出部４０、対地進路算出部５０、および、対地船速算出部６０は、別体であってもよい。さらに、航跡予測装置１０、１０Ａに対して、現在位置算出部２０、および、角速度算出部３０は、別体であってもよい。

１０、１０Ａ：航跡予測装置
２０：現在位置算出部
３０：角速度算出部
４０：姿勢角算出部
５０：対地進路算出部
６０：対地船速算出部
７０：水平対地速度算出部
８０：予測位置算出部
９０：表示器
１００：船舶

Claims

移動体の姿勢角、対地進路、および、対地船速から水平対地速度を算出する水平対地速度算出部と、
前記移動体の角速度を計測または算出する角速度算出部と、
現在時刻から予測時刻までの時間と前記水平対地速度とを用い、前記角速度が前記回動検出閾値を超えていれば前記角速度の積分演算を用いて予測位置を算出する予測位置算出部と、
を備える、航跡予測装置。
請求項１に記載の航跡予測装置であって、
前記予測位置算出部は、
前記角速度が回動検出閾値以下であれば前記角速度を用いずに予測位置を算出する、
航跡予測装置。
請求項１または請求項２に記載の航跡予測装置であって、
測位信号の搬送波位相を用いて、前記姿勢角を算出する姿勢角算出部を備える、航跡予測装置。
請求項１乃至請求項３のいずれかに記載の航跡予測装置であって、
測位信号の搬送波位相を用いて、前記対地進路を算出する対地進路算出部を備える、航跡予測装置。
請求項３に記載の航跡予測装置であって、
前記姿勢角算出部は、前記姿勢角における少なくともヨー角を算出する、
航跡予測装置。
請求項１乃至請求項５のいずれかに記載の航跡予測装置であって、
前記角速度算出部は、
測位信号の搬送波位相または慣性センサの出力を用いて前記角速度を算出する、
航跡予測装置。
請求項１乃至請求項６のいずれかに記載の航跡予測装置であって、
前記予測位置算出部は、複数の前記予測時刻に対する予測位置を算出して、これらを繋ぐ予測航跡を算出する、
航跡予測装置。
請求項７に記載の航跡予測装置であって、
前記予測位置と前記予測航跡とを表示する表示部を備える、
航跡予測装置。
移動体の姿勢角、対地進路、および、対地船速から水平対地速度を算出し、
前記移動体の角速度を計測または算出し、
現在時刻から予測時刻までの時間と前記水平対地速度とを用い、前記角速度が前記回動検出閾値を超えていれば前記角速度の積分演算を用いて予測位置を算出する、
航跡予測方法。
請求項９に記載の航跡予測方法であって、
前記予測位置を算出するときに、前記角速度が回動検出閾値以下であれば前記角速度を用いずに前記予測位置を算出する、
航跡予測方法。
請求項９または請求項１０に記載の航跡予測方法であって、
測位信号の搬送波位相を用いて、前記姿勢角を算出する、
航跡予測方法。
請求項９乃至請求項１１のいずれかに記載の航跡予測方法であって、
測位信号の搬送波位相を用いて、前記対地進路を算出する、
航跡予測方法。
請求項１１に記載の航跡予測方法であって、
前記姿勢角を算出する際に、前記姿勢角における少なくともヨー角を算出する、
航跡予測方法。
請求項９乃至請求項１３のいずれかに記載の航跡予測方法であって、
前記角速度を算出する際に、
測位信号の搬送波位相または慣性センサの出力を用いて前記角速度を算出する、
航跡予測方法。
移動体の姿勢角、対地進路、および、対地船速から水平対地速度を算出し、
前記移動体の角速度を計測または算出し、
現在時刻から予測時刻までの時間と前記水平対地速度とを用い、前記角速度が前記回動検出閾値を超えていれば前記角速度の積分演算を用いて前記予測位置を算出する、
処理を、演算処理器に実行させる、航跡予測プログラム。
請求項１５に記載の航跡予測プログラムであって、
前記予測位置を算出するときに、前記角速度が回動検出閾値以下であれば前記角速度を用いずに前記予測位置を算出する、
処理を、前記演算処理器に実行させる、航跡予測プログラム。
請求項１５または請求項１６に記載の航跡予測プログラムであって、
測位信号の搬送波位相を用いて、前記姿勢角を算出する、
処理を、前記演算処理器に実行させる、航跡予測プログラム。
請求項１５乃至請求項１７のいずれかに記載の航跡予測プログラムであって、
測位信号の搬送波位相を用いて、前記対地進路を算出する、
処理を、前記演算処理器に実行させる、航跡予測プログラム。
請求項１７に記載の航跡予測プログラムであって、
前記姿勢角を算出する際に、前記姿勢角における少なくともヨー角を算出する、
処理を、前記演算処理器に実行させる、航跡予測プログラム。
請求項１５乃至請求項１９のいずれかに記載の航跡予測プログラムであって、
前記角速度を算出する際に、
測位信号の搬送波位相または慣性センサの出力を用いて前記角速度を算出する、
処理を、前記演算処理器に実行させる、航跡予測プログラム。