JP6189186B2 - 水中航走体及びその制御装置並びに制御方法 - Google Patents

Description

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

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

特開平８−２８２５８９号公報

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

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

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

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

本態様によれば、船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表すとともに、各時刻の状態に適応するように変化させることで、船体の全運転範囲において適応可能な時変線形化状態方程式を求め、この時変線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて逆に解くことにより、導出された干渉補償演算式を用いて、運動軸の相互干渉を抑制するための干渉補償舵角を算出する。
このように、船体の全運転範囲において適応可能な時変線形化状態方程式を用いて導出された干渉補償演算式を用いるので、リアルタイムでその時々の平衡点における干渉補償舵角を容易に得ることができ、また、フィードフォワード系の制御の精度を向上させることが可能となる。そして、この干渉補償舵角を制御部のフィードバック制御に反映させることで、軸間の相互干渉を効果的に低減させることが可能となる。
なお、上記線形化状態方程式は、一般的に、非線形で表される状態方程式を、ある平衡点まわりにテーラー展開し、１次近似を行うことにより、得ることが可能である。
また、上記干渉補償演算式は、相互干渉を抑制する対象軸に関する要素の速度や加速度を変数として含むことを特徴としている。これは、線形化状態方程式に基づいて干渉補償演算式が作成されるからである。例えば、ピッチ角−深度間の相互干渉を抑制する場合には、深度に関する速度及び加速度、並びにピッチ角に関する速度及び加速度の少なくともいずれかが変数として干渉補償演算式に含まれることとなる。

上記水中航走体の制御装置において、前記平衡点及び前記状態変数には、前記船体の運動に関する目標値を用いることとしてもよい。

このように、平衡点として目標値を採用しているので、目標値が変更された場合の軸間の相互干渉を効果的に低減させることができる。また、状態変数として目標値自体を用いるので、後述する目標値の微小変化量を用いる場合と比較して、演算処理の負担を軽減することができる。

上記水中航走体の制御装置において、複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部とが含まれ、前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度及びｚ軸方向の目標速度が含まれ、前記状態変数として、ｚ軸方向の目標速度及び目標加速度、前記ｙ軸に対する回転軸の目標角速度及び目標角加速度、並びに目標ピッチ角が含まれることとしてもよい。

このような構成によれば、深度とピッチ角との間の相互干渉を低減することが可能となる。

上記水中航走体の制御装置において、複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部と、前記ｘ軸に対する回転軸に対応するロール角制御部と、前記ｚ軸に対する回転軸に対応する方位角制御部とが含まれ、前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度、ｚ軸方向の目標速度、ｙ軸方向の目標速度、ｘ軸周りの目標角速度及び目標角度、ｙ軸周りの目標角速度及び目標角度、ｚ軸周りの目標角速度が含まれ、前記状態変数として、ｚ軸方向の目標速度及び目標加速度、前記ｙ軸に対する回転軸の目標角速度及び目標角加速度、目標ピッチ角、及びｘ軸周りの目標角加速度、目標角速度及び目標角度、ｚ軸周りの目標角加速度、目標角速度が含まれることとしてもよい。尚、ｘ軸方向の目標速度とｙ軸方向の目標速度は、観測量を使用してもよい。

このような構成によれば、深度、ピッチ角、ロール角、方位角の間の相互干渉を低減することが可能となる。

上記水中航走体の制御装置において、前記平衡点には、前記船体の運動に関する目標値が用いられ、前記状態変数には、前記船体の運動に関する目標値の微小変化量が用いられることとしてもよい。

上記水中航走体の制御装置において、複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部とが含まれ、前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度の微小変化量及びｚ軸方向の目標速度の微小変化量が含まれ、前記状態変数として、ｚ軸方向の速度の微小変化量及び加速度の微小変化量、前記ｙ軸に対する回転軸の角速度の微小変化量及び角加速度の微小変化量、並びに目標ピッチ角の微小変化量が含まれることとしてもよい。

このような構成によれば、深度とピッチ角との間の相互干渉を低減することが可能となる。

