特許 6806462 水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6806462

(24)【登録日】2020年12月8日

(45)【発行日】2021年1月6日

(54)【発明の名称】水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラム

(51)【国際特許分類】

   B63C 11/48 20060101AFI20201221BHJP

【ＦＩ】

   B63C11/48 D

【請求項の数】5

【全頁数】21

(21)【出願番号】特願2016-100630(P2016-100630)

(22)【出願日】2016年5月19日

(65)【公開番号】特開2017-206154(P2017-206154A)

(43)【公開日】2017年11月24日

【審査請求日】2019年4月25日

(73)【特許権者】

【識別番号】000006208

【氏名又は名称】三菱重工業株式会社

(74)【代理人】

【識別番号】100149548

【弁理士】

【氏名又は名称】松沼 泰史

(74)【代理人】

【識別番号】100162868

【弁理士】

【氏名又は名称】伊藤 英輔

(74)【代理人】

【識別番号】100161702

【弁理士】

【氏名又は名称】橋本 宏之

(74)【代理人】

【識別番号】100189348

【弁理士】

【氏名又は名称】古都 智

(74)【代理人】

【識別番号】100196689

【弁理士】

【氏名又は名称】鎌田 康一郎

(74)【代理人】

【識別番号】100210572

【弁理士】

【氏名又は名称】長谷川 太一

(74)【代理人】

【識別番号】100134544

【弁理士】

【氏名又は名称】森 隆一郎

(74)【代理人】

【識別番号】100064908

【弁理士】

【氏名又は名称】志賀 正武

(74)【代理人】

【識別番号】100108578

【弁理士】

【氏名又は名称】高橋 詔男

(74)【代理人】

【識別番号】100126893

【弁理士】

【氏名又は名称】山崎 哲男

(72)【発明者】

【氏名】望月 慶佑

(72)【発明者】

【氏名】粟屋 伊智郎

(72)【発明者】

【氏名】中田 昌宏

(72)【発明者】

【氏名】井田 匡彦

【審査官】 福田 信成

(56)【参考文献】

【文献】 特開２０１０−１４３２３８（ＪＰ，Ａ）

【文献】 特開２０１４−０７３７２９（ＪＰ，Ａ）

【文献】 特開平０８−０２０３９１（ＪＰ，Ａ）

【文献】 特開２０１３−１３９１８５（ＪＰ，Ａ）

【文献】 特開２０１５−０１６８２６（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１１／０２９７０７０（ＵＳ，Ａ１）

【文献】 中国特許出願公開第１０３９５５２１８（ＣＮ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ６３Ｃ １１／００ − １１／５２

Ｂ６３Ｇ ８／１４ − ８／４２

Ｇ０５Ｄ １／００

Ｇ０５Ｄ １／０８

(57)【特許請求の範囲】

【請求項1】

複数のアクチュエータから発生する推力により水中を航走する水中航走体を制御する水中航走体制御装置であって、
前記水中航走体に搭載されたセンサを通じて得られた当該水中航走体の状態量の実測値と、与えられた前記状態量の指令値と、の偏差に基づいて、当該偏差を小さくするために前記水中航走体に与えるべき推力を示す推力指令値を算出するフィードバック制御部と、
前記推力指令値に基づく所定の外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を前記水中航走体に与えるために、複数の前記アクチュエータの各々が発生すべき推力を示すアクチュエータ別推力指令値を算出する推力配分部と、
前記推力指令値と前記状態量の実測値とに基づいて、前記水中航走体に作用する外乱力を示す外乱推定値を算出する外乱推定部と、
を備え、
前記外乱補償推力指令値は、
前記フィードバック制御部によって算出された前記推力指令値から、前記外乱推定部によって算出された前記外乱推定値が差し引かれてなり、
前記状態量の実測値に基づいて、前記フィードバック制御部におけるフィードバック制御の特性を示す制御パラメータを算出する制御パラメータ演算部を更に備え、
前記フィードバック制御部は、
前記制御パラメータ演算部によって算出された制御パラメータに基づいて、前記偏差に応じた前記推力指令値を算出し、
前記制御パラメータ演算部は、
前記フィードバック制御部において予め規定された規定パラメータに、前記外乱推定部の帯域と、前記水中航走体の前記状態量に応じた当該水中航走体の固有値と、に基づいて算出される補償値を乗算することで、前記制御パラメータを算出する
水中航走体制御装置。

【請求項2】

前記外乱推定部は、
前記推力配分部、前記アクチュエータ、及び、前記水中航走体の機体を一つの慣性体と見なしてなる慣性体モデルに基づいて、前記外乱推定値を算出する
請求項１に記載の水中航走体制御装置。

【請求項3】

請求項１または請求項２に記載の水中航走体制御装置と、
前記水中航走体と、
を備える水中航走システム。

【請求項4】

複数のアクチュエータから発生する推力により水中を航走する水中航走体を制御する方法であって、
前記水中航走体に搭載されたセンサを通じて得られた当該水中航走体の状態量の実測値と、与えられた前記状態量の指令値と、の偏差に基づいて、当該偏差を小さくするために前記水中航走体に与えるべき推力を示す推力指令値を算出するフィードバック制御ステップと、
前記推力指令値に応じた所定の外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を前記水中航走体に与えるために、複数の前記アクチュエータの各々が発生すべき推力を示すアクチュエータ別推力指令値を算出する推力配分ステップと、
外乱推定部が、前記推力指令値と前記状態量の実測値とに基づいて、前記水中航走体に作用する外乱力の推定結果を示す外乱推定値を算出する外乱推定ステップと、
を有し、
前記外乱補償推力指令値は、
前記フィードバック制御ステップにおいて算出された推力指令値から前記外乱推定ステップにおいて算出された外乱推定値が差し引かれてなり、
前記状態量の実測値に基づいて、前記フィードバック制御ステップにおけるフィードバック制御の特性を示す制御パラメータを算出する制御パラメータ演算ステップを更に有し、
前記フィードバック制御ステップでは、
前記制御パラメータ演算ステップで算出された制御パラメータに基づいて、前記偏差に応じた前記推力指令値を算出し、
前記制御パラメータ演算ステップでは、
前記フィードバック制御ステップにおいて予め規定された規定パラメータに、前記外乱推定部の帯域と、前記水中航走体の前記状態量に応じた当該水中航走体の固有値と、に基づいて算出される補償値を乗算することで、前記制御パラメータを算出する
水中航走体制御方法。

【請求項5】

