JP2022073731A - 追尾制御装置 - Google Patents

Abstract

【課題】 自船を対象船に追尾させる技術を提供する。【解決手段】 自船を対象船に追尾させる船舶追尾装置１であって、取得された対象船の船位に基づく対象船の速度運動モデルに基づいて対象船の運動を推定する運動推定部１１と、推定された対象船の運動に基づいて、対象船の船位と自船の船位との離間距離を最小化する自船の軌道計画を算出する軌道計画部１２と、軌道計画に自船を追従させるように自船を制御する追尾制御部１３とを備えた。【選択図】図２

Description

本発明は、対象船を追尾する技術に関する。

近年、船舶の航行技術において、離着桟航行に関する研究開発が精力的に進められている。一方、他の船舶を対象船として追尾するように自船を航行させる追尾航行は特殊用途のため、離着桟航行ほどには精力的に研究開発が進められていない。しかしながら、追尾航行は、救難、救助、海洋資源調査、漁業支援、船団の隊列航行などに利用することができるため、追尾航行には潜在的な需要がある。

なお、関連する技術として、仮想追尾対象体の仮想コースを仮想座標平面上に設け、この仮想コースに対しての図形的な第１条件を満たす仮想移動地点を設定し、追尾対象体の位置を検出して、仮想座標平面上に仮想追尾対象体の仮想位置を描き、仮想位置が仮想コースに対しての図形的な第２条件を満たすときを移動開始時点とし、この移動開始時点となったときに移動許可信号の入力を得て若しくは移動許可信号の入力を得ることなく、仮想移動地点に対応する実際の移動地点に向かって自船を移動する自動操船を行う船舶の自動操縦システム、が知られている（特許文献１参照）。

特開２０１８－１０３９５０号公報

本発明が解決しようとする課題は、自船を対象船に追尾させる技術を提供することにある。

実施形態の船舶追尾装置は、自船を対象船に追尾させる船舶追尾装置であって、取得された前記対象船の船位に基づく前記対象船の速度運動モデルに基づいて前記対象船の運動を推定する運動推定部と、前記推定された前記対象船の運動に基づいて、該対象船の船位と前記自船の船位との離間距離を最小化する該自船の軌道計画を算出する軌道計画部と、前記軌道計画に前記自船を追従させるように前記自船を制御する追尾制御部とを備える。

本発明によれば、自船を対象船に追尾させることができる。

船舶追尾装置を含むシステムの全体構成を示すブロック図である。 船舶追尾装置の構成を示すブロック図である。 旋回時の対地速度ベクトルを示す図である。 拡張カルマンフィルタの構成を示す図である。 多重モデルフィルタの構成を示す図である。 追尾軌道の一例を示す図である。 自船の軌道計画の一例を示す図である。 追尾軌道が実現できない領域を示す図である。 追尾軌道を更新する条件を示す図である。 追尾制御システムの誤差を示す図である。 追尾制御部の構成を示すブロック図である。 航路制御部の構成を示すブロック図である。 船体速度と潮流成分を示す図である。 参照信号の時系列を示す図である。 参照軌跡のリーチ見積もりを示す図である。 対地方位ＣＯＧと対地速度ＳＯＧとを示す図である。 船位航跡を示す図である。 船位時系列を示す図である。 検出方位に含まれるノイズ成分の時系列を示す図である。 ＩＭＭ Ｆｉｌｔｅｒ結果を示す図である。 ＥＫＦとＩＭＭとの比較を示す図である。 ＥＫＦによるＣＯＧとＳＯＧの推定を示す図である。 追尾制御の追跡結果を示す図である。 方位制御システムの誤差と舵角を示す図である。 船体速度と潮流速度を示す図である。

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

（１ 船舶追尾装置の構成）
まず、本実施形態に係る船舶追尾装置を含むシステムについて説明する。図１は、船舶追尾装置を含むシステムの全体構成のブロック図である。図２は、船舶追尾装置の構成を示すブロック図である。

図１に示すように、本実施形態における船舶追尾装置１は、推進駆動装置３２、舵駆動装置３３、センサ類３４が備えられた船体３１を有する自船を制御するものであって、推進駆動装置３２及び舵駆動装置３３を制御することによって、船体速度及び方位を制御する装置である。また、船舶追尾装置１は、自船とは異なる船舶である対象船を追尾対象とし、この対象船を追尾するように、具体的には自船と対象船とが接近するように自船の制御を行う。