上記水中航走体の制御装置において、複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部と、前記ｘ軸に対する回転軸に対応するロール角制御部と、前記ｚ軸に対する回転軸に対応する方位角制御部とが含まれ、前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度、ｚ軸方向の目標速度、ｙ軸方向の目標速度、ｘ軸周りの目標角速度及び目標角度、ｙ軸周りの目標角速度及び目標角度、ｚ軸周りの目標角速度が含まれ、前記状態変数として、ｚ軸方向の速度の微小変化量及び加速度の微小変化量、前記ｙ軸に対する回転軸の角速度の微小変化量及び角加速度の微小変化量、目標ピッチ角の微小変化量、ｘ軸周りの目標角加速度の微小変化量、目標角速度の微小変化量及び目標角度の微小変化量、ｚ軸周りの目標角加速度の微小変化量、目標角速度の微小変化量が含まれることとしてもよい。尚、ｘ軸方向の目標速度とｙ軸方向の目標速度は、観測量を使用してもよい。

このような構成によれば、深度、ピッチ角、ロール角、及び方位角の間の相互干渉を低減することが可能となる。

上記水中航走体の制御装置において、前記非干渉化制御部は、前記船体の操作量の組み合わせに対する状態量の平衡点を予め平衡点マップとして有しており、前記制御周期毎の前記船体の操作量に対応する前記状態量の平衡点を前記平衡点マップから取得し、取得した前記状態量の平衡点を前記干渉補償演算式に用いて、前記干渉補償舵角を演算することとしてもよい。

上記水中航走体の制御装置において、前記干渉補償演算式に含まれる状態変数には、相互干渉が生ずる前記軸に関係する状態変数のみが用いられていることが好ましい。
このように、干渉補償演算式に含まれる状態変数を制限することにより、干渉に関係のない演算を排除することができ、演算処理の低減を図ることができる。

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

本発明の第３態様は、船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸についてそれぞれ設けられたフィードバック制御系と、軸制御の相互干渉を打ち消すための干渉補償舵角を演算するフィードフォワード系とを有する水中航走体の制御方法であって、前記フィードフォワード系では、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列が平衡点の関数として表して、前記船体の全運転範囲において適応可能な時変線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて解くことにより導出された干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得る水中航走体の制御方法である。

本発明によれば、フィードフォワード系の制御の精度を向上させることができ、舵駆動系の動力消費を低減させることができるという効果を奏する。

本発明の一実施形態に係る水中航走体の概略構成を示した図である。 図１に示した水中航走体を船尾から見たときの舵の配置について模式的に示した図である。 本発明の一実施形態に係る水中航走体の運動の自由度について説明するための図である。 本発明の第１実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。 本発明の第１実施形態に係る水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。 本発明の第２実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。 本発明の第２実施形態に係る水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。 本発明の第３実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。 平衡点マップの一例を示した図である。 本発明の第３実施形態に係る水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。 本発明の第４実施形態に係る水中航走体の制御装置において、深度制御、ロール角制御、ピッチ角制御、方位角制御に関する機能ブロックについて示した図である。 本発明の第４実施形態に係る水中航走体の制御装置において、深度制御、ロール角制御、ピッチ角制御、方位角制御に関する機能ブロックについて示した図である。 本発明の第５実施形態に係る水中航走体の制御装置において、深度制御、ロール角制御、ピッチ角制御、方位角制御に関する機能ブロックについて示した図である。 本発明の第５実施形態に係る水中航走体の制御装置において、深度制御、ロール角制御、ピッチ角制御、方位角制御に関する機能ブロックについて示した図である。 図１４に示した目標値設定部の一構成例を示した図である。 本発明の第６実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。 図１６に示した水中航走体の制御装置の変形例を示した図である。 本発明の第７実施形態に係る水中航走体の制御装置において、深度制御とピッチ角制御に関する機能ブロックについて示した図である。 理想応答モデルの一例を示した図である。 従来の水中航走体の制御装置による深度変更時のシミュレーション結果の一例を示した図である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

