JP6487264B2 - ウェイポイント生成装置 - Google Patents

Description

本発明は、移動体の航路を定めるウェイポイントを生成する技術に関する。

船舶用自動操舵装置は、舵角を制御して参照方位に船首方位を追従させる方位制御システム（ＨＣＳ：Ｈｅａｄｉｎｇ Ｃｏｎｔｒｏｌ Ｓｙｓｔｅｍ）と、舵角を制御して計画航路に船体位置を追従させる航路制御システム（ＴＣＳ：Ｔｒａｃｋ Ｃｏｎｔｒｏｌ Ｓｙｓｔｅｍ）とに分けられる。参照方位によって計画航路に追従させる方位制御システムによれば、船体位置が航路から離れてしまうために適宜方位を修正する必要がある。一方、航路制御システムによれば、計画航路に船体位置を追従させるため、方位制御システムと比較して、目的地までの航海を簡易に実現することができる。なお、この航路制御システムにおいて、計画航路は、その上位システムである電子海図情報表示システム（ＥＣＤＩＳ：Ｅｌｅｃｔｒｏｎｉｃ Ｃｈａｒｔ Ｄｉｓｐｌａｙ ａｎｄ Ｉｎｆｏｒｍａｔｉｏｎ Ｓｙｓｔｅｍ）において、船舶の目的地までの通過点であるウェイポイントから定められるため、船舶用自動操舵装置として航路制御システムを導入するには、電子海図情報表示システムを導入する必要があった。

また、電子海図情報表示システムに関連する技術として、移動体の位置及び向きを取得し、複数のウェイポイントを直線上に並べたルートを縦軸とし、このルートに対する移動体又は他物標の左右方向の位置関係を横軸とする情報を直線ルート表示領域に表示し、ルートに対する移動体又は他物標の表示位置を制御することにより、ルートを直線状に変換して表示するとともに、ルートに対する移動体又は他物標の位置関係を表示するルート表示装置、が知られている（特許文献１参照）。

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

近年、運航における低エネルギー、低コスト及び短時間で行うという観点から、小型または中型の船舶においても、航路制御システムによる運航が求められている。しかしながら、小型または中型の船舶において計画航路に船体位置を追従させようとした場合、小型または中型の船舶の船橋の広さが電子海図情報表示システムを設置するのに十分でなく、また電子海図情報表示システムを導入するためのコストが船舶の規模に対して高いため、船体位置を追従させる計画航路を定めるためのウェイポイントを設定することができない、という問題があった。

本発明の実施形態は、上述した問題点を解決するためになされたものであり、電子海図情報表示システムを導入することなくウェイポイントを設定することができるウェイポイント生成装置を提供することを目的とする。

上述した課題を解決するため、本実施形態のウェイポイント生成装置は、移動体の目的地座標Ｐ（ｘ_ｐ，ｙ_ｐ）と、前記移動体の参照方位である初期方位ψ_０と、前記移動体の現在位置座標Ｏ（ｘ_ｏ，ｙ_ｏ）と、前記移動体の旋回径における旋回半径Ｒとを取得するデータ取得部と、前記現在位置座標Ｏから前記初期方位ψ_０方向に前記移動体の空走距離だけ離れた第１ウェイポイントＳ（ｘ_ｓ，ｙ_ｓ）と前記目的地座標Ｐとを、前記現在位置座標Ｏを原点とした前記初期方位ψ_０のマイナス方向に座標変換して正規化する第１座標変換部と、前記正規化された座標において、正規化された第１ウェイポイントＳ’（ｘ_ｓ’，ｙ_ｓ’）と前記旋回半径Ｒとに基づいて、第１ウェイポイントＳ’に対して初期方位ψ_０方向に所定の間隔距離を有した位置にある第２ウェイポイントＡ’（ｘ_ａ’，ｙ_ａ’）と、該第２ウェイポイントＡ’と正規化された目的地座標Ｐとを結ぶ線分上に前記第２ウェイポイントＡ’に対して前記間隔距離を有した位置にある第３ウェイポイントＦ’（ｘ_ｆ’，ｙ_ｆ’）とを生成するウェイポイント生成部と、前記ウェイポイント生成部により生成された第２ウェイポイントＡ’及び第３ウェイポイントＦ’を、前記現在位置座標Ｏを原点として前記初期方位ψ_０のプラス方向に座標変換する第２座標変換部と、前記第１ウェイポイントＳと前記第２座標変換部により座標変換された第２ウェイポイントＡ及び第３ウェイポイントＦとを出力する出力部とを備える。

