(57)【特許請求の範囲】
【請求項１】
移動体の目的地座標Ｐ（ｘｐ，ｙｐ）と、前記移動体の参照方位である初期方位ψ０と、前記移動体の現在位置座標Ｏ（ｘｏ，ｙｏ）と、前記移動体の旋回径における旋回半径Ｒとを取得するデータ取得部と、
前記現在位置座標Ｏから前記初期方位ψ０方向に前記移動体の空走距離だけ離れた第１ウェイポイントＳ（ｘｓ，ｙｓ）と前記目的地座標Ｐとを、前記現在位置座標Ｏを原点とした前記初期方位ψ０のマイナス方向に座標変換して正規化する第１座標変換部と、
前記正規化された座標において、正規化された第１ウェイポイントＳ’（ｘｓ’，ｙｓ’）と前記旋回半径Ｒとに基づいて、第１ウェイポイントＳ’に対して初期方位ψ０方向に所定の間隔距離を有した位置にある第２ウェイポイントＡ’（ｘａ’，ｙａ’）と、該第２ウェイポイントＡ’と正規化された目的地座標Ｐとを結ぶ線分上に前記第２ウェイポイントＡ’に対して前記間隔距離を有した位置にある第３ウェイポイントＦ’（ｘｆ’，ｙｆ’）とを生成するウェイポイント生成部と、
前記ウェイポイント生成部により生成された第２ウェイポイントＡ’及び第３ウェイポイントＦ’を、前記現在位置座標Ｏを原点として前記初期方位ψ０のプラス方向に座標変換する第２座標変換部と、
前記第１ウェイポイントＳと前記第２座標変換部により座標変換された第２ウェイポイントＡ及び第３ウェイポイントＦとを出力する出力部と
を備えるウェイポイント生成装置。
【請求項２】
前記ウェイポイント生成部は、前記移動体の変針量をΔψ、Δｘ＝ｘｐ’−ｘｓ’，Δｙ＝ｙｐ’−ｙｓ’＝ｙｐ’として、関数
【数１】
0006487264.tif 000027
を解くことによって、前記正規化された座標における第２ウェイポイントＡ’及び第３ウェイポイントＦ’を生成することを特徴とする請求項１に記載のウェイポイント生成装置。
【請求項３】
前記ウェイポイント生成部は、初期値Δψ０を与えた黄金分割探索により前記関数を解くことを特徴とする請求項２に記載のウェイポイント生成装置。
【請求項４】
前記初期値Δψ０＝ｔａｎ−１（ｙｐ’−ｙｓ’）÷（ｘｐ’−ｘｓ’）であることを特徴とする請求項３に記載のウェイポイント生成装置。
【請求項５】
前記ウェイポイント生成部は、正規化された第１ウェイポイントＳ’と前記旋回半径Ｒとに基づいて、前記正規化された座標において、前記移動体の旋回径の旋回中心Ｃ’（ｘｃ’，ｙｃ’）を生成し、
前記第２座標変換部は、前記旋回中心Ｃ’を、前記現在位置座標Ｏを原点として前記初期方位ψ０のプラス方向に座標変換し、
前記出力部は、前記第２座標変換部により座標変換された旋回中心Ｃを出力することを特徴とする請求項１乃至請求項４のいずれか一項に記載のウェイポイント生成装置。
【請求項６】
前記ウェイポイント生成部は、前記旋回中心Ｃ’（ｘｃ’，ｙｃ’）を、ｓｉｇｎを引数の符号を出力する符号関数として、
【数２】
0006487264.tif 000028
により生成することを特徴とする請求項５に記載のウェイポイント生成装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の航路を定めるウェイポイントを生成する技術に関する。
【背景技術】
【０００２】
船舶用自動操舵装置は、舵角を制御して参照方位に船首方位を追従させる方位制御システム（ＨＣＳ：Ｈｅａｄｉｎｇ  Ｃｏｎｔｒｏｌ  Ｓｙｓｔｅｍ）と、舵角を制御して計画航路に船体位置を追従させる航路制御システム（ＴＣＳ：Ｔｒａｃｋ  Ｃｏｎｔｒｏｌ  Ｓｙｓｔｅｍ）とに分けられる。参照方位によって計画航路に追従させる方位制御システムによれば、船体位置が航路から離れてしまうために適宜方位を修正する必要がある。一方、航路制御システムによれば、計画航路に船体位置を追従させるため、方位制御システムと比較して、目的地までの航海を簡易に実現することができる。なお、この航路制御システムにおいて、計画航路は、その上位システムである電子海図情報表示システム（ＥＣＤＩＳ：Ｅｌｅｃｔｒｏｎｉｃ  Ｃｈａｒｔ  Ｄｉｓｐｌａｙ  ａｎｄ  Ｉｎｆｏｒｍａｔｉｏｎ  Ｓｙｓｔｅｍ）において、船舶の目的地までの通過点であるウェイポイントから定められるため、船舶用自動操舵装置として航路制御システムを導入するには、電子海図情報表示システムを導入する必要があった。
【０００３】
また、電子海図情報表示システムに関連する技術として、移動体の位置及び向きを取得し、複数のウェイポイントを直線上に並べたルートを縦軸とし、このルートに対する移動体又は他物標の左右方向の位置関係を横軸とする情報を直線ルート表示領域に表示し、ルートに対する移動体又は他物標の表示位置を制御することにより、ルートを直線状に変換して表示するとともに、ルートに対する移動体又は他物標の位置関係を表示するルート表示装置、が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１４−２０６４５２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
近年、運航における低エネルギー、低コスト及び短時間で行うという観点から、小型または中型の船舶においても、航路制御システムによる運航が求められている。しかしながら、小型または中型の船舶において計画航路に船体位置を追従させようとした場合、小型または中型の船舶の船橋の広さが電子海図情報表示システムを設置するのに十分でなく、また電子海図情報表示システムを導入するためのコストが船舶の規模に対して高いため、船体位置を追従させる計画航路を定めるためのウェイポイントを設定することができない、という問題があった。
【０００６】
本発明の実施形態は、上述した問題点を解決するためになされたものであり、電子海図情報表示システムを導入することなくウェイポイントを設定することができるウェイポイント生成装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上述した課題を解決するため、本実施形態のウェイポイント生成装置は、移動体の目的地座標Ｐ（ｘｐ，ｙｐ）と、前記移動体の参照方位である初期方位ψ０と、前記移動体の現在位置座標Ｏ（ｘｏ，ｙｏ）と、前記移動体の旋回径における旋回半径Ｒとを取得するデータ取得部と、前記現在位置座標Ｏから前記初期方位ψ０方向に前記移動体の空走距離だけ離れた第１ウェイポイントＳ（ｘｓ，ｙｓ）と前記目的地座標Ｐとを、前記現在位置座標Ｏを原点とした前記初期方位ψ０のマイナス方向に座標変換して正規化する第１座標変換部と、前記正規化された座標において、正規化された第１ウェイポイントＳ’（ｘｓ’，ｙｓ’）と前記旋回半径Ｒとに基づいて、第１ウェイポイントＳ’に対して初期方位ψ０方向に所定の間隔距離を有した位置にある第２ウェイポイントＡ’（ｘａ’，ｙａ’）と、該第２ウェイポイントＡ’と正規化された目的地座標Ｐとを結ぶ線分上に前記第２ウェイポイントＡ’に対して前記間隔距離を有した位置にある第３ウェイポイントＦ’（ｘｆ’，ｙｆ’）とを生成するウェイポイント生成部と、前記ウェイポイント生成部により生成された第２ウェイポイントＡ’及び第３ウェイポイントＦ’を、前記現在位置座標Ｏを原点として前記初期方位ψ０のプラス方向に座標変換する第２座標変換部と、前記第１ウェイポイントＳと前記第２座標変換部により座標変換された第２ウェイポイントＡ及び第３ウェイポイントＦとを出力する出力部とを備える。
【発明の効果】
【０００８】
本発明の実施形態によれば、電子海図情報表示システムを導入することなくウェイポイントを設定することができる。
【図面の簡単な説明】
【０００９】
【図１】ウェイポイント生成装置、船舶用自動操舵装置及び制御対象を含む全体ブロック図である。
【図２】フィードバック制御部の構成を示すブロック図である。
【図３】航路制御システムで用いる座標系を示す図である。
【図４】航路保持制御に係るフィードバック制御ループを示すブロック図である。
【図５】ψＲとδＲの時系列の一例を示す図である。
【図６】参照方位による旋回航跡と参照航跡の関係を示す図である。
【図７】ウェイポイント生成装置の機能構成を示すブロック図である。
【図８】計画航路の座標変換を示す図である。
【図９】１回のウェイポイント生成例を示す図である。
【図１０】複数回のウェイポイント生成例を示す図である。
【図１１】方位制御モードから航路制御モードに遷移した場合の過渡応答を示す図である。
【図１２】航路制御モードで曲線追従した場合の過渡応答を示す図である。
【発明を実施するための形態】
【００１０】
以下、図面を参照しながら、本発明の実施形態について説明する。以降の説明において、各記号は、変数、修飾、添字として、以下の表のように定義される。
【００１１】
【表１】
0006487264.tif 000002
【００１２】
【表２】
0006487264.tif 000003
【００１３】
【表３】
0006487264.tif 000004
【００１４】
１．  船舶用自動操舵装置の構成
まず、本発明の船舶用自動操舵装置を含むシステムについて説明する。図１は、船舶用自動操舵装置と制御対象の全体のブロック図である。図２は、フィードバック制御部の構成を示すブロック図である。
【００１５】
図１に示すように、船舶用自動操舵装置１は、計画航路に船体位置を追従させるために舵を制御する装置であり、ウェイポイント生成装置１０、軌道計画部１１、軌道航路誤差演算部１２、フィードバック制御部１３、加算器１４及び各パラメータを同定する図示しない同定器を備えている。
【００１６】
船体２のセンサ類３は、船体２のｓｕｒｇｅ速度ｕを検出するスピードログ、船体２の船首方位ψを検出するジャイロコンパス、ＧＰＳ等の衛星測位システム（ＧＮＳＳ）からの船体位置（ｘ，ｙ）を検出するＧＮＳＳセンサを含む。
【００１７】
ウェイポイント生成装置１０は、センサ類３から入力されるｓｕｒｇｅ速度ｕ、船首方位ψ及び軌道計画部１１から入力される参照方位ψＲに基づいて、計画航路を出力する。
【００１８】
軌道計画部１１は、ウェイポイント生成装置１０から入力される計画航路、センサ類３から入力されるｓｕｒｇｅ速度ｕに基づいて、参照方位ψＲ、参照方位ｘＲ、ｙＲといった参照信号を出力するとともに、変針中にはフィードフォワード舵角δＦＦを出力する。
【００１９】
軌道航路誤差演算部１２は、センサ類３から入力される船首方位ψ、船体位置（ｘ，ｙ）と、軌道計画部１１から入力される参照方位ψＲ、参照位置ｘＲ，ｙＲとの比較を行い、方位誤差ψｅ、航路誤差ｘｅ，ｙｅを出力する。
【００２０】
船舶用自動操舵装置１の閉ループ系は、船体モデルと外乱モデルとからなる制御対象と、フィードバック制御部１３とから構成される。フィードバック制御部１３は、図２に示すように、推定器１３１と制御系フィードバックゲイン器１３２とを備える。推定器１３１は、軌道航路誤差演算部１２から入力された方位誤差ψｅ、航路誤差ｘｅ、ｙｅに基づいて、外乱が除去された推定方位誤差ψｅ＾、推定旋回角誤差ｒ＾といった方位に関する状態量の推定値と、横方向の推定航路誤差ｙｅ＾といった航路に関する状態量の推定値とを出力する。また、推定器１３１は対地座標系での演算を行い、推定潮流ベクトルｕｃ＾，ｖｃ＾といった潮流に関する推定値を出力する。制御系フィードバックゲイン器１３２は、推定方位誤差ψｅ＾、推定旋回角誤差ｒ＾に対してフィードバックゲインを掛けるとともに、推定航路誤差ｙｅ＾に対してフィードバックゲインを掛け、これらの結果を加算してフィードバック舵角δＦＢを出力する。
【００２１】
船体２の操舵機は、上述した構成による船舶用自動操舵装置１による命令舵角δＣに比例した舵角を動かすため、船体３は、舵角によって旋回角速度を生じ、方位、位置が変化する。また旋回角速度の発生と共に、斜航角（横方向速度）が発生する。
【００２２】
２．  定式化
２．１  座標系
ここで、航路座標系について説明する。図３は、航路制御システムで用いる座標系を示す図である。
【００２３】
図３に示すように、航路制御システムで用いる座標系は、地球固定座標系Ｏ−ＸＹ、船体固定座標系Ｇ−ＸＢＹＢと参照座標系ＯＲ−ＸＲＹＲからなり、いずれも右手系３軸直交座標系である。これらの座標系においてＺ軸は重力方向を正とし、回転極性は右ねじ方向を正とする。なお、座標系はＸ軸、Ｙ軸の２次元を用いるため、図３においてＺ軸は省略される。また、地球固定座標系はＸ軸を北向きにとり、船体固定座標系はＸ軸を船首方位にとり、参照座標系はＸ軸を前進方向にとる。
【００２４】
航路制御システムにおいて計画航路は、図３においてＷＰとして示されるウェイポイントを結ぶ経路として設定され、直線航路と曲線航路からなる。参照航路は計画航路上に置かれ、直線航路上においては原点を適当な位置に固定したものとして、また、曲線航路上においては旋回中心と船体位置とを結ぶ線との交点に置いたものとして定められる。なお、図３において、ｕはｓｕｒｇｅ速度、ｖはｓｗａｙ速度、ｒはｙａｗ（旋回）角速度、ψは船首方位、ψＲは参照方位、δは舵角、ｘ，ｙは船体位置、ｘＲ，ｙＲは参照航路、Ｒは旋回半径、ｘＣ，ｙＣは旋回中心、Ｕは対水速度でＵ＝√（ｕ２＋ｖ２）、添字ｃは潮流成分を示す。これらより、方位誤差ψｅ及び航路誤差ｘｅ，ｙｅは次式になる。
【００２５】
【数１】
0006487264.tif 000005
ここで、ｄｉｓｔｘ＝ｘ’−ｘｃ、ｄｉｓｔｙ＝ｙ’−ｙｃ、Ｍは座標変換行列、
【００２６】
【数２】
0006487264.tif 000006
【００２７】
２．２  制御対象
ここで、制御対象について説明する。制御対象は船体モデルと外乱モデルから構成される。船体モデルは、ｓｕｒｇｅ速度を一定として、野本の応答モデルを改良した本発明者による応答モデルを用いて、
【００２８】
【数３】
0006487264.tif 000007
に定める。ここでｓはラプラス演算子、Δｃ（ｓ）、δｃは命令舵角、Ｋｒは旋回力ゲイン、Ｋｖは横流れゲイン、Ｔｒ，Ｔｒ３，Ｔｖは時定数を示す。本実施の形態においては、安定船を対象とし、δｃは操舵機応答が船体運動に比べて十分に速いものとしてδｃ≒δとおく。この船体モデルは野本の船体モデルと比較して、Ｔｒ３を有し、Ｔｖ＝Ｔｒという特徴をもつ。船体方位モデルの状態空間表現は（５）式より次式になる
【００２９】
【数４】
0006487264.tif 000008
ここでｘｗ＝［ψ  ｒｘ］Ｔ，添字Ｔは転置行列、ＣＴ３＝１−（Ｔｒ３／Ｔｒ）、
【００３０】
【数５】
0006487264.tif 000009
【００３１】
外乱モデルは、舵角オフセットδｏｒ，δｏｖ、波浪成分ψｗ及び図３に示される潮流成分ｕｃ，ｖｃとする。ここで、δｏｒ，δｏｖは、風や船体特性などに誘起された方位軸回り（ｙａｗ方向）に作用する角速度、ｓｗａｙ方向に作用する速度をそれぞれ舵角換算したものであり、方位誤差と航路誤差を発生させる。ψｗは白色ノイズνが入力した狭帯域フィルタ出力を方位換算したものであり、δｃに無効舵を発生させる。ｕｃ，ｖｃは航路誤差を発生させる。このとき
【００３２】
【数６】
0006487264.tif 000010
とする。ここでｘｗ＝［ξ  ψｗ］Ｔ、ξは変数、Ｋｗ，ζｗ，ωｗはそれぞれ波浪モデルのゲイン、減衰係数、固有周波数として、
【００３３】
【数７】
0006487264.tif 000011
【００３４】
２．３  航路誤差モデル
航路誤差モデルを導出する。船体の対地速度は
【００３５】
【数８】
0006487264.tif 000012
と定める。ここで添字ｇは対地速度成分を示す。これにより参照座標における対地速度の誤差は、
【００３６】
【数９】
0006487264.tif 000013
ここでｕＲ，ｖＲは参照速度を示す。ｖＲ＝０，ｃｏｓψｅ≒１，ｓｉｎψｅ≒ψｅを用い、
【００３７】
【数１０】
0006487264.tif 000014
【００３８】
３．航路制御システム
航路制御システムとして、フィードバック制御部１３が航路保持制御を行い、軌道計画部１１が参照信号を出力するとともに、フィードフォワード舵角δＦＦを出力することによって曲線追従制御を行う。以下、航路保持制御及び曲線追従制御について説明する。
【００３９】
３．１  航路保持制御
まず、航路保持制御について説明する。図４は、航路保持制御に係るフィードバック制御ループを示すブロック図である。
【００４０】
図４に示すように、航路保持制御は、推定器１３１及び制御系フィードバックゲイン器１３２による閉ループ制御によって、閉ループ安定性及び外乱除去性を確保する。閉ループ安定性の状態空間表現は、
【００４１】
【数１１】
0006487264.tif 000015
になる。ここでｘｈ＝［ｘｒ  ｘｗ  δｏｒ］Ｔ、ψ‐＝−ψ−ψｗ：検出方位、ｘｔ＝［ｙｅ  ｖｅｏ］Ｔ、ｙｅ‐は検出航路誤差、ｖｅｏ＾はオフセット成分、添字ｈ，ｔ，＾，‐はそれぞれ方位制御ループ、航路制御ループ、推定値、検出値、Ｋは推定ゲイン、Ｆはフィードバックゲイン、Ｏｉ×ｊ：ｉ行ｊ列のゼロ行列を用いて、
【００４２】
【数１２】
0006487264.tif 000016
【００４３】
潮流成分ｕｃ，ｖｃは航行中に変化するため、これらは常に推定される。なお、潮流推定は船体位置の移動速度から求めるため、風浪や船体２の不釣合いによる移動速度を分離できず含む。よって、ｕｃ，ｖｃは推定器１３１によって次式より推定される。
【００４４】
【数１３】
0006487264.tif 000017
ここでｘｃｕ＝［ｘ  ｕｃ］Ｔ，ｘｃｖ＝［ｙ  ｖｃ］Ｔ、ｘ‐，ｙ‐は船体位置検出量、Ｋｃは潮流推定ゲインを示す。
【００４５】
３．２  曲線追従制御
次に、曲線追従制御について説明する。曲線追従制御は、軌道計画部１１による参照信号の出力とフィードフォワード制御からなる閉ループ制御によって、参照方位及び参照航路に船首方位及び船体位置を追従させる。軌道計画部１１により出力される軌道計画に次の条件を組み込むことによって適切な変針が実現できる。参照方位ψＲは、変針条件（変針量Δψ０、指定旋回角速度ｒ０）、操舵機条件（舵角設定値δ０、舵速度設定値δ０・）及び方位初期値Ｃ１ａ＝ψＲ・・（０），Ｃ２ａ＝ψＲ・（０）を満足する関数で
【００４６】
【数１４】
0006487264.tif 000018
になる。ここでηは定数。
【００４７】
フィードフォワード舵角δＦＦ（参照舵角δＲ）は方位伝達関数Ｐψ（ｓ）の逆モデルを用いると（簡単化のためＴｒ３＝０）、船首方位は、
【００４８】
【数１５】
0006487264.tif 000019
になり、参照方位に一致する。
【００４９】
ψＲとδＲの時系列の一例を図５に示す。この図において、Ｔは時間、添字ａ，ｖ，ｄはそれぞれ加速、等速、減速の各モード、Ｋｒは旋回力ゲイン、Ｔｒは時定数を示す。
【００５０】
参照方位による旋回航跡と参照航跡の関係を図６に示す。この図において、旋回航跡はＸＹ系として点線で示され、参照航跡はξη系として実線で示される。参航跡は船体横流れ速度ｖＲのため、旋回航跡を原点Ｏ周りに斜航角βＲ＝ｓｉｎ−１（Ｕ／ｖＲ）相当の座標回転したものに近似できる。よって、船体航跡を参照航跡に乗せるため、図６より参照方位はｘβＲ相当（リーチ）だけ手前から変針を開始させる。
【００５１】
４．  ウェイポイント生成装置
次に、ウェイポイント生成装置によるウェイポイントの生成について説明する。
【００５２】
４．１  ウェイポイント生成の概要
ウェイポイント生成装置１０は、船体２の現在位置とその時点での参照方位の方向に位置する仮想点を直線で結んだ航程線を生成し、航程線に船体位置を航路制御システムによって追従させる。ここで仮想点は現在位置から十分に離れたものとし、本実施の形態においては、数ＮＭから数十ＮＭとする。このような航程線を船舶が航行している間に新たな目的地が入力された場合、ウェイポイント生成装置１０は、計画航路を再構築して軌道計画部１１に出力する。この計画航路の構築において、ウェイポイント生成装置１０は、船舶が参照方位（初期方位）ψ０で航行中に目的地Ｐが与えられたとき、現在位置をＯとして、後述する３点のウェイポイントＳ，Ａ，Ｆと変針量Δψを求める。
【００５３】
４．２  ウェイポイント生成装置の構成及び動作
ウェイポイント生成装置の機能構成及び動作について説明する。図７は、ウェイポイント生成装置の機能構成を示すブロック図である。図８は、計画航路の座標変換を示す図である。
【００５４】
ウェイポイント生成装置１０は、機能として、データ取得部１０１、空走距離算出部１０２、第１座標変換部１０３、ウェイポイント生成部１０４、第２座標変換部１０５、出力部１０６を備え、これらの機能は、ウェイポイント生成装置１がハードウェアとして備えるＣＰＵとメモリが協働することによって実現される。以下、各機能について説明する。
【００５５】
＜データ取得部＞
データ取得部１０１は、パラメータとして、航海情報及び旋回条件を取得する。航海情報は、入力された目的地の座標Ｐと、座標Ｐが与えられた時点の参照方位である初期方位ψ０と、同時点における現在位置の座標Ｏとを含む。旋回条件は、船舶の旋回径における旋回半径Ｒと、空走距離ＥＤまたは空走距離ＥＤを算出するための空走距離パラメータとを含む。ここで、空走距離ＥＤは、現在位置Ｏから航程線上に位置するウェイポイントＳまでの距離であり、空走距離パラメータは船体２の船長Ｌまたは船速ｕであり、空走距離ＥＤ、船長Ｌ、船速ｕのいずれかが取得されれば良い。これらのうち、空走距離ＥＤ及び船長Ｌは操船者によって入力されるパラメータであり、船速ｕはセンサ類３から入力されるｓｕｒｇｅ速度ｕである。なお、空走距離ＥＤは予め定められた所定の値であっても良い。
【００５６】
＜空走距離算出部＞
空走距離算出部１０２は、データ取得部１０１により空走距離ＥＤが取得されない場合、即ち、船長Ｌ、船速ｕのいずれかが取得された場合、取得された船長Ｌまたは船速ｕに基づいて、以下の式により空走距離ＥＤを算出する。
【００５７】
【数１６】
0006487264.tif 000020
ここでＣｅｄは空走係数、ｔｅｄは空走時間を示し、いずれも予め設定された値とする。このとき、ウェイポイントＳの座標は次式になる。
【００５８】
【数１７】
0006487264.tif 000021
ここでｘ０，ｙ０は現在位置Ｏの座標であり、簡単化のためゼロとする。また、添字ｓはウェイポイントＳを示し、以降の説明における添字ｃ，ｐ，ａ，ｆはそれぞれ、現在位置（原点）Ｏ、目的地Ｐ、ウェイポイントＡ、ウェイポイントＦを示す。
【００５９】
＜第１座標変換部＞
第１座標変換部１０３は、ウェイポイントＳ及び目的地Ｐを逆座標変換することにより正規化する。具体的には、第１座標変換部１０３は、図８に示すように、点Ｓ（ｘｓ，ｙｓ）、点Ｐ（ｘｐ，ｙｐ）を、現在位置Ｏを原点として初期方位ψ０のマイナス方向に座標変換する。この座標変換によれば、座標変換された点Ｓ、Ｐは、それぞれ、
【００６０】
【数１８】
0006487264.tif 000022
になる。ここで添字’は正規化した座標（初期方位が北向き）、ｙｓ’＝０、Ｍは行列、添字Ｔは転置行列を示す。
【００６１】
＜ウェイポイント生成部＞
ウェイポイント生成部１０４は、正規化した座標において、旋回径の半径中心Ｃ、ウェイポイントＡ、ウェイポイントＦ及び変針量Δψを求める。ここで、ウェイポイントＡは初期方位を向く航程線上においてウェイポイントＳと所定の間隔距離を有した位置にある点であり、ウェイポイントＦはウェイポイントＡと目的地Ｐとを結ぶ航程線上においてウェイポイントＡとウェイポイントＳとの間隔距離と同様の間隔距離を有した位置にある点である。
【００６２】
ウェイポイント生成部１０４は、正規化された旋回径の半径中心Ｃ’の座標を次式により求める。
【００６３】
【数１９】
0006487264.tif 000023
ここでｓｉｇｎは引数の符号を出力する符号関数を示す。
【００６４】
また、ウェイポイント生成部１０４は、ウェイポイントＡ’、ウェイポイントＦ’を、正規化した座標上のウェイポイント間の位置関係から求める。ウェイポイントＡ’、ウェイポイントＦ’及び変針量Δψの関係式は
【００６５】
【数２０】
0006487264.tif 000024
ここで｜Δψ｜はΔψ＜０のための符号操作。上式を整理すると、次の方程式
【００６６】
【数２１】
0006487264.tif 000025
を得る。ここでΔｘ＝ｘｐ’−ｘｓ’，Δｙ＝ｙｐ’−ｙｓ’＝ｙｐ’。上式は超越関数のため、ウェイポイント生成部１０４は、この関数を数値計算によって解くことにより、ウェイポイントＳ’、ウェイポイントＡ’、ウェイポイントＦ’及び変針量Δψを算出する。この数値計算には、初期値Δψ０を与え、例えば黄金分割探索を用いると良い。この場合、｜ｆ（Δψ）｜または（ｆ（Δψ））２が用いられる。また、初期値はΔψ０＝ｔａｎ−１（ｙｐ’−ｙｓ’）÷（ｘｐ’−ｘｓ’）から与えられる。
【００６７】
＜第２座標変換部＞
第２座標変換部１０５は、ウェイポイント生成部１０４により生成された正規化されたウェイポイント及び半径中心を正座標変換することによりウェイポイントを得る。具体的には、第２座標変換部１０５は、ウェイポイントＡ’、ウェイポイントＦ’及び半径中心Ｃ’を、現在位置Ｏを原点として初期方位ψ０の正方向に座標変換する。Ａ’、Ｆ’Ｃ’は次式から求まる。
【００６８】
【数２２】
0006487264.tif 000026
【００６９】
＜出力部＞
出力部１０６は、得られたウェイポイントＳ、Ａ、Ｆ、及び旋回径の半径中心Ｃを計画航路として、軌道計画部１１へ出力する。
５．  検証
本実施形態をシミュレーションによって検証する。
【００７０】
＜ウェイポイント生成＞
１回のウェイポイント生成例を図９に示す。ここで条件は、Ｌ＝２００ｍ，ＥＤ＝１０Ｌ［ｍ］，Ｒ＝１ＮＭ＝１８５２ｍ，ψ０＝３０ｄｅｇ，Ｏ＝［０，０］ｍ，Ｐ＝［５０００，５０００］ｍとする。なお、変針量は、初期値Δψ０＝２０．８ｄｅｇ、Δψ＝２２．３ｄｅｇになる。
【００７１】
複数回のウェイポイント生成例を図１０に示す。ここで条件は、ＥＤ＝３Ｌ［ｍ］，ψ０＝２０ｄｅｇ，Ｐｉ，ｉ＝１〜４を与えた場合である。
【００７２】
＜航路制御のシミュレーション結果＞
図１１は、方位制御モードから航路制御モードに遷移した場合の過渡応答を示す。方位制御モードでは方位偏差がゼロであるが，航路制御モードでは航路誤差がゼロになる。その際潮流成分に対抗するため，斜航角はβ＝ｓｉｎ−１（５／１０）＝３０［ｄｅｇ］、航路誤差は最大−２００ｍ弱生じる。条件は、船体パラメータ：ｕ＝５．１４ｍ／ｓ，Ｋｒ＝０．０７８５１／ｓ，Ｋｖ＝−４．７３ｍ／ｓ，Ｔｒ＝８３．５ｓ，Ｔｒ３＝４．５ｓ，Ｔｖ３＝０ｓ。潮流成分：Ｕｃ＝５ｋｎ，ψｃ＝３１０ｄｅｇ。制御システム：Ｋｒ＝０．０２６７１／ｓ，Ｋｖ＝−２．６７ｍ／ｓ，Ｔｒ＝３６．１ｓ，Ｔｒ３＝３．６ｓ。制御ゲイン：ｆｐ＝１．５，ｆｄ＝２２．７ｓ，ｆｙ＝０．００１９７ｍ／ｓ。初期方位４０ｄｅｇ，変針量１０［ｄｅｇ］。
【００７３】
図１２は航路制御モードで曲線追従した場合の過渡応答を示す。船体モデルのパラメータが制御対象と制御システムとで異なるが、顕著な誤差は生じていない。したがって、ウェイポイント生成装置１０により出力されたウェイポイントは有効である。
【００７４】
本発明の実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。
【符号の説明】
【００７５】
１０  ウェイポイント生成装置
１０１  データ取得部
１０３  第１座標変換部
１０４  ウェイポイント生成部
１０５  第２座標変換部
１０６  出力部
【図１】
0006487264.tif 000029
【図２】
0006487264.tif 000030
【図３】
0006487264.tif 000031
【図４】
0006487264.tif 000032
【図５】
0006487264.tif 000033
【図６】
0006487264.tif 000034
【図７】
0006487264.tif 000035
【図８】
0006487264.tif 000036
【図９】
0006487264.tif 000037
【図１０】
0006487264.tif 000038
【図１１】
0006487264.tif 000039
【図１２】
0006487264.tif 000040