複数のアクチュエータから発生する推力により水中を航走する水中航走体を制御する水中航走体制御装置のコンピュータを、
前記水中航走体に搭載されたセンサを通じて得られた当該水中航走体の状態量の実測値と、与えられた前記状態量の指令値と、の偏差に基づいて、当該偏差を小さくするために前記水中航走体に与えるべき推力を示す推力指令値を算出するフィードバック制御部、
前記推力指令値に応じた所定の外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を前記水中航走体に与えるために、複数の前記アクチュエータの各々が発生すべき推力を示すアクチュエータ別推力指令値を算出する推力配分部、
前記推力指令値と前記状態量の実測値とに基づいて、前記水中航走体に作用する外乱力の推定結果を示す外乱推定値を算出する外乱推定部、
として機能させ、
前記外乱補償推力指令値は、
前記フィードバック制御部によって算出された推力指令値から前記外乱推定部によって算出された外乱推定値が差し引かれてなり、
前記状態量の実測値に基づいて、前記フィードバック制御部におけるフィードバック制御の特性を示す制御パラメータを算出する制御パラメータ演算部を更に備え、
前記フィードバック制御部は、
前記制御パラメータ演算部によって算出された制御パラメータに基づいて、前記偏差に応じた前記推力指令値を算出し、
前記制御パラメータ演算部は、
前記フィードバック制御部において予め規定された規定パラメータに、前記外乱推定部の帯域と、前記水中航走体の前記状態量に応じた当該水中航走体の固有値と、に基づいて算出される補償値を乗算することで、前記制御パラメータを算出する
プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラムに関する。

【背景技術】

【0002】

潜水艦、自律型無人潜水機（autonomous underwater vehicle：ＡＵＶ）等の水中航走体は、機体に備えられた複数のアクチュエータ（スラスタ、舵など）で発生させた推力によって水中を航走する。このような水中航走体を所望に航走させるためには、水中において６軸（前後、左右、上下、ロール、ピッチ、ヨー）の状態（位置・速度）を同時に制御する必要がある。しかしながら、一般に、与えられた推力に対する水中航走体の運動は、上述の６軸についての非線形な運動方程式で表現されるため、制御が難しい。

【0003】

水中航走体の運動特性については、例えば、速度に応じて特性が変化する非線形性が代表例である。例えば、ヨー角度を変更する旋回動作では、水中航走体の速度が大きいほど曲がりやすく、速度が小さいほど曲がりにくい（旋回に大きな力を要する）、という特性を有している。
また、各軸の特性は、他軸からの干渉も受け得る。例えば、水中航走体のピッチ角を変化させると、機体が傾いて、機体の重心位置（上下方向の位置）も同時に変化してしまう。
更に、水中航走体を動作させるスラスタや舵自体も、水中航走体の速度等に応じて非線形な特性を持つ。

【0004】

水中航走体に対する最も簡単な制御方法の一例としては、例えば、アクチュエータと機体とを一体として制御対象とみなし、運動指令（深度指令等）と実際のセンサ値との偏差を算出して、当該偏差に基づくＰＩＤ制御（若しくはそれに準じるもの、Ｉ−ＰＤ制御等）を行うことである。
また、機体の速度等、アクチュエータや機体の特性を変化させる要因の状態に応じて、あらかじめ設定したゲインテーブルを用いる、又は、あらかじめ構築した数式モデルの関係式を利用して、ＰＩＤ制御器のゲインを動的に変化させる手法（ゲインスケジューリング）が利用されている（例えば、特許文献１参照）。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２０１０−１４３２３８号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

上述のようなＰＩＤ制御等におけるゲインスケジューリングを実現するためには、水中航走体の機体及びアクチュエータに固有の特性を精密に再現する数式モデルを構築するとともに、当該数式モデルを詳細に解析し、速度等の運動条件毎に適切な制御ゲインを予め計算しておく必要がある。
しかしながら、機体及びアクチュエータの非線形的な特性を網羅して数式モデルを構築するには多大な時間を要する。また、時間をかけて構築した複雑な数式モデルであっても、幾分かのモデル化誤差は常に存在し、更に、数式モデルに現れない外乱要素（波によって受ける力等）や、他軸からの干渉も存在する。
そのため、ゲインスケジューリングを用いたとしても、水中航走体の運動制御における十分な制御精度を確保するのは困難である。

【0007】

本発明の目的は、簡素な設計で、水中航走体に対する精度の高い運動制御を実現可能な水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラムを提供することにある。

【課題を解決するための手段】

【0008】

本発明の一態様に係る水中航走体制御装置は、複数のアクチュエータから発生する推力により水中を航走する水中航走体を制御する水中航走体制御装置であって、前記水中航走体に搭載されたセンサを通じて得られた当該水中航走体の状態量の実測値と、与えられた前記状態量の指令値と、の偏差に基づいて、当該偏差を小さくするために前記水中航走体に与えるべき推力を示す推力指令値を算出するフィードバック制御部と、前記推力指令値に基づく所定の外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を前記水中航走体に与えるために、複数の前記アクチュエータの各々が発生すべき推力を示すアクチュエータ別推力指令値を算出する推力配分部と、前記推力指令値と前記状態量の実測値とに基づいて、前記水中航走体に作用する外乱力を示す外乱推定値を算出する外乱推定部と、を備えている。ここで、前記外乱補償推力指令値は、前記フィードバック制御部によって算出された前記推力指令値から、前記外乱推定部によって算出された前記外乱推定値が差し引かれてなる。

【0009】

また、本発明の一態様によれば、前記外乱推定部は、前記推力配分部、前記アクチュエータ、及び、前記水中航走体の機体を一つの慣性体と見なしてなる慣性体モデルに基づいて、前記外乱推定値を算出する。

【0010】

また、本発明の一態様に係る水中航走体制御装置は、前記状態量の実測値に基づいて、前記フィードバック制御部におけるフィードバック制御の特性を示す制御パラメータを算出する制御パラメータ演算部を更に備えている。また、前記フィードバック制御部は、前記制御パラメータ演算部によって算出された制御パラメータに基づいて、前記偏差に応じた前記推力指令値を算出する。

【0011】

また、本発明の一態様によれば、前記制御パラメータ演算部は、前記フィードバック制御部において予め規定された規定パラメータに、前記外乱推定部の帯域と、前記水中航走体の前記状態量に応じた当該水中航走体の固有値と、に基づいて算出される補償値を乗算することで、前記制御パラメータを算出する。

【0012】

また、本発明の一態様に係る水中航走システムは、上述の水中航走体制御装置と、前記水中航走体と、を備えている。

【0013】

また、本発明の一態様は、複数のアクチュエータから発生する推力により水中を航走する水中航走体を制御する方法であって、前記水中航走体に搭載されたセンサを通じて得られた当該水中航走体の状態量の実測値と、与えられた前記状態量の指令値と、の偏差に基づいて、当該偏差を小さくするために前記水中航走体に与えるべき推力を示す推力指令値を算出するフィードバック制御ステップと、前記推力指令値に応じた所定の外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を前記水中航走体に与えるために、複数の前記アクチュエータの各々が発生すべき推力を示すアクチュエータ別推力指令値を算出する推力配分ステップと、前記推力指令値と前記状態量の実測値とに基づいて、前記水中航走体に作用する外乱力の推定結果を示す外乱推定値を算出する外乱力推定ステップと、を有し、前記外乱補償推力指令値は、前記フィードバック制御ステップにおいて算出された推力指令値から前記外乱推定ステップにおいて算出された外乱推定値が差し引かれてなる水中航走体制御方法である。

【0014】