本発明の実施形態によれば、電子海図情報表示システムを導入することなくウェイポイントを設定することができる。

ウェイポイント生成装置、船舶用自動操舵装置及び制御対象を含む全体ブロック図である。 フィードバック制御部の構成を示すブロック図である。 航路制御システムで用いる座標系を示す図である。 航路保持制御に係るフィードバック制御ループを示すブロック図である。 ψ_Ｒとδ_Ｒの時系列の一例を示す図である。 参照方位による旋回航跡と参照航跡の関係を示す図である。 ウェイポイント生成装置の機能構成を示すブロック図である。 計画航路の座標変換を示す図である。 １回のウェイポイント生成例を示す図である。 複数回のウェイポイント生成例を示す図である。 方位制御モードから航路制御モードに遷移した場合の過渡応答を示す図である。 航路制御モードで曲線追従した場合の過渡応答を示す図である。

以下、図面を参照しながら、本発明の実施形態について説明する。以降の説明において、各記号は、変数、修飾、添字として、以下の表のように定義される。

１． 船舶用自動操舵装置の構成
まず、本発明の船舶用自動操舵装置を含むシステムについて説明する。図１は、船舶用自動操舵装置と制御対象の全体のブロック図である。図２は、フィードバック制御部の構成を示すブロック図である。

図１に示すように、船舶用自動操舵装置１は、計画航路に船体位置を追従させるために舵を制御する装置であり、ウェイポイント生成装置１０、軌道計画部１１、軌道航路誤差演算部１２、フィードバック制御部１３、加算器１４及び各パラメータを同定する図示しない同定器を備えている。

船体２のセンサ類３は、船体２のｓｕｒｇｅ速度ｕを検出するスピードログ、船体２の船首方位ψを検出するジャイロコンパス、ＧＰＳ等の衛星測位システム（ＧＮＳＳ）からの船体位置（ｘ，ｙ）を検出するＧＮＳＳセンサを含む。

ウェイポイント生成装置１０は、センサ類３から入力されるｓｕｒｇｅ速度ｕ、船首方位ψ及び軌道計画部１１から入力される参照方位ψ_Ｒに基づいて、計画航路を出力する。

軌道計画部１１は、ウェイポイント生成装置１０から入力される計画航路、センサ類３から入力されるｓｕｒｇｅ速度ｕに基づいて、参照方位ψ_Ｒ、参照方位ｘ_Ｒ、ｙ_Ｒといった参照信号を出力するとともに、変針中にはフィードフォワード舵角δ_ＦＦを出力する。

軌道航路誤差演算部１２は、センサ類３から入力される船首方位ψ、船体位置（ｘ，ｙ）と、軌道計画部１１から入力される参照方位ψ_Ｒ、参照位置ｘ_Ｒ，ｙ_Ｒとの比較を行い、方位誤差ψ_ｅ、航路誤差ｘ_ｅ，ｙ_ｅを出力する。

船舶用自動操舵装置１の閉ループ系は、船体モデルと外乱モデルとからなる制御対象と、フィードバック制御部１３とから構成される。フィードバック制御部１３は、図２に示すように、推定器１３１と制御系フィードバックゲイン器１３２とを備える。推定器１３１は、軌道航路誤差演算部１２から入力された方位誤差ψ_ｅ、航路誤差ｘ_ｅ、ｙ_ｅに基づいて、外乱が除去された推定方位誤差ψ_ｅ＾、推定旋回角誤差ｒ＾といった方位に関する状態量の推定値と、横方向の推定航路誤差ｙ_ｅ＾といった航路に関する状態量の推定値とを出力する。また、推定器１３１は対地座標系での演算を行い、推定潮流ベクトルｕ_ｃ＾，ｖ_ｃ＾といった潮流に関する推定値を出力する。制御系フィードバックゲイン器１３２は、推定方位誤差ψ_ｅ＾、推定旋回角誤差ｒ＾に対してフィードバックゲインを掛けるとともに、推定航路誤差ｙ_ｅ＾に対してフィードバックゲインを掛け、これらの結果を加算してフィードバック舵角δ_ＦＢを出力する。

