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

Abstract

【課題】フィードバック系の制御の精度を向上させ、外乱による船体の揺動を効果的に抑制することを目的とする。【解決手段】水中航走体の制御装置であって、制御対象である少なくとも２軸に対応してそれぞれ設けられたフィードバック系の制御部と、干渉補償演算式を用いて、軸間の相互干渉を打ち消すための干渉補償舵角を演算し、制御部に出力するフィードバック系の非干渉制御部１６と、を備え、干渉補償演算式は、船体の任意の平衡点近傍における線形化状態方程式において、係数行列を船体の状態量及び船体の操作量の関数として表して、船体の全運転範囲において適応可能なように変化させた線形化状態方程式を、所望の運動を行うために必要な舵操作量に関するパラメータについて逆問題解析することにより導出された演算式であり、非干渉制御部１６は、干渉補償演算式に、時間に応じた状態量と目標値の偏差を代入することにより、干渉補償舵角を得る。【選択図】図５

Description

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

従来、水中航走体の制御では、目標値と現在の状態量との差分に対してＰＩＤ制御（Ｐｒｏｐｏｒｔｉｏｎａｌ−Ｉｎｔｅｇｒａｌ−Ｄｅｒｉｖａｔｉｖｅ Ｃｏｎｔｒｏｌｌｅｒ）を行うことで舵角指令を設定する、いわゆるフィードバック制御が主に行われている。

舵角指令を設定するためには、船体における深度、ロール角、ピッチ角、方位角等の各軸に対して制御を行っているが、特定の軸間には相互干渉が生ずる。
特に、フィードバック制御系が各制御軸上で独立に設計されている場合には、船体が揺動したとしても、各制御軸にそれぞれ対応するフィードバック制御系が、それぞれ独立して揺動を抑制しようとするため、各制御軸間に干渉力が働いて、揺動の収束にかなりの時間を要するという問題があった。

このため、水中航走体の運動の目標値を変更したときの複数の制御軸間に作用する干渉力を非干渉化するために、各舵に対する操作量の補償値をフィードフォワード制御系で算出し、各制御軸にそれぞれ対応するフィードバック制御系に補償値を与える制御方法が提案されている（特許文献１）。

特開２０１５−３３９９７号公報

しかしながら、特許文献１に記載の発明では、フィードフォワード制御による補償ため、水中航走体の運動の目標値が変化した場合の各制御軸間で発生する干渉力を抑制することはできるものの、例えば波等の外乱によって船体が揺動し、状態量が変動した場合において発生した各制御軸間の干渉力については抑制されない。

このため、外乱によって船体が揺動した場合には、各制御軸間の相互干渉を効果的に抑制することができず、船体全体の揺動の収束にかなりの時間を要していた。

本発明は、このような事情に鑑みてなされたものであって、フィードバック系の制御の精度を向上させ、外乱による船体の揺動を効果的に抑制することができる水中航走体の制御装置及び水中航走体並びに水中航走体の制御方法を提供することを目的とする。

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

このような構成によれば、船体の全運転範囲において適応可能な時変線形化状態方程式を用いて導出された干渉補償演算式を用いるので、リアルタイムでその時々の平衡点における干渉補償舵角を容易に得ることができ、また、フィードバック系の制御の精度を向上させることが可能となる。そして、この干渉補償舵角を各制御軸上に独立して設計されたフィードバック制御系に反映させることで、各制御軸間の相互干渉を効果的に低減させることが可能となる。
なお、上記線形化状態方程式は、一般的に、非線形で表される状態方程式を、ある平衡点まわりにテーラー展開し、１次近似を行うことにより、得ることが可能である。
また、上記干渉補償演算式は、相互干渉を抑制する対象軸に関する要素の速度や加速度を変数として含むことを特徴としている。これは、線形化状態方程式に基づいて干渉補償演算式が作成されるからである。

