特許 6189119 水中航走体及びその制御装置並びに制御方法

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

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

(11)【特許番号】6189119

(24)【登録日】2017年8月10日

(45)【発行日】2017年8月30日

(54)【発明の名称】水中航走体及びその制御装置並びに制御方法

(51)【国際特許分類】

   B63G 8/14 20060101AFI20170821BHJP

   B63G 8/18 20060101ALI20170821BHJP

   G05D 1/08 20060101ALI20170821BHJP

   G05B 11/32 20060101ALI20170821BHJP

【ＦＩ】

   B63G8/14

   B63G8/18

   G05D1/08 Z

   G05B11/32 A

【請求項の数】9

【全頁数】27

(21)【出願番号】特願2013-146645(P2013-146645)

(22)【出願日】2013年7月12日

(65)【公開番号】特開2015-16826(P2015-16826A)

(43)【公開日】2015年1月29日

【審査請求日】2016年4月8日

(73)【特許権者】

【識別番号】000006208

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

(74)【代理人】

【識別番号】100112737

【弁理士】

【氏名又は名称】藤田 考晴

(74)【代理人】

【識別番号】100118913

【弁理士】

【氏名又は名称】上田 邦生

(72)【発明者】

【氏名】粟屋 伊智郎

(72)【発明者】

【氏名】安達 丈泰

(72)【発明者】

【氏名】中田 昌宏

【審査官】 岸 智章

(56)【参考文献】

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

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

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

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

【文献】 特公昭５８−０５７３６０（ＪＰ，Ｂ２）

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

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

Ｂ６３Ｂ ３９／００−３９−１４

Ｇ０５Ｂ １１／００−１１／６０

Ｇ０５Ｄ １／０８

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

【請求項1】

船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸における制御が可能な水中航走体の制御装置であって、
制御対象である各軸に対応してそれぞれ設けられた複数のフィードバック制御部と、
外乱補償演算式を用いて、外乱を打ち消すための外乱補償舵角を前記軸毎に演算する外乱補償部と、
各前記フィードバック制御部から出力される軸毎の舵角指令を前記外乱補償部から出力される対応する前記軸の外乱補償舵角を用いて補正し、前記軸毎の舵角指令を生成する舵角指令設定部とを備え、
前記外乱補償演算式は、外乱をパラメータとして含むとともに、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、前記外乱について逆に解くことにより導出された演算式であり、
前記外乱補償部は、前記外乱補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記外乱補償舵角を得る水中航走体の制御装置。

【請求項2】

前記外乱補償演算式の平衡点には、前記船体の実速度または前記船体の位置情報から演算により得られる速度演算値が用いられる請求項１に記載の水中航走体の制御装置。

【請求項3】

前記外乱補償演算式の平衡点には、前記船体の目標速度または前記目標速度から演算により得られる速度演算値が用いられる請求項１に記載の水中航走体の制御装置。

【請求項4】

前記状態変数には、前記船体に取り付けられたセンサによって検出される検出値または該検出値から演算により得られる演算値が用いられる請求項１から請求項３のいずれかに記載の水中航走体の制御装置。

【請求項5】

前記状態変数には、前記船体に取り付けられたセンサによって検出される検出値の微小変化量または該検出値から演算により得られる演算値の微小変化量が用いられる請求項１から請求項３のいずれかに記載の水中航走体の制御装置。

【請求項6】

干渉補償演算式を用いて、軸間の相互干渉を打ち消すための干渉補償舵角を演算し、前記舵角指令設定部に出力するフィードフォワード系の非干渉化制御部を更に備え、
前記非干渉化制御部は、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列が平衡点の関数として表されることにより、前記船体の全運転範囲において適応可能とされた線形化状態方程式を、舵操作量に関するパラメータについて解くことにより導出された干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得る請求項１から請求項５のいずれかに記載の水中航走体の制御装置。

【請求項7】

干渉補償演算式を用いて、軸間の相互干渉を打ち消すための干渉補償舵角を演算し、前記舵角指令設定部に出力するフィードフォワード系の非干渉化制御部を更に備え、
前記干渉補償演算式は、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表すとともに、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて逆に解くことにより導出された演算式であり、
前記非干渉化制御部は、前記干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得る請求項１から請求項５のいずれかに記載の水中航走体の制御装置。

【請求項8】

請求項１から請求項７のいずれかに記載の水中航走体の制御装置を備える水中航走体。

【請求項9】

船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸における制御が可能な水中航走体の制御方法であって、
制御対象である各軸の舵角指令を演算するフィードバック制御過程と、
外乱補償演算式を用いて、外乱を打ち消すための外乱補償舵角を前記制御対象の軸毎に演算する外乱補償過程と、
各前記フィードバック制御過程において演算された前記軸毎の舵角指令を前記外乱補償過程において演算された対応する前記軸の外乱補償舵角を用いて補正し、前記軸毎の舵角指令を生成する舵角指令設定過程と
を有し、
前記外乱補償過程では、外乱をパラメータとして含むとともに、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、前記外乱について逆に解くことにより導出された外乱補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記外乱補償舵角を得る水中航走体の制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、水中航走体及びその制御装置並びに制御方法に関するものである。

【背景技術】

【0002】

従来、水中航走体の姿勢制御では、目標姿勢と現在の姿勢との差分に対してＰＩＤ制御を行うことで舵角指令を設定する、いわゆるフィードバック制御が主に行われている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開平０８−２８２５８９号公報

【発明の概要】

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

【0004】

船体における深度、ロール角、ピッチ角、ヨー角等の各軸制御において、特定の軸間には相互干渉が生ずる。例えば、ピッチ角を０度に保持したまま深度のみを変更する場合、深度の変化により、ピッチ角が変動してしまう。フィードバック制御では、ピッチ角の変動を抑制しようと舵が操作されることから、舵駆動系の動力消費が多くなるという問題がある。
また、このような干渉を抑制するために、従来、フィードバック制御に加えて、フィードフォワード制御が用いられている。しかしながら、従来のフィードフォワード制御では、干渉を効果的に解消することができず、相当量の舵駆動系の動力消費が生じていた。

【0005】

図１５に、フィードフォワード系を含む従来の船体制御による深度変更時のシミュレーション結果の一例を示す。図１５において、（ａ）は船体の速力、（ｂ）は深度、（ｃ）はピッチ角、（ｄ）は深度に関する舵角、（ｅ）はピッチ角に関する舵角の時間的推移を示している。また、図１５（ｄ）、（ｅ）において、舵角指令は破線で、実舵角は実線で示されている。この図に示されるように、深度の変化によりピッチ角が影響をうけ、ピッチ角に関する舵角成分がゼロを介して前後に振られており、無駄な動力消費が行われているのがわかる。