また、本発明の一態様は、複数のアクチュエータから発生する推力により水中を航走する水中航走体を制御する水中航走体制御装置のコンピュータを、前記水中航走体に搭載されたセンサを通じて得られた当該水中航走体の状態量の実測値と、与えられた前記状態量の指令値と、の偏差に基づいて、当該偏差を小さくするために前記水中航走体に与えるべき推力を示す推力指令値を算出するフィードバック制御部、前記推力指令値に応じた所定の外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を前記水中航走体に与えるために、複数の前記アクチュエータの各々が発生すべき推力を示すアクチュエータ別推力指令値を算出する推力配分部、前記推力指令値と前記状態量の実測値とに基づいて、前記水中航走体に作用する外乱力の推定結果を示す外乱推定値を算出する外乱推定部、として機能させ、前記外乱補償推力指令値は、前記フィードバック制御部によって算出された推力指令値から前記外乱推定部によって算出された外乱推定値が差し引かれてなるプログラムである。

【発明の効果】

【0015】

上述の水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラムによれば、簡素な設計で、水中航走体に対する精度の高い運動制御が実現可能となる。

【図面の簡単な説明】

【0016】

【図1】第１の実施形態に係る水中航走システムの全体構成を示す図である。

【図2】第１の実施形態に係る水中航走体の構成を示す図である。

【図3】第１の実施形態に係る水中航走体制御装置の機能構成を示す図である。

【図4】第１の実施形態に係る外乱推定部の機能を説明する図である。

【図5】第１の実施形態に係る外乱推定部の構成を示す図である。

【図6】第１の実施形態に係る制御パラメータ演算部の機能を説明する第１の図である。

【図7】第１の実施形態に係る制御パラメータ演算部の機能を説明する第２の図である。

【図8】第１の実施形態に係る制御パラメータ演算部の処理フローを示す図である。

【発明を実施するための形態】

【0017】

＜第１の実施形態＞
以下、第１の実施形態に係る水中航走システム及び水中航走体制御装置について、図１〜図８を参照しながら詳細に説明する。

【0018】

（水中航走システムの全体構成）
図１は、第１の実施形態に係る水中航走システムの全体構成を示す図である。
図１に示すように、第１の実施形態に係る水中航走システム１は、無人潜水機である水中航走体１０と、当該水中航走体１０のコントローラである水中航走体制御装置２と、を備えている。また、水中航走体１０は、アクチュエータの一態様である複数のスラスタ１００と、機体１０１と、を有してなる。

【0019】

水中航走体１０は、機体１０１に具備された複数のスラスタ１００から発生する推力の組み合わせに応じて、機体前後方向（±Ｘ方向）、機体左右方向（±Ｙ方向）、機体上下方向（±Ｚ方向）に移動する。また、水中航走体１０は、上記推力の組み合わせに応じて、ロール（Ｘ軸回りの回転）、ピッチ（Ｙ軸回りの回転）、ヨー（Ｚ軸回りの回転）も所望に調整可能とする。

【0020】

水中航走体制御装置２は、水中航走体１０に対する操作者からの指令（位置指令、速度指令等）を受け付ける。また、水中航走体制御装置２は、操作者から受け付けた指令に応じた、水中航走体１０のスラスタ１００各々に対する回転数指令値を出力する。この回転数指令値を受け付けたスラスタ１００は、当該回転数指令値に応じた回転数（回転速度）で回転する。これにより、水中航走体１０が操作者の指令にしたがって駆動する。

【0021】

なお、本実施形態に係る水中航走システム１では、水中航走体１０は無人潜水機であって、地上に配置された水中航走体制御装置２から遠隔操作される態様としているが、他の実施形態においてはこの態様に限定されない。例えば、他の実施形態に係る水中航走システム１では、水中航走体１０は有人の潜水艦であって、当該潜水艦の内部（操舵室）に設置された水中航走体制御装置２を用いて、当該潜水艦の搭乗者が操作する態様であってもよい。
また、他の実施形態に係る水中航走システム１は、水中航走体１０に予め指令が設定された制御装置が搭載され、当該指令に規定された動作パターンに従って自動的に動作する態様であってもよい。
また、他の実施形態に係る水中航走システム１は、水中航走体１０に搭載されたセンサを通じて海底地形を照合しながら、自身で目標指令を算出して自動航行する（ある決まった海域を自動で探査する）機能を有する態様であってもよい。

【0022】

（水中航走体の構成）
図２は、第１の実施形態に係る水中航走体の構成を示す図である。
図２に示すように、第１の実施形態に係る水中航走体１０は、７個のスラスタ１００（ＴＨ１〜ＴＨ７）が設けられている。各スラスタ１００は、機体１０１の各軸方向に沿って、当該機体１０１の内部を一方側から他方側まで伸びる筒状の空孔をなしている。各スラスタ１００における空孔の内部にはプロペラが設けられている。各スラスタ１００は、当該プロペラが所望の回転数で回転することで、各軸方向に沿った推力を発生させる。

【0023】

図２の背面図に示すように、水中航走体１０には、機体１０１の背面側から正面側にかけて、２個のスラスタＴＨ１、ＴＨ２が設けられている。このスラスタＴＨ１、ＴＨ２は、機体１０１の前後方向（±Ｘ方向）制御及びヨー（Ｚ軸回りの角度）制御に用いられる。
また、機体１０１の側面には、２個のスラスタＴＨ３、ＴＨ４が設けられている。このスラスタＴＨ３、ＴＨ４は、機体１０１の左右方向（±Ｙ方向）制御及びヨー制御に用いられる（図２の側面図参照）。
更に、機体１０１の上面側から下面側にかけては、３個のスラスタＴＨ５、ＴＨ６、ＴＨ７が設けられている。このスラスタＴＨ５〜ＴＨ７は、機体１０１の上下方向（±Ｚ方向）制御、ロール（Ｘ軸回りの角度）制御、及び、ピッチ（Ｙ軸回りの角度）制御に用いられる（図２の平面図参照）。

【0024】

このように、第１の実施形態に係る水中航走体１０は、機体１０１に設けられた７個のスラスタＴＨ１〜ＴＨ７の各々から発生する推力に基づいて、水中において６軸（前後、左右、上下、ロール、ピッチ、ヨー）の状態（位置・速度）を所望に変更することができる。

【0025】

なお、図２に示した水中航走体１０の態様はあくまで一例であって、他の実施形態においては図２に示した態様に限定されない。例えば、他の実施形態に係る水中航走体１０は、図２に示した態様とは異なる位置に、異なる数のスラスタ１００が設けられていてもよい。
また、スラスタＴＨ１〜ＴＨ７は、推力の発生源となる「アクチュエータ」の一態様であり、アクチュエータはこれに限定されない。例えば、他の実施形態に係る水中航走体１０は、アクチュエータとして「舵」を具備する態様であってもよい。更に、水中航走体１０は、アクチュエータとして、スラスタ１００及び「舵」の両方を具備する態様であってもよい。

【0026】

（水中航走体制御装置の機能構成）
図３は、第１の実施形態に係る水中航走体制御装置の機能構成を示す図である。
図３に示すように、水中航走体制御装置２は、フィードバック制御部２０と、推力配分部２１と、回転数指令変換部２２と、外乱推定部２３と、制御パラメータ演算部２４と、を備えている。

【0027】