〔第４実施形態〕
以下に、本発明の第４実施形態に係る水中航走体の制御装置並びに制御方法について、図面を参照して説明する。
上述した第１〜第３実施形態に係る水中航走体の制御装置は、深度とピッチ角との間に生ずる干渉を低減する場合について説明したが、本実施形態では、深度ｚ、ピッチ角θ、ロール角φ、及び方位角ψの相互間に生じる干渉を低減する場合について説明する。すなわち、ロール角φ及び方位角ψについてもさらに考慮する点で、上記の各実施形態と異なる。
以下、第２実施形態に係る制御装置１０ｂと同一の構成については同一の符号を付すとともに説明を省略し、第２実施形態と異なる点について主に説明する。

図１１、１２は、本実施形態に係る制御装置１０ｇにおいて、深度制御、ピッチ角制御、ロール角制御、及び方位角制御に関する機能ブロックについて示した図である。
図１１、１２に示すように、制御装置１０ｇは、深度ｚを制御する深度制御部１１と、ピッチ角θを制御するピッチ角制御部１２と、ロール角を制御するロール角制御部７０と、方位角を制御する方位角制御部８０と、非干渉化制御部９０とを主な構成として備えている。

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

方位角制御部８０も、上記ロール角制御部７０と同様に、離散的な値である設定方位角ψ_ｓｅｔから連続的な目標方位角ψ^＊を設定する目標値設定部８１、目標値設定部８１からの目標方位角ψ^＊と船体１の実方位角ψとの差分を算出する差分演算部８２、差分演算部８２からの差分から方位角フィードバック舵角δｒ_ｌＦＢを設定するフィードバック制御部８３、及び方位角フィードバック舵角δｒ_ｌＦＢを後述する非干渉化制御部９０からの干渉補償舵角δｒ_ｌＦＦを用いて補正し、方位角舵角指令δｒ_ｌ ^＊を生成する方位角舵角設定部８４を主な構成として備えている。

非干渉化制御部９０はフィードフォワード系であり、線形化された船体１の状態方程式から逆問題解析によって導出された干渉補償演算式を用いて、深度ｚ、ピッチ角θ、ロール角φ、及び方位角ψの相互干渉を抑制するための干渉補償舵角δｂ_ＦＦ、δｒ_ｃＦＦ、δｒ_ｒＦＦ、及びδｒ_ｌＦＦをそれぞれ演算する。ここで、非干渉化制御部９０の各構成については、上述した第２実施形態とほぼ同様であるが、本実施形態では、深度ｚ、ピッチ角θ、ロール角φ、及び方位角ψの相互干渉を抑制することから演算に用いる演算式が異なる。以下、各演算式について、説明する。

船体１の任意の平衡点近傍における線形化状態方程式は、以下の（１６）式及び（１７）式で表される。ここで、平衡点については、目標値において船体が平衡状態となると仮定し、船体運動の目標値を平衡点としている。

（１６）−（１９）式において、ｕ、ｖ、ｗ、ｐ、ｑ、ｒ、φ、θ、ψは、図３において定義した通りであり、ｎは回転数[ｒｐｓ]、δｂは舵３ｅの舵角、換言すると、深度に関する舵角[ｒａｄ]、δｒ１、δｒ２、δｒ３、δｒ４はそれぞれ舵３ａ、舵３ｂ、舵３ｃ、舵３ｄの舵角である。

続いて、任意の操作量Ｕに対して、平衡状態での状態量が、あるＸに落ち着く平衡点（ｄＸ／ｄｔ＝０）が存在すると仮定すると、以下の（２０）式が成立する。

ｆ（Ｘ^＊，Ｕ^＊）＝０ （２０）

この状態からの微小変動量をΔＸ，ΔＵとすると、以下の（２１）式が得られる。

上記（２１）式を平衡点の近傍でテーラー展開し、１次近似とすると、以下の（２２）式で表される平衡点周りの状態方程式が得られる。

（２２）式において、Ａは以下の（２３）式、Ｂは以下の（２４）式で与えらえる。

上記から線形化状態方程式は、以下の（２５）式で表される。