センサ類３４は、船体３１の船首方位を検出するジャイロコンパス、船体３１の対水速度を検出する速度計、ＧＰＳ等の衛星測位システム（ＧＮＳＳ）からの船体位置を検出するＧＮＳＳセンサを含む。なお、センサ類３４は、船首方位、船体位置をそれぞれ検出可能なセンサを含むものであれば良い。また、対象船の船体位置の時系列データは、位置検出装置４により検出され、船舶追尾装置１は、この船体位置の時系列データに基づいて対象船の追尾を行うものとする。なお、本実施形態においては、位置検出装置４は、船体３１に搭載されたレーダ装置とするが、船体位置の時系列データを検出可能な装置であれば良く、例えば、ＡＩＳ（Ａｕｔｏｍａｔｉｃ Ｉｄｅｎｔｉｆｉｃａｔｉｏｎ Ｓｙｓｔｅｍ）としても良い。

図２に示すように、船舶追尾装置１は、対象船及び自船の運動を周期的に推定する運動推定部１１と、運動推定部１１による推定に基づいて、対象船と自船とを遭遇させる自船の軌道計画を追尾軌道として算出する軌道計画部１２と、軌道計画部１２により算出された追尾軌道に追従するように自船の航行を制御する追尾制御部１３とを備える。

（２ 対象船の運動補足）
（２．１ 船体モデル）
船体モデルについて説明する。船体運動を捕捉するため、以下の３つの船体運動モデルを定める。

（２．１．１ 速度運動モデル）
一定速度の運動モデルＣＶを１次元で表すと

になる。ここで、ｘは位置、ｕはｘの速度である。

上式を２次元の差分式で表すと

になる。ここで、ｙは位置、ｖはｙの速度、ｈは差分時間、ｋは更新番号である。

（２．１．２ 加速度運動モデル）
一定加速度の運動モデルＣＡを１次元で表すと

になる。ここで、ａはｘの加速度である。

上式を２次元の差分式で表すと

になる。ここで、ｂはｖの加速度である。

（２．１．３ 旋回運動モデル）
旋回運動モデルについて説明する。図３は、旋回時の対地速度ベクトルを示す図である。

一定旋回の運動モデルＣＴを図３に示す。ただし、速度は一定とする。このとき、２次元
の運動は

になる。ここで、ωは旋回角速度、ｔは時間である。

上式を差分式で表すと

になる。ここで、θ＝ωｈである。

（２．１．４ 船体運動モデルの評価）
上記３つの運動モデルを船体運動に照らして評価する。船体運動は車両や飛行機と比べると、質量が大きく、加速度が小さく、旋回角速度が小さい特徴をもつ。加速度運動モデルは加速度ａが小さいため、速度運動モデルで近似でき、この速度運動モデルは（３）式から

になる。

旋回運動モデルは旋回角速度ωが小さいため、速度運動モデルで近似でき、この速度運動モデルは（５）式から

になる。ここで、ｃｏｓ（ωｔ）≒１，ｓｉｎ（ωｔ）≒ωｔ，ｖ ｓｉｎ（ωｔ）＝ｕ ｓｉｎ（ωｔ）≒０である。よって、船体運動モデルとして速度運動モデルを用いることができる。

（２．２ 運動推定フィルタ）
船舶追尾装置１の運動推定部１１が船体運動を推定するためのフィルタについて説明する。このフィルタは離散型線形であり、拡張カルマンフィルタ（ＥＫＦ：Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）と多重モデルフィルタ（ＩＭＭ：Ｉｎｔｅｒａｃｔｉｖｅ Ｍｕｌｔｉｐｌｅ Ｍｏｄｅｌ Ｆｉｌｔｅｒ）を利用することができる。

推定対象の状態を推定して、船体運動を求める。船体運動は

になる。ここで、添字＾は推定値、Ｕ＾_Ｔは対地速度、ψ＾_Ｔは対地方位である。

（２．２．１ 推定対象）
推定対象はプラント状態式と観測式から、次式になる。

ここで，太字はベクトル成分であることを示し、ｘ（太字）は状態，ｕ（太字）は入力、ｙ^－（太字）は観測（出力）、ｗ（太字）はプラント外乱、ｖ（太字）は観測外乱、外乱は正規分布Ｎ（０，σ^２）に従い、σ^２は分散、Ａ（太字）はプラント行列、Ｂ（太字）は入力行列、Ｃ（太字）は観測行列、Ｇ（太字）は外乱行列である。ただし本実施形態において、推定対象の入力量ｕ（太字）は未知のため，Ｂｕ（太字）_ｋ項はゼロになる。

プラント行列Ａ（太字）は、推定対象のヤコブ行列になる。船体運動モデルにおいて、一定速度の運動モデルＣＶと一定加速度の運動モデルＣＡのヤコブ行列はそれぞれ（２）式と（４）式に等しい。また、一定旋回の運動モデルＣＴのヤコブ行列は

になる。ここで、

である。