フィードバック制御部２０は、水中航走体１０に搭載されたセンサ（後述する速度センサＳｎ１、位置センサＳｎ２）を通じて得られた水中航走体１０の状態量（速度、位置）の実測値と、別途与えられた状態量の指令値（速度指令値、位置指令値）と、を取得する。そして、フィードバック制御部２０は、取得した実測値と指令値との偏差に基づいて、当該偏差を小さくするために水中航走体１０に与えるべき推力を示す推力指令値を算出する。

【0028】

具体的には、フィードバック制御部２０は、位置制御器２００と、速度制御器２０１と、を備えている。
位置制御器２００は、操作者による操作に応じて入力された位置の指令値と、水中航走体１０の位置センサＳｎ２より得られた位置の実測値との偏差を算出する。そして、位置制御器２００は、当該偏差に所定の比例ゲイン（位置制御ゲインＫｐ）を乗算することで、水中航走体１０に対して付与すべき速度を示す速度指令値を算出する。
また、速度制御器２０１は、位置制御器２００から受け付けた速度指令値と、水中航走体１０の速度センサＳｎ１より得られた速度の実測値との偏差を算出する。そして、速度制御器２０１は、当該偏差に所定の比例ゲイン（速度制御ゲインＫｖ）を乗算することで、水中航走体１０全体に与えるべき（作用させるべき）推力を示す推力指令値を算出する。

【0029】

推力配分部２１は、フィードバック制御部２０からの推力指令値に基づく所定の外乱補償推力指令値（後述）を入力し、当該外乱補償推力指令値に示される推力を水中航走体１０に与えるために、７個のスラスタ１００の各々が発生すべき推力を示すスラスタ別推力指令値（アクチュエータ別推力指令値）を算出する。
ここで、フィードバック制御部２０は、６軸の各運動モード（前後、左右、上下、ロール、ピッチ、ヨー）の各々についての推力指令値を出力する。具体的には、フィードバック制御部２０が出力する推力指令値は、機体前後方向（±Ｘ方向）の推進力Ｆ_ｘ、機体左右方向（±Ｙ方向）の推進力Ｆ_ｙ、機体上下方向（±Ｚ方向）の推進力Ｆ_ｚ、機体Ｘ軸回りの回転モーメントＭ_ｘ、機体Ｙ軸回りの回転モーメントＭ_ｙ、及び、機体Ｚ軸回りの回転モーメントＭ_ｚからなる。なお、後述する外乱補償推力指令値についても同様である。以下の説明において、上記各運動モードについての推力指令値（及び外乱補償推力指令値）を総称して「運動モード別推力指令値」とも表記する。
他方、図２に示したスラスタ１００（スラスタＴＨ１、ＴＨ２、・・・、ＴＨ７）の各々が発生すべき推力であるスラスタ別推力指令値をＴ_１、Ｔ_２、・・・、Ｔ_７とする。ここで、スラスタ別推力指令値（Ｔ_１、Ｔ_２、・・・、Ｔ_７）と、上述の運動モード別推力指令値（Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ、Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ）との関係は、式（１）で表される。

【0030】

【数1】

【0031】

式（１）において、「ｘ_ｉ、ｙ_ｉ、ｚ_ｉ（ｉ＝１、２、・・・、７）」は、スラスタＴＨ１〜ＴＨ７の各々の配置座標である。

【0032】

例えば、水中航走体１０を機体前後方向のみに移動させる場合には、推力配分部２１は、運動モード別推力指令値（機体前後方向の推進力）Ｆ_ｘを、Ｔ_１＝Ｔ_２＝１／２・Ｆ_ｘ等と配分する。このような運動モード別推力指令値と、スラスタ別推力指令値との関係を、全てのスラスタ別推力指令値（Ｔ_１、Ｔ_２、・・・、Ｔ_７）、及び、全ての運動モード別推力指令値（Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ、Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ）について表すと、式（２）のようになる。

【0033】

【数2】

【0034】

式（２）において、「Ｖ」は、推力配分行列である。
推力配分部２１は、予め用意された推力配分行列Ｖに対し、入力された外乱補償推力指令値（運動モード別推力指令値（Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ、Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ））を乗算することで、スラスタ別推力指令値（Ｔ_１、Ｔ_２、・・・、Ｔ_７）を算出する。

【0035】

回転数指令変換部２２は、推力配分部２１から入力されたスラスタ別推力指令値（Ｔ_１、Ｔ_２、・・・、Ｔ_７）を、スラスタ１００を直接的に制御するための、より具体的なスラスタ制御用パラメータ（本実施形態においては「スラスタ回転数指令」）に変換する。例えば、回転数指令変換部２２は、スラスタＴＨ１（図２）についてのスラスタ別推力指令値Ｔ_１を受け付けた場合、当該スラスタＴＨ１が、推力Ｔ_１を実際に発生させるために達成すべき「回転数」を算出する。そして、回転数指令変換部２２は、当該回転数を示す回転数指令値Ｒ_１を、スラスタＴＨ１に向けて出力する。これにより、水中航走体１０のスラスタＴＨ１が回転数ｎ_１で回転駆動する。

【0036】

ここで、第１の実施形態に係る回転数指令変換部２２は、スラスタ１００の特性（推力と回転数との関係性）を模擬した数式モデルの逆モデルを予め有している。スラスタ１００の特性を模擬する数式モデル（以下、「スラスタモデル」とも記載する）は、例えば、式（３）のように表される。

【0037】

【数3】

【0038】

式（３）において、「ｎ_ｉ」（ｉ＝１、２、・・・、７）は、各スラスタＴＨ１、ＴＨ２、・・・、ＴＨ７の回転数である。また、「ｕ」は、水中航走体１０の機体前後方向（±Ｘ方向）の速度である。このように、回転数ｎ_ｉで回転する各スラスタＴＨ１、ＴＨ２、・・・、ＴＨ７から発生する推力Ｔ_１、Ｔ_２、・・・、Ｔ_７は、当該回転数ｎ_ｉ及び機体前後方向の速度ｕを変数とするスラスタモデルｇ_ｉで表現することができる。即ち、各スラスタＴＨ１、ＴＨ２、・・・、ＴＨ７から発生する推力Ｔ_１、Ｔ_２、・・・、Ｔ_７は、機体前後方向の速度ｕとスラスタ自身の回転数ｎ_ｉとにより計算される。
回転数指令変換部２２は、スラスタ１００固有の特性が模擬されたスラスタモデルｇ_ｉの逆モデルを用いて、スラスタ別推力指令値（Ｔ_１、Ｔ_２、・・・、Ｔ_７）を、回転数指令値（ｎ_１、ｎ_２、・・・、ｎ_７）に変換する。そして、変換した各回転数指令値（ｎ_１、ｎ_２、・・・、ｎ_７）を、水中航走体１０のスラスタ１００（スラスタＴＨ１、ＴＨ２、・・・、ＴＨ７）の各々に向けて出力する。

【0039】

外乱推定部２３は、フィードバック制御部２０からの推力指令値（Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ、Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ）と水中航走体１０の状態量（速度）の実測値とに基づいて、水中航走体１０の機体１０１に作用する外乱力を示す外乱推定値を算出する。

【0040】

ここで、水中航走体１０に作用する外乱力について簡単に説明する。
まず、水中航走体１０全体についての運動モデルは、以下の式（４）のように表すことができる。

【0041】

【数4】

【0042】