船体２の操舵機は、上述した構成による船舶用自動操舵装置１による命令舵角δ_Ｃに比例した舵角を動かすため、船体３は、舵角によって旋回角速度を生じ、方位、位置が変化する。また旋回角速度の発生と共に、斜航角（横方向速度）が発生する。

２． 定式化
２．１ 座標系
ここで、航路座標系について説明する。図３は、航路制御システムで用いる座標系を示す図である。

図３に示すように、航路制御システムで用いる座標系は、地球固定座標系Ｏ−ＸＹ、船体固定座標系Ｇ−Ｘ_ＢＹ_Ｂと参照座標系Ｏ_Ｒ−Ｘ_ＲＹ_Ｒからなり、いずれも右手系３軸直交座標系である。これらの座標系においてＺ軸は重力方向を正とし、回転極性は右ねじ方向を正とする。なお、座標系はＸ軸、Ｙ軸の２次元を用いるため、図３においてＺ軸は省略される。また、地球固定座標系はＸ軸を北向きにとり、船体固定座標系はＸ軸を船首方位にとり、参照座標系はＸ軸を前進方向にとる。

航路制御システムにおいて計画航路は、図３においてＷ_Ｐとして示されるウェイポイントを結ぶ経路として設定され、直線航路と曲線航路からなる。参照航路は計画航路上に置かれ、直線航路上においては原点を適当な位置に固定したものとして、また、曲線航路上においては旋回中心と船体位置とを結ぶ線との交点に置いたものとして定められる。なお、図３において、ｕはｓｕｒｇｅ速度、ｖはｓｗａｙ速度、ｒはｙａｗ（旋回）角速度、ψは船首方位、ψ_Ｒは参照方位、δは舵角、ｘ，ｙは船体位置、ｘ_Ｒ，ｙ_Ｒは参照航路、Ｒは旋回半径、ｘ_Ｃ，ｙ_Ｃは旋回中心、Ｕは対水速度でＵ＝√（ｕ^２＋ｖ^２）、添字_ｃは潮流成分を示す。これらより、方位誤差ψ_ｅ及び航路誤差ｘ_ｅ，ｙ_ｅは次式になる。

ここで、ｄｉｓｔ_ｘ＝ｘ’−ｘ_ｃ、ｄｉｓｔ_ｙ＝ｙ’−ｙ_ｃ、Ｍは座標変換行列、

２．２ 制御対象
ここで、制御対象について説明する。制御対象は船体モデルと外乱モデルから構成される。船体モデルは、ｓｕｒｇｅ速度を一定として、野本の応答モデルを改良した本発明者による応答モデルを用いて、

に定める。ここでｓはラプラス演算子、Δ_ｃ（ｓ）、δ_ｃは命令舵角、Ｋ_ｒは旋回力ゲイン、Ｋ_ｖは横流れゲイン、Ｔ_ｒ，Ｔ_ｒ３，Ｔ_ｖは時定数を示す。本実施の形態においては、安定船を対象とし、δ_ｃは操舵機応答が船体運動に比べて十分に速いものとしてδ_ｃ≒δとおく。この船体モデルは野本の船体モデルと比較して、Ｔ_ｒ３を有し、Ｔ_ｖ＝Ｔ_ｒという特徴をもつ。船体方位モデルの状態空間表現は（５）式より次式になる

ここでｘ_ｗ＝［ψ ｒ_ｘ］^Ｔ，添字^Ｔは転置行列、Ｃ_Ｔ３＝１−（Ｔ_ｒ３／Ｔ_ｒ）、

外乱モデルは、舵角オフセットδ_ｏｒ，δ_ｏｖ、波浪成分ψ_ｗ及び図３に示される潮流成分ｕ_ｃ，ｖ_ｃとする。ここで、δ_ｏｒ，δ_ｏｖは、風や船体特性などに誘起された方位軸回り（ｙａｗ方向）に作用する角速度、ｓｗａｙ方向に作用する速度をそれぞれ舵角換算したものであり、方位誤差と航路誤差を発生させる。ψ_ｗは白色ノイズνが入力した狭帯域フィルタ出力を方位換算したものであり、δ_ｃに無効舵を発生させる。ｕ_ｃ，ｖ_ｃは航路誤差を発生させる。このとき

とする。ここでｘ_ｗ＝［ξ ψ_ｗ］^Ｔ、ξは変数、Ｋ_ｗ，ζ_ｗ，ω_ｗはそれぞれ波浪モデルのゲイン、減衰係数、固有周波数として、

