(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024119713
(43)【公開日】2024-09-03
(54)【発明の名称】水中グライダのベクトル場における経路追跡の制御方法
(51)【国際特許分類】
G05D 1/00 20240101AFI20240827BHJP
B63C 11/48 20060101ALI20240827BHJP
B63C 11/00 20060101ALI20240827BHJP
B63H 25/04 20060101ALI20240827BHJP
B63B 79/40 20200101ALI20240827BHJP
G05D 1/46 20240101ALI20240827BHJP
【ＦＩ】
G05D1/00 A
B63C11/48 D
B63C11/00 B
B63H25/04 D
B63B79/40
G05D1/10
【審査請求】有
【請求項の数】1
【出願形態】ＯＬ
(21)【出願番号】P 2023092087
(22)【出願日】2023-06-05
(11)【特許番号】
(45)【特許公報発行日】2024-06-18
(31)【優先権主張番号】202310149488.3
(32)【優先日】2023-02-22
(33)【優先権主張国・地域又は機関】CN
(71)【出願人】
【識別番号】515352847
【氏名又は名称】大連海事大学
(74)【代理人】
【識別番号】100095407
【弁理士】
【氏名又は名称】木村 満
(74)【代理人】
【識別番号】100132883
【弁理士】
【氏名又は名称】森川 泰司
(74)【代理人】
【識別番号】100148633
【弁理士】
【氏名又は名称】桜田 圭
(74)【代理人】
【識別番号】100147924
【弁理士】
【氏名又は名称】美恵 英樹
(72)【発明者】
【氏名】王 浩亮
(72)【発明者】
【氏名】劉 陸
(72)【発明者】
【氏名】古 楠
(72)【発明者】
【氏名】彭 周華
(72)【発明者】
【氏名】王 丹
(72)【発明者】
【氏名】王 安青
(72)【発明者】
【氏名】盧 麗宇
(72)【発明者】
【氏名】尹 晨陽
(72)【発明者】
【氏名】仝 世豪
【テーマコード（参考）】
5H301
【Ｆターム（参考）】
5H301AA05
5H301CC04
5H301CC07
5H301CC10
5H301GG07
(57)【要約】 （修正有）
【課題】水中グライダの経路追跡のコースをより滑らかにし、水中グライダの動作中のリスクを低減し、水中グライダの経路追跡性能を向上させる。
【解決手段】水中グライダのベクトル場における経路追跡の制御方法において、水中グライダの運動モデルを作成し、所望経路と所望速度を設定し、所望経路をパラメータ化して、パラメータ化された所望経路の情報を取得し、パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算し、パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて水中グライダの誘導角速度を計算し、所望速度と経路追跡誤差における縦方向の経路追跡誤差に基づいて、水中グライダの前進速度を計算し、水中グライダが誘導角速度と前進速度によって航行することを含む。
【選択図】図１
【解決手段】水中グライダのベクトル場における経路追跡の制御方法において、水中グライダの運動モデルを作成し、所望経路と所望速度を設定し、所望経路をパラメータ化して、パラメータ化された所望経路の情報を取得し、パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算し、パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて水中グライダの誘導角速度を計算し、所望速度と経路追跡誤差における縦方向の経路追跡誤差に基づいて、水中グライダの前進速度を計算し、水中グライダが誘導角速度と前進速度によって航行することを含む。
【選択図】図１
【特許請求の範囲】
【請求項1】
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含むことを特徴とする水中グライダのベクトル場における経路追跡の制御方法。
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含むことを特徴とする水中グライダのベクトル場における経路追跡の制御方法。
【請求項2】
前記ステップ２は、ステップＳ１とステップＳ２を含み、
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、所望経路をパラメータ化し、
（１）
式（１）中、ｘｄ、ｙｄ、ｚｄは、それぞれ所望経路をパラメータ化するための関数であり、Ｐｄは座標系{Ｆ}における所望経路であり、χは所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
（２）
式（２）中、
であり、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角である、ことを特徴とする請求項１に記載の水中グライダのベクトル場における経路追跡の制御方法。
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、所望経路をパラメータ化し、
【数1】
式（１）中、ｘｄ、ｙｄ、ｚｄは、それぞれ所望経路をパラメータ化するための関数であり、Ｐｄは座標系{Ｆ}における所望経路であり、χは所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
【数2】
式（２）中、
【数3】
【請求項3】
前記ステップ３は、ステップＳ１１とステップＳ１２を含み、
ステップＳ１１、回転行列である
を定義し、前記
は座標系{Ｆ}から慣性座標系{Ｉ}に変換した回転行列であり、
は式（３）で表し、
（３）
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２、経路追跡誤差である
を定義し、その中、ｘｅは縦方向の経路追跡誤差であり、ｙｅは横方向の経路追跡誤差であり、ｚｅは垂直方向の経路追跡誤差であり、式（４）により、経路追跡誤差を計算し、
（４）
式（４）中、
、
であり、ηは水中グライダの実際位置情報であり、ηｄは仮想基準点の位置情報である、ことを特徴とする請求項２に記載の水中グライダのベクトル場における経路追跡の制御方法。
ステップＳ１１、回転行列である
【数4】
【数5】
【数6】
【数7】
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２、経路追跡誤差である
【数8】
【数9】
式（４）中、
【数10】
【数11】
【請求項4】
前記ステップ４は、ステップＳ２１とステップＳ２２を含み、
ステップＳ２１、式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
（５）
式（５）中、
はベクトル場の方向を表し、κ∈Ｒ、κ≧２であり、
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
（６）
（７）
（８）
（９）
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である、ことを特徴とする請求項３に記載の水中グライダのベクトル場における経路追跡の制御方法。
ステップＳ２１、式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
【数12】
式（５）中、
【数13】
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
【数14】
【数15】
【数16】
【数17】
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である、ことを特徴とする請求項３に記載の水中グライダのベクトル場における経路追跡の制御方法。
【請求項5】
前記ステップ５は、式（１０）により、水中グライダの前進速度Ｕｄを計算することを含み、
（１０）
式（１０）中、ｖｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である、ことを特徴とする請求項４に記載の水中グライダのベクトル場における経路追跡の制御方法。
【数18】
式（１０）中、ｖｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である、ことを特徴とする請求項４に記載の水中グライダのベクトル場における経路追跡の制御方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、水中グライダの制御分野に関し、特に水中グライダのベクトル場における経路追跡の制御方法に関する。
【背景技術】
【0002】
海洋は地球の重要な構成部分であり、豊富な生物資源、石油・ガス資源、鉱物資源を有し、全地球の海洋面積は約３．６億平方キロメートルで、地球表面積の約７１％を占めている。今の時代、地球上の人口はますます増加し、陸上の資源はますます乏しくなっているため、世界各国は海洋資源の重要性を認識しつつ、海洋資源の開発と利用を始めている。水中グライダは、海洋観測、早期警報偵察、資源探査のための重要なツールであり、軍事分野や民間分野において非常に広く応用できる。
【0003】
従来の有人潜水艦と無人水中航走体に比べて、水中グライダは小型、軽量、活動範囲が広く、安全性が高いなどの利点を持っているため、水中グライダは悪い環境に適応し、所期の任務をよりよく達成することができる。水中グライダは、ある程度の搭載能力を持っており、水中の移動探査プラットフォームとして、さまざまなセンサを搭載することで、さまざまな目標の探査を実現することができ、海洋情報の収集と海洋の分析などの基本的な要件を満たす。水中観察探査をより良く行うために、高精度の経路追跡制御が任務達成の鍵となっている。
【0004】
水中グライダの経路追跡制御について、国内外の学者らにより、いくつかの研究がなされており、採用された経路追跡制御方法は、主に単純追跡誘導法、平行接近誘導法、視線誘導法、ベクトル場誘導方法などがある。単純追跡誘導法と平行接近誘導法は、２点誘導法で、つまり、追跡の過程で航走体と仮想目標点の位置だけを考慮した方法であり、視線誘導法は３点誘導法で、つまり、追跡の過程で航走体と仮想目標点の位置に加えて、経路接線上の仮想基準点の位置も考慮した方法である。
【0005】
なお、経路追跡の過程で、視線誘導法は、所望の経路が連続的に誘導されることが要求されるが、ベクトル場誘導法は、所望の経路の周囲に航走体を誘導して所望の経路に接近させるベクトル場を構築することで、航走体を希望の経路に徐々に収束させ、追跡誤差がゼロになるようにする方法で、その所望の経路は、離散的な経路点であってもよく、経路への制約が比較的に低い。
【0006】
特許文献１は、共同ベクトル場における複数無人潜水艦の同期経路追跡システムを提供し、経路変数の協調設計を導入することで、複数無人艇の同期経路追跡を実現している。
【0007】
特許文献２は、無人船のベクトル場における経路追跡の制御装置及びその設計方法を提供し、所望の経路の周囲にベクトル場を生成して、より良い追跡誤差の収束効果を得、無人船の経路追跡性能を向上させる。
【先行技術文献】
【特許文献】
【0008】
【特許文献1】中国特許出願公開第１１５０３２９８７号明細書
【特許文献2】中国特許出願公開第１１３２１９９７０号明細書
【発明の概要】
【発明が解決しようとする課題】
【0009】
しかしながら、上記特許文献には、水平面上のベクトル場での誘導方法だけが開示され、水中の３次元空間で運動する水中グライダの経路追跡の制御に適用できないため、本発明は、水中グライダに適した３次元ベクトル場における経路追跡の制御方法を提供し、水中グライダの３次元空間での経路追跡制御を実現する。
【0010】
本発明は、上記の技術的課題に鑑みてなされたもので、水中グライダのベクトル場における経路追跡の制御方法を提供する。
【課題を解決するための手段】
【0011】
本発明の一形態は水中グライダのベクトル場における経路追跡の制御方法であって、
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含む。
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含む。
【0012】
好ましくは、前記ステップ２は、以下のステップＳ１とステップＳ２を含み、
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系｛Ｆ｝を作成し、式（１）により、所望経路をパラメータ化し、
（１）
式（１）中、ｘｄ、ｙｄ、ｚｄは、それぞれ所望経路をパラメータ化するための関数であり、Ｐｄは座標系｛Ｆ｝における所望経路であり、χは所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報はピッチ角の接線角とヨー角の接線角とを含み、
（２）
式（２）中、
であり、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角である。
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系｛Ｆ｝を作成し、式（１）により、所望経路をパラメータ化し、
【数1】
式（１）中、ｘｄ、ｙｄ、ｚｄは、それぞれ所望経路をパラメータ化するための関数であり、Ｐｄは座標系｛Ｆ｝における所望経路であり、χは所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報はピッチ角の接線角とヨー角の接線角とを含み、
【数2】
式（２）中、
【数3】
【0013】
好ましくは、前記ステップ３は、以下のステップＳ１１とステップＳ１２を含み、
ステップＳ１１、回転行列である
を定義し、前記
は座標系｛Ｆ｝から慣性座標系｛Ｉ｝に変換した回転行列であり、
は式（３）で表し、
（３）
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２、経路追跡誤差である
と経路追跡誤差の二乗和である
を定義し、その中、ｘｅは縦方向の経路追跡誤差であり、ｙｅは横方向の経路追跡誤差であり、ｚｅは垂直方向の経路追跡誤差であり、式（４）により、経路追跡誤差を計算し、
（４）
式（４）中、
、
であり、ηは水中グライダの実際位置情報であり、ηｄは仮想基準点の位置情報である。
ステップＳ１１、回転行列である
【数4】
【数5】
【数6】
【数7】
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２、経路追跡誤差である
【数8】
【数9】
【数10】
式（４）中、
【数11】
【数12】
【0014】
好ましくは、前記ステップ４は、以下のステップＳ２１とステップＳ２２を含み、
ステップＳ２１、式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
（５）
式（５）中、
であってベクトル場の方向を表し、κ∈Ｒ、κ≧２であり、
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
（６）
（７）
（８）
（９）
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である。
ステップＳ２１、式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
【数13】
式（５）中、
【数14】
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
【数15】
【数16】
【数17】
【数18】
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である。
【0015】
好ましくは、前記ステップ５は、式（１０）により、水中グライダの前進速度Ｕｄを計算することを含み、
（１０）
式（１０）中、vｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である。
【数19】
式（１０）中、vｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である。
【発明の効果】
【0016】
本発明は、水中グライダのベクトル場における経路追跡の制御方法を提供し、仮想基準点のベクトル場の空間ベクトル方向に基づいて水中グライダの誘導針路データを計算し、水中グライダの実際位置と仮想基準点の位置情報に基づいて水中グライダの誘導前進速度信号を取得した。ベクトル場の作用下で、水中グライダは力線に沿って滑らかに運動することができ、最終的に所望経路に収束される。
【0017】
本発明は、ベクトル場方法に基づいて、水中グライダの過渡状態の制御性能を考慮して、水中グライダの経路追跡のコースをより滑らかにし、水中グライダの運動中のリスクを低減し、また、アクチュエータ機構の動作機能を考慮して、水中の追跡任務を効果的に完成でき、追跡誤差がより小さい収束効果が得られ、水中グライダの経路追跡性能を向上させる。
【図面の簡単な説明】
【0018】
本発明の実施例または従来技術における技術的手段をより明確に説明するために、以下、実施例または先行技術に関連した図面について簡単に紹介するが、以下の図面は本発明の一部の実施例であり、当業者であれば、創造的労働を行なわずにこれらの図面に基づいて他の図面を得ることができることはいうまでもない。
【0019】
【図1】本発明に係る方法のフローチャートである。
【図2】本発明に係る水中グライダのベクトル場における経路追跡の制御装置の構成を示す模式図である。
【図3】本発明に係るシミュレーションでの水中グライダの経路追跡を示す図である。
【図4】本発明に係るシミュレーションでの水中グライダの経路追跡誤差を示す図である。
【図5】本発明に係るシミュレーションでの水中グライダ経路のパラメータの更新を示す図である。
【図6】本発明に係るシミュレーションでの水中グライダの針路追跡の効果を示す図である。
【図7】本発明に係るシミュレーションでの水中グライダの速度追跡の効果を示す図である。
【発明を実施するための形態】
【0020】
本発明に係る実施例の目的、技術的手段及びメリットをより明らかにするために、以下、本発明の実施例における図面を参照しながら、本発明の実施例における技術手段を明らか且つ完全に説明し、説明される実施例が全ての実施例ではなく、本発明の一部の実施例に過ぎないことはいうまでもない。当業者が本発明における実施例に基づいて創造的労動を行うことなく得た他の実施例は、全て本発明が保護する範囲に含まれるものとする。
【0021】
図１は、本発明に係るフローチャートであり、図１に示すように、本実施例の方法は、
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差に基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含む。
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差に基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含む。
【0022】
本発明は、ベクトル場方法によって、所望経路の周囲に空間ベクトルを生成する。生成されたベクトル場の空間ベクトルの方向に基づいて、水中グライダの誘導針路信号を計算し、誘導針路信号は誘導角速度を含む。水中グライダの位置と経路パラメータ点の位置情報に基づいて、水中グライダの誘導前進速度信号を取得した。ベクトル場の作用下で、水中グライダは力線に沿って滑らかに運動することができ、最終的に所望経路に収束される。
【0023】
本発明は、水中グライダの過渡状態の制御性能を考慮して、水中グライダの経路追跡のコースをより滑らかにし、水中グライダの運動中のリスクを低減し、また、アクチュエータ機構の動作機能を考慮して、水中の追跡任務を効果的に完成することができる。ベクトル場の誘導方法によれば、追跡誤差がより小さい収束効果が得られ、水中グライダの経路追跡性能を向上させる。また、経路追跡の制御の過程で、必要な制御入力がより合理的で、水中グライダの応用において、より省エネになる。
【0024】
具体的には、ステップ１では、水中グライダの運動モデルを作成し、所望経路と所望速度を設定し、下記式（I）は水中グライダの運動モデルであり、
（I）
式(I)中、ｘ,ｙ,ｚ∈Ｒはそれぞれ水中グライダの慣性座標系における位置を示し、θ,ψはそれぞれ水中グライダの慣性座標系におけるピッチ角とヨー角を示し、ｕ,ｖ,ｗはそれぞれ水中グライダの機体座標系のＸ軸方向、Ｙ軸方向及びＺ軸方向に沿う線速度を示し、ｑ,ｒはそれぞれ水中グライダの機体座標系のＹ軸方向とＺ軸方向を回る角速度を示す。
【数20】
式(I)中、ｘ,ｙ,ｚ∈Ｒはそれぞれ水中グライダの慣性座標系における位置を示し、θ,ψはそれぞれ水中グライダの慣性座標系におけるピッチ角とヨー角を示し、ｕ,ｖ,ｗはそれぞれ水中グライダの機体座標系のＸ軸方向、Ｙ軸方向及びＺ軸方向に沿う線速度を示し、ｑ,ｒはそれぞれ水中グライダの機体座標系のＹ軸方向とＺ軸方向を回る角速度を示す。
【0025】
運動制御に適した３次元の経路追跡の制御装置を設計するため、流体座標系{Ａ}を作成し、その座標系において、水中グライダの総速度が
と示され、水中グライダの迎え角と横滑り角がぞれぞれα=ａｒｃｔａｎ(ｗ/ｕ)とβ=ａｒｃｔａｎ(ｖ/ｕ)となり、ｕ＞０である。
【数21】
【0026】
横揺れと横揺れ角速度の影響を考慮せず、水中グライダの３次元空間での運動は、以下の運動方程式（II）で示される。
（II）
式（II）中、ΘとΨは、それぞれ水中グライダの流体座標系における経路角と方位角を示す。
【数22】
式（II）中、ΘとΨは、それぞれ水中グライダの流体座標系における経路角と方位角を示す。
【0027】
ステップ２では、所望経路をパラメータ化し、パラメータ化された所望経路の情報を得る。
【0028】
前記ステップ２は、以下のステップＳ１とステップＳ２を含み、
ステップＳ１：Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、所望経路をパラメータ化し、
（１）
式（１）中、ｘｄ、ｙｄとｚｄは、それぞれ所望経路をパラメータ化する関数であり、χは所望経路の経路パラメータであり、Ｐｄは座標系｛Ｆ｝における所望経路であり、
ステップＳ２：式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
（２）
式（２）中、
であり、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角である。
ステップＳ１：Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、所望経路をパラメータ化し、
【数23】
式（１）中、ｘｄ、ｙｄとｚｄは、それぞれ所望経路をパラメータ化する関数であり、χは所望経路の経路パラメータであり、Ｐｄは座標系｛Ｆ｝における所望経路であり、
ステップＳ２：式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
【数24】
式（２）中、
【数25】
【0029】
ステップ３では、パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算する。
【0030】
前記ステップ３は、以下のステップＳ１１とステップＳ１２を含み、
ステップＳ１１：座標系{Ｆ}から慣性座標系{Ｉ}に変換した回転行列を
と定義し、
は式（３）で表し、
（３）
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２：経路追跡誤差である
と経路追跡誤差の二乗和である
を定義し、その中、ｘｅは縦方向の経路追跡誤差であり、ｙｅは横方向の経路追跡誤差であり、ｚｅは垂直方向の経路追跡誤差であり、具体的に式（４）により、経路追跡誤差を計算し、
（４）
式（４）中、
、
であり、ηは水中グライダの実際位置情報であり、ηｄは仮想基準点の位置情報である。
上記ｘｅ,ｙｅ,ｚｅはそれぞれ式ｘｅ=ｘ-ｘｄ、ｙｅ=ｙ-ｙｄ、ｚｅ=ｚ-ｚｄで表す。
ステップＳ１１：座標系{Ｆ}から慣性座標系{Ｉ}に変換した回転行列を
【数26】
【数27】
【数28】
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２：経路追跡誤差である
【数29】
【数30】
【数31】
式（４）中、
【数32】
【数33】
上記ｘｅ,ｙｅ,ｚｅはそれぞれ式ｘｅ=ｘ-ｘｄ、ｙｅ=ｙ-ｙｄ、ｚｅ=ｚ-ｚｄで表す。
【0031】
ステップ４では、パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算する。
【0032】
前記ステップ４は、以下のステップＳ２１とステップＳ２２を含み、
ステップＳ２１：式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
（５）
式（５）中、
は誘導ベクトル場の方向を表し、κ∈Ｒ、κ≧２であり、その中、Ｒは実数集合を表し、κの異なる値は異なる類型のベクトル場を表し、本願でのベクトル場の類型は吸引ベクトル場であるためκ≧２であり、αｘ,αｙ,αｚは水中グライダの追跡経路の三次元接線方向を表し、
ステップＳ２２：式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
（６）
（７）
（８）
（９）
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である。
ステップＳ２１：式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
【数34】
式（５）中、
【数35】
ステップＳ２２：式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
【数36】
【数37】
【数38】
【数39】
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である。
【0033】
予め設定された所望速度と縦方向の経路追跡誤差に基づいて、所望経路を更新し、更新後の所望経路に基づいて、更新後のパラメータ化された水中グライダの所望経路の情報を取得する。
【0034】
具体的には、以下の式（１１）により、経路パラメータの更新率
を計算し、
（１１）
式（11）中、ｖｓは予め設定された所望速度であり、ｋ３は経路パラメータの更新率ゲインを表し、ｋ３∈(０,１０]であり、ｔａｎｈは双曲正接関数であり、ｘｅは縦方向の経路追跡誤差である。
【数40】
【数41】
式（11）中、ｖｓは予め設定された所望速度であり、ｋ３は経路パラメータの更新率ゲインを表し、ｋ３∈(０,１０]であり、ｔａｎｈは双曲正接関数であり、ｘｅは縦方向の経路追跡誤差である。
【0035】
パラメータの更新率
を得た後、更新後の経路パラメータを運動方程式に代入して水中グライダの所定経路の位置情報を再度計算し、その後、更新後の経路追跡誤差と水中グライダの誘導角速度を計算する。
【数42】
【0036】
ステップ５では、所望速度と経路追跡誤差における縦方向の経路追跡誤差に基づいて、水中グライダの前進速度を計算する。
【0037】
前記ステップ５は、式（１０）により、水中グライダの前進速度Ｕｄを計算することを含み、
（１０）
式（１０）中、ｖｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である。
【数43】
式（１０）中、ｖｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である。
【0038】
ステップ６では、水中グライダが誘導角速度と前進速度によって航行する。
従来の水中グライダの経路追跡の制御方法は、制御構造が複雑で、制御モジュールが多いため、製品設計と製造のコストが高く、普及に不利である。
従来の水中グライダの経路追跡の制御方法は、制御構造が複雑で、制御モジュールが多いため、製品設計と製造のコストが高く、普及に不利である。
【0039】
従来の制御方法は、誘導則の設計が複雑で、パラメータ基準の設定が困難で、ロバスト性能が悪く、応用分野が狭いので、本実施例は水中グライダのベクトル場における経路追跡の制御装置を提供し、図２に示すように、ベクトル場の作用下で使用され、水中グライダの誘導角速度と前進速度を取得して、水中グライダの位置センサに送信し、これにより水中グライダを所望経路に沿って運動させる。
【0040】
水中グライダのベクトル場における経路追跡の制御装置は、複数無人潜水艦の編隊制御、共同経路追跡制御、含有制御等の集合制御の場合に、より便利に拡大され、複数無人潜水艦が多シーンで多任務の共同制御に係る問題を解決するのに有利であり、具体的には、水中グライダのベクトル場における経路追跡の制御装置は、参照経路モジュール、経路追跡誤差モジュール、誘導ベクトル場モジュール、経路パラメータ更新モジュール、誘導前進速度モジュールを含む。
【0041】
参照経路モジュールは、その入力端が経路パラメータ更新モジュールに接続され、その出力端が経路追跡誤差モジュールと誘導ベクトル場モジュールとにぞれぞれ接続される。
【0042】
経路追跡誤差モジュールは、その入力端が参照経路モジュールと水中グライダの位置センサとに接続され、その出力端が経路パラメータ更新モジュールと誘導前進速度モジュールとに接続される。
【0043】
誘導ベクトル場モジュールは、その入力端が参照経路モジュールと水中グライダの位置センサとに接続され、その出力端が水中グライダの位置センサに接続される。
経路パラメータ更新モジュールは、その入力端が誤差モジュールの出力端に接続され、その出力端が参照経路モジュールの入力端に接続される。
経路パラメータ更新モジュールは、その入力端が誤差モジュールの出力端に接続され、その出力端が参照経路モジュールの入力端に接続される。
【0044】
誘導前進速度モジュールは、その入力端が経路追跡誤差モジュールに接続され、その出力端が水中グライダの位置センサに接続される。
【0045】
参照経路モジュールは、経路パラメータ更新モジュールにより入力された経路パラメータに基づいて、水中グライダのパラメータ化された所望経路の情報を取得する。
【0046】
経路追跡誤差モジュールは、水中グライダのパラメータ化された所望経路の情報と水中グライダの実際位置情報とに基づいて、水中グライダの所望経路の位置と水中グライダの実際位置との間の経路の誤差を取得する。
【0047】
誘導ベクトル場モジュールは、水中グライダのパラメータ化された所望経路の情報と水中グライダの実際位置情報とに基づいて、水中グライダの誘導針路を取得する。
【0048】
経路パラメータ更新モジュールは、予め設定された所望速度と縦方向の経路追跡誤差とに基づいて、更新後の所望経路を取得して参照経路モジュールに送信する。
【0049】
誘導前進速度モジュールは、所望速度と縦方向の経路追跡誤差に基づいて、水中グライダの前進速度を取得する。
【0050】
シミュレーションの結果は、図３～図７に示す。図３は、ベクトル場の経路追跡における水中グライダの運動軌跡を示す図であり、図３において破線は所定の参照経路を示し、実線は水中グライダの実際の運動軌跡を示す。図３から分かるように、水中グライダは、一定の距離を航行してから所定のパラメータ化された経路を追跡し、一定の時間の後により良い追跡効果が得られる。
【0051】
【0052】
【0053】
図６は無人船のベクトル場の経路追跡における針路追跡の効果を示す図であり、図６において破線は参考針路を示し、実線は実際の針路を示し、図６から分かるように、水中グライダの針路が短時間で所定の針路までよく追跡することができる。
【0054】
【0055】
本発明は、以下のメリットがある。
本発明は、水中グライダのベクトル場における経路追跡の制御方法を提供し、仮想基準点のベクトル場の空間ベクトル方向に基づいて、水中グライダの誘導針路データを計算し、水中グライダの実際位置と仮想基準点の位置情報に基づいて水中グライダの誘導前進速度信号を取得した。ベクトル場の作用下で、水中グライダは力線に沿って滑らかに運動することができ、最終に所望経路に収束される。
本発明は、水中グライダのベクトル場における経路追跡の制御方法を提供し、仮想基準点のベクトル場の空間ベクトル方向に基づいて、水中グライダの誘導針路データを計算し、水中グライダの実際位置と仮想基準点の位置情報に基づいて水中グライダの誘導前進速度信号を取得した。ベクトル場の作用下で、水中グライダは力線に沿って滑らかに運動することができ、最終に所望経路に収束される。
【0056】
本発明は、ベクトル場方法において、水中グライダの過渡状態の制御性能を考慮して、水中グライダの経路追跡のコースをより滑らかにし、水中グライダの運動中のリスクを低減し、また、アクチュエータ機構の動作機能を考慮して、水中の追跡任務を効果的に完成でき、追跡誤差がより小さい収束効果が得られ、水中グライダの経路追跡性能を向上させる。
【0057】
最後に以下のことを説明すべきである。以上の各実施例は本発明の技術的手段を説明するためのものにすぎなく、それを限定するものではない。上述した各実施例を参照しながら本発明を詳細に説明したが、上述した各実施例に記載の技術的手段を修正するか、またはその技術的特徴の一部または全部に同等な取り替えを実施することも可能であり、それらの修正や取り替えによって、対応する技術的手段の本質が本発明の各実施例の技術的手段の範囲から逸脱しないことは当業者に理解されよう。
【0058】
（付記）
（付記１）
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含むことを特徴とする水中グライダのベクトル場における経路追跡の制御方法。
（付記１）
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
所望経路をパラメータ化し、パラメータ化された所望経路の情報を取得するステップ２と、
パラメータ化された所望経路の情報に基づいて、仮想基準点の位置情報を取得し、仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された所望経路の情報に基づいて、誘導ベクトル場情報を取得し、誘導ベクトル場情報と水中グライダの実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
所望速度と経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが誘導角速度と前進速度によって航行するステップ６と、を含むことを特徴とする水中グライダのベクトル場における経路追跡の制御方法。
【0059】
（付記２）
前記ステップ２は、ステップＳ１とステップＳ２を含み、
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、所望経路をパラメータ化し、
（１）
式（１）中、ｘｄ、ｙｄ、ｚｄは、それぞれ所望経路をパラメータ化するための関数であり、Ｐｄは座標系{Ｆ}における所望経路であり、χは所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
（２）
式（２）中、
であり、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角である、ことを特徴とする付記１に記載の水中グライダのベクトル場における経路追跡の制御方法。
前記ステップ２は、ステップＳ１とステップＳ２を含み、
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、所望経路をパラメータ化し、
【数44】
式（１）中、ｘｄ、ｙｄ、ｚｄは、それぞれ所望経路をパラメータ化するための関数であり、Ｐｄは座標系{Ｆ}における所望経路であり、χは所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
【数45】
式（２）中、
【数46】
【0060】
（付記３）
前記ステップ３は、ステップＳ１１とステップＳ１２を含み、
ステップＳ１１、回転行列である
を定義し、前記
は座標系{Ｆ}から慣性座標系{Ｉ}に変換した回転行列であり、
は式（３）で表し、
（３）
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２、経路追跡誤差である
を定義し、その中、ｘｅは縦方向の経路追跡誤差であり、ｙｅは横方向の経路追跡誤差であり、ｚｅは垂直方向の経路追跡誤差であり、式（４）により、経路追跡誤差を計算し、
（４）
式（４）中、
、
であり、ηは水中グライダの実際位置情報であり、ηｄは仮想基準点の位置情報である、ことを特徴とする付記２に記載の水中グライダのベクトル場における経路追跡の制御方法。
前記ステップ３は、ステップＳ１１とステップＳ１２を含み、
ステップＳ１１、回転行列である
【数47】
【数48】
【数49】
【数50】
式（３）中、θｄ(χ)はピッチ角の接線角であり、ψｄ(χ)はヨー角の接線角であり、
ステップＳ１２、経路追跡誤差である
【数51】
【数52】
式（４）中、
【数53】
【数54】
【0061】
（付記４）
前記ステップ４は、ステップＳ２１とステップＳ２２を含み、
ステップＳ２１、式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
（５）
式（５）中、
はベクトル場の方向を表し、κ∈Ｒ、κ≧２であり、
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
（６）
（７）
（８）
（９）
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である、ことを特徴とする付記３に記載の水中グライダのベクトル場における経路追跡の制御方法。
前記ステップ４は、ステップＳ２１とステップＳ２２を含み、
ステップＳ２１、式（５）により、仮想基準点から生成される誘導ベクトル場を作成し、
【数55】
式（５）中、
【数56】
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑｄと水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒｄを含み、
【数57】
【数58】
【数59】
【数60】
式中、Θｄは水中グライダの経路角であり、Ψｄは水中グライダの方位角であり、ｋ１∈Ｒは誘導ピッチ角速度のゲインを表し、ｋ１∈(０,１０]であり、ｋ２∈Ｒは誘導ヨー角速度のゲインを表し、ｋ２∈(０,１０]である、ことを特徴とする付記３に記載の水中グライダのベクトル場における経路追跡の制御方法。
【0062】
（付記５）
前記ステップ５は、式（１０）により、水中グライダの前進速度Ｕｄを計算することを含み、
（１０）
式（１０）中、ｖｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である、ことを特徴とする付記４に記載の水中グライダのベクトル場における経路追跡の制御方法。
前記ステップ５は、式（１０）により、水中グライダの前進速度Ｕｄを計算することを含み、
【数61】
式（１０）中、ｖｓは所望速度であり、ｋ４∈Ｒは前進速度のゲインを表し、ｋ４∈(０,１０]であり、ｔａｎｈは双曲正接関数である、ことを特徴とする付記４に記載の水中グライダのベクトル場における経路追跡の制御方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【手続補正書】
【提出日】2024-03-18
【手続補正1】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項1】
水中グライダの運動モデルを作成し、所望経路と所望速度を設定するステップ１と、
前記所望経路をパラメータ化し、パラメータ化された前記所望経路の情報を取得するステップ２と、
パラメータ化された前記所望経路の情報に基づいて、仮想基準点の位置情報を取得し、前記仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された前記所望経路の情報に基づいて、誘導ベクトル場情報を取得し、前記誘導ベクトル場情報と水中グライダの前記実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
前記所望速度と前記経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが前記誘導角速度と前記前進速度によって航行するステップ６と、を含み、
前記ステップ２は、ステップＳ１とステップＳ２を含み、
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、前記所望経路をパラメータ化し、
（１）
式（１）中、ｘ ｄ 、ｙ ｄ 、ｚ ｄ は、それぞれ前記所望経路をパラメータ化するための関数であり、Ｐ ｄ は座標系{Ｆ}における所望経路であり、χは前記所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された前記所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
（２）
式（２）中、
であり、θ
ｄ
(χ)は前記ピッチ角の接線角であり、ψ
ｄ
(χ)は前記ヨー角の接線角であり、
前記ステップ３は、ステップＳ１１とステップＳ１２を含み、
ステップＳ１１、回転行列である
を定義し、前記
は座標系{Ｆ}から慣性座標系{Ｉ}に変換した回転行列であり、
は式（３）で表し、
（３）
式（３）中、θ ｄ (χ)は前記ピッチ角の接線角であり、ψ ｄ (χ)は前記ヨー角の接線角であり、
ステップＳ１２、前記経路追跡誤差である
を定義し、その中、ｘ
ｅ
は前記縦方向の経路追跡誤差であり、ｙ
ｅ
は横方向の経路追跡誤差であり、ｚ
ｅ
は垂直方向の経路追跡誤差であり、式（４）により、前記経路追跡誤差を計算し、
（４）
式（４）中、
、
であり、ηは水中グライダの実際位置情報であり、η
ｄ
は前記仮想基準点の位置情報であり、
前記ステップ４は、ステップＳ２１とステップＳ２２を含み、
ステップＳ２１、式（５）により、前記仮想基準点から生成される誘導ベクトル場を作成し、
（５）
式（５）中、
はベクトル場の方向を表し、κ∈Ｒ、κ≧２であり、
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの前記誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑ ｄ と水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒ ｄ を含み、
（６）
（７）
（８）
（９）
式中、Θ ｄ は水中グライダの経路角であり、Ψ ｄ は水中グライダの方位角であり、ｋ １ ∈Ｒは前記誘導ピッチ角速度のゲインを表し、ｋ １ ∈(０,１０]であり、ｋ ２ ∈Ｒは前記誘導ヨー角速度のゲインを表し、ｋ ２ ∈(０,１０]であり、
前記ステップ５は、式（１０）により、水中グライダの前記前進速度Ｕ ｄ を計算することを含み、
（１０）
式（１０）中、ｖ ｓ は前記所望速度であり、ｋ ４ ∈Ｒは前記前進速度のゲインを表し、ｋ ４ ∈(０,１０]であり、ｔａｎｈは双曲正接関数である、
ことを特徴とする水中グライダのベクトル場における経路追跡の制御方法。
前記所望経路をパラメータ化し、パラメータ化された前記所望経路の情報を取得するステップ２と、
パラメータ化された前記所望経路の情報に基づいて、仮想基準点の位置情報を取得し、前記仮想基準点と水中グライダの実際位置との間の経路追跡誤差を計算するステップ３と、
パラメータ化された前記所望経路の情報に基づいて、誘導ベクトル場情報を取得し、前記誘導ベクトル場情報と水中グライダの前記実際位置に基づいて、水中グライダの誘導角速度を計算するステップ４と、
前記所望速度と前記経路追跡誤差における縦方向の経路追跡誤差とに基づいて、水中グライダの前進速度を計算するステップ５と、
水中グライダが前記誘導角速度と前記前進速度によって航行するステップ６と、を含み、
前記ステップ２は、ステップＳ１とステップＳ２を含み、
ステップＳ１、Ｓｅｒｒｅｔ－Ｆｒｅｎｅｔ座標系{Ｆ}を作成し、式（１）により、前記所望経路をパラメータ化し、
【数1】
式（１）中、ｘ ｄ 、ｙ ｄ 、ｚ ｄ は、それぞれ前記所望経路をパラメータ化するための関数であり、Ｐ ｄ は座標系{Ｆ}における所望経路であり、χは前記所望経路の経路パラメータであり、
ステップＳ２、式（２）により、パラメータ化された前記所望経路の情報を取得し、前記パラメータ化された情報は、ピッチ角の接線角とヨー角の接線角を含み、
【数2】
式（２）中、
【数3】
前記ステップ３は、ステップＳ１１とステップＳ１２を含み、
ステップＳ１１、回転行列である
【数4】
【数5】
【数6】
【数7】
式（３）中、θ ｄ (χ)は前記ピッチ角の接線角であり、ψ ｄ (χ)は前記ヨー角の接線角であり、
ステップＳ１２、前記経路追跡誤差である
【数8】
【数9】
式（４）中、
【数10】
【数11】
前記ステップ４は、ステップＳ２１とステップＳ２２を含み、
ステップＳ２１、式（５）により、前記仮想基準点から生成される誘導ベクトル場を作成し、
【数12】
式（５）中、
【数13】
ステップＳ２２、式（６）、（７）、（８）、（９）により、水中グライダの前記誘導角速度を計算し、前記誘導角速度は水中グライダのＹ軸方向に沿って回る誘導ピッチ角速度ｑ ｄ と水中グライダのＺ軸方向に沿って回る誘導ヨー角速度ｒ ｄ を含み、
【数14】
【数15】
【数16】
【数17】
式中、Θ ｄ は水中グライダの経路角であり、Ψ ｄ は水中グライダの方位角であり、ｋ １ ∈Ｒは前記誘導ピッチ角速度のゲインを表し、ｋ １ ∈(０,１０]であり、ｋ ２ ∈Ｒは前記誘導ヨー角速度のゲインを表し、ｋ ２ ∈(０,１０]であり、
前記ステップ５は、式（１０）により、水中グライダの前記前進速度Ｕ ｄ を計算することを含み、
【数18】
式（１０）中、ｖ ｓ は前記所望速度であり、ｋ ４ ∈Ｒは前記前進速度のゲインを表し、ｋ ４ ∈(０,１０]であり、ｔａｎｈは双曲正接関数である、
ことを特徴とする水中グライダのベクトル場における経路追跡の制御方法。