式（４）において、「ｕ」は、水中航走体１０の機体前後方向（±Ｘ方向）の速度である。また、「ｖ」は、水中航走体１０の機体左右方向（±Ｙ方向）の速度である。また、「ｗ」は、水中航走体１０の機体上下方向（±Ｚ方向）の速度である。また、「ｐ」は、水中航走体１０のロール（Ｘ軸回り）の角速度である。また、「ｑ」は、水中航走体１０のピッチ（Ｙ軸回り）の角速度である。また、「ｒ」は、水中航走体１０のヨー（Ｚ軸回り）の角速度である。

【0043】

式（４）は、水中航走体１０についての運動方程式であり、運動モード別の加速度（角加速度）（ｕドット、ｖドット、ｗドット、ｐドット、ｑドット、ｒドット）は、Ａ行列と運動モード別の速度（角速度）（ｕ、ｖ、ｗ、ｐ、ｑ、ｒ）との積、及び、Ｂ行列とスラスタ１００の推力（Ｔ_１、Ｔ_２、・・・、Ｔ_７）との積の和として表される。
ここで、Ａ行列は、機体１０１の形状等に起因する固有パラメータ群であって、１つの運動モードの加速度又は角加速度（例えば、ｕドット）に対する、他軸を含む各運動モードの速度又は角速度（ｕ、ｖ、ｗ、ｐ、ｑ、ｒ）の各々についての影響（干渉）の度合いを示す行列である。
また、Ｂ行列は、各スラスタ１００の機体１０１における設置位置、設置方向に依存する固有パラメータ群である。例えば、図２を用いて説明した通り、機体前後方向の加速度ｕドットに対しては、機体１０１の背面側から正面側にかけて設けられた２個のスラスタＴＨ１、ＴＨ２からの推力Ｔ_１、Ｔ_２が影響する。

【0044】

式（４）に基づけば、例えば、水中航走体１０の機体前後方向についての運動特性は、式（５）のようにモデル化される。

【0045】

【数5】

【0046】

式（５）に示すように、水中航走体１０の機体前後方向の運動特性に関しては、推力Ｔ_１、Ｔ_２の項のみが作用する。
また、式（５）における関数ｆは、Ａ行列で規定された固有パラメータ群に基づく関数であって、機体前後方向の速度ｕに応じて作用する力、及び、他軸の速度、角速度（ｖ、ｗ、ｐ、ｑ、ｒ）からの干渉を受けて作用する力の度合いを表している。即ち、関数ｆの変数には、機体前後方向の速度ｕのみならず、機体左右方向の速度ｖ、機体上下方向の速度ｗ、ロール角速度ｐ、ピッチ角速度ｑ、及び、ヨー角速度ｒも含まれる。

【0047】

ここで、他軸の速度、角速度（ｖ、ｗ、ｐ、ｑ、ｒ）からの干渉の項を、機体前後方向に作用する「外乱力」として一まとめにすると、式（５）は、水中航走体１０の（機体前後方向の運動に作用する）質量ｍ_ｘと、機体前後方向の推進力Ｆ_ｘ（＝Ｔ_１＋Ｔ_２（式（１）参照））と、機体前後方向の外乱力Ｄ_ｘとを利用して、式（６）のように変形することができる。

【0048】

【数6】

【0049】

式（６）によれば、水中航走体１０に入力された全ての力（Ｆ_ｘ＋Ｄ_ｘ）を質量（ｍ_ｘ）で除算したものが、その方向の加速度（ｕドット）となっている。即ち、式（６）は、水中航走体１０全体を質量ｍ_ｘの一つの慣性体と見なし、その慣性体の運動特性を模擬する慣性体モデルと言える。
外乱推定部２３は、図３に示すブロック線図において、フィードバック制御部２０からの推力指令値（Ｆ_ｘ）を入力とし、速度（ｕ）を出力とする伝達系（推力配分部２１、回転数指令変換部２２、スラスタ１００、及び、機体１０１の各伝達特性の直列結合）を一つの慣性体と見なしてなる慣性体モデル（式（６））に基づいて、外乱力（Ｄ_ｘ）を推定する。以下、推力配分部２１、回転数指令変換部２２、スラスタ１００、及び、機体１０１の各伝達特性の直列結合をまとめて「制御対象ｅ」とも表記する。

【0050】

（外乱推定部の機能）
図４は、第１の実施形態に係る外乱推定部の機能を説明する図である。
図３に示すブロック線図によれば、制御対象ｅには、フィードバック制御部２０によって算出された推力指令値（Ｆ_ｘ）から、外乱推定部２３によって算出された外乱推定値（Ｄ_ｘ）が差し引かれてなる外乱補償推力指令値（Ｆ_ｘ−Ｄ_ｘ）が入力される。ここで、外乱推定部２３によって算出された外乱推定値（Ｄ_ｘ）が、制御対象ｅ（水中航走体１０）に作用する外乱力と完全に一致していたと仮定すると、制御対象ｅには、当該制御対象ｅの内部において発生し得る外乱力を打ち消す外乱補償推力指令値（Ｆ_ｘ−Ｄ_ｘ）が入力されることとなる。したがって、制御対象ｅと、外乱推定部２３と、を含む拡大系の制御対象Ｅは、フィードバック制御部２０からの推力指令値（Ｆ_ｘ）がそのまま作用する（外乱力Ｄ_ｘが生じない）質量ｍ_ｘの慣性体と見なすことができる。
したがって、図３に示すブロック線図は、外乱推定部２３が水中航走体１０に作用する外乱力を正しく推定できている限りにおいては、図４に示すブロック線図のように簡略化される。即ち、入力を「力」（推力指令値）とし、出力を「速度」として、質量ｍの慣性体として振る舞う拡大系の制御対象Ｅの伝達関数は、「１／（ｍｓ）」に成形される。これにより、フィードバック制御部２０の設計（位置制御器２００及び速度制御器２０１のパラメータ決定）を行う際には、質量ｍの慣性体（拡大系の制御対象Ｅ）の状態量（位置、速度）を制御するものとして容易に設計することができる。

【0051】

図３において、制御パラメータ演算部２４は、水中航走体１０の状態量（速度）の実測値に基づいて、フィードバック制御部２０におけるフィードバック制御の特性を示す制御パラメータを算出する。また、フィードバック制御部２０は、制御パラメータ演算部２４によって算出された制御パラメータに基づいて、速度指令値と速度の実測値との偏差に応じた推力指令値を算出する。

【0052】

（外乱推定部の構成）
図５は、第１の実施形態に係る外乱推定部の構成を示す図である。
図５に示すように、外乱推定部２３は、近似微分演算部２３０と、慣性体モデル２３１と、ローパスフィルタ２３２と、を備えている。
近似微分演算部２３０は、速度センサＳｎ１から速度の実測値を入力して、その微分値（加速度の推定値）を算出する。ここで、近似微分演算部２３０の伝達関数に含まれる「ω」は、外乱推定部２３の帯域である。帯域ωは、外乱推定部２３の特性値であって、外乱推定部２３の性能の指標となる。即ち、外乱推定部２３は、帯域ω以下の外乱力についてのみ補償可能となる。
慣性体モデル２３１は、推力配分部２１から機体１０１までの伝達系（制御対象ｅ）を、質量ｍ_ｍの一つの慣性体とみなしてなるモデルである。慣性体モデル２３１は、近似微分演算部２３０から入力された加速度に対し質量ｍ_ｍを乗算し、その演算結果を、制御対象ｅに実際に作用した力（Ｆ_ｘ＋Ｄ_ｘ）の推定値として出力する。
ローパスフィルタ２３２は、フィードバック制御部２０からの推力指令値のうち、外乱推定部２３の帯域ωよりも低い周波数成分のみを通過させる。