また、例えば、一般的なフィードフォワード制御系と一般的なフィードバック制御系を併用した場合は、目標値が変更されたときに外乱が発生すると、目標値が変更されたことによるフィードフォワード制御系の動作と、外乱が発生したことによるフィードバック制御系の動作とが重複して、制御系が互いに悪影響を及ぼす可能性がある。しかし、本発明の第１態様に係るフィードバック系の非干渉制御部では、干渉補償演算式に、目標値に対する状態量の偏差を代入して干渉補償舵角を得る構成としたため、一般的なフィードフォワード制御系（非干渉機能を有するものに限らない）と併用した場合であっても、それぞれ相互に影響することなく、目標値の変化に対する制御はフィードフォワード制御系が行い、外乱に対する制御は非干渉制御部が行うことができる。このため、フィードフォワード制御系と非干渉制御部との切替が不要であり、それぞれの制御を同時に実行することができる。
また、フィードフォワード制御系を有していない構成であっても、本発明の第１態様に係る非干渉制御部は、干渉補償演算式に、目標値に対する状態量の偏差を代入して干渉補償舵角を得る構成としたため、目標値が変更されたとしても、目標値が変更されたことを外乱として誤検知することなく、正確に非干渉制御を行うことが可能である。

上記水中航走体の制御装置において、前記状態量には、前記船体の状態に関する測定値が用いられることとしてもよい。

このように、船体の状態に関する測定値を状態量として、干渉補償演算式より干渉補償舵角を得ることができるので、外乱等で船体が揺動しても、各制御軸間の相互干渉を効果的に低減させて、揺動を低減することがでる。

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

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

上記水中航走体の制御装置において、前記干渉補償演算式に含まれる状態変数には、相互干渉が生ずる前記軸に関係する状態変数のみが用いられていることとしてもよい。

このように、干渉補償演算式に含まれる状態変数を制限することにより、干渉に関係のない演算を排除することができ、演算処理に係る負荷の低減及び処理速度の向上を図ることができる。

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

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

本発明によれば、フィードバック系の制御の精度を向上させ、外乱による船体の揺動を効果的に抑制することができるという効果を奏する。

本発明の一実施形態に係る水中航走体の概略構成を示した図である。 図１に示した水中航走体を船尾から見たときの舵の配置について模式的に示した図である。 本発明の一実施形態に係る水中航走体の運動の自由度について説明するための図である。 本発明の一実施形態に係る水中航走体の制御装置において、深度制御、ロール角制御、ピッチ角制御、方位角制御に関する機能ブロックについて示した図である。 本発明の一実施形態に係る水中航走体の制御装置において、非干渉制御部に関する機能ブロックについて示した図である。 本発明の一実施形態に係る水中航走体の制御装置による波外乱印加時の深度に関するシミュレーション結果を示した図である。 本発明の一実施形態に係る水中航走体の制御装置による波外乱印加時のピッチ角に関するシミュレーション結果を示した図である。 本発明の一実施形態に係る水中航走体の制御装置による波外乱印加時の方位角に関するシミュレーション結果を示した図である。

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

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

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

以下に、本発明の一実施形態に係る水中航走体の制御装置１０及び水中航走体並びに水中航走体の制御方法について、図面を参照して説明する。本実施形態に係る水中航走体の制御装置１０は、上述した６軸に対してそれぞれ設けられた複数の制御部及びこれら軸間における干渉を抑制するための非干渉制御部１６を有するが、以下の説明においては、便宜上、ｚ軸方向の制御（深度制御）、ｘ軸に対応する回転軸の制御（ロール角φ制御）、ｙ軸に対応する回転軸の制御（ピッチ角θ制御）及びｚ軸に対応する回転軸の制御（方位角ψ制御）を一例として取り上げ、深度、ロール角、ピッチ角及び方位角の各制御軸間に生ずる干渉を低減する場合について説明する。なお、制御軸間の干渉については、相互干渉のため、制御軸のうち少なくとも２軸以上を適切に選択することができ、上記の深度、ロール角、ピッチ角及び方位角の組に限定されない。