２．３ 航路誤差モデル
航路誤差モデルを導出する。船体の対地速度は

と定める。ここで添字_ｇは対地速度成分を示す。これにより参照座標における対地速度の誤差は、

ここでｕ_Ｒ，ｖ_Ｒは参照速度を示す。ｖ_Ｒ＝０，ｃｏｓψ_ｅ≒１，ｓｉｎψ_ｅ≒ψ_ｅを用い、

３．航路制御システム
航路制御システムとして、フィードバック制御部１３が航路保持制御を行い、軌道計画部１１が参照信号を出力するとともに、フィードフォワード舵角δ_ＦＦを出力することによって曲線追従制御を行う。以下、航路保持制御及び曲線追従制御について説明する。

３．１ 航路保持制御
まず、航路保持制御について説明する。図４は、航路保持制御に係るフィードバック制御ループを示すブロック図である。

図４に示すように、航路保持制御は、推定器１３１及び制御系フィードバックゲイン器１３２による閉ループ制御によって、閉ループ安定性及び外乱除去性を確保する。閉ループ安定性の状態空間表現は、

になる。ここでｘ_ｈ＝［ｘ_ｒ ｘ_ｗ δ_ｏｒ］^Ｔ、ψ^‐＝−ψ−ψ_ｗ：検出方位、ｘ_ｔ＝［ｙ_ｅ ｖ_ｅｏ］^Ｔ、ｙ_ｅ ^‐は検出航路誤差、ｖ_ｅｏ＾はオフセット成分、添字_ｈ，_ｔ，＾，^‐はそれぞれ方位制御ループ、航路制御ループ、推定値、検出値、Ｋは推定ゲイン、Ｆはフィードバックゲイン、Ｏ_ｉ×ｊ：ｉ行ｊ列のゼロ行列を用いて、

潮流成分ｕ_ｃ，ｖ_ｃは航行中に変化するため、これらは常に推定される。なお、潮流推定は船体位置の移動速度から求めるため、風浪や船体２の不釣合いによる移動速度を分離できず含む。よって、ｕ_ｃ，ｖ_ｃは推定器１３１によって次式より推定される。

ここでｘ_ｃｕ＝［ｘ ｕ_ｃ］^Ｔ，ｘ_ｃｖ＝［ｙ ｖ_ｃ］^Ｔ、ｘ^‐，ｙ^‐は船体位置検出量、Ｋ_ｃは潮流推定ゲインを示す。

３．２ 曲線追従制御
次に、曲線追従制御について説明する。曲線追従制御は、軌道計画部１１による参照信号の出力とフィードフォワード制御からなる閉ループ制御によって、参照方位及び参照航路に船首方位及び船体位置を追従させる。軌道計画部１１により出力される軌道計画に次の条件を組み込むことによって適切な変針が実現できる。参照方位ψ_Ｒは、変針条件（変針量Δψ_０、指定旋回角速度ｒ_０）、操舵機条件（舵角設定値δ_０、舵速度設定値δ_０ ^・）及び方位初期値Ｃ_１ａ＝ψ_Ｒ ^・・（０），Ｃ_２ａ＝ψ_Ｒ ^・（０）を満足する関数で

になる。ここでηは定数。

フィードフォワード舵角δ_ＦＦ（参照舵角δ_Ｒ）は方位伝達関数Ｐ_ψ（ｓ）の逆モデルを用いると（簡単化のためＴ_ｒ３＝０）、船首方位は、

になり、参照方位に一致する。

ψ_Ｒとδ_Ｒの時系列の一例を図５に示す。この図において、Ｔは時間、添字_ａ，_ｖ，_ｄはそれぞれ加速、等速、減速の各モード、Ｋ_ｒは旋回力ゲイン、Ｔ_ｒは時定数を示す。

参照方位による旋回航跡と参照航跡の関係を図６に示す。この図において、旋回航跡はＸＹ系として点線で示され、参照航跡はξη系として実線で示される。参航跡は船体横流れ速度ｖ_Ｒのため、旋回航跡を原点Ｏ周りに斜航角β_Ｒ＝ｓｉｎ^−１（Ｕ／ｖ_Ｒ）相当の座標回転したものに近似できる。よって、船体航跡を参照航跡に乗せるため、図６より参照方位はｘ_βＲ相当（リーチ）だけ手前から変針を開始させる。