【0006】

本発明は、このような事情に鑑みてなされたものであって、舵駆動系の動力消費の低減を図ることのできる水中航走体及びその制御装置並びに制御方法を提供することを目的とする。

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

【0007】

本発明の第１態様は、船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸における制御が可能な水中航走体の制御装置であって、制御対象である各軸に対応してそれぞれ設けられた複数のフィードバック制御部と、外乱補償演算式を用いて、外乱を打ち消すための外乱補償舵角を前記軸毎に演算する外乱補償部と、各前記フィードバック制御部から出力される軸毎の舵角指令を前記外乱補償部から出力される対応する前記軸の外乱補償舵角を用いて補正し、前記軸毎の舵角指令を生成する舵角指令設定部とを備え、前記外乱補償演算式は、外乱をパラメータとして含むとともに、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、前記外乱について逆に解くことにより導出された演算式であり、前記外乱補償部は、前記外乱補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記外乱補償舵角を得る水中航走体の制御装置である。

【0008】

本態様によれば、船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表すとともに、各時刻の状態に適応するように変化させることで、船体の全運転範囲において適応可能な時変線形化状態方程式を求め、この時変線形化状態方程式を、外乱について解くことにより、導出された外乱補償演算式を用いて、外乱を抑制するための外乱補償舵角を算出する。
このように、船体の全運転範囲において適応可能とされた線形化状態方程式を用いて導出された外乱補償演算式を用いるので、リアルタイムでその時々の平衡点における外乱補償舵角を容易に得ることができる。これにより、未知の外乱成分を低減することができ、船体の制御精度を向上させることができる。この結果、舵駆動系の動力消費を低減させることができる。
なお、上記線形化状態方程式は、一般的に、非線形で表される状態方程式を、ある平衡点まわりにテーラー展開し、１次近似を行うことにより、得ることが可能である。

【0009】

上記水中航走体の制御装置において、前記外乱補償演算式の平衡点には、前記船体の実速度または前記船体の位置情報から演算により得られる速度演算値が用いられることとしてもよい。

【0010】

上記水中航走体の制御装置において、前記外乱補償演算式の平衡点には、前記船体の目標速度または前記目標速度から演算により得られる速度演算値が用いられることとしてもよい。

【0011】

上記水中航走体の制御装置において、前記状態変数には、前記船体に取り付けられたセンサによって検出される検出値または該検出値から演算により得られる演算値が用いられることとしてもよい。

【0012】

上記水中航走体の制御装置において、前記状態変数には、前記船体に取り付けられたセンサによって検出される検出値の微小変化量または該検出値から演算により得られる演算値の微小変化量が用いられることとしてもよい。

【0013】

上記水中航走体の制御装置は、干渉補償演算式を用いて、軸間の相互干渉を打ち消すための干渉補償舵角を演算し、前記制御部に出力するフィードフォワード系の非干渉化制御部を更に備え、前記非干渉化制御部は、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列が平衡点の関数として表されることにより、前記船体の全運転範囲において適応可能とされた線形化状態方程式を、舵操作量に関するパラメータについて解くことにより導出された干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得ることとしてもよい。

【0014】

上記水中航走体の制御装置は、干渉補償演算式を用いて、軸間の相互干渉を打ち消すための干渉補償舵角を演算し、前記制御部に出力するフィードフォワード系の非干渉化制御部を更に備え、前記干渉補償演算式は、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて逆に解くことにより導出された演算式であり、前記非干渉化制御部は、前記干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得ることとしてもよい。

【0015】

本発明の第２態様は、上記の制御装置を備える水中航走体である。

【0016】

本発明の第３態様は、船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸における制御が可能な水中航走体の制御方法であって、制御対象である各軸の舵角指令を演算するフィードバック制御過程と、外乱補償演算式を用いて、外乱を打ち消すための外乱補償舵角を前記制御対象の軸毎に演算する外乱補償過程と、各前記フィードバック制御過程において演算された前記軸毎の舵角指令を前記外乱補償過程において演算された対応する前記軸の外乱補償舵角を用いて補正し、前記軸毎の舵角指令を生成する舵角指令設定過程とを有し、前記外乱補償過程では、外乱をパラメータとして含むとともに、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、前記外乱について逆に解くことにより導出された外乱補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記外乱補償舵角を得る水中航走体の制御方法である。

【発明の効果】

【0017】

本発明によれば、舵駆動系の動力消費を低減させることができるという効果を奏する。

【図面の簡単な説明】

【0018】

【図1】本発明の一実施形態に係る水中航走体の概略構成を示した図である。

【図2】図１に示した水中航走体を船尾から見たときの舵の配置について模式的に示した図である。

【図3】本発明の一実施形態に係る水中航走体の運動の自由度について説明するための図である。

【図4】本発明の第１実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。

【図5】本発明の第１実施形態に係る水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。

【図6】本発明の第２実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。

【図7】本発明の第２実施形態に係る水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。

【図8】本発明の第３実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。

【図9】平衡点マップの一例を示した図である。

【図10】本発明の第３実施形態に係る水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。

【図11】本発明の第４実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。

【図12】図１１に示した水中航走体の制御装置の変形例を示した図である。

【図13】本発明の第５実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。

【図14】理想応答モデルの一例を示した図である。

【図15】従来の水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。

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

【0019】

以下に、本発明の水中航走体及び水中航走体及びその制御装置並びに制御方法の各実施形態について、図面を参照して説明する。
図１は、本発明の一実施形態に係る水中航走体の概略構成を示した図、図２は図１に示した水中航走体を船尾から見たときの舵の配置について模式的に示した図、図３は水中航走体の運動の自由度について説明するための図である。

【0020】

図３に示すように、水中航走体の船体１は、船尾軸線（以下「ｘ軸」という）、左右軸線（以下「ｙ軸」という）、上下軸線（以下「ｚ軸」という）からなる互いに直交する３つの直線軸と、これら各直線軸に対する３つの回転軸とからなる６軸（６自由度）の制御が可能な構成とされている。

【0021】