図４は、本実施形態に係る水中航走体の制御装置（以下、単に「制御装置１０」という）において、深度制御、ロール角制御、ピッチ角制御及び方位角制御に関する機能ブロックについて示した図である。
制御装置１０は、深度を制御する深度制御部１１と、ロール角を制御するロール角制御部１２と、ピッチ角を制御するピッチ角制御部１３と、方位角を制御する方位角制御部１４と、フィードフォワード制御部（以下、単に「ＦＦ制御部１５」という）と、非干渉制御部１６とを備えている。

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

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

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

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

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

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

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

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

ＦＦ制御部１５は、フィードフォワード系であり、船体１の運動の目標値を変更したときの複数の制御軸間に作用する干渉力を非干渉化するために、各舵に対する操作量の補償舵角δｂ_ＦＦ、δｒ_ｒＦＦ、δｒ_ｃＦＦ、δｒ_ｌＦＦを算出する。例えば、目標深度ｚ^＊、目標ロール角φ^＊、目標ピッチ角θ^＊、目標方位角ψ^＊等の目標値が入力される。ＦＦ制御部１５は、船体１の運動の目標値等に基づいて、舵角を補償する制御方法であれば、各制御軸間の干渉力を非干渉化するものでなくてもよく、上記構成に限られない。

非干渉制御部１６は、フィードバック系であり、線形化された船体１の状態方程式から逆問題解析によって導出された干渉補償演算式に対して、時間に応じた状態量及び目標値に対する状態量の偏差を代入することによって、深度ｚ、ロール角φ、ピッチ角θ、方位角ψとの相互干渉を抑制するための干渉補償舵角δｂ_ｆｂ、δｒ_ｒｆｂ、δｒ_ｃｆｂ、δｒ_ｌｆｂを演算する。ここでは、非干渉制御部１６の各構成について説明する前に、干渉補償演算式の導出過程について説明する。

一般的に、船体１の運動は非線形であるが、平衡点近傍では線形化できる。例えば、船体１の任意の平衡点近傍における線形化状態方程式は、以下の（３）式で表される。

（３）式において、ｕはｘ軸方向の速力［ｍ／ｓ］、ｖはｙ軸方向の速力［ｍ／ｓ］、ｗはｚ軸方向の速力[ｍ／ｓ]、ｐはロール角速度 [ｒａｄ／ｓ]、ｑはピッチ角速度［ｒａｄ／ｓ］、ｒは方位角速度［ｒａｄ／ｓ］、ｚは深度［ｍ］、φはロール角［ｒａｄ］、θはピッチ角［ｒａｄ]、ψは方位角［ｒａｄ］、ｎは回転数［ｒｐｓ］、δｂは舵３ｅの舵角、換言すると、深度に関する舵角指令［ｒａｄ］、δｒ_φ、δｒ_θ、δｒ_ψは舵３ａ、舵３ｂ、舵３ｃ、舵３ｄの舵角に関連し、換言すると、ロール角φ、ピッチ角θ、方位角ψに関する舵角指令［ｒａｄ］である。

続いて、任意の平衡点近傍における線形化状態方程式の係数行列Ａ，Ｂを任意の平衡点の関数として表現することにより、局所的に成立していた線形化を船体１の全運転領域において連続的に成立するよう変形する。ここで、平衡点とは、船体１の現在の状態を表し、現在の状態とは、船体１の現在の深度、ロール角、ピッチ角、方位角といった船体１に設けたセンサ等により測定して取得できる情報（状態量）及び、船体１の現在の深度舵角指令δｂ^＊、ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊といった舵の操作に関する情報（操作量）である。

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

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

（４）式に、深度ｚ、ロール角φ、ピッチ角θ、方位角ψに関係する制御系に特化した線形化状態方程式の一例を示す。また、（５）−（３３）式は、（４）式に含まれる係数行列Ａ、Ｂの各要素を示したものである。（５）−（３３）式に示されているように、係数行列Ａ、Ｂの各要素は平衡点における船体１の状態量及び操作量の関数として数式化されている（なお、一部の要素は定数として表される）。

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