上記Ａ、Ｂ行列の各要素ａ_１１〜ｂ_９６は、ある平衡点Ｘ^＊、Ｕ^＊における関数または定数として表現される。例えば、要素ａ_１４、ａ_１５、ａ_１６は、以下の（２６）−（２８）式で表される。なお、その他の各要素についても上記の（１６）〜（２２）式から当然のごとく導出される。

（２６）−（２８）式において、係数Ｋの添え字については、上述した第１実施形態における係数Ｋの添え字と同様である。

次に、上記線形化状態方程式を逆問題解析し、干渉を打ち消すための舵角指令について解く。すなわち、任意の状態目標値に制御したいときの操作量は、上記（２２）式を逆問題解析して、以下の（２９）式で演算できる。

また、ΔＵは操作量の指令値の微小変化量、ΔＸ^＊は状態量の微小変化量である。上記（２９）式に対して、深度、ロール角、ピッチ角、方位角に関するパラメータのそれぞれに、各目標値に基づく値を代入して連立方程式を作成し、この連立方程式を各舵角指令の微小変化Δδｂ、Δδｒ１、Δδｒ２、Δδｒ３、Δδｒ４について解くことにより、相互間の干渉を低減させるための干渉補償舵角指令が得られる。
例えば、方位角ψのみを変化させ、深度ｚ、ロール角φ、ピッチ角θについては現在値を維持したい場合には、方位角ψに関するパラメータには目標となる方位角ψに基づく値を代入し、方位角ψ以外のパラメータには全て０を設定する。そして、そのときの連立方程式を各舵角指令の微小変化Δδｂ、Δδｒ１、Δδｒ２、Δδｒ３、Δδｒ４について解くことにより、方位角ψを変化による他のパラメータへの影響を回避するための舵角指令を得ることができる。

ここで、本実施形態では、上記（２９）式に示した微小変化量を絶対値に置き換えた逆解析演算式を干渉補償演算式として用いて、干渉補償舵角を演算する。すなわち、本実施形態に係る干渉補償演算式は、以下の（３０）式で表される。

このように、微小変化量でなく絶対値に置き換えることにより、微小変化量を用いる場合に比べて構成を簡素化することができ、演算処理の負担を軽減することができる。以下、上記（３０）式に示した干渉補償演算式を採用して非干渉制御を行う場合の非干渉化制御部９０の具体的な構成について主に図１１を参照して説明する。

図１１に示すように、非干渉化制御部９０は、係数行列算出部９１、微分器９２ａ〜９２ｈ、干渉補償舵角算出部９３及び座標変換部９４を主な構成として備えている。

係数行列算出部９１には、ｘ軸方向における目標速度ｕ^＊及びｙ軸方向における目標速度ｖ^＊が入力される。目標速度ｕ^＊及びｖ^＊は、例えば、上位装置から入力される値である。また、係数行列算出部９１には、目標値設定部２１において設定された目標深度ｚ^＊を微分して得られるｚ軸方向における目標速度ｗ^＊、目標ロール角設定部７１によって設定された目標ロール角φ^＊及び該目標ロール角φを微分して得られるロール角速度ｐ^＊、目標値設定部３１によって設定された目標ピッチ角θ^＊及び該目標ピッチ角θ^＊を微分して得られるピッチ角速度ｑ^＊、及び目標値設定部８１によって設定された目標方位角ψ^＊を微分して得られる目標方位角速度ｒ^＊が入力される。厳密には、絶対座標系（ｚ,φ,θ,ψ）から相対座標系（ｗ，ｐ，ｑ，ｒ）への変換は、微分以外の補正式が必要であるが、ここでは省略している。

係数行列算出部９１は、上記（２６）式〜（２８）式に例示されるように、係数行列Ａ、Ｂの各要素を算出するための関数（一部は、定数やゼロである）をそれぞれ保有しており、これらの関数に上記入力値を代入することにより、現在の平衡点における係数行列Ａ（ｎ），Ｂ（ｎ）を算出する。