図３に示すように、本実施形態では、船体座標系における上記ｘ軸方向、ｙ軸方向、ｚ軸方向の速度をそれぞれｕ，ｖ，ｗとし、それぞれの軸周りの角速度をｐ，ｑ，ｒと定義する。更に、絶対座標系（地球座標系）におけるｘ軸周りの回転角度をロール角φ、ｙ軸周りの回転角度をピッチ角θ、ｚ軸周りの回転角度をヨー角ψと定義する。
図１、図２に示すように、船体１には、複数の舵３ａ〜３ｅが設けられている。舵３ａ〜３ｄは、主にピッチ角θに関する制御に用いられ、舵３ｅは主に深度に関する制御に用いられる。
そして、これらの舵３ａ〜３ｅが、後述する各実施形態に係る制御装置において生成される各舵角指令に基づいて操作されることにより、６軸の状態（主に、ｚ軸方向における位置である深度及びピッチ角）をそれぞれの目標値に追従させる制御が実現される。

【0022】

〔第１実施形態〕
以下に、本発明の第１実施形態に係る水中航走体の制御装置並びに制御方法について、図面を参照して説明する。本実施形態に係る水中航走体の制御装置は、上述した６軸に対してそれぞれ設けられた複数の制御部及びこれら軸間における干渉を抑制するための非干渉化制御部を有するが、以下の説明においては、便宜上、ｚ軸方向の制御（深度制御）及びｙ軸に対応する回転軸の制御（ピッチ角θ制御）を一例として取り上げ、深度とピッチ角との間に生ずる干渉を低減する場合について説明する。

【0023】

図４は、本実施形態に係る水中航走体の制御装置（以下、単に「制御装置」という）１０ａにおいて、深度制御とピッチ角制御に関する機能ブロックについて示した図である。
制御装置１０ａは、深度ｚを制御する深度制御部１１と、ピッチ角θを制御するピッチ角制御部１２と、非干渉化制御部１３とを備えている。

【0024】

深度制御部１１は、離散的な値である設定深度ｚ_ｓｅｔから連続的な目標深度ｚ^＊を設定する目標深度設定部２１、目標深度設定部２１からの目標深度ｚ^＊と船体１の実深度ｚとの差分を算出する差分演算部２２、差分演算部２２からの差分から深度フィードバック舵角δｂ_ＦＢを設定するフィードバック制御部２３、及び深度フィードバック舵角δｂ_ＦＢを後述する非干渉化制御部１３からの干渉補償舵角δｂ_ＦＦを用いて補正し、深度舵角指令δｂ^＊を生成する深度舵角設定部２４を主な構成として備えている。

【0025】

目標深度設定部２１は、例えば、上位装置（図示略）から入力される離散的な値である設定深度ｚ_ｓｅｔに対して所定の応答モデルを用いて、連続的な目標深度ｚ^＊を得る。応答モデルの一例としては、以下の（１）式に示すような、２次のローパスフィルタが挙げられる。

【0026】

【数1】

【0027】

（１）式において、ζは減衰率［−］、ω_ｎは応答周波数［ｒａｄ／ｓ］であり、以下の（２）式で表される。例えば、深度制御系においては、オーバーシュートなしとして、ζ＝１と設定される。

【0028】

【数2】

【0029】

（２）式において、Ｔｓは、整定時間［ｓｅｃ］であり、運転条件に応じて任意に設定される。

【0030】

フィードバック制御部２３は、例えば、差分演算部２２からの差分に対してＰＩＤ制御を行うことにより、深度フィードバック舵角δｂ_ＦＢを設定する。
深度舵角設定部２４は、例えば、深度フィードバック舵角δｂ_ＦＢと干渉補償舵角δｂ_ＦＦとを加算する加算器として実現される。

【0031】

ピッチ角制御部１２は、離散的な値である設定ピッチ角θ_ｓｅｔから連続的な目標ピッチ角θ^＊を設定する目標ピッチ角設定部３１、目標ピッチ角設定部３１からの目標ピッチ角θ^＊と船体１の実ピッチ角θとの差分を算出する差分演算部３２、差分演算部３２からの差分からピッチ角フィードバック舵角δｒ_ＦＢを設定するフィードバック制御部３３、及びピッチ角フィードバック舵角δｒ_ＦＢを後述する非干渉化制御部１３からの干渉補償舵角δｒ_ＣＦＦを用いて補正し、ピッチ角舵角指令δｒ_Ｃ^＊を生成するピッチ角舵角設定部３４を主な構成として備えている。
ピッチ角制御部１２の各構成は、深度制御部１１とほぼ同様であるため、詳細説明は省略する。

【0032】

非干渉化制御部１３は、フィードフォワード系であり、線形化された船体１の状態方程式から逆問題解析によって導出された干渉補償演算式を用いて、深度ｚとピッチ角θとの相互干渉を抑制するための干渉補償舵角δｂ_ＦＦ、δｒ_ＣＦＦを演算する。ここでは、非干渉化制御部１３の各構成について説明する前に、干渉補償演算式の導出過程について説明する。

【0033】

一般的に、船体１の運動は非線形であるが、平衡点近傍（平衡点まわりの微小区間）では線形化できる。例えば、船体１の任意の平衡点近傍における線形化状態方程式は、以下の（３）式で表される。ここで、平衡点については、目標値において船体が平衡状態となると仮定し、船体運動の目標値を平衡点としている。

【0034】

【数3】

【0035】

（３）式において、Δｕはｘ軸方向の速度の微小変化[ｍ／ｓ]、Δｖはｙ軸方向の速度の微小変化[ｍ／ｓ]、Δｗはｚ軸方向の速度の微小変化[ｍ／ｓ]、Δｐはロール角速度の微小変化[ｒａｄ／ｓ]、Δｑはピッチ角速度の微小変化[ｒａｄ／ｓ]、Δｒはヨー角速度の微小変化[ｒａｄ／ｓ]、Δφはロール角の微小変化[ｒａｄ]、Δθはピッチ角の微小変化[ｒａｄ]、Δｎは回転数の微小変化[ｒｐｓ]、Δδｂは舵３ｅの舵角の微小変化、換言すると、深度に関する舵角指令の微小変化[ｒａｄ]、Δδｒ１、Δδｒ２、Δδｒ３、Δδｒ４はそれぞれ舵３ａ、舵３ｂ、舵３ｃ、舵３ｄの舵角の微小変化、換言すると、ピッチ角θに関する舵角指令の微小変化[ｒａｄ]である。

【0036】