例えば、（６）式に記載のＫ_ｗｖｐは、船体１のｚ軸方向の深度速力ｗに関する運動方程式（（３４）式）における、ｖ・ｐ項に係る係数Ｋ_ｗｖｐであることを示している。

同様に、例えば、（８）式に記載のＫ_{ｗｕｕｄｂ}は、船体１のｚ軸方向の深度速力ｗに関する運動方程式（（３５）式）における、ｕ^２・δｂ項に係る係数Ｋ_{ｗｕｕｄｂ}であることを示している。

また、（５）−（３３）式で表される係数行列Ａ、Ｂの各要素において、ｄｒ１、ｄｒ２、ｄｒ３、ｄｒ４は、舵３ａ〜３ｄのそれぞれの舵角を表している。例えば、舵３ａの舵角が変更された場合には、船体のピッチ方向に寄与する横舵成分とヨー方向に寄与する縦舵成分とロール方向に寄与するロール舵成分の３つの成分の力が船体に作用する。このため、係数行列Ａ、Ｂの各要素において用いられるｄｒ１、ｄｒ２、ｄｒ３、ｄｒ４は、ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊をベクトル合成することにより算出される。

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

具体的には、上記（４）式の状態方程式を、深度に関する舵角指令
δｂ[ｒａｄ]、ロール角φに関する舵角指令 δｒ_φ[ｒａｄ]、ピッチ角θに関する舵角指令δｒ_θ[ｒａｄ]、方位角ψに関する舵角指令δｒ_ψ[ｒａｄ]について解くと、以下の（３６）−（３９）式が得られる。なお、（３６）−（３９）式の導出過程において、状態変数であるｚ軸方向の深度速力ｗ[ｍ／ｓ]、ロール角速度ｐ[ｒａｄ／ｓ]、ピッチ角速度ｑ[ｒａｄ／ｓ]、方位角速度ｒ[ｒａｄ／ｓ]については、それぞれ、目標値と状態量の偏差に置換した。具体的には、深度速力ｗを深度速力ｗと目標深度速力ｗ^＊との偏差である深度速力偏差ｅ_ｗとし、ロール角速度ｐをロール角速度ｐと目標ロール角速度ｐ^＊との偏差であるロール角速度偏差ｅ_ｐとし、ピッチ角速度ｑをピッチ角速度ｑと目標ピッチ角速度ｑ^＊との偏差であるピッチ角速度偏差ｅ_ｑとし、方位角速度ｒを方位角速度ｒと目標方位角速度ｒ^＊との偏差である方位角速度偏差ｅ_ｒとしている。なお、深度加速度ｄｗ／ｄｔ、ロール角加速度ｄｐ／ｄｔ、ピッチ角加速度ｄｑ／ｄｔ、方位角加速度ｄｒ／ｄｔについても、それぞれ、深度加速度ｄｗ／ｄｔと目標深度加速度ｄｗ^＊／ｄｔとの偏差である深度加速度偏差ｅ_ｄｗ、ロール角加速度ｄｐ／ｄｔと目標ロール角加速度ｄｐ^＊／ｄｔとの偏差であるロール角加速度偏差ｅ_ｄｐ、ピッチ角加速度ｄｑ／ｄｔと目標ピッチ角加速度ｄｑ^＊／ｄｔとの偏差であるピッチ角加速度偏差ｅ_ｄｑ、方位角加速度ｄｒ／ｄｔと目標方位角加速度ｄｒ^＊／ｄｔとの偏差である方位角加速度偏差ｅ_ｄｒに置換した。ロール角φ、ピッチ角θについても、それぞれ、ロール角偏差ｅ_φ、ピッチ角偏差ｅ_θに置換した。