干渉補償舵角算出部９３には、係数行列算出部９１によって算出された係数行列Ａ（ｎ），Ｂ（ｎ）が入力されるとともに、目標深度速度ｗ^＊及び目標深度加速度ｄｗ^＊／ｄｔ、目標ピッチ角θ^＊、目標ピッチ角速度ｑ^＊及び目標ピッチ角加速度ｄｑ^＊／ｄｔ、目標ロール角φ^＊、目標ロール角速度ｐ^＊及び目標ロール角加速度ｄｐ^＊／ｄｔ、及び目標方位角速度ｒ^＊及び目標方位角加速度ｄｒ^＊／ｄｔが入力される。なお、これらを演算するために、それぞれ微分器９２ａ〜９２ｈが設けられている。

干渉補償舵角算出部９３は、干渉補償演算式（例えば、上述した（３０）式）を保有しており、これらの式に、上記入力値を代入することで、干渉補償舵角δｂ_ＦＦ、δｒ１、δｒ２、δｒ３、δｒ４をそれぞれ得る。
座標変換部９４は、干渉補償舵角算出部９３によって得られた干渉補償舵角δｒ１、δｒ２、δｒ３、δｒ４をロール角、ピッチ角、方位角のそれぞれに対応する干渉補償舵角δｒ_ｒＦＦ、δｒ_ｃＦＦ、δｒ_ｌＦＦに変換する。これは、幾何学的な舵の配置に基づき、４つの舵がロール、ピッチ、方位へ作用する舵力に変換するものである。

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

ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊は、座標変換部８５によって各舵３ａ〜３ｄへの舵制御指令δｒ１^＊、δｒ２^＊、δｒ３^＊、δｒ４^＊にそれぞれ変換される。これにより、舵３ｅが舵角指令δｂ^＊に基づいて、舵３ａが舵制御指令δｒ１^＊に基づいて、舵３ｂが舵制御指令δｒ２^＊に基づいて、舵３ｃが舵制御指令δｒ３^＊に基づいて、舵３ｄが舵制御指令δｒ４^＊に基づいて、それぞれ駆動制御されることにより、深度ｚ、ロール角φ、ピッチ角θ、方位角ψ間の相互干渉を抑制しながら、船体姿勢を目標姿勢に追従させることが可能となる。

以上、本実施形態に係る水中航走体の制御装置１０ｇによれば、深度ｚ、ロール角φ、ピッチ角θ、及び方位角ψの相互干渉を抑制することが可能となる。したがって、船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。
なお、本実施形態においては、干渉補償演算式に用いる状態変数を目標値の絶対値としたが、第１実施形態のように、微小変化量を用いることとしてもよい。この場合、上述した第１実施形態と同様に、遅延部４２、微小変化量演算部４４、積算部４６の機能を有する構成が必要となる。

〔第５実施形態〕
以下に、本発明の第５実施形態に係る水中航走体の制御装置並びに制御方法について、図面を参照して説明する。
上述した第４実施形態に係る水中航走体の制御装置１０ｇでは、微分器９２ａ〜９２ｈを用いて、速度・角速度及び加速度・角加速度を得ていた。これに代えて、本実施形態では、図１３、１４に示すように、目標値設定部２１´、３１´、７１´、８１´が備える理想応答モデルの内部状態を取り出すことで、速度・角速度及び加速度・角加速度を出力することとしている。

図１３、１４は、本実施形態に係る制御装置１０ｈの機能ブロック図を示した図である。図１３、１４に示すように、本実施形態に係る制御装置１０ｈは、図１１、１２に示した微分器９２ａ〜９２ｈが省略され、各目標値設定部２１´、３１´、７１´、８１´から各速度・角速度及び加速度・角加速度が出力されるような構成とされている。なお、その他の構成については、図１１、１２に示した構成と同様のため、説明は省略する。