続いて、任意の平衡点近傍における線形化状態方程式の係数行列Ａ，Ｂを平衡点の関数として表現することにより、局所的に成立していた線形化を船体１の全運転領域において連続的に成立するよう変形する。
上記のような係数行列Ａ，Ｂの要素は、ある任意の平衡点について、線形化したとき、平衡点の関数として表される。従って、船体の平衡点の変化に対して、係数行列Ａ，Ｂを適応させることで、局所的にのみ成立する線形化状態方程式を全運転領域で成立させることが理論的に可能と考えられる。

【0037】

続いて、相互干渉が生ずる軸、すなわち、深度ｚとピッチ角θに関係する状態変数を特定し、特定した状態変数からなる線形化状態方程式となるように、モデルリダクションを行い、行列サイズを低減する。このように、深度・ピッチ角制御系に特化した線形化状態方程式とすることで、相互干渉に関係のない状態変数を除くことができ、処理負担を低減することが可能となる。

【0038】

（４）式に、深度・ピッチ角制御系に特化した線形化状態方程式の一例を示す。また、（５）式は、（４）式に含まれる係数行列の各要素を示したものである。（５）式に示されているように、係数行列の各要素は平衡点の関数として数式化されている（なお、一部の要素は定数として表される）。

【0039】

【数4】

【0040】

【数5】

【0041】

上記（５）式の各々において、係数Ｋは、船体固有の運動特性によって決定される定数である。係数Ｋの添え字は、最初の文字が運動方程式の軸を表し、２番目以降の文字が状態変数を示している。

【0042】

例えば、Ｋ_ｑθは、船体のｑ軸に関する運動方程式（（６）式）における、θ項に係る係数Ｋ_ｑθであることを示している。

【0043】

【数6】

【0044】

同様に、例えば、Ｋ_{ｗｕｕｄｒ}は、船体のｗ軸に関する運動方程式（（７）式）における、ｕ^２・δｒ項に係る係数Ｋ_{ｗｕｕｄｒ}であることを示している。

【0045】

【数7】

【0046】

次に、モデルリダクション後の線形化状態方程式を逆問題解析し、干渉を打ち消すための舵角指令について解く。

【0047】

具体的には、上記（４）式から深度速度ｗとピッチ角速度ｑの式を抽出すると、以下の（８）式、（９）式となる。

【0048】

【数8】

【0049】

ここで、例えば、ピッチ角θが常に０に設定される場合を想定すると、ピッチ角に関するパラメータは全て０、すなわち、ｄｑ／ｄｔ＝０、ｑ＝０、θ＝０とおける。また、ピッチ角θに関する舵３ａ〜３ｄの舵角の微小変化がいずれも等しいとすると、δｒ_ｃ＝δｒ１＝δｒ２＝δｒ３＝δｒ４とおけ、以下の（１０）式、（１１）式が得られる。
なお、上記想定は一例であり、例えば、ピッチ角θを所定のピッチ角に維持したいのであれば、ピッチ角θには所望の設定値を入力すればよい。

【0050】

【数9】

【0051】

そして、上記（１０）式、（１１）式の連立方程式を、深度ｚに関する舵角指令の微小変化Δδｂ、ピッチ角θに関する舵角指令の微小変化Δδｒ_ｃについて解くと、以下の（１２）式及び（１３）式が得られる。
この（１２）式及び（１３）式が干渉補償演算式の一例であり、Δδｂが非干渉化に必要な深度に関する微小干渉補償舵角Δδｂ_ＦＦ、Δδｒ_ｃが非干渉化に必要なピッチ角に関する微小干渉補償舵角Δδｒ_ｃＦＦとなる。

【0052】

【数10】

【0053】

次に、図３に戻り、本実施形態に係る非干渉化制御部１３の具体的な構成について、説明する。非干渉化制御部１３は、係数行列算出部４１、遅延部４２、微分器４３ａ〜４３ｄ、微小変化量演算部４４、干渉補償舵角算出部４５、及び積算部４６を主な構成として備えている。

【0054】

係数行列算出部４１には、ｘ軸方向における設定速度＝目標速度ｕ^＊及び目標深度ｗ^＊が入力される。目標速度ｕ^＊は、例えば、上位装置から入力される値であり、目標深度ｗ^＊は、目標深度設定部２１において設定された目標深度ｚ^＊を微分器４３ａで微分した値である。
係数行列算出部４１は、上記（５）式で表される係数行列の各要素の数式を保有しており、これらの数式に目標速度ｕ^＊及び目標深度ｗ^＊を代入することにより、現在の平衡点における係数行列Ａ（ｎ），Ｂ（ｎ）を算出する。

【0055】

算出された係数行列Ａ（ｎ），Ｂ（ｎ）は、遅延部４２に入力される。遅延部４２は、今回の制御周期で得られた係数行列Ａ（ｎ），Ｂ（ｎ）を記憶するとともに、記憶していた前回の制御周期で得られた係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）を干渉補償舵角算出部４５に出力する。このように、１周期前の制御周期で得られた係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）を出力するのは、後述する微小変化量演算部４４から出力される各状態変数との整合をとるためである。

【0056】

微小変化量演算部４４には、目標深度速度ｗ^＊及び目標深度加速度ｄｗ^＊／ｄｔ、目標ピッチ角θ^＊に対応する回転軸の目標角速度ｑ^＊及び目標角加速度ｄｑ^＊／ｄｔ、並びに目標ピッチ角θ^＊が入力される。目標深度速度ｗ^＊は、目標深度設定部２１により設定された目標深度ｚ^＊を微分器４３ａにより１回微分した値、目標加速度ｄｗ^＊／ｄｔは、目標深度ｚ^＊を微分器４３ａ、４３ｂにより２回微分した値である。目標角速度ｑ^＊は、目標ピッチ角設定部３１により設定された目標ピッチ角θ^＊を微分器４３ｃにより１回微分した値、目標角加速度ｄｑ^＊／ｄｔは目標ピッチ角θ^＊を微分器４３ｃ、４３ｄにより２回微分した値である。

【0057】

なお、上記微分器４３ａ〜４３ｄ及び微小変化量演算部４４は、後述する干渉補償舵角算出部４５において、干渉補償演算式に代入される状態変数を算出するための構成である。したがって、例えば、上記の状態変数のうち、干渉補償演算式に用いられない状態変数については、微分器の一部を省略したり、演算を省略したりすることが可能である。

【0058】