次に、本実施形態に係る非干渉制御部１６の具体的な構成について説明する。図５は、本発明の一実施形態に係る水中航走体の制御装置１０において、非干渉制御部１６に関する機能ブロックについて示した図である。非干渉制御部１６は、係数行列演算部３１、微分部３２〜３９、座標変換演算部４０、偏差演算部４１〜４８、及び非干渉補償舵角演算部５０、を主な構成として備えている。

係数行列演算部３１には、航行速度ｕ、横滑り速度ｖ、深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒ、深度舵角指令δｂ^＊、ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊が入力される。係数行列演算部３１は、上記（５）−（３３）式で表される係数行列Ａ、Ｂの各要素の数式を保有しており、これらの数式に入力された航行速度ｕ、横滑り速度ｖ、深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒ、深度舵角指令δｂ^＊、ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊を代入することにより、現在の平衡点における係数行列Ａ（ｎ）、Ｂ（ｎ）を算出する。そして、現在の平衡点における係数行列Ａ（ｎ）、Ｂ（ｎ）を記憶し、前回の制御周期で得られた係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）を非干渉補償舵角演算部５０へ出力する。

座標変換演算部４０は、絶対座標系（地球座標系）におけるロール角φ、ピッチ角θ、微分部３２より出力された深度速度ｄｚ／ｄｔ、微分部３３より出力されたロール角速度ｄφ／ｄｔ、微分部３４より出力されたピッチ角速度ｄθ／ｄｔ、微分部３５より出力された方位角速度ｄψ／ｄｔを、船体座標系における深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒに座標変換する。具体的には、座標変換演算部４０は、座標変換演算式（（４０）式）を有しており、入力されるロール角φ、ピッチ角θ、微分部３２より出力されたｚ、微分部３３より出力されたロール角速度ｄφ／ｄｔ、微分部３４より出力されたピッチ角速度ｄθ／ｄｔ、微分部３５より出力された方位角速度ｄψ／ｄｔを座標変換演算式へ代入することにより、深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒを得る。

偏差演算部４１は、座標変換演算部４０から出力された深度速力ｗと目標深度速力ｗ^＊との偏差ｅ_ｗを算出する。他の偏差演算部４２〜４４についても同様に、ロール角速度ｐと目標ロール角速度ｐ^＊との偏差ｅ_ｐ、ピッチ角速度ｑと目標ピッチ角速度ｑ^＊との偏差ｅ_ｑ、方位角速度ｒと目標方位角速度ｒ^＊との偏差ｅ_ｒを算出する。
偏差演算部４５は、座標変換演算部４０から出力され、微分部３６により微分処理された深度加速度ｄｗ／ｄｔと目標深度加速度ｄｗ^＊／ｄｔとの偏差ｅ_ｄｗを算出する。他の偏差演算部４６〜４８についても同様に、ロール角加速度ｄｐ／ｄｔと目標ロール角加速度ｄｐ^＊／ｄｔとの偏差ｅ_ｄｐ、ピッチ角加速度ｄｑ／ｄｔと目標ピッチ角加速度ｄｑ^＊／ｄｔとの偏差ｅ_ｄｑ、方位角加速度ｄｒ／ｄｔと目標方位角加速度ｄｒ^＊／ｄｔとの偏差ｅ_ｄｒを算出する。

非干渉補償舵角演算部５０は、干渉補償演算式（例えば、上述した（３６）−（３９）式）を保有しており、これらの式に、係数行列演算部３１及び偏差演算部４１〜４８から出力された値を代入することで、干渉補償舵角δｂ_ｆｂ、δｒ_ｒｆｂ、δｒ_ｃｆｂ、δｒ_ｌｆｂを得る。

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

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

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

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

ＦＦ制御部１５では、船体１の運動の目標値が入力されることによって、舵角を補償する補償舵角δｂ_ＦＦ、δｒ_ｒＦＦ、δｒ_ｃＦＦ、δｒ_ｌＦＦが算出される。