図１５は、ピッチ角制御部１２´が備える目標値設定部３１´の一構成例を示した図である。なお、深度制御部１１´の目標値設定部２１´、ロール角制御部７０´の目標値設定部７１´、及び方位角制御部８０´の目標値設定部８１´についても同様の構成とされている。図１５に示すように、目標値設定部３１´は、減算器１０１、１０２、比例器１０３、１０４、及び積分器１０５、１０６を備えている。この目標値設定部３１´にピッチ角設定値θｓｅｔが入力された場合、積分器１０５の入力が目標ピッチ角加速度ｄｑ^＊／ｄｔとなり、積分器１０６の入力が目標ピッチ角速度ｑ^＊となり、出力が目標ピッチ角θ^＊となる。目標ピッチ角加速度ｄｑ^＊／ｄｔ、目標ピッチ角速度ｑ^＊、及び目標ピッチ角θ^＊は、非干渉化制御部９０に出力される。ここで、比例器１０３の比例ゲインＫｐは以下の（３１）式で、比例器１０４の比例ゲインＫｖは以下の（３２）式で与えられる。

Ｋｐ＝ω_ｎ／２ζ （３１）
Ｋｖ＝２ζω_ｎ （３２）

（３１）、（３２）式において、ω_ｎは固有角周波数、ζは減衰率である。固有角周波数ω_ｎは、外部から与える設定値であるが、速力ｕが変化すると船体の応答性が変わるので、速力ｕに応じて設定値を変化させることが好ましい。例えば、速力ｕが比較的遅いときにはピッチ角の応答性が鈍く、速力ｕが比較的早いときにはピッチ角の応答性がよい。したがって、このような特性に応じて、速力ｕと固有角周波数ω_ｎとを関連付けたテーブルなどを用意しておき、このテーブルから速力ｕに応じた固有周波数ω_ｎを逐次設定することとすればよい。また、固有角周波数ω_ｎは、目標値設定部２１´、３１´、７１´、及び８１´のそれぞれにおいて、異なる値を設定することとしてもよい。これは、速力ｕに応じてそれぞれのパラメータの応答性が異なるからである。
減衰率ζは固定値であるが、例えば、比例ゲインＫｐとＫｖとで異なる値を設定してもよい。

このように、本実施形態に係る水中航走体の制御装置及び制御方法によれば、目標値設定部２１´、３１´、７１´、８１´が備える理想応答モデルの演算過程で発生する状態変数である速度（角速度）、加速度（角加速度）を、非干渉化制御部９０に入力するので、微分器９２ａ〜９２ｈを省略することができる。
なお、第１〜第３実施形態に係る水中航走体の制御装置においても、目標値設定部２１、３１に代えて、上記目標値設定部２１´、３１´を採用することとしてもよい。これにより、微分器４３ａ〜４３ｄを省略することが可能となる。

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

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

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

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

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

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

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

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

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

Δｂ_ｄｉｓ＝ｋ１・Δｄ_１ （３５）
Δｒ_ｃｄｉｓ＝ｋ２・Δｄ_２ （３６）

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

１ 船体
１０ａ〜１０ｈ 水中航走体の制御装置
１１、１１´ 深度制御部
１２、１２´ ピッチ角制御部
１３ａ〜１３ｃ、１３ａ´、９０ 非干渉化制御部
１５ａ、１５ｂ、１５ｃ 外乱補償部
７０、７０´ ロール角制御部
８０、８０´ 方位角制御部

Claims (11)

1. 船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸における制御が可能な水中航走体の制御装置であって、
   制御対象である各軸に対応してそれぞれ設けられたフィードバック系の複数の制御部と、
   干渉補償演算式を用いて、軸間の相互干渉を打ち消すための干渉補償舵角を演算し、前記制御部に出力するフィードフォワード系の非干渉化制御部と
   を備え、
   前記干渉補償演算式は、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて逆に解くことにより導出された演算式であり、
   前記非干渉化制御部は、前記干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得る水中航走体の制御装置。
2. 前記平衡点及び前記状態変数には、前記船体の運動に関する目標値が用いられる請求項１に記載の水中航走体の制御装置。
3. 複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部とが含まれ、
   前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度及びｚ軸方向の目標速度が含まれ、前記状態変数として、ｚ軸方向の目標速度及び目標加速度、前記ｙ軸に対する回転軸の目標角速度及び目標角加速度、並びに目標ピッチ角が含まれる請求項１または請求項２に記載の水中航走体の制御装置。