微小変化量演算部４４は、これら各目標値の微小変化量Δｗ^＊、Δｄｗ^＊／ｄｔ、Δｑ^＊、Δｄｑ^＊／ｄｔ、Δθ^＊を算出して、干渉補償舵角算出部４５に出力する。
具体的には、微小変化量演算部４４は、各入力値について、制御周期の前回値と今回値との差分を算出することにより、微小変化量を算出する。

【0059】

干渉補償舵角算出部４５は、干渉補償演算式（例えば、上述した（１２）式及び（１３）式）を保有しており、これらの式に、遅延部４２及び微小変化量演算部４４からの入力値を代入することで、微小干渉補償舵角Δδｂ_ＦＦ、Δδｒ_ｃＦＦを得る。
積算部４６は、干渉補償舵角算出部４５で算出された微小干渉補償舵角Δδｂ_ＦＦ、Δδｒ_ｃＦＦを前回の制御周期における干渉補償舵角δｂ_ＦＦ、δｒ_ｃＦＦに加算することにより、今回の制御周期における深度に関する干渉補償舵角δｂ_ＦＦ及びピッチ角に関する干渉補償舵角δｒ_ｃＦＦを算出する。すなわち、演算式を表すと以下の通りとなる。

【0060】

δｂ_ＦＦ（今回値）＝δｂ_ＦＦ（前回値）＋Δδｂ_ＦＦ
δｒ_ｃＦＦ（今回値）＝δｒ_ｃＦＦ（前回値）＋Δδｒ_ｃＦＦ

【0061】

このようにして得られた、深度に関する干渉補償舵角δｂ_ＦＦは、深度制御部１１の深度舵角設定部２４において、深度フィードバック舵角δｂ_ＦＢに加算されることで、深度舵角指令δｂ^＊が得られる。
同様に、ピッチ角に関する干渉補償舵角δｒ_ｃＦＦは、ピッチ角制御部１２のピッチ角舵角設定部３４において、ピッチ角フィードバック舵角δｒ_ｃＦＢに加算されることで、ピッチ舵角指令δｒ_ｃ^＊が算出される。

【0062】

次に、本実施形態に係る水中航走体の制御装置１０ａの動作について説明する。
上位装置において設定された設定深度ｚ_ｓｅｔおよび設定ピッチ角θ_ｓｅｔ（＝０）が入力されると、設定深度ｚ_ｓｅｔは深度制御部１１に、ピッチ角設定値θ_ｓｅｔはピッチ角制御部１２に入力される。
深度制御部１１において、設定深度ｚ_ｓｅｔは目標深度設定部２１に入力され、離散的な値から連続的な値である目標深度ｚ^＊に変換される。目標深度ｚ^＊は、非干渉化制御部１３及び差分演算部２２に出力される。差分演算部２２では、目標深度ｚ^＊と船体１の実深度ｚとの差分が算出され、フィードバック制御部２３にて、この差分に基づく深度フィードバック舵角δｂ_ＦＢが設定される。

【0063】

同様に、ピッチ角制御部１２において、設定ピッチ角θ_ｓｅｔは目標ピッチ角設定部３１に入力され、離散的な値から連続的な値である目標ピッチ角θ^＊に変換される。目標ピッチ角θ^＊は、非干渉化制御部１３及び差分演算部３２に出力される。差分演算部３２では、目標ピッチ角θ^＊と船体１の実ピッチ角θとの差分が算出され、フィードバック制御部３３にて、この差分に基づくピッチ角フィードバック舵角δｒ_ｃＦＢが設定される。

【0064】

非干渉化制御部１３では、目標深度ｚ^＊及び目標ピッチ角θ^＊が微分器４３ａ〜４３ｄに入力されることにより、目標深度ｚ^＊から目標深度速度ｗ^＊、目標深度加速度ｄｗ^＊／ｄｔが算出されるとともに、目標ピッチ角θ^＊から目標ピッチ角速度ｒ^＊、目標ピッチ角加速度ｄｒ^＊／ｄｔが算出さる。目標深度速度ｗ^＊は、係数行列算出部４１及び微小変化量演算部４４に出力され、目標深度加速度ｄｗ^＊／ｄｔ、目標ピッチ角速度ｒ^＊、目標ピッチ角加速度ｄｒ^＊／ｄｔは、微小変化量演算部４４に出力される。

【0065】

係数行列演算部４１では、上位装置から入力されたｘ軸方向における設定速度＝目標速度ｕ^＊と目標深度速度ｗ^＊とを用いて、係数行列Ａ（ｎ）、Ｂ（ｎ）が算出され、遅延部４２に出力される。遅延部４２では今回値が記憶されるとともに、記憶されていた係数行列の前回値Ａ（ｎ−１）、Ｂ（ｎ−１）が干渉補償舵角算出部４５に出力される。
一方、微小変化量演算部４４では、入力された各パラメータに対して前回値との差分がそれぞれ演算されることにより、それぞれのパラメータについて微小変化量が算出される。算出されたそれぞれの微小変化量Δｗ^＊、Δｄｗ^＊／ｄｔ、Δｒ^＊、Δｄｒ^＊／ｄｔ、Δθ^＊は、干渉補償舵角算出部４５に出力される。

【0066】

干渉補償舵角算出部４５では、入力された係数行列の前回値Ａ（ｎ−１）、Ｂ（ｎ−１）及び各微小変化量Δｗ^＊、Δｄｗ^＊／ｄｔ、Δｒ^＊、Δｄｒ^＊／ｄｔ、Δθ^＊を、予め保有している干渉舵角演算式に代入することにより、微小干渉補償舵角Δδｂ_ＦＦ、Δδｒ_ｃＦＦが算出される。微小干渉補償舵角Δδｂ_ＦＦ、Δδｒ_ｃＦＦは、積算部４６において、前回の干渉補償舵角にそれぞれ加算されることにより、今回の干渉補償舵角δｂ_ＦＦ、δｒ_ｃＦＦが得られる。

【0067】

このようにして算出された深度に関する干渉補償舵角δｂ_ＦＦは、深度制御部１１の深度舵角設定部２４において深度フィードバック舵角δｂ_ＦＢに加算され、深度舵角指令δｂ^＊が算出される。
同様に、ピッチ角に関する干渉補償舵角δｒ_ｃＦＦは、ピッチ角制御部１２のピッチ角舵角設定部３４において、ピッチ角フィードバック舵角δｒ_ｃＦＢに加算され、ピッチ舵角指令δｒ_ｃ^＊が算出される。
そして、深度舵角指令δｂ^＊及びピッチ舵角指令δｒ_ｃ^＊に基づいて、船体１に設けられた舵３ａ〜３ｅの舵角が制御される。
これにより、ピッチ角θをゼロに保ちながら深度ｚを目標深度ｚ^＊に追従させることが可能となり、最終的に深度ｚを設定深度ｚ_ｓｅｔに一致させることができる。