非干渉制御部１６では、深度ｚ、ロール角φ、ピッチ角θ、方位角ψが微分部３２〜３５に入力されることにより、深度速度ｄｚ／ｄｔ、ロール角速度ｄφ／ｄｔ、ピッチ角速度ｄθ／ｄｔ、方位角速度ｄψ／ｄｔが出力される。出力された深度速度ｄｚ／ｄｔ、ロール角速度ｄφ／ｄｔ、ピッチ角速度ｄθ／ｄｔ、方位角速度ｄψ／ｄｔ並びにロール角φ、ピッチ角θは座標変換演算部４０へ入力され、船体座標系における深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒに座標変換される。

座標変換された深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒは、偏差演算部４１〜４４によって、深度速力ｗと目標深度速力ｗ^＊との深度速力偏差ｅ_ｗ、ロール角速度ｐと目標ロール角速度ｐ^＊とのロール角速度偏差ｅ_ｐ、ピッチ角速度ｑと目標ピッチ角速度ｑ^＊とのピッチ角速度偏差ｅ_ｑ、方位角速度ｒと目標方位角速度ｒ^＊との方位角速度偏差ｅ_ｒが算出される。

また、座標変換された深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒは、それぞれ微分部３６〜３９を介して、偏差演算部４５〜４８に入力される。そして、偏差演算部４５〜４８によって、深度加速度ｄｗ／ｄｔと目標深度加速度ｄｗ^＊／ｄｔとの偏差ｅ_ｄｗ、ロール角加速度ｄｐ／ｄｔと目標ロール角加速度ｄｐ^＊／ｄｔとの偏差ｅ_ｄｐ、ピッチ角加速度ｄｑ／ｄｔと目標ピッチ角加速度ｄｑ^＊／ｄｔとの偏差ｅ_ｄｑ、方位角加速度ｄｒ／ｄｔと目標方位角加速度ｄｒ^＊／ｄｔとの偏差ｅ_ｄｒが算出される。

係数行列演算部３１では、上位装置から入力された航行速度ｕ、横滑り速度ｖ、深度速力ｗ、ロール角速度ｐ、ピッチ角速度ｑ、方位角速度ｒ、深度舵角指令δｂ^＊、ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊を用いて、係数行列Ａ（ｎ）、Ｂ（ｎ）が算出される。そして、現在の平衡点における係数行列Ａ（ｎ）、Ｂ（ｎ）を記憶し、前回の制御周期で得られた係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）を非干渉補償舵角演算部５０へ出力する。

非干渉補償舵角演算部５０には、係数行列演算部３１で算出された係数行列Ａ（ｎ−１）、Ｂ（ｎ−１）、偏差演算部４１〜４８で算出された深度速力偏差ｅ_ｗ、ロール角速度偏差ｅ_ｐ、ピッチ角速度偏差ｅ_ｑ、方位角速度偏差ｅ_ｒ、深度加速度偏差ｅ_ｄｗ、ロール角加速度偏差ｅ_ｄｐ、ピッチ角加速度偏差ｅ_ｄｑ、方位角加速度偏差ｅ_ｄｒ、航行速度ｕ、横滑り速度ｖ、ロール角偏差ｅ_φ、ピッチ角偏差ｅ_θが入力される。入力された各値は、非干渉補償舵角演算部５０が保有している干渉補償演算式に代入され、干渉補償舵角δｂ_ｆｂ、δｒ_ｒｆｂ、δｒ_ｃｆｂ、δｒ_ｌｆｂが算出される。