４． ウェイポイント生成装置
次に、ウェイポイント生成装置によるウェイポイントの生成について説明する。

４．１ ウェイポイント生成の概要
ウェイポイント生成装置１０は、船体２の現在位置とその時点での参照方位の方向に位置する仮想点を直線で結んだ航程線を生成し、航程線に船体位置を航路制御システムによって追従させる。ここで仮想点は現在位置から十分に離れたものとし、本実施の形態においては、数ＮＭから数十ＮＭとする。このような航程線を船舶が航行している間に新たな目的地が入力された場合、ウェイポイント生成装置１０は、計画航路を再構築して軌道計画部１１に出力する。この計画航路の構築において、ウェイポイント生成装置１０は、船舶が参照方位（初期方位）ψ_０で航行中に目的地Ｐが与えられたとき、現在位置をＯとして、後述する３点のウェイポイントＳ，Ａ，Ｆと変針量Δψを求める。

４．２ ウェイポイント生成装置の構成及び動作
ウェイポイント生成装置の機能構成及び動作について説明する。図７は、ウェイポイント生成装置の機能構成を示すブロック図である。図８は、計画航路の座標変換を示す図である。

ウェイポイント生成装置１０は、機能として、データ取得部１０１、空走距離算出部１０２、第１座標変換部１０３、ウェイポイント生成部１０４、第２座標変換部１０５、出力部１０６を備え、これらの機能は、ウェイポイント生成装置１がハードウェアとして備えるＣＰＵとメモリが協働することによって実現される。以下、各機能について説明する。

＜データ取得部＞
データ取得部１０１は、パラメータとして、航海情報及び旋回条件を取得する。航海情報は、入力された目的地の座標Ｐと、座標Ｐが与えられた時点の参照方位である初期方位ψ_０と、同時点における現在位置の座標Ｏとを含む。旋回条件は、船舶の旋回径における旋回半径Ｒと、空走距離ＥＤまたは空走距離ＥＤを算出するための空走距離パラメータとを含む。ここで、空走距離ＥＤは、現在位置Ｏから航程線上に位置するウェイポイントＳまでの距離であり、空走距離パラメータは船体２の船長Ｌまたは船速ｕであり、空走距離ＥＤ、船長Ｌ、船速ｕのいずれかが取得されれば良い。これらのうち、空走距離ＥＤ及び船長Ｌは操船者によって入力されるパラメータであり、船速ｕはセンサ類３から入力されるｓｕｒｇｅ速度ｕである。なお、空走距離ＥＤは予め定められた所定の値であっても良い。

＜空走距離算出部＞
空走距離算出部１０２は、データ取得部１０１により空走距離ＥＤが取得されない場合、即ち、船長Ｌ、船速ｕのいずれかが取得された場合、取得された船長Ｌまたは船速ｕに基づいて、以下の式により空走距離ＥＤを算出する。

ここでＣ_ｅｄは空走係数、ｔ_ｅｄは空走時間を示し、いずれも予め設定された値とする。このとき、ウェイポイントＳの座標は次式になる。

ここでｘ_０，ｙ_０は現在位置Ｏの座標であり、簡単化のためゼロとする。また、添字_ｓはウェイポイントＳを示し、以降の説明における添字_ｃ，_ｐ，_ａ，_ｆはそれぞれ、現在位置（原点）Ｏ、目的地Ｐ、ウェイポイントＡ、ウェイポイントＦを示す。

＜第１座標変換部＞
第１座標変換部１０３は、ウェイポイントＳ及び目的地Ｐを逆座標変換することにより正規化する。具体的には、第１座標変換部１０３は、図８に示すように、点Ｓ（ｘ_ｓ，ｙ_ｓ）、点Ｐ（ｘ_ｐ，ｙ_ｐ）を、現在位置Ｏを原点として初期方位ψ_０のマイナス方向に座標変換する。この座標変換によれば、座標変換された点Ｓ、Ｐは、それぞれ、

になる。ここで添字’は正規化した座標（初期方位が北向き）、ｙ_ｓ’＝０、Ｍは行列、添字^Ｔは転置行列を示す。