【0068】

図５に、本実施形態に係る水中航走体の制御装置１０ａによる深度変更時のシミュレーション結果を示す。図５において、（ａ）は船体の速力、（ｂ）は深度、（ｃ）はピッチ角、（ｄ）は深度に関する舵角、（ｅ）はピッチ角に関する舵角の時間的推移を示している。また、図５（ｄ）、（ｅ）において、舵角指令は破線で、実舵角は実線で示されている。この図からわかるように、図１５に示した従来のシミュレーション結果と比較して、深度ｚが設定値まで速やかに変化しているとともに、ピッチ角θの変動が抑制されている。
したがって、本実施形態に係る水中航走体の制御装置及び制御方法によれば、船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。

【0069】

〔第２実施形態〕
次に、本発明の第２実施形態に係る水中航走体及びその制御装置並びに制御方法について図を参照して説明する。
上述した第１実施形態では、干渉補償演算式に用いる状態変数を目標値の微小変化量としていた。これは、線形化が平衡点近傍において局所的に成立するという理論に基づくものである。しかしながら、目標値の微小変化量を状態変数として用いた場合、微分計算が多く、処理負担が大きい。そこで、本実施形態では、干渉補償演算式に用いる状態変数を目標値自体とし、微分計算による処理負担を軽減している。

【0070】

すなわち、本実施形態では、上記（１２）式、（１３）式における各状態変数Δｗ^＊、Δｄｗ^＊／ｄｔ、Δｒ^＊、Δｄｒ^＊／ｄｔ、Δθ^＊に代えて、目標値自体である状態変数ｗ^＊、ｄｗ^＊／ｄｔ、ｒ^＊、ｄｒ^＊／ｄｔ、θ^＊を用いる。これにより、（１２）式、（１３）式は、以下の（１４）式、（１５）式で表される。

【0071】

【数11】

【0072】

図６に、本実施形態に係る水中航走体の制御装置１０ｂの機能ブロック図の一例を示す。図６において、図４に示した構成と同一の構成については、同一の符号を付している。図６に示すように、第１実施形態に係る制御装置１０ａの構成に比べて、遅延部４２、微小変化量演算部４４、及び積算部４６が省略され、構成が簡素化されている。
このように、本実施形態に係る水中航走体の制御装置１０ｂによれば、第１実施形態に比べて構成を簡素化することができ、演算処理の負担を軽減することが可能となる。

【0073】

図７に、本実施形態に係る水中航走体の制御装置１０ｂによる深度変更時のシミュレーション結果を示す。図７において、（ａ）は船体の速力、（ｂ）は深度、（ｃ）はピッチ角、（ｄ）は深度に関する舵角、（ｅ）はピッチ角に関する舵角の時間的推移を示している。また、図７（ｄ）、（ｅ）において、舵角指令は破線で、実舵角は実線で示されている。この図からわかるように、図１５に示した従来のシミュレーション結果と比較して、深度が設定値まで速やかに変化しているとともに、ピッチ角の変動が抑制されている。
したがって、本実施形態に係る水中航走体の制御装置及び制御方法によれば、船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。

【0074】

〔第３実施形態〕
次に、本発明の第３実施形態に係る水中航走体及びその制御装置並びに制御方法について図を参照して説明する。
上述した第１又は第２実施形態では、状態方程式における平衡点を目標値としておいていたが、本実施形態では、平衡点を後述する平衡点マップから取得する点で異なる。
平衡点マップは、船体１の舵操作量（例えば、プロペラ回転数や、各直交軸に関する舵角成分など）の組み合わせに対する状態量（例えば、ｘ軸方向の速度ｕ、ｙ軸方向の速度ｖ、ｚ軸方向の速度ｗ、ロール角速度ｐ、ピッチ角速度ｑ、ヨー角速度ｒ、ロール角φ、ピッチ角θ）の平衡点を予め数値解析により求め、数値解析の結果をマップ化したものである。図９に、速力設定２ｋｔ相当時の平衡点マップの一例を示す。図９に示した平衡点マップでは、速力設定が２ｋｔの場合に、深度の舵角成分及びピッチ角の舵角成分から速度ｕを取得することができる。

【0075】

図８に、本実施形態に係る水中航走体の制御装置１０ｃのうち、深度制御とピッチ角制御に関する機能ブロックについて示す。
図６に示した構成と同一の構成については、同一の符号を付している。
図８に示すように、本実施形態に係る制御装置１０ｃは、第２実施形態に係る制御装置１０ｂの構成に対して、平衡点設定部４８と、パラメータ変換部４９とが追加された構成とされている。
平衡点設定部４８は、船体１の舵操作量と状態量とを関連付けた平衡点マップを有し、この平衡点マップと現在の操作量とから状態量の平衡点ｕ_ｅｐ、ｗ_ｅｐを設定する。
パラメータ変換部４９は、設定速度ｕ_ｓｅｔを回転数指令ｎ^＊に換算する。設定速度ｕ_ｓｅｔを回転数指令ｎ^＊に変換する理由は、平衡点マップにおける舵操作量の一つとして、回転数指令ｎ^＊が用いられているからである。

【0076】

このような制御装置１０ｃによれば、平衡点設定部４８には、設定速度ｕ_ｓｅｔから換算された回転数指令ｎ^＊と、深度制御部１１により設定された深度舵角指令δｂ^＊、ピッチ角制御部１２により設定されたピッチ角舵角指令δｒ_ｃ^＊が入力される。平衡点設定部４８は、これら入力値の組み合わせから一義的に特定される平衡点ｕ_ｅｐ、ｗ_ｅｐを平衡点マップを参照することにより取得し、取得した平衡点ｕ_ｅｐ、ｗ_ｅｐを係数行列算出部４１に出力する。なお、図８では、第２実施形態で説明したように、目標値自体を状態変数として用いた場合の構成例を示しているが、図４に示しように、それぞれのパラメータの微小変化量を状態変数として用いる非干渉制御に適用することとしてもよい。

【0077】