（２．２．２ ＥＫＦ）
ＥＫＦを定める。

ＥＫＦは、観測更新と時間更新からなり、観測更新は制御対象の出力ｙ^－を用いて、次式になる。

ここで、添字^Ｔは転置行列、ｘ（太字）＾_ｋ｜ｋはｘ（太字）_ｋの推定値、Ｋ（太字）_ｋは推定ゲイン、Ｐ（太字）_ｋ｜ｋは誤差共分散行列、Ｒ（太字）_ｋは重み行列（分散行列）でＲ（太字）_ｋ＝Ｅ（ｖ（太字）_ｋｖ（太字）^Ｔ _ｋ）、Ｅは期待値、Ｉ（太字）は適当な単位行列

である。

また、時間更新は次式になる。

ここで、ｘ（太字）＾_{ｋ＋１｜ｋ}はｘ（太字）＾_ｋ｜ｋの時間更新、Ｑ（太字）_ｋは重み行列でＱ（太字）_ｋ＝Ｅ（ｗ（太字）_ｋｗ（太字）^Ｔ _ｋ）である。また、Ｅ（ｗ（太字）_ｋｗ（太字）^Ｔ _ｋ）＝０とする。

（２．２．３ ＩＭＭ）
ＩＭＭを定める。図４、図５は、それぞれ、ＥＫＦ，ＩＭＭの構成を示す図である。

図４、図５に示すように、ＩＭＭは、ＥＫＦが単独に構成されるのに対して、３つのＥＫＦと推定状態を調整する３つの要素から構成される。本実施形態においては、既存のＩＭＭを利用するため、その詳細構成については省略する。ＩＭＭは全体として１つのＥＫＦのように機能し、検出信号ｙ_ｋを入力して推定信号ｘ＾_ｋを出力する。

（２．２．４ 運動推定フィルタの比較）
運動推定フィルタとしてのＥＫＦとＩＭＭとを比較する。

上述したように、船体運動モデルは一定速度の運動モデルＣＶを含み、この運動モデルＣＶはＥＫＦ及びＩＭＭのそれぞれに組み込まれる。ＩＭＭは３つの運動モデルＣＶ，ＣＡ，ＣＴを含む。したがって、運動モデルＣＶの選択が適切であれば、ＥＫＦとＩＭＭとの性能差は顕著ではない。また、ＩＭＭの計算量は、少なくともＥＫＦの３倍となるため、計算量の低減という観点においては、運動推定フィルタとしてＥＫＦを用いることが望ましい。

（３ 追尾軌道）
追尾軌道について説明する。図６は、追尾軌道の一例を示す図である。

本実施形態に係る船舶追尾装置１における軌道計画部１２は、対象船を追尾する軌道を推測航法によって計画する。追尾軌道は、対象船、自船それぞれの運動状態を一定として、自船の軌道計画に基づいて、自船と対象船とが出会う遭遇域の位置と時刻を求めるものである。図６に示すように、自船の追尾軌道は、対象船の補足後に計画され、直線と円弧により構成される。
（３．１ 対象船の軌道計画）
対象船の軌道計画について説明する。対象船の位置は、上述したように補足した状態を設定値に用いると、次式になる。

ここで、太字はベクトル成分、添字^ｏｂ、添字_ｇ、添字（０）は、それぞれ、対象船の設定値、対地成分、初期値を示し、ｘ^ｏｂは船体位置（船位）、ｕ^ｏｂ _ｇは速度、τは補足した時点をゼロとした場合の追尾時刻である。速度ｕ^ｏｂ _ｇは次式で表すことができる。

ここで、ｕ^ｏｂ _ｇ，ｖ^ｏｂ _ｇは、それぞれ、北方向、東方向の対地速度であり、Ｕ^ｏｂ _ｇは対地速度（ＳＯＧ：Ｓｐｅｅｄ Ｏｖｅｒ Ｇｒｏｕｎｄ）、ψ^ｏｂ _ｇは対地方位（ＣＯＧ：Ｃｏｕｒｓｅ Ｏｖｅｒ Ｇｒｏｕｎｄ）である。よって、対象船の船位は追尾時刻の関数になる。

（３．２ 自船の軌道計画）
自船の軌道計画について説明する。図７は、自船の軌道計画の一例を示す図である。なお、図７において、点Ｏ’は自船の出発点、点Ｓは旋回開始点、点Ｆは旋回終了点、点Ｐは到達点をそれぞれ示す。

図７に示すように、点Ｐを自船の位置ｘとすれば、点Ｐは点Ｐ‘を座標変換することにより求められる。座標変換は自船の出発点Ｏ’周りに回転したものになる。点Ｐは関数ｆを用いて