【0053】

外乱推定部２３は、ローパスフィルタ２３２を通じて入力された推力指令値（Ｆ_ｘ）と、制御対象ｅを模した慣性体モデル２３１を通じて推定された力（制御対象ｅに実際に作用した力（Ｆ_ｘ＋Ｄ_ｘ））と、の差分を出力する。外乱推定部２３が算出した当該差分は、慣性体モデル２３１に基づいて推定された外乱力（Ｄ_ｘ）となる。
制御対象ｅには、フィードバック制御部２０から入力される推力指令値（Ｆ_ｘ）に対し、外乱推定値（Ｄ_ｘ）を差し引いて得られる外乱補償推力指令値が入力される。このようにして得られた外乱補償推力指令値は、制御対象ｅに作用する外乱力を補償する（打ち消す）。これにより、制御対象ｅと外乱推定部２３とを含む拡大系の制御対象Ｅは、外乱力が打ち消された（発生しない）慣性体と見なすことができる（図４参照）。

【0054】

（制御パラメータ演算部の機能）
図６、図７は、それぞれ、第１の実施形態に係る制御パラメータ演算部の機能を説明する第１の図、第２の図である。
以下、図６、図７を参照しながら、制御パラメータ演算部２４の機能について説明する。

【0055】

図４に示したように、外乱推定部２３が水中航走体１０に作用する外乱力を正しく推定できている限りにおいて、拡大系の制御対象Ｅを、シンプルな“質量ｍの慣性体”と見なすことができる旨を説明した。しかしながら、実際の制御対象ｅには、外乱推定部２３による外乱補償処理（即ち、推力指令値（Ｆ_ｘ）に対する外乱推定値（Ｄ_ｘ）のフィードバック処理）によっては打ち消すことができない外乱要素が存在する。
そこで、図５における制御対象ｅの、より厳密な運動特性について説明する。

【0056】

水中航走体１０の機体前後方向（±Ｘ方向）の運動特性を模擬する慣性体モデルは、質量ｍ_ｘ、前後方向の推進力Ｆ_ｘ、及び、前後方向の外乱力Ｄ_ｘによって、式（６）のように表現されることを説明した。
ここで、機体前後方向の推進力Ｆ_ｘは、フィードバック制御部２０からの推力指令値であって、スラスタ１００を通じて水中航走体１０に与えられる推力（具体的には、Ｔ_１＋Ｔ_２）である。この機体前後方向の推進力Ｆ_ｘと、外乱力Ｄ_ｘとが釣り合っている平衡状態においては、式（６）に示す慣性体モデルは、機体前後方向の速度ｕと、機体前後方向の推進力Ｆ_ｘと、を用いて式（７）のように表すことができる。

【0057】

【数7】

【0058】

式（７）において、「ａ_ｕ」は、式（４）のＡ行列に含まれる要素であって、水中航走体１０の固有値である。ただし、固有値ａ_ｕは、水中航走体１０の機体前後方向の速度ｕに応じて時々刻々と変化する特性を有する。

【0059】

同様に、機体左右方向（±Ｙ方向）の推進力Ｆ_ｙと外乱力Ｄ_ｙとが釣り合う平衡状態における、水中航走体１０の機体左右方向の運動特性を模擬する慣性体モデルは、機体左右方向の速度ｖと、機体左右方向の推進力Ｆ_ｙと、を用いて、式（８）のように表すことができる。

【0060】

【数8】

【0061】

式（８）において、「ａ_ｖ」は、式（４）のＡ行列に含まれる要素であって、水中航走体１０の固有値である。ただし、固有値ａ_ｖは、水中航走体１０の機体左右方向の速度ｖ及び機体前後方向の速度ｕに応じて時々刻々と変化する特性を有する。

【0062】

同様に、機体上下方向（±Ｚ方向）の推進力Ｆ_ｚと外乱力Ｄ_ｚとが釣り合う平衡状態における、水中航走体１０の機体上下方向（±Ｚ方向）の運動特性を模擬する慣性体モデルは、機体上下方向の速度ｗと、機体上下方向の推進力Ｆ_ｚと、を用いて式（９）のように表すことができる。

【0063】

【数9】

【0064】

式（９）において、「ａ_ｗ」は、式（４）のＡ行列に含まれる要素であって、水中航走体１０の固有値である。ただし、固有値ａ_ｗは、水中航走体１０の機体上下方向の速度ｗ及び機体前後方向の速度ｕに応じて時々刻々と変化する特性を有する。

【0065】

更に、ロールの角速度ｐ、ピッチの角速度ｑ、ヨーの角速度ｒ、及び、各軸回りの回転モーメントＭ_ｘ、Ｍ_ｙ、Ｍ_ｚについての慣性体モデルも、各々の平衡状態においては、式（７）〜式（９）と同様に、Ａ行列に含まれる固有値（ａ_ｐ、ａ_ｑ、ｑ_ｒ）を用いて表現される（ただし、回転モーメントＭ_ｘ、Ｍ_ｙ、Ｍ_ｚに対しては、質量ｍに相当するイナーシャＪが乗算される）。
このように、スラスタ１００によって水中航走体１０に与えられる推進力と、当該水中航走体１０に作用する外乱力とが釣り合う平衡状態においては、当該外乱力は、水中航走体１０の速度（角速度）（ｕ、ｖ、ｗ、ｐ、ｑ、ｒ）と、当該運動モード別の固有値（ａ_ｕ、ａ_ｖ、ｑ_ｗ、ａ_ｐ、ａ_ｑ、ｑ_ｒ）との積で表すことができる。
そして、上述したように、運動モード別の固有値（ａ_ｕ、ａ_ｖ、ｑ_ｗ、ａ_ｐ、ａ_ｑ、ｑ_ｒ）自身も、水中航走体１０の速度又は角速度（ｕ、ｖ、ｗ、ｐ、ｑ、ｒ）に応じて、時々刻々と変化する特性を有している。

【0066】

式（７）〜式（９）等のように線形化された制御対象ｅの伝達関数Ｇは、式（１０）のように表される。

【0067】

【数10】

【0068】