図１０に、本実施形態に係る水中航走体の制御装置１０ｃによる深度変更時のシミュレーション結果を示す。図１０において、（ａ）は船体の速力、（ｂ）は深度、（ｃ）はピッチ角、（ｄ）は深度に関する舵角、（ｅ）はピッチ角に関する舵角の時間的推移を示している。また、図１０（ｄ）、（ｅ）において、舵角指令は破線で、実舵角は実線で示されている。この図からわかるように、図１５に示した従来のシミュレーション結果と比較して、深度が目標値まで速やかに変化しているとともに、ピッチ角の変動が抑制されている。
したがって、本実施形態に係る水中航走体の制御装置及び制御方法によれば、船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。

【0078】

〔第４実施形態〕
次に、本発明の第４実施形態に係る水中航走体及びその制御装置並びに制御方法について図を参照して説明する。
上述した各実施形態では、各軸間の制御における干渉を抑制するための非干渉化制御部１３、１３ａ、１３ｂを設け、フィードフォワード制御により干渉の影響を抑制していたが、未知の外乱などが発生した場合には、干渉抑制効果が低減してしまう。

【0079】

そこで、本実施形態に係る制御装置１０ｄ（図１１参照）では、フィードフォワード制御によって補償しきれなかった干渉力、未知の外乱等により発生する制御誤差を低減させるための外乱補償部１５ａ（図１１参照）を備えることを特徴としている。

【0080】

図１１は、本実施形態に係る制御装置１０ｄにおいて、深度制御とピッチ角制御に関する機能ブロックについて示した図である。図１１において、上述した第１実施形態と共通する構成要素については、同一の符号を付している。なお、非干渉化制御部１３ａについては、第２実施形態に係る非干渉化制御部１３ｂ、第３実施形態に係る非干渉化制御部１３ｃを採用することも可能であり、また、従来の非干渉化制御部を採用することも可能である。また、非干渉化制御部を省略した構成としてもよい。このように、非干渉化制御部の有無及びその構成については、特に限定されない。

【0081】

外乱補償部１５ａは、線形化された船体１の状態方程式から逆問題解析によって導出された外乱補償演算式を用いて、外乱を抑制するための外乱補償舵角を演算する。ここでは、外乱補償部１５ａの各構成について説明する前に、外乱補償演算式の導出過程について説明する。なお、考え方については、上述した干渉補償演算式とほぼ同様である。

【0082】

まず、任意の平衡点近傍における船体１の線形化状態方程式において、係数行列を平衡点の関数として表現するとともに、船体１の全運転領域において連続的に成立するように適応的に変化させた線形化状態方程式を作成する。ここで、本実施形態においても、船体運動の平衡点として目標値を採用する。

【0083】

続いて、相互干渉が生ずる軸に関係する状態変数を特定し、特定した状態変数からなる線形化状態方程式となるように、モデルリダクションを行い、行列サイズを低減する。
たとえば、深度ｚとピッチ角θとの間に相互干渉が生ずる場合には、深度ｚとピッチ角θに関連する状態変数を抽出し、深度・ピッチ角制御系に特化した線形化状態方程式にモデルリダクションする。ここで、本実施形態では、外乱を抑制することを目的の一つとしているため、外乱に関する項を線形化状態方程式に含める。
（１６）式に、モデルリダクション後の線形化状態方程式の一例を示す。なお、（１６）式における係数行列の各要素は、上述した（５）式の通りである。

【0084】

【数12】

【0085】

上記（１６）式において、ｄ_１は外乱の深度方向（ｚ軸方向）に関する成分、ｄ_２は外乱のピッチ角に関する成分である。

【0086】

次に、外乱を含めたモデルリダクション後の線形化状態方程式を逆問題解析し、外乱ｄ_１、ｄ_２について解くと、（１７）式が得られる。

【0087】

【数13】

【0088】

このようにして、微小外乱成分Δｄ_１、Δｄ_２が算出されると、算出された外乱を相殺するための微小外乱補償舵角Δｂ_ｄｉｓ、Δｒ_ｃｄｉｓは、変換係数をｋ１、ｋ２を用いて以下のように得ることができる。

【0089】

Δｂ_ｄｉｓ＝ｋ１・Δｄ_１ （１８）
Δｒ_ｃｄｉｓ＝ｋ２・Δｄ_２ （１９）

【0090】

（１８）式、（１９）式において、Δｂ_ｄｉｓはｚ軸方向における微小外乱成分Δｄ_１を打ち消すための微小外乱補償舵角、Δｒ_ｃｄｉｓはｙ軸方向における微小外乱成分Δｄ_２を打ち消すための微小外乱補償舵角である。

【0091】

次に、本実施形態に係る外乱補償部１５ａの構成について、図１１を参照して説明する。
外乱補償部１５ａは、係数行列算出部５１、遅延部５２、微分器５３ａ〜５３ｄ、微小変化量演算部５４、外乱補償舵角算出部５５及び積算部５６を主な構成として備えている。

【0092】

係数行列算出部５１には、ｘ軸方向における実速度ｕ及びｚ軸方向における実速度ｗが入力される。実速度ｕは、例えば、船体１に取り付けられたセンサなどにより検出される値であり、実速度ｗは、船体１に取り付けられたセンサなどにより検出される実深度ｚが微分器５３ａで微分された値が用いられる。
係数行列算出部５１は、上記（５）式で表される各要素の数式を保有しており、これらの数式に実速度ｕ及び実深度ｗを代入することにより、現在の船体１の状態に応じた係数行列Ａ（ｎ）、Ｂ（ｎ）を算出する。

【0093】

算出された係数行列Ａ（ｎ）、Ｂ（ｎ）は、遅延部５２に入力される。遅延部５２は、前回の制御周期で得られた係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）を外乱補償舵角算出部５５に出力する。このように、１周期前の制御周期で得られた係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）を出力するのは、後述する微小変化量演算部５４との整合をとるためである。
微小変化量演算部５４には、ｚ軸方向の実速度ｗ及び実加速度ｄｗ／ｄｔ、ピッチ角に対応する回転軸の実角速度ｑ及び実角加速度ｄｑ／ｄｔ、並びに実ピッチ角θが入力される。実速度ｗは、実深度ｚを微分器５３ａにより１回微分した値、実加速度ｄｗ／ｄｔは、実深度ｚを微分器５３ａ、５３ｂにより２回微分した値である。実角速度ｑは、船体１に取り付けられたセンサなどによって検出された実ピッチ角θを微分器５３ｃにより１回微分した値、実角加速度ｄｑ／ｄｔは実ピッチ角θを微分器５３ｃ、５３ｄにより２回微分した値である。

【0094】