になる。ここで、ｘ（太字）は自船の船位、ψｓｅｔは旋回角∠Ｓ’Ｃ’Ｆ’、ｄ_１は線分Ｏ’Ｓ’の距離である。

関数ｆの内容を示す。点Ｏ’，Ｓ’，Ｆ’，Ｐ’の間の距離と時刻とをそれぞれ求めると、

になる。ここで、ｄ_２は円弧Ｓ’Ｆ’の距離、ｄ_３は線分Ｆ’Ｐ’の距離、ρは旋回半径の設定値、Ｕ_ｇは対地速度の設定値である。ただし、Ｕ_ｇ＞Ｕ^ｏｂ _ｇである。よって、点Ｏ’から点Ｐ’までの追尾時刻はτで、追尾距離は

になる。

点Ｓ’，Ｆ’，Ｐ’の座標をそれぞれ求めると、

になる。ここで、Ｏ’＝［０，０］^Ｔ、添字^Ｔは転置行列、ｓｉｇｎはψの符号関数で

である。

よって、点Ｐは次式のように点Ｐ’を座標変換することにより求められ、船位ｘが定まる。

ここで、ψ_ｇ（０）は対地方位の初期値、Ω（太字）は座標変換行列で

である。

（３．３ 追尾軌道の算法）
追尾軌道の算法について説明する。図８は追尾軌道が実現できない領域を示す図である。図９は追尾軌道を更新する条件を示す図である。

追尾軌道は、自船と対象船との船位を一致させるために、３つのパラメータτ，ψ_ｓｅｔ，ｄ_１を求めることによって得られる。追尾軌道の算法は、多変数関数の最小化問題に帰着させるものである。また、追尾軌道の仕様は、自船と対象船との船位を一致させ、自船と対象船との出会い時間を最短とし、追尾軌道における旋回角を最小にするように定められる。

このように定められた追尾軌道の評価関数Ｊは

になる。ここで、Ｊ＞０、ｄ_ｘは自船と対象船との距離誤差で

である。

評価関数ＪはパラメータＸ（太字）＝｛τ，ψ_ｓｅｔ，ｄ_１｝を変数とする関数になる。変数は初期値から収束値に達する。そのとき、評価量の極小値は、変数に関する偏微係数（勾配ベクトル）がゼロになるときの値に相当し

になる。ここで、∂／∂Ｘ（太字）は勾配ベクトル、０（太字）はゼロベクトル、ｍｉｎは極小値である。

パラメータの初期値と制約値は、

になる。ここで、Ｐａｒはパラメータ、添字（０）は初期値、添字^Ｕは上限値、添字_Ｌは下限値である。なお、（３７）式に制約値を加えたものについてはその説明を省略する。

図８に示すように、旋回半径ρ_Ｌと制約値とに基づいて、追尾軌道が実現できない領域が存在する。ここで、制約値はψ^Ｕ _ｓｅｔ＝π，ψ_ｓｅｔＬ=－πの場合である。ただし、自船と対象船との船位が近接している場合には追尾軌道は用いられない。なお、図８は一回の追尾軌道の算出の結果であり、追尾軌道は必要に応じて更新される。

追尾軌道を更新する条件は、図９に示すように、自船及び対象船のそれぞれの推測船位が出会い範囲を超えた場合

になる。ここで、添字＾は予測値、ｘ_ｅｚは現在の出会い位置、ρ_ｅｚは許容半径である。

すなわち、出会い位置と推定値としての対象船の船体位置との離間距離が設定閾値以上である場合、または、出会い位置と予測値としての自船の船体位置との離間距離が設定閾値以上である場合、追尾軌道が更新される。

（４ 追尾制御）
追尾軌道に自船を追従させる追尾制御システムについて説明する。図１０は、追尾制御システムの誤差を示す図である。図１１は、追尾制御部の構成を示すブロック図である。

追尾制御部１３は、軌道計画上で設定された時刻に対する位置に自船の位置を追従させるものである。軌道計画は、図１０に示すように、直線と円弧により構成される。図１０において、点Ｐは自船位置、点Ｈは軌道計画上の点Ｐに対する垂足位置、点Ｒは軌道計画上の参照位置、点Ｃは旋回中心点、ρは旋回半径、Ｓは旋回の開始点、Ｆは旋回の終端点をそれぞれ示す。また、ｄ_Ｈは直線または円弧における基準点から点Ｈまでの距離、ｄ_Ｒは直線または円弧における基準点から点Ｒまでの距離、ｄ_ｅは距離誤差としての点Ｈ，Ｒ間の距離、ｙ_ｅは航路誤差としての点Ｈ，Ｐ間の距離をそれぞれ示す。

