JP7170019B2 - 追尾制御装置 - Google Patents

追尾制御装置 Download PDF

Info

Publication number
JP7170019B2
JP7170019B2 JP2020183899A JP2020183899A JP7170019B2 JP 7170019 B2 JP7170019 B2 JP 7170019B2 JP 2020183899 A JP2020183899 A JP 2020183899A JP 2020183899 A JP2020183899 A JP 2020183899A JP 7170019 B2 JP7170019 B2 JP 7170019B2
Authority
JP
Japan
Prior art keywords
ship
motion
tracking
trajectory
target
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2020183899A
Other languages
English (en)
Other versions
JP2022073731A (ja
Inventor
冬希 羽根
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tokyo Keiki Inc
Original Assignee
Tokyo Keiki Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tokyo Keiki Inc filed Critical Tokyo Keiki Inc
Priority to JP2020183899A priority Critical patent/JP7170019B2/ja
Publication of JP2022073731A publication Critical patent/JP2022073731A/ja
Application granted granted Critical
Publication of JP7170019B2 publication Critical patent/JP7170019B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Feedback Control In General (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Description

本発明は、対象船を追尾する技術に関する。
近年、船舶の航行技術において、離着桟航行に関する研究開発が精力的に進められている。一方、他の船舶を対象船として追尾するように自船を航行させる追尾航行は特殊用途のため、離着桟航行ほどには精力的に研究開発が進められていない。しかしながら、追尾航行は、救難、救助、海洋資源調査、漁業支援、船団の隊列航行などに利用することができるため、追尾航行には潜在的な需要がある。
なお、関連する技術として、仮想追尾対象体の仮想コースを仮想座標平面上に設け、この仮想コースに対しての図形的な第1条件を満たす仮想移動地点を設定し、追尾対象体の位置を検出して、仮想座標平面上に仮想追尾対象体の仮想位置を描き、仮想位置が仮想コースに対しての図形的な第2条件を満たすときを移動開始時点とし、この移動開始時点となったときに移動許可信号の入力を得て若しくは移動許可信号の入力を得ることなく、仮想移動地点に対応する実際の移動地点に向かって自船を移動する自動操船を行う船舶の自動操縦システム、が知られている(特許文献1参照)。
特開2018-103950号公報
本発明が解決しようとする課題は、自船を対象船に追尾させる技術を提供することにある。
実施形態の船舶追尾装置は、自船を対象船に追尾させる船舶追尾装置であって、取得された前記対象船の船位に基づく前記対象船の速度運動モデルに基づいて前記対象船の運動を推定する運動推定部と、前記推定された前記対象船の運動に基づいて、該対象船の船位と前記自船の船位との離間距離を最小化する該自船の軌道計画を算出する軌道計画部と、前記軌道計画に前記自船を追従させるように前記自船を制御する追尾制御部とを備える。
本発明によれば、自船を対象船に追尾させることができる。
船舶追尾装置を含むシステムの全体構成を示すブロック図である。 船舶追尾装置の構成を示すブロック図である。 旋回時の対地速度ベクトルを示す図である。 拡張カルマンフィルタの構成を示す図である。 多重モデルフィルタの構成を示す図である。 追尾軌道の一例を示す図である。 自船の軌道計画の一例を示す図である。 追尾軌道が実現できない領域を示す図である。 追尾軌道を更新する条件を示す図である。 追尾制御システムの誤差を示す図である。 追尾制御部の構成を示すブロック図である。 航路制御部の構成を示すブロック図である。 船体速度と潮流成分を示す図である。 参照信号の時系列を示す図である。 参照軌跡のリーチ見積もりを示す図である。 対地方位COGと対地速度SOGとを示す図である。 船位航跡を示す図である。 船位時系列を示す図である。 検出方位に含まれるノイズ成分の時系列を示す図である。 IMM Filter結果を示す図である。 EKFとIMMとの比較を示す図である。 EKFによるCOGとSOGの推定を示す図である。 追尾制御の追跡結果を示す図である。 方位制御システムの誤差と舵角を示す図である。 船体速度と潮流速度を示す図である。
以下、図面を参照しながら、本発明の実施形態について説明する。
(1 船舶追尾装置の構成)
まず、本実施形態に係る船舶追尾装置を含むシステムについて説明する。図1は、船舶追尾装置を含むシステムの全体構成のブロック図である。図2は、船舶追尾装置の構成を示すブロック図である。
図1に示すように、本実施形態における船舶追尾装置1は、推進駆動装置32、舵駆動装置33、センサ類34が備えられた船体31を有する自船を制御するものであって、推進駆動装置32及び舵駆動装置33を制御することによって、船体速度及び方位を制御する装置である。また、船舶追尾装置1は、自船とは異なる船舶である対象船を追尾対象とし、この対象船を追尾するように、具体的には自船と対象船とが接近するように自船の制御を行う。
センサ類34は、船体31の船首方位を検出するジャイロコンパス、船体31の対水速度を検出する速度計、GPS等の衛星測位システム(GNSS)からの船体位置を検出するGNSSセンサを含む。なお、センサ類34は、船首方位、船体位置をそれぞれ検出可能なセンサを含むものであれば良い。また、対象船の船体位置の時系列データは、位置検出装置4により検出され、船舶追尾装置1は、この船体位置の時系列データに基づいて対象船の追尾を行うものとする。なお、本実施形態においては、位置検出装置4は、船体31に搭載されたレーダ装置とするが、船体位置の時系列データを検出可能な装置であれば良く、例えば、AIS(Automatic Identification System)としても良い。
図2に示すように、船舶追尾装置1は、対象船及び自船の運動を周期的に推定する運動推定部11と、運動推定部11による推定に基づいて、対象船と自船とを遭遇させる自船の軌道計画を追尾軌道として算出する軌道計画部12と、軌道計画部12により算出された追尾軌道に追従するように自船の航行を制御する追尾制御部13とを備える。
(2 対象船の運動補足)
(2.1 船体モデル)
船体モデルについて説明する。船体運動を捕捉するため、以下の3つの船体運動モデルを定める。
(2.1.1 速度運動モデル)
一定速度の運動モデルCVを1次元で表すと
Figure 0007170019000001
になる。ここで、xは位置、uはxの速度である。
上式を2次元の差分式で表すと
Figure 0007170019000002
になる。ここで、yは位置、vはyの速度、hは差分時間、kは更新番号である。
(2.1.2 加速度運動モデル)
一定加速度の運動モデルCAを1次元で表すと
Figure 0007170019000003
になる。ここで、aはxの加速度である。
上式を2次元の差分式で表すと
Figure 0007170019000004
になる。ここで、bはvの加速度である。
(2.1.3 旋回運動モデル)
旋回運動モデルについて説明する。図3は、旋回時の対地速度ベクトルを示す図である。
一定旋回の運動モデルCTを図3に示す。ただし、速度は一定とする。このとき、2次元
の運動は
Figure 0007170019000005
になる。ここで、ωは旋回角速度、tは時間である。
上式を差分式で表すと
Figure 0007170019000006
になる。ここで、θ=ωhである。
(2.1.4 船体運動モデルの評価)
上記3つの運動モデルを船体運動に照らして評価する。船体運動は車両や飛行機と比べると、質量が大きく、加速度が小さく、旋回角速度が小さい特徴をもつ。加速度運動モデルは加速度aが小さいため、速度運動モデルで近似でき、この速度運動モデルは(3)式から
Figure 0007170019000007
になる。
旋回運動モデルは旋回角速度ωが小さいため、速度運動モデルで近似でき、この速度運動モデルは(5)式から
Figure 0007170019000008
になる。ここで、cos(ωt)≒1,sin(ωt)≒ωt,v sin(ωt)=u sin(ωt)≒0である。よって、船体運動モデルとして速度運動モデルを用いることができる。
(2.2 運動推定フィルタ)
船舶追尾装置1の運動推定部11が船体運動を推定するためのフィルタについて説明する。このフィルタは離散型線形であり、拡張カルマンフィルタ(EKF:Extended Kalman Filter)と多重モデルフィルタ(IMM:Interactive Multiple Model Filter)を利用することができる。
推定対象の状態を推定して、船体運動を求める。船体運動は
Figure 0007170019000009
になる。ここで、添字^は推定値、U^は対地速度、ψ^は対地方位である。
(2.2.1 推定対象)
推定対象はプラント状態式と観測式から、次式になる。
Figure 0007170019000010
ここで,太字はベクトル成分であることを示し、x(太字)は状態,u(太字)は入力、y(太字)は観測(出力)、w(太字)はプラント外乱、v(太字)は観測外乱、外乱は正規分布N(0,σ)に従い、σは分散、A(太字)はプラント行列、B(太字)は入力行列、C(太字)は観測行列、G(太字)は外乱行列である。ただし本実施形態において、推定対象の入力量u(太字)は未知のため,Bu(太字)項はゼロになる。
プラント行列A(太字)は、推定対象のヤコブ行列になる。船体運動モデルにおいて、一定速度の運動モデルCVと一定加速度の運動モデルCAのヤコブ行列はそれぞれ(2)式と(4)式に等しい。また、一定旋回の運動モデルCTのヤコブ行列は
Figure 0007170019000011
になる。ここで、
Figure 0007170019000012
である。
(2.2.2 EKF)
EKFを定める。
EKFは、観測更新と時間更新からなり、観測更新は制御対象の出力yを用いて、次式になる。
Figure 0007170019000013
ここで、添字は転置行列、x(太字)^k|kはx(太字)の推定値、K(太字)は推定ゲイン、P(太字)k|kは誤差共分散行列、R(太字)は重み行列(分散行列)でR(太字)=E(v(太字)v(太字) )、Eは期待値、I(太字)は適当な単位行列
Figure 0007170019000014
である。
また、時間更新は次式になる。
Figure 0007170019000015
ここで、x(太字)^k+1|kはx(太字)^k|kの時間更新、Q(太字)は重み行列でQ(太字)=E(w(太字)w(太字) )である。また、E(w(太字)w(太字) )=0とする。
(2.2.3 IMM)
IMMを定める。図4、図5は、それぞれ、EKF,IMMの構成を示す図である。
図4、図5に示すように、IMMは、EKFが単独に構成されるのに対して、3つのEKFと推定状態を調整する3つの要素から構成される。本実施形態においては、既存のIMMを利用するため、その詳細構成については省略する。IMMは全体として1つのEKFのように機能し、検出信号yを入力して推定信号x^を出力する。
(2.2.4 運動推定フィルタの比較)
運動推定フィルタとしてのEKFとIMMとを比較する。
上述したように、船体運動モデルは一定速度の運動モデルCVを含み、この運動モデルCVはEKF及びIMMのそれぞれに組み込まれる。IMMは3つの運動モデルCV,CA,CTを含む。したがって、運動モデルCVの選択が適切であれば、EKFとIMMとの性能差は顕著ではない。また、IMMの計算量は、少なくともEKFの3倍となるため、計算量の低減という観点においては、運動推定フィルタとしてEKFを用いることが望ましい。
(3 追尾軌道)
追尾軌道について説明する。図6は、追尾軌道の一例を示す図である。
本実施形態に係る船舶追尾装置1における軌道計画部12は、対象船を追尾する軌道を推測航法によって計画する。追尾軌道は、対象船、自船それぞれの運動状態を一定として、自船の軌道計画に基づいて、自船と対象船とが出会う遭遇域の位置と時刻を求めるものである。図6に示すように、自船の追尾軌道は、対象船の補足後に計画され、直線と円弧により構成される。
(3.1 対象船の軌道計画)
対象船の軌道計画について説明する。対象船の位置は、上述したように補足した状態を設定値に用いると、次式になる。
Figure 0007170019000016
ここで、太字はベクトル成分、添字ob、添字、添字(0)は、それぞれ、対象船の設定値、対地成分、初期値を示し、xobは船体位置(船位)、uob は速度、τは補足した時点をゼロとした場合の追尾時刻である。速度uob は次式で表すことができる。
Figure 0007170019000017
ここで、uob ,vob は、それぞれ、北方向、東方向の対地速度であり、Uob は対地速度(SOG:Speed Over Ground)、ψob は対地方位(COG:Course Over Ground)である。よって、対象船の船位は追尾時刻の関数になる。
(3.2 自船の軌道計画)
自船の軌道計画について説明する。図7は、自船の軌道計画の一例を示す図である。なお、図7において、点O’は自船の出発点、点Sは旋回開始点、点Fは旋回終了点、点Pは到達点をそれぞれ示す。
図7に示すように、点Pを自船の位置xとすれば、点Pは点P‘を座標変換することにより求められる。座標変換は自船の出発点O’周りに回転したものになる。点Pは関数fを用いて
Figure 0007170019000018
になる。ここで、x(太字)は自船の船位、ψsetは旋回角∠S’C’F’、dは線分O’S’の距離である。
関数fの内容を示す。点O’,S’,F’,P’の間の距離と時刻とをそれぞれ求めると、
Figure 0007170019000019
になる。ここで、dは円弧S’F’の距離、dは線分F’P’の距離、ρは旋回半径の設定値、Uは対地速度の設定値である。ただし、U>Uob である。よって、点O’から点P’までの追尾時刻はτで、追尾距離は
Figure 0007170019000020
になる。
点S’,F’,P’の座標をそれぞれ求めると、
Figure 0007170019000021
になる。ここで、O’=[0,0]、添字は転置行列、signはψの符号関数で
Figure 0007170019000022
である。
よって、点Pは次式のように点P’を座標変換することにより求められ、船位xが定まる。
Figure 0007170019000023
ここで、ψ(0)は対地方位の初期値、Ω(太字)は座標変換行列で
Figure 0007170019000024
である。
(3.3 追尾軌道の算法)
追尾軌道の算法について説明する。図8は追尾軌道が実現できない領域を示す図である。図9は追尾軌道を更新する条件を示す図である。
追尾軌道は、自船と対象船との船位を一致させるために、3つのパラメータτ,ψset,dを求めることによって得られる。追尾軌道の算法は、多変数関数の最小化問題に帰着させるものである。また、追尾軌道の仕様は、自船と対象船との船位を一致させ、自船と対象船との出会い時間を最短とし、追尾軌道における旋回角を最小にするように定められる。
このように定められた追尾軌道の評価関数Jは
Figure 0007170019000025
になる。ここで、J>0、dは自船と対象船との距離誤差で
Figure 0007170019000026
である。
評価関数JはパラメータX(太字)={τ,ψset,d}を変数とする関数になる。変数は初期値から収束値に達する。そのとき、評価量の極小値は、変数に関する偏微係数(勾配ベクトル)がゼロになるときの値に相当し
Figure 0007170019000027
になる。ここで、∂/∂X(太字)は勾配ベクトル、0(太字)はゼロベクトル、minは極小値である。
パラメータの初期値と制約値は、
Figure 0007170019000028
になる。ここで、Parはパラメータ、添字(0)は初期値、添字は上限値、添字は下限値である。なお、(37)式に制約値を加えたものについてはその説明を省略する。
図8に示すように、旋回半径ρと制約値とに基づいて、追尾軌道が実現できない領域が存在する。ここで、制約値はψ set=π,ψsetL=-πの場合である。ただし、自船と対象船との船位が近接している場合には追尾軌道は用いられない。なお、図8は一回の追尾軌道の算出の結果であり、追尾軌道は必要に応じて更新される。
追尾軌道を更新する条件は、図9に示すように、自船及び対象船のそれぞれの推測船位が出会い範囲を超えた場合
Figure 0007170019000029
になる。ここで、添字^は予測値、xezは現在の出会い位置、ρezは許容半径である。
すなわち、出会い位置と推定値としての対象船の船体位置との離間距離が設定閾値以上である場合、または、出会い位置と予測値としての自船の船体位置との離間距離が設定閾値以上である場合、追尾軌道が更新される。
(4 追尾制御)
追尾軌道に自船を追従させる追尾制御システムについて説明する。図10は、追尾制御システムの誤差を示す図である。図11は、追尾制御部の構成を示すブロック図である。
追尾制御部13は、軌道計画上で設定された時刻に対する位置に自船の位置を追従させるものである。軌道計画は、図10に示すように、直線と円弧により構成される。図10において、点Pは自船位置、点Hは軌道計画上の点Pに対する垂足位置、点Rは軌道計画上の参照位置、点Cは旋回中心点、ρは旋回半径、Sは旋回の開始点、Fは旋回の終端点をそれぞれ示す。また、dは直線または円弧における基準点から点Hまでの距離、dは直線または円弧における基準点から点Rまでの距離、dは距離誤差としての点H,R間の距離、yは航路誤差としての点H,P間の距離をそれぞれ示す。
図11に示すように、追尾制御部13は、距離制御システムと航路制御システムとを備える。距離制御システムは、軌道計画上の参照時刻に対応する参照位置Rに自船の前進方向位置Hを追従させるものであり、距離誤差dを算出する距離誤差算出部131と、距離誤差dに基づいて船体の対水速度を制御する距離制御部132とにより構成される。また、航路制御システムは、軌道計画上の垂線位置Hに自船の横方向位置Pを追従させるものであり、航路誤差yを算出する航路誤差算出部133と、航路誤差yに基づいて船体のsway方向速度を制御する航路制御部134とにより構成される。
(4.1 距離制御システム)
距離制御システムについて説明する。
(4.1.1 距離誤差)
距離誤差は、軌道計画上にある位置までの距離によって
Figure 0007170019000030
に定める。ここで、dは距離誤差、dは自船位置から参照位置(点R)までの距離、dは自船位置から軌道計画上の垂線位置(点H)までの距離であり、
Figure 0007170019000031
である。ここで、Uは対地速度の設定値、tは時間、添字、添字、添字(0)はそれぞれ、直線軌道、円弧軌道、初期値を示し、
Figure 0007170019000032
である。ここで、ψは点Hの方位である。
(4.1.2 制御器の要素)
距離制御部132における制御器の要素は
Figure 0007170019000033
に定める。ここで、sはラプラス演算子、Λ(s)は制御量、C(s)は制御器で
Figure 0007170019000034
である。ここで、Kは比例ゲイン、Tはフィルタの時定数、Tは積分器の時定数である。積分器は距離誤差に対して1形サーボ系、外乱成分(特記せず)に対して0型サーボ系を実現する。
(4.2 航路制御システム)
航路制御システムについて説明する。図12は航路制御部の構成を示すブロック図である。
航路制御部134は航路誤差yをゼロに収斂させるものであり、航路保持と航路旋回の機能をもつ。航路保持は軌道計画における直線部分(図10における(a))において実行され、航路旋回は軌道計画における円弧部分(図10における(b))において実行される。
航路制御部134は、図12に示すように、フィードバック制御部134Aと参照信号を用いるフィードフォワード制御部134Bとを有し、航路保持及び航路旋回はフィードバック制御部134A及びフィードフォワード制御部134Bによって実現される。参照信号は参照方位ψRと参照軌跡からなる。図12において、δFB,δFF,δは、それぞれ、フィードバック舵角、フィードフォワード舵角、命令舵角を示す。なお、点Pに基づくyの算出についてはその説明を省略する。
(4.2.1 航路誤差モデル)
航路誤差算出部により算出される航路誤差について説明する。図13は船体速度と潮流成分を示す図である。なお、図13において、XOYは地球固定座標を示し、Xは船体固定座標を示すものとする。
航路誤差モデルは速度ベクトル成分から導出される。船体速度及び潮流速度の対地成分は図13に示すように、船体成分と潮流成分の和で、
Figure 0007170019000035
になる。ここで、添字は船体速度の対地成分、添字は船体速度の地球固定座標成分であり、
Figure 0007170019000036
である。ここで、Ω は船体固定座標から地球固定座標に変換する行列であり、
Figure 0007170019000037
である。ここで、添字は転置行列である。
参照方位における対地速度は次式になる。
Figure 0007170019000038
ここで、添字は参照方位に関し、ψ=ψ-ψは方位偏差であり、
Figure 0007170019000039
である。
(51)式を線形近似すると、次式になる。
Figure 0007170019000040
ここで、ψeは微小角として線形化され、cos ψ≒1,sin ψ≒ψ、uψ≒uψであり、uは一定値であり、ucR,ucRは、それぞれ、参照軌跡上の接線方向、法線方向の速度成分で
Figure 0007170019000041
である。ここで、ψcR=ψ-ψである。
参照軌跡での船体速度成分は(53)式になり、潮流成分ucRが接線方向に、潮流成分vcRが法線方向に作用する。よって、直線軌道や円弧軌道を実現するためには、ucR,vcRの影響を修正する必要がある。
(4.2.2 フィードバック制御部)
フィードバック制御部について説明する。
フィードバック制御部134Aは制御対象との閉ループ系を構成し航路誤差をゼロに収束させ、潮流成分による誤差を修正する。フィードバック制御部134Aは推定器とフィルタに状態フィードバックゲインを加えたものであり、次式になる。
Figure 0007170019000042
ここで、添字は方位制御、添字は航路制御、添字^は推定値に関する。また、x(太字)^h=[x(太字)^,x(太字)^,δ^ro,x(太字)^=[ψ^,r^,x(太字)^=[ξ^,ψ^である。また、添字は転置行列に関し、ξは変数、K(太字)は推定ゲイン、F(太字)はフィードバックゲイン、fは航路ゲイン、Tはフィルタ時定数である。また、γは潮流成分の修正項であり、
Figure 0007170019000043
である。ここで、v^=Kδro、O×はi行j列のゼロ行列、δroは舵角オフセットであり、K,K,T,Tr3は船体パラメータでそれぞれ旋回力ゲイン、横流れゲインと2つの時定数であり、ζ,ω,σは波浪パラメータでそれぞれ減衰係数、固有周波数と強さのゲインである。
(4.2.3 潮流成分の推定)
潮流成分は航行中に変化するため、航路制御においては潮流成分が常時推定される。なお、潮流成分の推定(潮流推定)は船位の移動速度から求めるため、風浪による成分を分離できずに含む。潮流推定を対地座標で構成すると、次式になる。
Figure 0007170019000044
ここで、添字^は推定値、添字は検出値、x^=[x^ u^,y^=[y^ u^であり、x,yは船位、u,vは潮流成分、u,vは船体速度、K=[kc1c2は潮流推定ゲインであり、
Figure 0007170019000045
である。
(4.2.4 フィードフォワード制御部)
フィードフォワード制御部について説明する。図14は参照信号の時系列を示す図である。
フィードフォワード制御部134Bは、参照信号による開ループ制御系によって、船位を半径一定の円弧軌道に追従させるものであり、航路誤差を扱わない。参照信号は図14に示すように、参照方位ψと参照舵角δからなり、方位・舵角の変針条件を満足して、ψ=ψを実現する。ψ,δは時間関数で
Figure 0007170019000046
になる。ここで、aは係数、tは変針時間である。
参照方位の角速度r=ψ’が一定かつ前進速度uが一定のとき、参照軌跡は半径一定ρ=u÷rの円弧を描く。ここでρは旋回半径である。
(4.2.4.1 潮流成分の修正)
旋回時に生じる潮流成分による影響を修正する方法について説明する。
潮流成分は(53)式からucR,vcRになり、以下に説明するように、それぞれ、vcRは斜航角の修正、ucRは角速度の修正として、その影響が低減される。
cRは参照軌跡の法線方向成分で航路誤差を生じさせるため、斜航角の修正として、潮流斜航角
Figure 0007170019000047
によって相殺される。このとき、(58)式に示したフィードバック制御部134Aにおける処理も併せて実施される。
cRは参照軌跡の接線方向成分で前進速度を変化させるため、旋回角速度を修正することで旋回半径一定を保持する。ugRは次の条件
Figure 0007170019000048
に達したら、参照方位を次式の設定値
Figure 0007170019000049
によって更新する。ここで、添字upは初期値または更新値、usetは速度しきい値、ψsetは変針量、ψup は更新時の値である。上式の更新後、変針量設定値はψup setがゼロになるまで、次式の置き換えをする。
Figure 0007170019000050
(4.2.4.2 横流れ速度の修正)
航路誤差を生じさせる横流れ速度の修正について説明する。図15は参照軌跡のリーチ見積もりを示す図である。
旋回時の船体航跡は横流れ速度により外側にリーチ相当移動し、航路誤差を生じさせる。そのため、変針開始点を軌道計画の開始点Sからリーチだけ手前のWOP(Wheel Over Point)にする。その移動をリーチ修正と呼ぶ。
リーチ量dは図15に示すように、旋回終端点P,Fにおける接線の切片x,xから求める。ここで,αは点Pの傾きである。なお、dはキック量で、修正しない。点Pの参照軌跡を船体速度と潮流成分から求めると、
Figure 0007170019000051
になる。ここで、
Figure 0007170019000052
である。(76)式は潮流成分の影響を受けるため、前述の潮流成分の修正方法が実施される。
(5 検証)
本実施形態に係る船舶追尾装置の有効性をシミュレーションによって検証する。検証対象は、対象船の運動補足と自船の追尾制御である。
(5.1 対象船の運動補足)
対象船の運動補足に関する検証について説明する。図16は、対地方位COGと対地速度SOGとを示す図である。図17は、船位航跡を示す図である。図18は、船位時系列を示す図である。図19は、検出方位に含まれるノイズ成分の時系列を示す図である。図20は、IMM Filter結果を示す図である。図21はEKFとIMMとの比較を示す図である。図22は、EKFによるCOGとSOGの推定を示す図である。
対象船の運動捕捉をEKFとIMMによって行う。EKFは運動モデルCVを利用し、IMMは、運動モデルCV、加速度モデルCA及び運動モデルCTを利用するものとする。EKFとIMMの計算はMATLAB(登録商標)のSensor Fusion and Tracking Toolboxで実施する。
シミュレーションにおける対象船の運動条件は、刻み時間1s、加減速度±1/30m・s-2、加減速時間200s、速度6.6m・s-1、速度時間100s、旋回角速度0.5deg・s-1、旋回時間360sである。シミュレーションにおける対象船の運動と船位の状態は図16~19に示す。
図20~22は対象船の運動捕捉の結果を示す。図20はIMMにおいて3つの運動モデルの確率で、CVが9割、CTが1割とCAがゼロである。図21はEKF(CVを利用)とIMMの誤差比較を示し、図21からIMMはEKFよりわずかに誤差が小さいことがわかる。図20と図21の結果から、対象船の運動条件では運動モデルはCVを、推定方法はEKFをそれぞれ用いて実用上問題ないことが確認できる。
図22はEKF(CV)による(9)式のSOGと(10)式のCOGの推定結果を示す。図22から対象船の速度が低い、特に前進速度が小さいとき、ノイズ成分の影響が強く作用し誤差が大きくなることがわかる。また、ノイズの大きさが大きいと、フィルタゲインを小さくするので、推定値に遅れが生じる。遅れ時間は図22においては8.3秒になる。
(5.2 自船の追尾制御)
自船の追尾制御に関する検証について説明する。図23は追尾制御の追跡結果を示す図である。図24は方位制御システムの誤差と舵角を示す図である。図25は船体速度と潮流速度を示す図である。
シミュレーションにおける自船の追尾制御の結果を図23に示す。図23には、対象船の捕捉後、自船の追尾軌道計画に則り、追尾制御システムによって追従した航跡が示される。
シミュレーションにおける条件を以下に示す。対象船はUob =10kn,ψob =70deg,xob(0)=(7,-2)Nmであり、自船はU(0)=15kn,Umax=20kn,ψ(0)=60deg,x(0)=(0,0)Nmであり、潮流成分はU=5kn,ψ=60degである。
制御システムにおいては、距離制御システムのゲインはK=0.01,T=30s,T=900sである。航路制御システムのゲインは、抜粋して、船体パラメータK=0.10s-1,K=-7.4m・s-1,T=43s,ω=0.48s-1,ζ=0.1であり、ゲインf=0.99m,f=18.8s,f=0.0013m-1である。
追尾軌道の計算はMATLAB(登録商標)のfminconを利用する。その結果、出会い時間τ=2803s、追尾距離d=11.7Nm、旋回までの距離d=3.0Nm、旋回角ψset=-43.6degになる。また初期値と制約は次式になる。
Figure 0007170019000053
WOPは図23示すように、旋回開始点S(不図示)からリーチdr=167.3mだけ手前になる。図23において、遭遇域での時刻はτ’=2801.6s、自船と対象船との距離d’=1.3mになる。また、図24に示すように、方位偏差は潮流斜航角を生じ、航路誤差が旋回時にy=84.4mを生じ,遭遇域でy=1.5mになる。図25に示すように、対水速度は対地前進速度ugR=15knを保持している。潮流推定値は潮流外乱成分に適切に追従している。よって、距離制御システムと航路制御システムは適切に動作していることが確認できた。
(6 まとめ)
上述した本実施形態に係る船舶追尾装置1によれば、対象船の運動推定に用いる船体運動モデルは速度モデルが有効であり、推定手法として拡張カルマンフィルタを用いることで、有意な運動推定をより少ない計算量により実現することができる。また、推測航法による追尾軌道において、その算法は対象船と自船の軌道計画から求めるもので、多変数関数の最小化問題に帰着させる方法を用いた。また、自船の追尾制御において、制御システムは追尾軌道計画に船位を追従し、距離制御と航路制御のシステムから構成される。
本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。この実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
1 船舶追尾装置
11 運動推定部
12 軌道計画部
13 追尾制御部

Claims (4)

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

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2020183899A JP7170019B2 (ja) 2020-11-02 2020-11-02 追尾制御装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2020183899A JP7170019B2 (ja) 2020-11-02 2020-11-02 追尾制御装置

Publications (2)

Publication Number Publication Date
JP2022073731A JP2022073731A (ja) 2022-05-17
JP7170019B2 true JP7170019B2 (ja) 2022-11-11

Family

ID=81604157

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2020183899A Active JP7170019B2 (ja) 2020-11-02 2020-11-02 追尾制御装置

Country Status (1)

Country Link
JP (1) JP7170019B2 (ja)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117270402B (zh) * 2023-11-09 2024-01-26 西北工业大学 一种适用于极端风场的无人机复合抗扰航迹跟踪控制方法
CN117406746B (zh) * 2023-11-20 2024-09-03 大连海洋大学 一种考虑恶劣海况的渔船航迹保持控制器的设计方法
CN117647933B (zh) * 2024-01-26 2024-03-29 中国人民解放军国防科技大学 一种面向精度提升的轨迹规划方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011011641A (ja) 2009-07-02 2011-01-20 National Maritime Research Institute 操縦性能向上を図った船舶
JP2016102743A (ja) 2014-11-28 2016-06-02 古野電気株式会社 追尾処理装置、レーダ装置、および、追尾処理方法
JP2018103949A (ja) 2016-12-28 2018-07-05 株式会社三井E&Sホールディングス 船舶の自動操縦システム、船舶、及び船舶の自動操縦方法
WO2018193596A1 (ja) 2017-04-20 2018-10-25 富士通株式会社 衝突リスク算出プログラム、衝突リスク算出方法および衝突リスク算出装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS58192109A (ja) * 1982-05-07 1983-11-09 Hitachi Ltd 船舶の自動操縦方式

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011011641A (ja) 2009-07-02 2011-01-20 National Maritime Research Institute 操縦性能向上を図った船舶
JP2016102743A (ja) 2014-11-28 2016-06-02 古野電気株式会社 追尾処理装置、レーダ装置、および、追尾処理方法
JP2018103949A (ja) 2016-12-28 2018-07-05 株式会社三井E&Sホールディングス 船舶の自動操縦システム、船舶、及び船舶の自動操縦方法
WO2018193596A1 (ja) 2017-04-20 2018-10-25 富士通株式会社 衝突リスク算出プログラム、衝突リスク算出方法および衝突リスク算出装置

Also Published As

Publication number Publication date
JP2022073731A (ja) 2022-05-17

Similar Documents

Publication Publication Date Title
JP7170019B2 (ja) 追尾制御装置
Fossen et al. Line-of-sight path following for dubins paths with adaptive sideslip compensation of drift forces
JP5993125B2 (ja) 船舶用自動操舵装置
WO2021230356A1 (ja) 船舶の自動誘導方法、船舶の自動誘導プログラム、船舶の自動誘導システム、及び船舶
Fossen et al. Nonlinear output feedback control of dynamically positioned ships using vectorial observer backstepping
JP5042906B2 (ja) 船舶用自動操舵装置
Dukan et al. Sea floor geometry approximation and altitude control of ROVs
JP5042905B2 (ja) 船舶用自動操舵装置
JP5052165B2 (ja) 船舶用自動操舵装置
CN115014355A (zh) 一种双体无人船的定点返航调控方法及装置
JP6658220B2 (ja) 航走制御方法および装置
Han et al. A Ship Path Tracking Control Method Using a Fuzzy Control Integrated Line-of-Sight Guidance Law
Lee et al. Design and experiment of a small boat track-keeping autopilot
EP4227214A1 (en) Aircraft control system, aircraft, aircraft control method, and program
de la Cruz et al. A streamlined nonlinear path following kinematic controller
JP7177116B2 (ja) 船体位置保持装置
Miller Model predictive ship trajectory tracking system based on line of sight method
JP7177117B2 (ja) 船体位置保持装置
Wang et al. Nonlinear path following of autonomous underwater vehicle considering uncertainty
Karras et al. Motion control for autonomous underwater vehicles: A robust model—Free approach
JP6373205B2 (ja) 船舶用自動操舵装置
Daly et al. A nonlinear path following controller for an underactuated unmanned surface vessel
WO2023089843A1 (ja) 船舶用自動操舵装置
WO2024204611A1 (ja) 操船制御装置
Li et al. Path following for underactuated ships control and simulation based on active disturbance rejection with sliding mode control

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20210601

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220621

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220808

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20221004

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20221031

R150 Certificate of patent or registration of utility model

Ref document number: 7170019

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150