このようにして算出された深度に関する干渉補償舵角δｂ_ｆｂは、深度制御部１１の深度舵角設定部１１ｄにおいて深度フィードバック舵角δｂ_ＦＢにネガティブフィードバックされる。具体的には、深度舵角設定部１１ｄからの深度フィードバック舵角δｂ_ＦＢにＦＦ制御部１５からの補償舵角δｂ_ＦＦが加算され、さらに、非干渉制御部１６からの干渉補償舵角δｂ_ｆｂが減算されることによって、深度舵角指令δｂ^＊が算出される。
同様に、ロール角に関する干渉補償舵角δｒ_ｒｆｂは、ロール角制御部１２のロール角舵角設定部１２ｄにおいて、ロール角フィードバック舵角δｒ_ｒＦＢにネガティブフィードバックされる。具体的には、ロール角設定部からのロール角フィードバック舵角δｒ_ｒＦＢにＦＦ制御部１５からの補償舵角δｒ_ｒＦＦが加算され、さらに、非干渉制御部１６からの干渉補償舵角δｒ_ｒｆｂが減算されることによって、ロール角舵角指令δｒ_ｒ ^＊が算出される。
同様に、ピッチ角に関する干渉補償舵角δｒ_ｃｆｂは、ピッチ角制御部１３のピッチ角舵角設定部１３ｄにおいて、ピッチ角フィードバック舵角δｒ_ｃＦＢにネガティブフィードバックされる。具体的には、ピッチ角設定部からのピッチ角フィードバック舵角δｒ_ｃＦＢにＦＦ制御部１５からの補償舵角δｒ_ｃＦＦが加算され、さらに、非干渉制御部１６からの干渉補償舵角δｒ_ｃｆｂが減算されることによって、ピッチ角舵角指令δｒ_ｃ ^＊が算出される。
同様に、方位角に関する干渉補償舵角δｒ_ｌｆｂは、方位角制御部１４の方位角舵角設定部１４ｄにおいて、方位角フィードバック舵角δｒ_ｌＦＢにネガティブフィードバックされる。具体的には、方位角舵角設定部１４ｄからの方位角フィードバック舵角δｒ_ｌＦＢにＦＦ制御部１５からの補償舵角δｒ_ｌＦＦが加算され、さらに、非干渉制御部１６からの干渉補償舵角δｒ_ｌｆｂが減算されることによって、方位角舵角指令δｒ_ｌ ^＊が算出される。

そして、船体運動系において、深度舵角指令δｂ^＊、ロール角舵角指令δｒ_ｒ ^＊、ピッチ角舵角指令δｒ_ｃ ^＊、方位角舵角指令δｒ_ｌ ^＊基づいて、船体１に設けられた舵３ａ〜３ｄの舵角が制御される。

図６に、本実施形態に係る水中航走体の制御装置１０による波外乱印加時の深度に関するシミュレーション結果を示す。図６において、（ａ）は深度と時間の関係を示し、（ｂ）は舵角と時間の関係を示している。一点鎖線が目標値を示しており、深度は目標値として一定値が設定されている。外乱が印加されると、従来制御（点線）では深度が目標値に対して大きく上下に振られてしまっているのに対し、本実施形態に係る制御では、舵角が適切に制御されることによって揺動が大きく抑制されていることがわかる。なお、従来制御とは、本実施形態に係る水中航走体の制御装置１０において、非干渉制御部１６を有していない場合の制御である。つまり、従来制御では、各制御軸間の干渉力を非干渉化せず、各制御軸上で独立に設計されたフィードバック制御系のみで、外乱による揺動に対して制御を行う。

図７に、本実施形態に係る水中航走体の制御装置１０による波外乱印加時のピッチ角に関するシミュレーション結果を示す。図７において、（ａ）はピッチ角と時間の関係を示し、（ｂ）は舵角と時間の関係を示している。一点鎖線が目標値を示しており、ピッチ角は目標値として一定値が設定されている。外乱が印加されると、従来制御（点線）ではピッチ角が目標値に対して大きく振られてしまっているのに対し、本実施形態に係る制御では、舵角が適切に制御されることによって揺動が大きく抑制されていることがわかる。