図１１に示すように、追尾制御部１３は、距離制御システムと航路制御システムとを備える。距離制御システムは、軌道計画上の参照時刻に対応する参照位置Ｒに自船の前進方向位置Ｈを追従させるものであり、距離誤差ｄ_ｅを算出する距離誤差算出部１３１と、距離誤差ｄ_ｅに基づいて船体の対水速度を制御する距離制御部１３２とにより構成される。また、航路制御システムは、軌道計画上の垂線位置Ｈに自船の横方向位置Ｐを追従させるものであり、航路誤差ｙ_ｅを算出する航路誤差算出部１３３と、航路誤差ｙ_ｅに基づいて船体のｓｗａｙ方向速度を制御する航路制御部１３４とにより構成される。

（４．１ 距離制御システム）
距離制御システムについて説明する。
（４．１．１ 距離誤差）
距離誤差は、軌道計画上にある位置までの距離によって

に定める。ここで、ｄ_ｅは距離誤差、ｄ_Ｒは自船位置から参照位置（点Ｒ）までの距離、ｄ_Ｈは自船位置から軌道計画上の垂線位置（点Ｈ）までの距離であり、

である。ここで、Ｕ_ｇは対地速度の設定値、ｔは時間、添字^Ｌ、添字^Ｃ、添字（０）はそれぞれ、直線軌道、円弧軌道、初期値を示し、

である。ここで、ψ_Ｈは点Ｈの方位である。

（４．１．２ 制御器の要素）
距離制御部１３２における制御器の要素は

に定める。ここで、ｓはラプラス演算子、Λ（ｓ）は制御量、Ｃ_ｄ（ｓ）は制御器で

である。ここで、Ｋ_ｐは比例ゲイン、Ｔ_ｆはフィルタの時定数、Ｔ_ｉは積分器の時定数である。積分器は距離誤差に対して１形サーボ系、外乱成分（特記せず）に対して０型サーボ系を実現する。

（４．２ 航路制御システム）
航路制御システムについて説明する。図１２は航路制御部の構成を示すブロック図である。

航路制御部１３４は航路誤差ｙ_ｅをゼロに収斂させるものであり、航路保持と航路旋回の機能をもつ。航路保持は軌道計画における直線部分（図１０における（ａ））において実行され、航路旋回は軌道計画における円弧部分（図１０における（ｂ））において実行される。

航路制御部１３４は、図１２に示すように、フィードバック制御部１３４Ａと参照信号を用いるフィードフォワード制御部１３４Ｂとを有し、航路保持及び航路旋回はフィードバック制御部１３４Ａ及びフィードフォワード制御部１３４Ｂによって実現される。参照信号は参照方位ψＲと参照軌跡からなる。図１２において、δ_ＦＢ，δ_ＦＦ，δ_ｃは、それぞれ、フィードバック舵角、フィードフォワード舵角、命令舵角を示す。なお、点Ｐに基づくｙ_ｅの算出についてはその説明を省略する。

（４．２．１ 航路誤差モデル）
航路誤差算出部により算出される航路誤差について説明する。図１３は船体速度と潮流成分を示す図である。なお、図１３において、ＸＯＹは地球固定座標を示し、Ｘ_ＢＯ_ＢＹ_Ｂは船体固定座標を示すものとする。

航路誤差モデルは速度ベクトル成分から導出される。船体速度及び潮流速度の対地成分は図１３に示すように、船体成分と潮流成分の和で、

になる。ここで、添字_ｇは船体速度の対地成分、添字_ｎは船体速度の地球固定座標成分であり、

である。ここで、Ω^Ｂ _Ｅは船体固定座標から地球固定座標に変換する行列であり、

である。ここで、添字^Ｔは転置行列である。

参照方位における対地速度は次式になる。

ここで、添字_Ｒは参照方位に関し、ψ_ｅ＝ψ－ψ_Ｒは方位偏差であり、

である。

（５１）式を線形近似すると、次式になる。

ここで、ψｅは微小角として線形化され、ｃｏｓ ψ_ｅ≒１，ｓｉｎ ψ_ｅ≒ψ_ｅ、ｕψ_ｅ≒ｕ_０ψ_ｅであり、ｕ_０は一定値であり、ｕ_ｃＲ，ｕ_ｃＲは、それぞれ、参照軌跡上の接線方向、法線方向の速度成分で

である。ここで、ψ_ｃＲ＝ψ_ｅ－ψ_Ｒである。

参照軌跡での船体速度成分は（５３）式になり、潮流成分ｕ_ｃＲが接線方向に、潮流成分ｖ_ｃＲが法線方向に作用する。よって、直線軌道や円弧軌道を実現するためには、ｕ_ｃＲ，ｖ_ｃＲの影響を修正する必要がある。