4. 複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部と、前記ｘ軸に対する回転軸に対応するロール角制御部と、前記ｚ軸に対する回転軸に対応する方位角制御部とが含まれ、
   前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度、ｚ軸方向の目標速度、ｙ軸方向の目標速度、ｘ軸周りの目標角速度及び目標角度、ｙ軸周りの目標角速度及び目標角度、ｚ軸周りの目標角速度が含まれ、
   前記状態変数として、ｚ軸方向の目標速度及び目標加速度、前記ｙ軸に対する回転軸の目標角速度及び目標角加速度、目標ピッチ角、及びｘ軸周りの目標角加速度、目標角速度及び目標角度、ｚ軸周りの目標角加速度、目標角速度が含まれる請求項１または請求項２に記載の水中航走体の制御装置。
5. 前記平衡点には、前記船体の運動に関する目標値が用いられ、
   前記状態変数には、前記船体の運動に関する目標値の微小変化量が用いられる請求項１に記載の水中航走体の制御装置。
6. 複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部とが含まれ、
   前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度及びｚ軸方向の目標速度が含まれ、前記状態変数として、ｚ軸方向の速度の微小変化量及び加速度の微小変化量、前記ｙ軸に対する回転軸の角速度の微小変化量及び角加速度の微小変化量、並びに目標ピッチ角の微小変化量が含まれる請求項１または請求項５に記載の水中航走体の制御装置。
7. 複数の前記制御部には、前記ｚ軸に対応する深度制御部と、前記ｙ軸に対する回転軸に対応するピッチ角制御部と、前記ｘ軸に対する回転軸に対応するロール角制御部と、前記ｚ軸に対する回転軸に対応する方位角制御部とが含まれ、
   前記干渉補償演算式には、平衡点としてｘ軸方向の目標速度、ｚ軸方向の目標速度、ｙ軸方向の目標速度、ｘ軸周りの目標角速度及び目標角度、ｙ軸周りの目標角速度及び目標角度、ｚ軸周りの目標角速度が含まれ、
   前記状態変数として、ｚ軸方向の速度の微小変化量及び加速度の微小変化量、前記ｙ軸に対する回転軸の角速度の微小変化量及び角加速度の微小変化量、目標ピッチ角の微小変化量、ｘ軸周りの目標角加速度の微小変化量、目標角速度の微小変化量及び目標角度の微小変化量、ｚ軸周りの目標角加速度の微小変化量、目標角速度の微小変化量が含まれる請求項１または請求項５に記載の水中航走体の制御装置。
8. 前記非干渉化制御部は、
   前記船体の操作量の組み合わせに対する状態量の平衡点を予め平衡点マップとして有しており、
   前記制御周期毎の前記船体の操作量に対応する前記状態量の平衡点を前記平衡点マップから取得し、取得した前記状態量の平衡点を前記干渉補償演算式に用いて、前記干渉補償舵角を演算する請求項１に記載の水中航走体の制御装置。
9. 前記干渉補償演算式に含まれる状態変数には、相互干渉が生ずる前記軸に関係する状態変数のみが用いられている請求項１から請求項８のいずれかに記載の水中航走体の制御装置。
10. 請求項１から請求項９のいずれかに記載の水中航走体の制御装置を備える水中航走体。
11. 船体のｘ軸、ｙ軸、ｚ軸からなる３つの直線軸と、前記直線軸の各々に対する３つの回転軸とからなる６軸のうち、少なくとも２軸についてそれぞれ設けられたフィードバック制御系と、軸制御の相互干渉を打ち消すための干渉補償舵角を演算するフィードフォワード系とを有する水中航走体の制御方法であって、
    前記フィードフォワード系では、前記船体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて逆に解くことにより導出された干渉補償演算式に、時間に応じた前記平衡点の値及び状態変数の値を代入することにより、前記干渉補償舵角を得る水中航走体の制御方法。
    

Images