図８に、本実施形態に係る水中航走体の制御装置１０による波外乱印加時の方位角に関するシミュレーション結果を示す。図８において、（ａ）は方位角と時間の関係を示し、（ｂ）は舵角と時間の関係を示している。一点鎖線が目標値を示しており、方位角は目標値として一定値が設定されている。外乱が印加されると、従来制御（点線）ではピッチ角が目標値に対して大きく振られてしまっているのに対し、本実施形態に係る制御では、舵角が適切に制御されることによって揺動が抑制されていることがわかる。

シミュレーション結果により、本実施形態に係る水中航走体の制御装置１０は、各制御軸上で独立に設計されたフィードバック制御系に対して、非干渉制御部１６にて算出した各舵角の補償値をフィードバックさせることによって、各制御軸間で発生した干渉力を効果的に抑制し、船体全体として、外乱に対して揺動を低減することができる。

以上説明してきたように、本実施形態に係る水中航走体の制御装置及び水中航走体並びに水中航走体の制御方法によれば、船体１の任意の平衡点において線形化した線形化状態方程式から、各制御軸毎に干渉量を推定し、干渉量を打ち消すための操作量を推定する。つまり、船体１の現在の状態に応じて、線形化状態方程式を逐次得ることができるため、どんな船体１の状態においても、各制御軸間の干渉量を非干渉化する操作量を推定することができる。よって、非線形な運動特性にも対応でき、各制御軸間の干渉を抑制することが可能である。

このため、波等の外乱によって船体１が揺動したとしても、各制御軸間の干渉を抑制しつつ、舵制御を行うことができ、効果的に船体１の揺動を抑制することができる。この効果により、荒天時においても安定した姿勢で船体の自動航行が実現でき、運動制御性能の向上に大きく寄与できる。

また、例えば、一般的なフィードフォワード制御系と一般的なフィードバック制御系を併用した場合は、目標値が変更されたときに外乱が発生すると、目標値が変更されたことによるフィードフォワード制御系の動作と、外乱が発生したことによるフィードバック制御系の動作とが重複して、制御系が互いに悪影響を及ぼす可能性がある。しかし、本実施形態に係るフィードバック系の非干渉制御部１６では、干渉補償演算式に、目標値に対する状態量の偏差を代入して干渉補償舵角を得る構成としたため、一般的なフィードフォワード制御系（非干渉機能を有するものに限らない）と併用した場合であっても、それぞれ相互に影響することなく、目標値の変化に対する制御はフィードフォワード制御系が行い、外乱に対する制御は非干渉制御部１６が行うことができる。このため、フィードフォワード制御系と非干渉制御部１６との切替が不要であり、それぞれの制御を同時に実行することができる。
また、フィードフォワード制御系を有していない構成であっても、本実施形態に係る非干渉制御部１６は、干渉補償演算式に、目標値に対する状態量の偏差を代入して干渉補償舵角を得る構成としたため、目標値が変更されたとしても、目標値が変更されたことを外乱として誤検知することなく、正確に非干渉制御を行うことが可能である。

１ ：船体
３ａ〜３ｅ ：舵
１０ ：制御装置
１１ ：深度制御部
１１ａ ：目標値設定部
１１ｂ ：差分演算部
１１ｃ ：フィードバック制御部
１１ｄ ：深度舵角設定部
１２ ：ロール角制御部
１２ａ ：目標値設定部
１２ｂ ：差分演算部
１２ｃ ：フィードバック制御部
１２ｄ ：ロール角舵角設定部
１３ ：ピッチ角制御部
１３ａ ：目標値設定部
１３ｂ ：差分演算部
１３ｃ ：フィードバック制御部
１３ｄ ：ピッチ角舵角設定部
１４ ：方位角制御部
１４ａ ：目標値設定部
１４ｂ ：差分演算部
１４ｃ ：フィードバック制御部
１４ｄ ：方位角舵角設定部
１５ ：ＦＦ制御部
１６ ：非干渉制御部
３１ ：係数行列演算部
３２〜３９ ：微分部
４０ ：座標変換演算部
４１〜４８ ：偏差演算部
５０ ：非干渉補償舵角演算部

Claims (6)

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

Images