（４．２．２ フィードバック制御部）
フィードバック制御部について説明する。

フィードバック制御部１３４Ａは制御対象との閉ループ系を構成し航路誤差をゼロに収束させ、潮流成分による誤差を修正する。フィードバック制御部１３４Ａは推定器とフィルタに状態フィードバックゲインを加えたものであり、次式になる。

ここで、添字_ｈは方位制御、添字_ｔは航路制御、添字＾は推定値に関する。また、ｘ（太字）＾ｈ＝［ｘ（太字）＾_ｒ，ｘ（太字）＾_ｗ，δ＾_ｒｏ］^Ｔ，ｘ（太字）＾_ｒ＝［ψ＾_ｅ，ｒ＾_ｘ］^Ｔ，ｘ（太字）＾_ｗ＝［ξ＾，ψ＾_ｗ］^Ｔである。また、添字^Ｔは転置行列に関し、ξは変数、Ｋ（太字）_ｈは推定ゲイン、Ｆ（太字）_ｈはフィードバックゲイン、ｆ_ｙは航路ゲイン、Ｔ_ｙはフィルタ時定数である。また、γ_ｔは潮流成分の修正項であり、

である。ここで、ｖ＾_ｏ＝Ｋ_ｖδ_ｒｏ、Ｏ_ｉ×_ｊはｉ行ｊ列のゼロ行列、δ_ｒｏは舵角オフセットであり、Ｋ_ｒ，Ｋ_ｖ，Ｔ_ｒ，Ｔ_ｒ３は船体パラメータでそれぞれ旋回力ゲイン、横流れゲインと２つの時定数であり、ζ_ｗ，ω_ｗ，σ_ｗは波浪パラメータでそれぞれ減衰係数、固有周波数と強さのゲインである。

（４．２．３ 潮流成分の推定）
潮流成分は航行中に変化するため、航路制御においては潮流成分が常時推定される。なお、潮流成分の推定（潮流推定）は船位の移動速度から求めるため、風浪による成分を分離できずに含む。潮流推定を対地座標で構成すると、次式になる。

ここで、添字＾は推定値、添字^－は検出値、ｘ＾_ｃ＝［ｘ＾ ｕ＾_ｃ］^Ｔ，ｙ＾_ｃ＝［ｙ＾ ｕ＾_ｃ］^Ｔであり、ｘ，ｙは船位、ｕ_ｃ，ｖ_ｃは潮流成分、ｕ_ｎ，ｖ_ｎは船体速度、Ｋ_ｃ＝［ｋ_ｃ１ ｋ_ｃ２］^Ｔは潮流推定ゲインであり、

である。

（４．２．４ フィードフォワード制御部）
フィードフォワード制御部について説明する。図１４は参照信号の時系列を示す図である。

フィードフォワード制御部１３４Ｂは、参照信号による開ループ制御系によって、船位を半径一定の円弧軌道に追従させるものであり、航路誤差を扱わない。参照信号は図１４に示すように、参照方位ψ_Ｒと参照舵角δ_Ｒからなり、方位・舵角の変針条件を満足して、ψ＝ψ_Ｒを実現する。ψ_Ｒ，δ_Ｒは時間関数で

になる。ここで、ａは係数、ｔ_Ｒは変針時間である。

参照方位の角速度ｒ_Ｒ＝ψ’_Ｒが一定かつ前進速度ｕが一定のとき、参照軌跡は半径一定ρ＝ｕ÷ｒ_Ｒの円弧を描く。ここでρは旋回半径である。

（４．２．４．１ 潮流成分の修正）
旋回時に生じる潮流成分による影響を修正する方法について説明する。

潮流成分は（５３）式からｕ_ｃＲ，ｖ_ｃＲになり、以下に説明するように、それぞれ、ｖ_ｃＲは斜航角の修正、ｕ_ｃＲは角速度の修正として、その影響が低減される。

ｖ_ｃＲは参照軌跡の法線方向成分で航路誤差を生じさせるため、斜航角の修正として、潮流斜航角

によって相殺される。このとき、（５８）式に示したフィードバック制御部１３４Ａにおける処理も併せて実施される。

ｕ_ｃＲは参照軌跡の接線方向成分で前進速度を変化させるため、旋回角速度を修正することで旋回半径一定を保持する。ｕ_ｇＲは次の条件

に達したら、参照方位を次式の設定値

によって更新する。ここで、添字^ｕｐは初期値または更新値、ｕ_ｓｅｔは速度しきい値、ψ_ｓｅｔは変針量、ψ^ｕｐ _Ｒは更新時の値である。上式の更新後、変針量設定値はψ^ｕｐ _ｓｅｔがゼロになるまで、次式の置き換えをする。