＜ウェイポイント生成部＞
ウェイポイント生成部１０４は、正規化した座標において、旋回径の半径中心Ｃ、ウェイポイントＡ、ウェイポイントＦ及び変針量Δψを求める。ここで、ウェイポイントＡは初期方位を向く航程線上においてウェイポイントＳと所定の間隔距離を有した位置にある点であり、ウェイポイントＦはウェイポイントＡと目的地Ｐとを結ぶ航程線上においてウェイポイントＡとウェイポイントＳとの間隔距離と同様の間隔距離を有した位置にある点である。

ウェイポイント生成部１０４は、正規化された旋回径の半径中心Ｃ’の座標を次式により求める。

ここでｓｉｇｎは引数の符号を出力する符号関数を示す。

また、ウェイポイント生成部１０４は、ウェイポイントＡ’、ウェイポイントＦ’を、正規化した座標上のウェイポイント間の位置関係から求める。ウェイポイントＡ’、ウェイポイントＦ’及び変針量Δψの関係式は

ここで｜Δψ｜はΔψ＜０のための符号操作。上式を整理すると、次の方程式

を得る。ここでΔｘ＝ｘ_ｐ’−ｘ_ｓ’，Δｙ＝ｙ_ｐ’−ｙ _ｓ ’＝ｙ_ｐ’。上式は超越関数のため、ウェイポイント生成部１０４は、この関数を数値計算によって解くことにより、ウェイポイントＳ’、ウェイポイントＡ’、ウェイポイントＦ’及び変針量Δψを算出する。この数値計算には、初期値Δψ_０を与え、例えば黄金分割探索を用いると良い。この場合、｜ｆ（Δψ）｜または（ｆ（Δψ））^２が用いられる。また、初期値はΔψ_０＝ｔａｎ^−１（ｙ_ｐ’−ｙ_ｓ’）÷（ｘ_ｐ’−ｘ _ｓ’）から与えられる。

＜第２座標変換部＞
第２座標変換部１０５は、ウェイポイント生成部１０４により生成された正規化されたウェイポイント及び半径中心を正座標変換することによりウェイポイントを得る。具体的には、第２座標変換部１０５は、ウェイポイントＡ’、ウェイポイントＦ’及び半径中心Ｃ’を、現在位置Ｏを原点として初期方位ψ_０の正方向に座標変換する。Ａ’、Ｆ’Ｃ’は次式から求まる。

＜出力部＞
出力部１０６は、得られたウェイポイントＳ、Ａ、Ｆ、及び旋回径の半径中心Ｃを計画航路として、軌道計画部１１へ出力する。
５． 検証
本実施形態をシミュレーションによって検証する。

＜ウェイポイント生成＞
１回のウェイポイント生成例を図９に示す。ここで条件は、Ｌ＝２００ｍ，ＥＤ＝１０Ｌ［ｍ］，Ｒ＝１ＮＭ＝１８５２ｍ，ψ_０＝３０ｄｅｇ，Ｏ＝［０，０］ｍ，Ｐ＝［５０００，５０００］ｍとする。なお、変針量は、初期値Δψ_０＝２０．８ｄｅｇ、Δψ＝２２．３ｄｅｇになる。

複数回のウェイポイント生成例を図１０に示す。ここで条件は、ＥＤ＝３Ｌ［ｍ］，ψ_０＝２０ｄｅｇ，Ｐ_ｉ，ｉ＝１〜４を与えた場合である。

＜航路制御のシミュレーション結果＞
図１１は、方位制御モードから航路制御モードに遷移した場合の過渡応答を示す。方位制御モードでは方位偏差がゼロであるが，航路制御モードでは航路誤差がゼロになる。その際潮流成分に対抗するため，斜航角はβ＝ｓｉｎ^−１（５／１０）＝３０［ｄｅｇ］、航路誤差は最大−２００ｍ弱生じる。条件は、船体パラメータ：ｕ＝５．１４ｍ／ｓ，Ｋ_ｒ＝０．０７８５１／ｓ，Ｋ_ｖ＝−４．７３ｍ／ｓ，Ｔ_ｒ＝８３．５ｓ，Ｔ_ｒ３＝４．５ｓ，Ｔ_ｖ３＝０ｓ。潮流成分：Ｕ_ｃ＝５ｋｎ，ψ_ｃ＝３１０ｄｅｇ。制御システム：Ｋ_ｒ＝０．０２６７１／ｓ，Ｋ_ｖ＝−２．６７ｍ／ｓ，Ｔ_ｒ＝３６．１ｓ，Ｔ_ｒ３＝３．６ｓ。制御ゲイン：ｆ_ｐ＝１．５，ｆ_ｄ＝２２．７ｓ，ｆ_ｙ＝０．００１９７ｍ／ｓ。初期方位４０ｄｅｇ，変針量１０［ｄｅｇ］。