式（１０）において、「Ｕ」は、伝達関数Ｇ（制御対象ｅ）への入力であって、具体的には、外乱補償推力指令値（運動モード別推力指令値（Ｆ_ｘ、Ｆ_ｙ、Ｆ_ｚ、Ｍ_ｘ、Ｍ_ｙ、Ｍ_ｚ））である。また、「Ｘ」は、伝達関数Ｇ（制御対象ｅ）からの出力であって、具体的には、速度センサＳｎ１を通じて取得された速度及び角速度（ｕ、ｖ、ｗ、ｐ、ｑ、ｒ）の実測値である。
また、式（１０）において「ａ」は、各運動モード別の固有値（ａ_ｕ、ａ_ｖ、ｑ_ｗ、ａ_ｐ、ａ_ｑ、ｑ_ｒ）である。また、「ｍ」は、水中航走体１０の機体１０１の、運動モード別の質量（ｍ_ｘ、ｍ_ｙ、ｍ_ｚ）（又は、イナーシャＪ）である。
以上より、制御対象ｅの伝達関数Ｇは、平衡状態における運動特性（式（７）〜式（９）等）に基づいて、「Ｋ／（ｓ＋ａ）」（Ｋ＝１／ｍ）と表現することができる。

【0069】

他方、外乱推定部２３が有する慣性体モデル２３１は、制御対象ｅを質量ｍ_ｍの慣性体と見なしたモデルである。慣性体モデル２３１は、伝達関数をＫ_ｍ／ｓ（Ｋ_ｍ＝１／ｍ_ｍ）としてモデル化する。外乱推定部２３としては、モデルの逆特性を利用するため、積分器１／ｓの逆特性は近似微分演算部２３０で模擬し、ゲインの逆数１／Ｋ_ｍを慣性体モデル２３１としてモデル化する。
このように、制御対象ｅは、実際には、慣性体とは異なる運動特性（伝達関数Ｇ＝Ｋ／（ｓ＋ａ））を有しているにもかかわらず、外乱推定部２３は、制御対象ｅを、慣性体（Ｋ_ｍ／ｓ）と見なして外乱力の推定を行っている。そのため、外乱推定部２３では吸収しきれない外乱要素が生じ得る。

【0070】

図６において、拡大系の制御対象Ｅの伝達関数は、制御対象ｅの伝達関数Ｇ、及び、外乱推定部２３の伝達関数を用いて、式（１１）のように表される。

【0071】

【数11】

【0072】

式（１１）のうち、「Ｋ_ｍ／ｓ」は、外乱推定部２３が有する慣性体モデル２３１（質量ｍ_ｍの慣性体モデル）の伝達特性である（Ｋ_ｍ／ｓ＝１／（ｍ_ｍｓ））。
式（１１）によれば、拡大系の制御対象Ｅの伝達関数は、外乱推定部２３が有する慣性体モデル２３１（質量ｍ_ｍの慣性体モデル）の伝達特性（Ｋ_ｍ／ｓ）と、制御対象ｅの慣性体モデル２３１からのずれ成分（Ｋ（ｓ＋ω）／（Ｋ_ｍｓ＋（Ｋ_ｍａ＋Ｋω）））との積で表現される。

【0073】

ここで、制御対象ｅのゲインＫ、及び、慣性体モデル２３１のゲインＫ_ｍは、水中航走体１０の機体１０１の実際の質量ｍ又はイナーシャＪに基づくパラメータである。通常行われる詳細な各種機構系のシミュレーション解析によれば、モデル値である質量ｍ_ｍと実際の機体１０１の質量ｍとのずれは、さほど大きくないことが分かっている。したがって、慣性体モデル２３１のゲインＫ_ｍが、制御対象ｅの実際のゲインＫに等しい（Ｋ_ｍ＝Ｋ）と仮定すると、式（１１）は式（１２）のように変形できる。

【0074】

【数12】

【0075】

式（１２）によれば、拡大系の制御対象Ｅの伝達関数は、外乱推定部２３の慣性体モデル２３１の伝達特性（Ｋ_ｍ／ｓ）×１次進み要素（帯域：ω）×１次遅れ要素（帯域：ω＋ａ）×誤差ゲイン（ω／（ω＋ａ））で表現される。ここで、上述の通り「ａ」は制御対象ｅ（機体１０１）の固有値であり、ａ＞０であるため、常にω＜ω＋ａが成り立つ。

【0076】

式（１２）より、拡大系の制御対象Ｅにおけるゲインの周波数特性の概念図は、図７に示すようなボード線図で表される。即ち、拡大系の制御対象Ｅの特性は、低周波数領域（帯域ω以下の周波数）では、誤差ゲインω／（ω＋ａ）の分だけ外乱推定部２３の慣性体モデル２３１からゲインが低下した慣性体（積分特性）となる。また、帯域ωより大きく周波数（ω＋ａ）以下の周波数領域では、ゲインがほぼフラットになり、高周波数領域（周波数（ω＋ａ）より大きい周波数）では慣性体モデル２３１と等しい慣性体（積分特性）になる。

【0077】

図７に示したように、拡大系の制御対象Ｅと設計モデル（慣性体モデル２３１）とのずれ方が把握できている。このずれ方は、外乱推定部２３の帯域ωと、水中航走体１０の固有値ａにより定まる。ここで、上述した通り、水中航走体１０の固有値ａは、水中航走体１０の前後方向の速度ｕ等に応じて、時々刻々と変化するパラメータである。

【0078】

図８は、第１の実施形態に係る制御パラメータ演算部の処理フローを示す図である。
図８に示すように、制御パラメータ演算部２４は、まず、速度センサＳｎ１を通じて、水中航走体１０の速度の実測値を取得する（ステップＳ０１）。
次に、制御パラメータ演算部２４は、予め用意したテーブル（若しくは、関係式）を参照して、速度センサＳｎ１から取得した速度の実測値から、その状態（速度）において適用すべき固有値ａを特定する（ステップＳ０２）。
更に、制御パラメータ演算部２４は、特定した固有値ａを用いて、フィードバック制御部２０の速度制御器２０１に適用すべき制御パラメータを算出する（ステップＳ０３）。具体的には、制御パラメータ演算部２４は、図７における誤差ゲインω／（ω＋ａ）の逆数（（ω＋ａ）／ω）を補償値として、元々規定されていた規定パラメータ（速度制御ゲインＫｖ）に乗算する。

【0079】

速度制御器２０１は、制御パラメータ演算部２４によって、速度制御ゲインＫｖに補償値（（ω＋ａ）／ω）が乗算された制御パラメータ（Ｋｖ・（ω＋ａ）／ω）に基づいて、推力指令値を算出する。これにより、帯域ω以下の周波数領域において拡大系の制御対象Ｅにて低下する分（誤差ゲインω／（ω＋ａ））が打ち消される。これにより、拡大系の制御対象Ｅは、見かけ上、慣性体モデル２３１（質量ｍ_ｍの慣性体）として振る舞う。

【0080】

（作用・効果）
以上の通り、第１の実施形態に係る水中航走体制御装置２は、速度センサＳｎ１を通じて得られた当該水中航走体１０の速度の実測値と、与えられた速度の指令値と、の偏差に基づいて、当該偏差を小さくするために水中航走体１０に与えるべき推力を示す推力指令値を算出するフィードバック制御部２０を備えている。
また、水中航走体制御装置２は、推力指令値に基づく外乱補償推力指令値を入力し、当該外乱補償推力指令値に示される推力を水中航走体１０に与えるために、複数のスラスタ１００の各々が発生すべき推力を示すスラスタ別推力指令値を算出する推力配分部２１を備えている。
更に、水中航走体制御装置２は、推力指令値と速度の実測値とに基づいて、水中航走体１０に作用していた外乱力を示す外乱推定値を算出する外乱推定部２３を備えている。
そして、外乱補償推力指令値は、フィードバック制御部２０によって算出された推力指令値から、外乱推定部２３によって算出された外乱推定値が差し引かれてなる。