（４．２．４．２ 横流れ速度の修正）
航路誤差を生じさせる横流れ速度の修正について説明する。図１５は参照軌跡のリーチ見積もりを示す図である。

旋回時の船体航跡は横流れ速度により外側にリーチ相当移動し、航路誤差を生じさせる。そのため、変針開始点を軌道計画の開始点Ｓからリーチだけ手前のＷＯＰ（Ｗｈｅｅｌ Ｏｖｅｒ Ｐｏｉｎｔ）にする。その移動をリーチ修正と呼ぶ。

リーチ量ｄ_ｒは図１５に示すように、旋回終端点Ｐ，Ｆにおける接線の切片ｘ_ｐ，ｘ_ｆから求める。ここで，αは点Ｐの傾きである。なお、ｄ_ｋはキック量で、修正しない。点Ｐの参照軌跡を船体速度と潮流成分から求めると、

になる。ここで、

である。（７６）式は潮流成分の影響を受けるため、前述の潮流成分の修正方法が実施される。

（５ 検証）
本実施形態に係る船舶追尾装置の有効性をシミュレーションによって検証する。検証対象は、対象船の運動補足と自船の追尾制御である。

（５．１ 対象船の運動補足）
対象船の運動補足に関する検証について説明する。図１６は、対地方位ＣＯＧと対地速度ＳＯＧとを示す図である。図１７は、船位航跡を示す図である。図１８は、船位時系列を示す図である。図１９は、検出方位に含まれるノイズ成分の時系列を示す図である。図２０は、ＩＭＭ Ｆｉｌｔｅｒ結果を示す図である。図２１はＥＫＦとＩＭＭとの比較を示す図である。図２２は、ＥＫＦによるＣＯＧとＳＯＧの推定を示す図である。

対象船の運動捕捉をＥＫＦとＩＭＭによって行う。ＥＫＦは運動モデルＣＶを利用し、ＩＭＭは、運動モデルＣＶ、加速度モデルＣＡ及び運動モデルＣＴを利用するものとする。ＥＫＦとＩＭＭの計算はＭＡＴＬＡＢ（登録商標）のＳｅｎｓｏｒ Ｆｕｓｉｏｎ ａｎｄ Ｔｒａｃｋｉｎｇ Ｔｏｏｌｂｏｘで実施する。

シミュレーションにおける対象船の運動条件は、刻み時間１ｓ、加減速度±１／３０ｍ・ｓ^－２、加減速時間２００ｓ、速度６．６ｍ・ｓ^－１、速度時間１００ｓ、旋回角速度０．５ｄｅｇ・ｓ^－１、旋回時間３６０ｓである。シミュレーションにおける対象船の運動と船位の状態は図１６～１９に示す。

図２０～２２は対象船の運動捕捉の結果を示す。図２０はＩＭＭにおいて３つの運動モデルの確率で、ＣＶが９割、ＣＴが１割とＣＡがゼロである。図２１はＥＫＦ（ＣＶを利用）とＩＭＭの誤差比較を示し、図２１からＩＭＭはＥＫＦよりわずかに誤差が小さいことがわかる。図２０と図２１の結果から、対象船の運動条件では運動モデルはＣＶを、推定方法はＥＫＦをそれぞれ用いて実用上問題ないことが確認できる。

図２２はＥＫＦ（ＣＶ）による（９）式のＳＯＧと（１０）式のＣＯＧの推定結果を示す。図２２から対象船の速度が低い、特に前進速度が小さいとき、ノイズ成分の影響が強く作用し誤差が大きくなることがわかる。また、ノイズの大きさが大きいと、フィルタゲインを小さくするので、推定値に遅れが生じる。遅れ時間は図２２においては８．３秒になる。

（５．２ 自船の追尾制御）
自船の追尾制御に関する検証について説明する。図２３は追尾制御の追跡結果を示す図である。図２４は方位制御システムの誤差と舵角を示す図である。図２５は船体速度と潮流速度を示す図である。

シミュレーションにおける自船の追尾制御の結果を図２３に示す。図２３には、対象船の捕捉後、自船の追尾軌道計画に則り、追尾制御システムによって追従した航跡が示される。

シミュレーションにおける条件を以下に示す。対象船はＵ^ｏｂ _ｇ＝１０ｋｎ，ψ^ｏｂ _ｇ＝７０ｄｅｇ，ｘ^ｏｂ（０）＝（７，－２）Ｎｍであり、自船はＵ（０）＝１５ｋｎ，Ｕ_ｍａｘ＝２０ｋｎ，ψ（０）＝６０ｄｅｇ，ｘ（０）＝（０，０）Ｎｍであり、潮流成分はＵ_ｃ＝５ｋｎ，ψ_ｃ＝６０ｄｅｇである。