微小変化量演算部５４は、これら各入力値の微小変化量Δｗ、Δｄｗ／ｄｔ、Δｑ、Δｄｑ／ｄｔ、Δθを算出して、外乱補償舵角算出部５５に出力する。具体的には、微小変化量演算部５４は、各入力値について、制御周期の前回値と今回値との差分を算出することにより、微小変化量を算出する。

【0095】

外乱補償舵角算出部５５は、上述した手順に従って予め用意した外乱補償演算式を保有しており、これらの式に、遅延部５２及び微小変化量演算部５４から入力された状態変数を代入することで、深度方向に関する微小外乱成分Δｄ_１及びピッチ角に関する微小外乱成分ｄ_２をそれぞれ相殺するための微小外乱補償舵角Δδ_ｄｉｓ、Δδｒ_ｃｄｉｓを算出する。

【0096】

積算部５６は、微小外乱補償舵角Δｂ_ｄｉｓ、Δｒ_ｃｄｉｓを前回の制御周期で得られた外乱補償舵角に加算することで、今回の制御周期における深度に関する外乱補償舵角δｂ_ｂｉｓ及びピッチ角に関する外乱補償舵角δｒ_ｃｂｉｓを算出する。
積算部５６から出力された深度に関する外乱補償舵角δｂ_ｄｉｓは、深度制御部１１の深度舵角設定部２４´において、深度フィードバック舵角δｂ_ＦＦに加算されることで、深度舵角指令δｂ^＊が算出される。
同様に、積算部５６から出力されたピッチ角に関する外乱補償舵角Δδｒ_ｃｄｉｓは、ピッチ角制御部１２のピッチ角舵角設定部３４´において、ピッチ角フィードバック舵角δｒ_ｃＦＢに加算されることで、ピッチ舵角指令δｒ_ｃ^＊が算出される。

【0097】

そして、深度舵角指令δｂ^＊及びピッチ舵角指令δｒ_ｃ^＊に基づいて、船体１に設けられた舵３ａ〜３ｅの舵角が制御されることにより、未知の外乱等による制御精度の低下を抑制することが可能となる。

【0098】

なお、本実施形態では、外乱補償演算式に用いる状態変数を検出値の微小変化量としていたが、上述した第２実施形態と同様、検出値自体とし、微分計算による処理負担を軽減することとしてもよい。

【0099】

また、上述した本実施形態では、係数行列の演算において、ｘ軸方向における実速度ｕ及びｚ軸方向における実速度ｗを用いて係数行列を算出したが、図１２に示した制御装置１０ｅのように、外乱補償部１５ｂの係数行列算出部５１に入力される値を、センサ検出値に代えて、ｘ軸方向における設定速度ｕ_ｓｅｔ及びｚ軸方向における目標速度ｗ^＊としてもよい。
例えば、センサ検出値を使用した場合には、ノイズや波の影響により好ましくない周波数帯域の信号を含んでしまう可能性があり、係数行列の算出精度が低下し、これによって、制御精度が低下するおそれがある。これに対し、設定値や目標値を用いて係数行列を算出することにより、センサ検出値に含まれるノイズ等による制御精度の低下を回避することが可能となる。

【0100】

〔第５実施形態〕
次に、本発明の第５実施形態に係る水中航走体及びその制御装置並びに制御方法について図を参照して説明する。
上述した第１または第４実施形態においては、設定値、目標値あるいは検出値を微分器４３ａ〜４３ｄ、５３ａ〜５３ｄによって微分して、速度や加速度を得ていた。しかしながら、これら微分器４３ａ〜４３ｄ、５３ａ〜５３ｄが帯域微分であった場合には、遅れが生じることから、位置・速度・加速度の各要素について共通の位相関係を維持することが困難である。また、特に、センサ検出値の微分は、ノイズ成分を含むため、微分信号に含まれるノイズ成分は更に増加してしまうおそれがある。
そこで、本実施形態では、微分器に代えて、理想応答モデルを用いて、速度や加速度を算出することにより、微分に起因する上述の不都合を解消することとしている。

【0101】

図１３は、本実施形態に係る制御装置１０ｆにおいて、深度制御とピッチ角制御に関する機能ブロックについて示した図である。
図１３に示すように、本実施形態に係る外乱補償部１５ｃは、微分器５３ａ〜５３ｄに代えて、深度に関する速度及び加速度を算出する速度・加速度演算部５８ａと、ピッチ角に関する角速度及び角加速度を算出する角速度・角加速度演算部５８ｂとを有している。

【0102】

速度・加速度演算部５８ａは、例えば、図１４に示すような理想応答モデルを有し、この理想応答モデルから、位置ｚが入力された場合の内部状態（位置、速度、加速度）を取り出すことにより、連続位置ｚｍ、速度ｄｚｍ／ｄｔ、加速度ｄ^２ｚｍ／ｄｔ^２を取得する。ここで、理想応答モデルは、図１４に示されるモデルに限られず、２次系でも、制御動特性を模擬したモデルでも、ローパス特性（例えば、１追従モデル等）を有するモデル等であってもよい。

【0103】

同様に、角速度・角加速度演算部５８ｂは、ピッチ角に関する理想応答モデルを用いて、角度、角速度、角加速度を取得する。
このように、微分を用いずに、速度、加速度の情報を得ることにより、ノイズ成分などを低減でき、制御精度を向上させることが可能となる。

【0104】

更に、本実施形態に係る制御装置１０ｆでは、深度制御部１１´の目標深度設定部２１´、目標ピッチ角設定部３１´で用いられる応答モデルを上述したような理想応答モデルに変更し、この理想応答モデルから連続的な目標値、速度（角速度）、加速度（角加速度）を得る。このようにすることで、微分器４３ａ〜４３ｄを省略することができるとともに、ノイズによる制御精度の低下を抑制することができる。

【0105】

以上、本発明の各実施形態について説明してきたが、本発明の範囲は、上述の実施形態のみに限定されるものではなく、発明の要旨を逸脱しない範囲において、例えば、上述した各実施形態を部分的または全体的に組み合わせる等して、種々変形実施が可能である。

【符号の説明】

【0106】

１ 船体
１０ａ、１０ｂ、１０ｃ、１０ｄ、１０ｅ 水中航走体の制御装置
１１、１１´ 深度制御部
１２、１２´ ピッチ角制御部
1３、１３´、１３ａ〜１３ｃ 非干渉化制御部
１５ａ、１５ｂ、１５ｃ 外乱補償部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】