【0081】

このようにすることで、推力配分部２１に入力される推力指令値（外乱補償推力指令値）は、フィードバック制御部２０によって算出された推力指令値から、外乱推定部２３によって算出された外乱推定値が差し引かれたものとなる。これにより、推力配分部２１が出力するスラスタ別推力指令値は、水中航走体１０に作用する外乱力を予め打ち消すように調整されたものとなる。これにより、フィードバック制御部２０による制御設計に際しては、水中航走体１０を、外乱力を受けずに運動する単純な慣性体とみなして設計すればよいため、水中航走体１０に対するフィードバック制御の設計工程を簡素化することができる。
以上より、簡素な設計で、水中航走体１０に対する精度の高い運動制御が実現可能となる。

【0082】

また、第１の実施形態に係る外乱推定部２３は、推力配分部２１、水中航走体１０のスラスタ１００、及び、機体１０１を一つの慣性体と見なしてなる慣性体モデル２３１に基づいて、外乱推定値を算出する。
このようにすることで、外乱推定部２３は、速度センサＳｎ１から取得した速度の実測値から、慣性体モデル２３１を通じて、水中航走体１０に作用していた全ての力を推定することができる。よって、外乱推定部２３は、推力指令値に示される力と、水中航走体１０に作用していた力の推定結果との偏差を、水中航走体１０に作用していた「外乱力」と見なして、外乱力推定値を算出することができる。

【0083】

更に、第１の実施形態に係る水中航走体制御装置２は、水中航走体１０の速度の実測値に基づいて、フィードバック制御部２０におけるフィードバック制御の特性を示す制御パラメータ（本実施形態においては速度制御ゲインＫｖ×補償値）を算出する制御パラメータ演算部２４を備えている。
このようにすることで、水中航走体１０を、完全な慣性体と見なすことができないために、外乱推定部２３によって吸収することができない外乱要素を、フィードバック制御部２０の制御パラメータを適宜調整して、打ち消すことができる。

【0084】

また、第１の実施形態に係る制御パラメータ演算部２４は、フィードバック制御部２０において予め規定された規定パラメータ（速度制御ゲインＫｖ）に、外乱推定部２３の帯域ωと、水中航走体１０の速度に応じた当該水中航走体１０の固有値ａと、に基づいて算出される補償値（（ω＋ａ）／ω）を乗算することで、フィードバック制御部２０の制御パラメータを算出する。
このようにすることで、フィードバック制御部２０の制御パラメータ（Ｋｖ・（ω＋ａ）／ω）は、水中航走体１０を慣性体と見なした場合に生じ得る誤差ゲイン（ω／（ω＋ａ））を打ち消すように、適宜調整される。

【0085】

以上、第１の実施形態に係る水中航走システム１及び水中航走体制御装置２について詳細に説明したが、水中航走システム１及び水中航走体制御装置２の具体的な態様は、上述のものに限定されることはなく、要旨を逸脱しない範囲内において種々の設計変更等を加えることは可能である。

【0086】

フィードバック制御部２０の態様は、第１の実施形態で説明した態様に限定されない。例えば、他の実施形態に係るフィードバック制御部２０は、位置制御器２００、速度制御器２０１のうちのいずれか一方のみに基づく比例制御（Ｐ）を行う態様であってもよい。また、フィードバック制御部２０は、位置制御器２００、速度制御器２０１の何れか一方又は両方において、積分制御（Ｉ）、微分制御（Ｄ）、位相進み・位相遅れ補償を組み合わせた制御を行ってもよい。

【0087】

また、制御パラメータ演算部２４は、フィードバック制御部２０のうち速度制御器２０１の規定パラメータ（速度制御ゲインＫｖ）を調整するものとして説明したが、他の実施形態においてはこの態様に限定されない。例えば、他の実施形態に係るフィードバック制御部２０は、フィードバック制御部２０のうち位置制御器２００の規定パラメータ（位置制御ゲインＫｐ）を調整するものとしてもよい。

【0088】

また、制御パラメータ演算部２４は、フィードバック制御部２０の速度制御ゲインＫｖに、（ω＋ａ）／ωなる補償値を乗算することで制御パラメータを算出するものとしたが、この場合、周波数（ω＋ａ）以上の周波数領域では、逆に慣性体モデルのゲインから外れてしまう（図７参照）。したがって、他の実施形態に係る制御パラメータ演算部２４は、如何なる周波数領域においても、拡大系の制御対象Ｅのゲインが慣性体モデルのゲインに一致するように、制御パラメータを算出してもよい。
具体的には、制御パラメータ演算部２４は、制御対象ｅの慣性体モデル２３１からのずれ成分（（ｓ＋ω）／（ｓ＋（ω＋ａ）））（式（１２）参照）の逆数を、規定パラメータ（速度制御ゲインＫｖ）に乗算することで、速度制御器２０１の制御パラメータを算出するようにしてもよい。このようにすることで、制御対象ｅの運動特性を、全ての周波数帯域において、質量ｍ_ｍの慣性体モデルに一致させることができる。

【0089】

また、第１の実施形態に係る水中航走体１０は、機体１０１に推力を与えるアクチュエータとしてスラスタ１００を備えるものとして説明したが、他の実施形態においてはこの態様に限定されない。水中航走体１０の機体１０１に推力を与える機構であれば如何なる態様であっても構わない。
また、第１の実施形態に係る水中航走体制御装置２は、スラスタ１００が発生すべき推力を示すスラスタ別推力指令値を、スラスタ１００が回転すべき回転数指令値に変換する回転数指令変換部２２を備えるものとして説明したが、他の実施形態においてはこの態様に限定されない。
即ち、回転数指令変換部２２は、機体１０１に推力を与えるアクチュエータの態様に応じて、適宜変更されてもよい。具体的には、他の実施形態に係る水中航走体制御装置２は、アクチュエータ別推力指令値を、当該アクチュエータに直接入力可能な指令値（回転数、電圧値、電流値等、アクチュエータの態様に応じた指令値）に変換する変換部を備える態様とされていればよい。

【0090】

また、上述の各実施形態においては、上述した水中航走体制御装置２の各種処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって上記各種処理が行われる。また、コンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしても良い。

【0091】

上記プログラムは、上述した機能の一部を実現するためのものであってもよい。さらに、上述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。更に、水中航走体制御装置２は、１台のコンピュータで構成されていても良いし、通信可能に接続された複数のコンピュータで構成されていてもよい。

【0092】

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

【符号の説明】

【0093】

１ 水中航走システム
１０ 水中航走体
１００ スラスタ（アクチュエータ）
１０１ 機体
２ 水中航走体制御装置
２０ フィードバック制御部
２００ 位置制御器
２０１ 速度制御器
２１ 推力配分部
２２ 回転数指令変換部
２３ 外乱推定部
２３０ 近似微分演算部
２３１ 慣性体モデル
２３２ ローパスフィルタ
２４ 制御パラメータ演算部
Ｓｎ１ 速度センサ
Ｓｎ２ 位置センサ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】