図１２は航路制御モードで曲線追従した場合の過渡応答を示す。船体モデルのパラメータが制御対象と制御システムとで異なるが、顕著な誤差は生じていない。したがって、ウェイポイント生成装置１０により出力されたウェイポイントは有効である。

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

１０ ウェイポイント生成装置
１０１ データ取得部
１０３ 第１座標変換部
１０４ ウェイポイント生成部
１０５ 第２座標変換部
１０６ 出力部

Claims (6)

1. 移動体の目的地座標Ｐ（ｘ_ｐ，ｙ_ｐ）と、前記移動体の参照方位である初期方位ψ_０と、前記移動体の現在位置座標Ｏ（ｘ_ｏ，ｙ_ｏ）と、前記移動体の旋回径における旋回半径Ｒとを取得するデータ取得部と、
   前記現在位置座標Ｏから前記初期方位ψ_０方向に前記移動体の空走距離だけ離れた第１ウェイポイントＳ（ｘ_ｓ，ｙ_ｓ）と前記目的地座標Ｐとを、前記現在位置座標Ｏを原点とした前記初期方位ψ_０のマイナス方向に座標変換して正規化する第１座標変換部と、
   前記正規化された座標において、正規化された第１ウェイポイントＳ’（ｘ_ｓ’，ｙ_ｓ’）と前記旋回半径Ｒとに基づいて、第１ウェイポイントＳ’に対して初期方位ψ_０方向に所定の間隔距離を有した位置にある第２ウェイポイントＡ’（ｘ_ａ’，ｙ_ａ’）と、該第２ウェイポイントＡ’と正規化された目的地座標Ｐとを結ぶ線分上に前記第２ウェイポイントＡ’に対して前記間隔距離を有した位置にある第３ウェイポイントＦ’（ｘ_ｆ’，ｙ_ｆ’）とを生成するウェイポイント生成部と、
   前記ウェイポイント生成部により生成された第２ウェイポイントＡ’及び第３ウェイポイントＦ’を、前記現在位置座標Ｏを原点として前記初期方位ψ_０のプラス方向に座標変換する第２座標変換部と、
   前記第１ウェイポイントＳと前記第２座標変換部により座標変換された第２ウェイポイントＡ及び第３ウェイポイントＦとを出力する出力部と
   を備えるウェイポイント生成装置。
2. 前記ウェイポイント生成部は、前記移動体の変針量をΔψ、Δｘ＝ｘ_ｐ’−ｘ_ｓ’，Δｙ＝ｙ_ｐ’−ｙ _ｓ ’＝ｙ_ｐ’として、関数
   

   

   を解くことによって、前記正規化された座標における第２ウェイポイントＡ’及び第３ウェイポイントＦ’を生成することを特徴とする請求項１に記載のウェイポイント生成装置。
3. 前記ウェイポイント生成部は、初期値Δψ_０を与えた黄金分割探索により前記関数を解くことを特徴とする請求項２に記載のウェイポイント生成装置。
4. 前記初期値Δψ_０＝ｔａｎ^−１（ｙ_ｐ’−ｙ_ｓ’）÷（ｘ_ｐ’−ｘ _ｓ’）であることを特徴とする請求項３に記載のウェイポイント生成装置。
5. 前記ウェイポイント生成部は、正規化された第１ウェイポイントＳ’と前記旋回半径Ｒとに基づいて、前記正規化された座標において、前記移動体の旋回径の旋回中心Ｃ’（ｘ_ｃ’，ｙ_ｃ’）を生成し、
   前記第２座標変換部は、前記旋回中心Ｃ’を、前記現在位置座標Ｏを原点として前記初期方位ψ_０のプラス方向に座標変換し、
   前記出力部は、前記第２座標変換部により座標変換された旋回中心Ｃを出力することを特徴とする請求項１乃至請求項４のいずれか一項に記載のウェイポイント生成装置。
6. 前記ウェイポイント生成部は、前記旋回中心Ｃ’（ｘ_ｃ’，ｙ_ｃ’）を、ｓｉｇｎを引数の符号を出力する符号関数として、
   

   

   により生成することを特徴とする請求項５に記載のウェイポイント生成装置。

Images