制御システムにおいては、距離制御システムのゲインはＫ_ｐ＝０．０１，Ｔ_ｆ＝３０ｓ，Ｔ_ｉ＝９００ｓである。航路制御システムのゲインは、抜粋して、船体パラメータＫ_ｒ＝０．１０ｓ^－１，Ｋ_ｖ＝－７．４ｍ・ｓ^－１，Ｔ_ｒ＝４３ｓ，ω_ｗ＝０．４８ｓ^－１，ζ_ｗ＝０．１であり、ゲインｆ_１＝０．９９ｍ，ｆ_２＝１８．８ｓ，ｆ_ｙ＝０．００１３ｍ^－１である。

追尾軌道の計算はＭＡＴＬＡＢ（登録商標）のｆｍｉｎｃｏｎを利用する。その結果、出会い時間τ＝２８０３ｓ、追尾距離ｄ＝１１．７Ｎｍ、旋回までの距離ｄ_１＝３．０Ｎｍ、旋回角ψ_ｓｅｔ＝－４３．６ｄｅｇになる。また初期値と制約は次式になる。

ＷＯＰは図２３示すように、旋回開始点Ｓ（不図示）からリーチｄｒ＝１６７．３ｍだけ手前になる。図２３において、遭遇域での時刻はτ’＝２８０１．６ｓ、自船と対象船との距離ｄ’_ｘ＝１．３ｍになる。また、図２４に示すように、方位偏差は潮流斜航角を生じ、航路誤差が旋回時にｙ_ｅ＝８４．４ｍを生じ，遭遇域でｙ_ｅ＝１．５ｍになる。図２５に示すように、対水速度は対地前進速度ｕ_ｇＲ＝１５ｋｎを保持している。潮流推定値は潮流外乱成分に適切に追従している。よって、距離制御システムと航路制御システムは適切に動作していることが確認できた。

（６ まとめ）
上述した本実施形態に係る船舶追尾装置１によれば、対象船の運動推定に用いる船体運動モデルは速度モデルが有効であり、推定手法として拡張カルマンフィルタを用いることで、有意な運動推定をより少ない計算量により実現することができる。また、推測航法による追尾軌道において、その算法は対象船と自船の軌道計画から求めるもので、多変数関数の最小化問題に帰着させる方法を用いた。また、自船の追尾制御において、制御システムは追尾軌道計画に船位を追従し、距離制御と航路制御のシステムから構成される。

本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。この実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１ 船舶追尾装置
１１ 運動推定部
１２ 軌道計画部
１３ 追尾制御部

Claims (7)

1. 自船を対象船に追尾させる船舶追尾装置であって、
   取得された前記対象船の船位に基づく前記対象船の速度運動モデルに基づいて前記対象船の運動を推定する運動推定部と、
   前記推定された前記対象船の運動に基づいて、該対象船の船位と前記自船の船位との離間距離を最小化する該自船の軌道計画を算出する軌道計画部と、
   前記軌道計画に前記自船を追従させるように前記自船を制御する追尾制御部と
   を備える船舶追尾装置。
2. 前記軌道計画部は、前記離間距離が最小となるまでの時間である遭遇時間が最小となる軌道計画を算出することを特徴とする請求項１に記載の船舶追尾装置。
3. 前記軌道計画部は、前記自船の旋回角が最小となる軌道計画を算出することを特徴とする請求項２に記載の船舶追尾装置。
4. 前記軌道計画部は、前記離間距離、前記遭遇時刻、及び前記旋回角を変数とする多変数関数の最小値を求めることによって前記軌道計画を算出することを特徴とする請求項３に記載の船舶追尾装置。
5. 前記運動推定部は、前記対象船の運動及び前記自船の運動を周期的に推定し、
   前記軌道計画部は、前記離間距離が最小となる位置である遭遇位置と、前記推定された運動に基づく前記対象船または前記自船の推定位置との距離が予め設定された距離以上である場合、前記軌道計画を更新することを特徴とする請求項１～請求項４のいずれか一項に記載の船舶追尾装置。
6. 前記追尾制御部は、前記軌道計画に対する前記自船の航路誤差を低減させるように前記自船のｓｗａｙ方向速度を制御する航路制御部と、前記軌道計画上の参照位置との距離誤差を低減させるように前記自船の対水速度を制御する距離制御部とを有することを特徴とする請求項１～請求項５のいずれか一項に記載の船舶追尾装置。
7. 前記運動推定部は、拡張カルマンフィルタを用いて前記対象船の運動を推定することを特徴とする請求項１～請求項６のいずれか一項に記載の船舶追尾装置。

Images