特許 7404033 制御システム及び構造体、並びに制御方法、並びに制御プログラム

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

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

(11)【特許番号】

(24)【登録日】2023-12-15

(45)【発行日】2023-12-25

(54)【発明の名称】制御システム及び構造体、並びに制御方法、並びに制御プログラム

(51)【国際特許分類】

   G05D 1/08 20060101AFI20231218BHJP

   G05B 11/36 20060101ALI20231218BHJP

   B63C 11/00 20060101ALI20231218BHJP

   B63C 11/48 20060101ALI20231218BHJP

   B63G 8/14 20060101ALI20231218BHJP

   B63G 8/20 20060101ALI20231218BHJP

【ＦＩ】

G05D1/08 Z

G05B11/36 G

B63C11/00 B

B63C11/48 D

B63G8/14

B63G8/20

G05B11/36 507H

【請求項の数】 7

(21)【出願番号】P 2019202331

(22)【出願日】2019-11-07

(65)【公開番号】P2021077014

(43)【公開日】2021-05-20

【審査請求日】2022-08-25

(73)【特許権者】

【識別番号】000006208

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

(74)【代理人】

【識別番号】100112737

【弁理士】

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

(74)【代理人】

【識別番号】100140914

【弁理士】

【氏名又は名称】三苫 貴織

(74)【代理人】

【識別番号】100136168

【弁理士】

【氏名又は名称】川上 美紀

(74)【代理人】

【識別番号】100172524

【弁理士】

【氏名又は名称】長田 大輔

(72)【発明者】

【氏名】安達 丈泰

(72)【発明者】

【氏名】小田 一道

(72)【発明者】

【氏名】粟屋 伊智郎

(72)【発明者】

【氏名】中田 昌宏

(72)【発明者】

【氏名】栗田 章央

【審査官】大古 健一

(56)【参考文献】

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

【文献】特開２００９－１７９０８７（ＪＰ，Ａ）

【文献】米国特許出願公開第２００４／０１６３５８１（ＵＳ，Ａ１）

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

Ｇ０５Ｂ １／００ － ７／０４

Ｇ０５Ｂ １１／００ －１３／０４

Ｇ０５Ｂ １７／００ －１７／０２

Ｇ０５Ｂ ２１／００ －２１／０２

Ｂ６３Ｃ １／００ －１５／００

Ｂ６３Ｇ １／００ －１３／０２

Ｇ０５Ｄ １／００ － １／１２

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

【請求項1】

機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段によって制御する構造体に適用される制御システムであって、
機体の状態量を取得する取得部と、
機体の状態量を状態変数とし、前記操作手段の操作量を入力とした状態方程式において、故障が発生した前記操作手段である故障操作手段に関する情報に基づいて前記故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段以外の前記操作手段の前記操作量を演算する演算部と、
を備え、
前記演算部は、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段以外の前記操作手段の前記操作量を演算する制御システム。

【請求項2】

前記状態方程式における各係数のうち少なくともいずれか１つは、機体の状態量に関するパラメータの関数として表される請求項１に記載の制御システム。

【請求項3】

前記状態方程式において、前記状態量には機体の前進速度が含まれており、前記前進速度にかかる係数は０に設定されている請求項１または２に記載の制御システム。

【請求項4】

前記操作手段は、アクチュエータにより駆動される請求項１から３のいずれか１項に記載の制御システム。

【請求項5】

機体の姿勢を制御する複数の操作手段と、
請求項１から４のいずれか１項に記載の制御システムと
を備える構造体。

【請求項6】

機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段によって制御する構造体に適用される制御方法であって、
機体の状態量を取得する取得工程と、
機体の状態量を状態変数とし、前記操作手段の操作量を入力とした状態方程式において、故障が発生した前記操作手段である故障操作手段に関する情報に基づいて前記故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段以外の前記操作手段の前記操作量を演算する演算工程と、
を有し、
前記演算工程において、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段以外の前記操作手段の前記操作量を演算する制御方法。

【請求項7】

機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段によって制御する構造体に適用される制御プログラムであって、
機体の状態量を取得する取得処理と、
機体の状態量を状態変数とし、前記操作手段の操作量を入力とした状態方程式において、故障が発生した前記操作手段である故障操作手段に関する情報に基づいて前記故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段以外の前記操作手段の前記操作量を演算する演算処理と、
をコンピュータに実行させ、
前記演算処理において、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段以外の前記操作手段の前記操作量を演算する制御プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、制御システム及び構造体、並びに制御方法、並びに制御プログラムに関するものである。

【背景技術】

【0002】

水中航走体や飛昇体等の構造体では、操作手段によって機体の姿勢を制御している（例えば特許文献１）。例えば水中航走体では、機体に設けた舵を制御することによって、機体の姿勢を制御している。

【先行技術文献】

【特許文献】

【0003】

【文献】国際公開第２０１８／１１０５９８号

【発明の概要】

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

【0004】

しかしながら、例えば岩礁に衝突した場合等では、舵の一部に機械的な損傷（例えば固着や変形）が生ずることがある。複数の舵のうちの一部が故障すると、機体の姿勢の制御に支障が生じる場合がある。具体的には、舵が一定の舵角に固着した場合等には、故障した舵の舵角に応じて機体の姿勢に影響が生じる可能性がある。このような場合には、継続的に運転を行うことが困難となる。

【0005】

本開示は、このような事情に鑑みてなされたものであって、より継続的に運転を行うことのできる制御システム及び構造体、並びに制御方法、並びに制御プログラムを提供することを目的とする。

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

【0006】

本開示の第１態様は、機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段によって制御する構造体に適用される制御システムであって、機体の状態量を取得する取得部と、機体の状態量を状態変数とし、前記操作手段の操作量を入力とした状態方程式において、故障が発生した前記操作手段である故障操作手段に関する情報に基づいて前記故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段以外の前記操作手段の前記操作量を演算する演算部と、を備え、前記演算部は、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段以外の前記操作手段の前記操作量を演算する制御システムである。

【0007】

本開示の第２態様は、機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段によって制御する構造体に適用される制御方法であって、機体の状態量を取得する取得工程と、機体の状態量を状態変数とし、前記操作手段の操作量を入力とした状態方程式において、故障が発生した前記操作手段である故障操作手段に関する情報に基づいて前記故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段以外の前記操作手段の前記操作量を演算する演算工程と、を有し、前記演算工程において、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段以外の前記操作手段の前記操作量を演算する制御方法である。

【0008】

本開示の第３態様は、機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段によって制御する構造体に適用される制御プログラムであって、機体の状態量を取得する取得処理と、機体の状態量を状態変数とし、前記操作手段の操作量を入力とした状態方程式において、故障が発生した前記操作手段である故障操作手段に関する情報に基づいて前記故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段以外の前記操作手段の前記操作量を演算する演算処理と、をコンピュータに実行させ、前記演算処理において、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段以外の前記操作手段の前記操作量を演算する制御プログラムである。

【発明の効果】

【0009】

本開示によれば、より継続的に運転を行うことができるという効果を奏する。

【図面の簡単な説明】

【0010】

【図1】本開示の第１実施形態に係る水中航走体を例示した図である。

【図2】本開示の第１実施形態に係るＸ舵を例示した図である。

【図3】本開示の第１実施形態に係る制御装置のハードウェア構成の一例を示した図である。

【図4】本開示の第１実施形態に係る制御装置が備える機能を示した機能ブロック図である。

【図5】本開示の第１実施形態に係る故障診断部の構成例を示す図である。

【図6】本開示の第１実施形態に係る故障候補特定部における故障舵の候補特定処理のフローチャートを示した図である。

【図7】本開示の第１実施形態に係る演算部における故障舵角算出処理のフローチャートを示した図である。

【図8】本開示の第１実施形態に係る縮退指令部における縮退処理のフローチャートを示した図である。

【図9】本開示の第１実施形態に係る耐故障制御部の構成例を示す図である。

【図10】本開示の第１実施形態に係る耐故障制御部における演算処理のフローチャートを示した図である。

【図11】本開示の第１実施形態に係る故障舵特定処理による効果を示す図である。

【図12】本開示の第１実施形態に係る耐故障制御部による非干渉化の効果を示す図である。

【図13】本開示の第２実施形態に係る故障診断部の構成例を示す図である。

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

【0011】

〔第１実施形態〕
以下に、本開示に係る制御システム及び構造体、並びに制御方法、並びに制御プログラムの第１実施形態について、図面を参照して説明する。制御システムは、冗長性のある操作手段（例えば舵）によって制御を行う構造体であれば幅広く適用できるものであって、以下に説明するような水中航走体１のみに限定されるものではない。例えば、制御システムは、航空機やロケット、ドローン等の構造体にも適用可能であり、有人及び無人に関わらず適用可能である。

【0012】

絶対座標系（地球座標系）におけるｘ軸周りの回転角度をロール角φ、ｙ軸周りの回転角度をピッチ角θ、ｚ軸周りの回転角度をヨー角ψと定義されている。ロール角φ、ピッチ角θ、及びヨー角ψが、水中航走体１における機体の姿勢を示している。すなわち、水中航走体１では、機体の姿勢に係るロール角φ、ピッチ角θ、及びヨー角ψを制御対象（制御量）として、後述する操作手段（例えば舵）によって姿勢制御を行う。なお、姿勢としてピッチ角θを制御することによって、深度Ｚ（ｚ軸方向）を変化させることができるため、ピッチ角θと深度Ｚとは互いに相関している。

【0013】

図１は、本開示の第１実施形態に係る制御システムを備えた水中航走体１を例示した図である。図１に示すように、水中航走体１は、船尾軸線（以下、「ｘ軸」という。）、左右軸線（以下、「ｙ軸」という。）、上下軸線（以下、「ｚ軸」という。）からなる互いに直交する３つの直線軸と、これら各直線軸に対する３つの回転軸とからなる船体座標系の６軸（６自由度）が定義される。そして、ｘ軸方向の速度を速度ｕ、ｙ軸方向の速度を速度ｖ、ｚ軸方向の速度を速度ｗとする。また、ｘ軸周りの回転角速度を角速度ｐ、ｙ軸周りの回転角速度を角速度ｑ、ｚ軸周りの回転角速度を角速度ｒとする。すなわち、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒは、機体の状態量となる。

【0014】

水中航走体１の状態量は、水中航走体１の状態を示す値である。本実施形態では、状態量として、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、及び角速度ｒを用いる。そしてさらに、状態量として、ロール角φ、ピッチ角θ、ヨー角ψ、及び深度Ｚを用いる。各状態量については、水中航走体１の状態（姿勢等）を表すものであり、水中航走体１に設けたセンサによって検出する。検出された各状態量は、後述する制御装置２０において用いられる。センサは、例えば、機体内に設けられた慣性航法装置である。各センサについては、機体の内側に設けることによって、水中耐圧や流体抵抗増加等の発生を抑制できる。なお、各状態量の取得方法については、センサによるものに限定されず、シミュレーションモデル等を用いて推定し取得する等の他の方法を用いることも可能である。機体の状態量が取得されれば、各状態量の特定方法（演算方法）については限定されない。

【0015】

制御装置２０は、機体に複数設けられた操作手段を制御して、水中航走体１のロール角φ、ピッチ角θ、及びヨー角ψを制御する（姿勢制御）。操作手段とは、水中航走体１の姿勢を制御する手段であり、具体的には舵である。操作手段については、機体の姿勢を制御でき、アクチュエータを用いて駆動される手段であれば舵に限定されない。例えば、スラスタ等を用いることとしてもよい。本実施形態においては、操作手段は、図２に示すような、１番舵Ｒ１、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４の４枚の舵（Ｘ舵）Ｒを用いる場合について説明する。なお、図２は、船尾から見た図であり、推進力を得るためのプロペラ２も示している。プロペラ２による推進力によって、ｘ軸方向への力を得る。制御装置２０では、Ｘ舵における各舵を制御して、機体の姿勢に係る制御対象であるロール角φ、ピッチ角θ、及びヨー角ψを制御する。このように、機体の姿勢に関する制御対象の数（本実施形態では３つ）に対して、操作手段の数（本実施形態では４つ）の方が多くなるため、本実施形態のような水中航走体１は冗長性のあるシステムとなる。制御対象は、姿勢として機体のピッチ角θ、ヨー角ψ、及びロール角φの３要素として、機体に４つ以上設けられた操作手段によって制御することとしてもよい。機体の姿勢に関する制御対象を、制御対象の数よりも多い複数の操作手段によって制御する構造体であれば、冗長性のあるシステムとして同様に適応することができる。

【0016】

具体的には、制御装置２０は、水中航走体１に設けられた１番舵用アクチュエータ、２番舵用アクチュエータ、３番舵用アクチュエータ、及び４番舵用アクチュエータに対して制御指令値を与え、機体の姿勢を制御する。各アクチュエータを駆動することで、各舵の角度（舵角）を変化させて、機体の姿勢を制御する。

【0017】

図３は、本実施形態に係る制御装置２０のハードウェア構成の一例を示した図である。
図３に示すように、制御装置２０は、コンピュータシステム（計算機システム）であり、例えば、ＣＰＵ１１と、ＣＰＵ１１が実行するプログラム等を記憶するためのＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２と、各プログラム実行時のワーク領域として機能するＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３と、大容量記憶装置としてのハードディスクドライブ（ＨＤＤ）１４と、ネットワーク等に接続するための通信部１５とを備えている。大容量記憶装置としては、ソリッドステートドライブ（ＳＳＤ）を用いることとしてもよい。これら各部は、バス１８を介して接続されている。

【0018】

また、制御装置２０は、キーボードやマウス等からなる入力部や、データを表示する液晶表示装置等からなる表示部などを備えていてもよい。

【0019】

なお、ＣＰＵ１１が実行するプログラム等を記憶するための記憶媒体は、ＲＯＭ１２に限られない。例えば、磁気ディスク、光磁気ディスク、半導体メモリ等の他の補助記憶装置であってもよい。

【0020】

後述の各種機能を実現するための一連の処理の過程は、プログラムの形式でハードディスクドライブ１４等に記録されており、このプログラムをＣＰＵ１１がＲＡＭ１３等に読み出して、情報の加工・演算処理を実行することにより、後述の各種機能が実現される。なお、プログラムは、ＲＯＭ１２やその他の記憶媒体に予めインストールしておく形態や、コンピュータ読み取り可能な記憶媒体に記憶された状態で提供される形態、有線又は無線による通信手段を介して配信される形態等が適用されてもよい。コンピュータ読み取り可能な記憶媒体とは、磁気ディスク、光磁気ディスク、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、半導体メモリ等である。

【0021】

図４は、制御装置（制御システム）２０が備える機能を示した機能ブロック図である。図４に示されるように、制御装置２０は、舵角指令算出部３０と、故障診断部４０と、縮退指令部５０と、耐故障制御部６０とを備えている。

【0022】

（舵角指令算出部の構成）
舵角指令算出部３０は、目標値に基づいて、Ｘ舵における各舵に対する指令値を算出する。このため、舵角指令算出部３０は、仮想舵角算出部３１と、分配部３２とを備えている。目標値とは、機体の姿勢に関する制御量に対する目標値である。制御量とは、制御対象に属する量のうちで、目的の値にまで制御することが望まれる量である。目標値は、ロール角φ、ピッチ角θ、及びヨー角ψのそれぞれに対応する目標値となる。ピッチ角θは、機体の深度Ｚに関連するため、本実施形態に係る仮想舵角算出部３１では、深度Ｚの目標値を用いて、深度制御器３３によりピッチ角θの目標値を算出する場合について説明する。しかしながら、目標値としては、機体の姿勢に関する制御量を用いることができるため、ロール角φ、ピッチ角θ、及びヨー角ψに対応する目標値が仮想舵角算出部３１へ入力されることとしてもよい。舵角指令算出部３０では、機体の姿勢に関する制御量に対する目標値（ロール角φ、ピッチ角θ、及びヨー角ψ）に基づいて、各舵に対応する指令値を算出することができれば、図４の構成に限定されない。

【0023】

仮想舵角算出部３１は、機体の姿勢に関する制御量の目標値に基づいて、仮想舵（横舵、縦舵、ロール舵）に対応する指令値を算出している。目標値は、深度目標値Ｚ*、ヨー角目標値ψ*、ロール角目標値φ*である。深度目標値Ｚ*については、ピッチ角目標値θ*を算出するために用いられるため、仮想舵の指令値は、ロール角目標値φ*、ピッチ角目標値θ*、及びヨー角目標値ψ*に基づいて算出されることとなる。仮想舵は、仮想的に設定された舵であり、具体的には横舵、縦舵、及びロール舵である。

【0024】

仮想舵角算出部３１には、各目標値と、各目標値に対応する状態量との差分が入力される。具体的には、深度目標値Ｚ*と深度の状態量との差分、ヨー角目標値ψ*とヨー角ψの状態量との差分、及びロール角目標値φ*とロール角φの状態量との差分が仮想舵角算出部３１へ入力される。なお、仮想舵角算出部３１へは、ピッチ角θの状態量についても入力される。

【0025】

仮想舵角算出部３１は、深度制御器３３と、姿勢角制御器３６と、方位制御器３４と、ロール制御器３５とを備えている。深度制御器３３は、深度目標値Ｚ*と深度の状態量との差分に基づいて、ピッチ角目標値θ*を算出する。そして、ピッチ角目標値θ*と、ピッチ角θの状態量との差分が姿勢角制御器３６へ入力される。姿勢角制御器３６では、ピッチ角目標値θ*とピッチ角θの状態量との差分に基づいて、横舵の指令値を算出する。すなわち、仮想舵である横舵の指令値は、ピッチ角目標値θ*に基づいて算出される。

【0026】

方位制御器３４は、ヨー角目標値ψ*とヨー角ψの状態量との差分に基づいて、縦舵の指令値を算出する。すなわち、仮想舵である縦舵の指令値は、ヨー角目標値ψ*に基づいて算出される。

【0027】

ロール制御器３５は、ロール角目標値φ*とロール角φの状態量との差分に基づいて、ロール舵の指令値を算出する。すなわち、仮想舵であるロール舵の指令値は、ロール角目標値φ*に基づいて算出される。

【0028】

横舵、縦舵、及びロール舵の指令値が算出されると、分配部３２へ出力される。仮想舵角算出部３１と、分配部３２の間には、無効化部３７が設けられている。無効化部３７は、後述する縮退指令部５０により制御される。具体的には、無効化部３７は、縮退指令部５０より縮退の指令がない場合には、仮想舵角算出部３１からの出力値をそのまま分配部３２へ出力する。そして、無効化部３７は、縮退指令部５０より縮退の指令がある場合には、仮想舵角算出部３１からの出力値（特定の出力）を無効化する。本実施形態では、ロール角φに対して優先順位を低く設定しているため、具体的な構成としては、ロール制御器３５の出力側に無効化部３７を設けている。すなわち、後述する縮退指令部５０より縮退の指令がある場合には、ロール制御器３５の出力が無効化され、ロール舵の指令値として０が分配部３２へ入力される。なお、縮退に関する優先順位の設定によって、ロール制御器３５の出力側に無効化部３７を設ける場合以外の構成とすることも可能である。

【0029】

分配部３２は、仮想舵である横舵、縦舵、及びロール舵の指令値に基づいて、実際の舵であるＸ舵（１番舵Ｒ１、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４）への指令値を算出する。例えば、横舵、縦舵、及びロール舵の各指令値を数値変換（例えば定数倍）し、数値変換された値を加減算することにより、Ｘ舵の指令値を算出する。例えば、１番舵Ｒ１に対する指令値は、横舵の指令値に基づく値と、縦舵の指令値に基づく値と、ロール舵の指令値に基づく値とを加減算することによって算出される。なお、仮想舵に基づくＸ舵の指令値の算出方法については上記に限定されない。

【0030】

分配部３２によって算出されたＸ舵への指令値は、Ｘ舵における各舵のアクチュエータへ出力され、舵が制御される。

【0031】

（故障診断部の構成）
故障診断部４０は、Ｘ舵に対して故障診断を行う。すなわち、故障診断部４０では、故障が発生している舵である故障舵を特定する。図５に示すように、故障診断部４０は、推定部４１と、判定部４２と、演算部（故障操作量演算部）４５とを備えている。故障診断部４０へは、水中航走体１の状態量として、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒ、ピッチ角θ、ロール角φが入力される。状態量とは、水中航走体１における所定のパラメータにおける現在の値（現状値）である。なお、故障診断部４０で使用される状態量については、上記に限定されず、後述するオブザーバの構成に応じて設定される。例えば、状態量としては、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒ、ピッチ角θ、ヨー角ψ、及びロール角φのうち少なくともいずれか１つを用いることが可能である。機体の状態量については、例えば機体に設けられたセンサ等によって計測され、取得部６１へ出力される。

【0032】

推定部４１は、構造体の状態量と、操作手段のうち１つの操作手段の指令値とに基づいて、操作手段のうち他の操作手段に対する操作量を推定する推定処理を、各操作手段に対応して行う。具体的には、推定部４１は、水中航走体１の状態量と、Ｘ舵のうち１つの舵（例えば１番舵Ｒ１）への指令値とに基づいて、他の舵（例えば、２番舵Ｒ２、３番舵Ｒ３、４番舵Ｒ４）における操作量を推定する（推定処理）。操作量とは、指令値に基づいて駆動された場合の舵の角度（舵角）である。そして該推定処理を、他の舵に対しても同様に行う。

【0033】

推定部４１は、図５に示すように、オブザーバを用いて、推定処理を行う。オブザーバは、各舵に対応してそれぞれ設けられている。図５では、オブザーバＯＳ１のみ構成を具体的に示しており、他のオブザーバ（オブザーバＯＳ２、オブザーバＯＳ３、オブザーバＯＳ４）については構成を略示しているが、オブザーバＯＳ１と同様の構成である。

【0034】

状態（状態変数）をｘとし、入力を各舵に対する操作量とした場合には、線形近似モデル（状態方程式）は、以下の式（１）となる。１番舵Ｒ１に対する操作量をδｒ１、２番舵Ｒ２に対する操作量をδｒ２、３番舵Ｒ３に対する操作量をδｒ３、４番舵Ｒ４に対する操作量をδｒ４とする。

【0035】

【数1】

【0036】

式（１）において、Ａは状態行列（Ａ行列）、Ｂ（ｂ１、ｂ２、ｂ３、ｂ４を要素とする行列）は入力行列（Ｂ行列）である。Ｂ行列は、舵の影響係数であり、揚力係数や感度係数とも呼ばれる。式（１）では、各舵に対する操作量と、状態変数との関係が示されている。状態変数は、推定部４１へ入力される各状態量（速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒ、ピッチ角θ、ロール角φ）の行列となる。式（１）において、例えば１番舵Ｒ１を正常とした場合には、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４に対するＢ行列を用いて、式（１）を以下の式（２）へ変形することができる。

【0037】

【数2】

【0038】

式（２）は、疑似逆行列を用いて、式（３）へ変形することができる。

【0039】

【数3】

【0040】

すなわち、１つの舵（式（３）では１番舵Ｒ１）を正常と仮定して（操作量と指令値が等しいとして）、各状態量を用いることによって、その他の舵の操作量を算出することができる。なお、Ａ行列やＢ行列の各要素は、水中航走体１の特性に基づいて予め設定されている。

【0041】

推定部４１では、式（３）の算出原理を用いたオブザーバによって、舵の操作量を推定する。具体的には、図５に示すように、オブザーバＯＳ１では、状態量にＡ行列を乗じ、また、状態量にｓを乗じている。状態量にｓを乗じることとは、時間領域において状態量を微分することに相当する。そして、１番舵Ｒ１に故障が発生していないと仮定して（すなわち、操作量と指令値とが等しいと仮定して）、１番舵Ｒ１の指令値にＢ行列の１番舵Ｒ１に対応する成分（ｂ１）を乗ずる。そして、状態量ｘを微分した値に対して、状態量ｘにＡ行列を乗じた値及び１番舵Ｒ１の指令値にｂ１を乗じた値を減算した値を算出する。該算出された値は、式（２）の左辺に相当する。そして、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４に対応するＢ行列成分の疑似逆行列Ｂ^＋ _２３４を乗ずることによって、式（３）の右辺の計算を行い、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４の舵角の推定値を算出する。

【0042】

他のオブザーバにおいても同様の計算が行われる。なお、オブザーバＯＳ１におけるＢ行列の１番舵Ｒ１に対応する成分（ｂ１）や、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４に対応するＢ行列成分の疑似逆行列は、各オブザーバに対応してそれぞれ設定されている。

【0043】

推定部４１では、上記のような１番舵Ｒ１に対応する推定処理を、他の舵に対しても行う。すなわち、１番舵Ｒ１を正常と仮定して推定処理を行うことによって、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４の舵角の推定値を算出する（オブザーバＯＳ１）。２番舵Ｒ２を正常と仮定して推定処理を行うことによって、１番舵Ｒ１、３番舵Ｒ３、及び４番舵Ｒ４の舵角の推定値を算出する（オブザーバＯＳ２）。３番舵Ｒ３を正常と仮定して推定処理を行うことによって、１番舵Ｒ１、２番舵Ｒ２、及び４番舵Ｒ４の舵角の推定値を算出する（オブザーバＯＳ３）。４番舵Ｒ４を正常と仮定して推定処理を行うことによって、１番舵Ｒ１、２番舵Ｒ２、及び３番舵Ｒ３の舵角の推定値を算出する（オブザーバＯＳ４）。このようにして、各推定処理によって各舵に対応した舵角の推定値が３つずつ（計１２つ）算出されることとなる。

【0044】

このように算出された各舵角の推定値は、判定部４２へ出力される。

【0045】

判定部４２は、推定された各操作手段にそれぞれ対応する各操作量に基づいて、各操作手段に対する故障判定を行う。具体的には、判定部４２は、各舵のそれぞれに対応する舵角の推定値に基づいて故障判定を行う。このため、図５に示すように、判定部４２は、故障候補特定部４３と、多数決部４４とを備えている。なお、本実施形態では、推定部４１により推定された各推定値に基づいて多数決ロジックにより故障診断を行う場合について説明するが、推定部４１により推定された各推定値に基づいて故障診断を行うことができれば、多数決ロジックによるものに限定されない。

【0046】

故障候補特定部４３は、同じ舵に対して推定された複数の舵角（操作量）に基づいて、故障舵の候補を特定する。具体的には、故障候補特定部４３は、操作手段に対して推定された各操作量のうち最も偏差の大きい操作量を異常操作量として特定し、特定された異常操作量に基づいて故障判定を行う。このため、故障候補特定部４３には、１番舵Ｒ１に対応した第１特定部Ｓ１と、２番舵Ｒ２に対応した第２特定部Ｓ２と、３番舵Ｒ３に対応した第３特定部Ｓ３と、４番舵Ｒ４に対応した第４特定部Ｓ４とが設けられている。すなわち、第１特定部Ｓ１には、１番舵Ｒ１に対して推定された各舵角が入力され、第２特定部Ｓ２には、２番舵Ｒ２に対して推定された各舵角が入力され、第３特定部Ｓ３には、３番舵Ｒ３に対して推定された各舵角が入力され、第４特定部Ｓ４には、４番舵Ｒ４に対して推定された各舵角が入力される。

【0047】

具体的には、第１特定部Ｓ１では、１番舵Ｒ１に対して推定された各舵角のうち、最も偏差の大きい舵角を異常舵角（異常と推定される舵角）として特定する。他と比較して偏差が最も大きいということは、推定処理に誤りがあったと想定されるため、異常舵角として特定された舵角の推定処理において正常と仮定した舵を、故障舵の候補とする。すなわち、第１特定部Ｓ１では、２番舵Ｒ２、３番舵Ｒ３、及び４番舵Ｒ４のいずれか１つが故障舵として特定される。故障舵として特定された舵の故障フラグを１、故障舵として特定されなかった舵の故障フラグを０として出力がされる。

【0048】

第１特定部Ｓ１においては、２番舵Ｒ２に対応した故障フラグＦ２１、３番舵Ｒ３に対応した故障フラグＦ３１、４番舵Ｒ４に対応した故障フラグＦ４１が出力される。第２特定部Ｓ２においては、１番舵Ｒ１に対応した故障フラグＦ１２、３番舵Ｒ３に対応した故障フラグＦ３２、４番舵Ｒ４に対応した故障フラグＦ４２が出力される。第３特定部Ｓ３においては、１番舵Ｒ１に対応した故障フラグＦ１３、２番舵Ｒ２に対応した故障フラグＦ２３、４番舵Ｒ４に対応した故障フラグＦ４３が出力される。第４特定部Ｓ４においては、１番舵Ｒ１に対応した故障フラグＦ１４、２番舵Ｒ２に対応した故障フラグＦ２４、３番舵Ｒ３に対応した故障フラグＦ３４が出力される。各特定部では、故障舵の候補とされた舵に対応する故障フラグが１として出力される。例えば、第１特定部Ｓ１において、２番舵Ｒ２が故障舵として特定された場合には、故障フラグＦ２１が１、故障フラグＦ３１が０、故障フラグＦ３１が０として出力がされる。

【0049】

図６は、故障候補特定部４３における故障舵の候補特定処理の一例を示すフローである。例えば、図６のフローは、各舵角が推定された後に開始される。図６については、１番舵Ｒ１に対して推定された各舵角に基づいて故障舵を推定する場合（第１特定部Ｓ１）について例示している。他の舵についても図６と同様の処理によって故障舵の候補特定処理を行うことができる。図６のフローでは、２番舵Ｒ２を正常と仮定した推定処理によって推定された１番舵Ｒ１の舵角をｄｒ１２とし、３番舵Ｒ３を正常と仮定した推定処理によって推定された１番舵Ｒ１の舵角をｄｒ１３とし、４番舵Ｒ４を正常と仮定した推定処理によって推定された１番舵Ｒ１の舵角をｄｒ１４として説明を行う。

【0050】

故障候補特定部４３では、１番舵Ｒ１に関して推定された各舵角（ｄｒ１２、ｄｒ１３、ｄｒ１４）を取得し（Ｓ１０１）、各舵角の平均値を算出する（Ｓ１０２）。そして、各舵角について、平均値に対する偏差（絶対値）を算出する（Ｓ１０３）。各舵角に対応する偏差が閾値未満であるか否かを判定する（Ｓ１０４）。閾値については、各舵角の推定ばらつき等によって設定され、各偏差が等しいとみなせる上限値として設定される。Ｓ１０４では、各舵角に対応する偏差がすべて等しい（すなわちすべて０である）かを判定することがより好ましい。各舵角に対応する偏差が閾値未満である場合（Ｓ１０４のＹＥＳ判定）には、各舵角は異常ではないとして、各舵の故障フラグを０として出力する（Ｓ１０５）。

【0051】

各舵角に対応する偏差が閾値未満でない場合（Ｓ１０４のＮＯ判定）には、ｄｒ１２の偏差が最大か否かを判定する（Ｓ１０６）。ｄｒ１２の偏差が最大である場合（Ｓ１０６のＹＥＳ判定）には、２番舵Ｒ２の故障フラグを１とする（Ｓ１０７）。ｄｒ１２の偏差が最大でない場合（Ｓ１０６のＮＯ判定）には、ｄｒ１３の偏差が最大か否かを判定する（Ｓ１０８）。ｄｒ１３の偏差が最大である場合（Ｓ１０８のＹＥＳ判定）には、３番舵Ｒ３の故障フラグを１とする（Ｓ１０９）。ｄｒ１３の偏差が最大でない場合（Ｓ１０８のＮＯ判定）には、ｄｒ１４の偏差が最大である場合であるため、４番舵Ｒ４の故障フラグを１とする（Ｓ１１０）。なお、１とされなかった故障フラグは０となる。

【0052】

このようにして、第１特定部Ｓ１では、１番舵Ｒ１に対して推定された各舵角に基づいて故障舵を特定し、故障フラグを１とする。第２特定部Ｓ２、第３特定部Ｓ３、及び第４特定部Ｓ４についても、同様に処理を行い、故障舵が特定された故障フラグが設定される。具体的には、第２特定部Ｓ２では、２番舵Ｒ２に対して推定された各舵角に基づいて故障舵を特定し、故障フラグを１とする。第３特定部Ｓ３では、３番舵Ｒ３に対して推定された各舵角に基づいて故障舵を特定し、故障フラグを１とする。第４特定部Ｓ４では、４番舵Ｒ４に対して推定された各舵角に基づいて故障舵を特定し、故障フラグを１とする。

【0053】

このように故障候補特定部４３において処理が行われることによって、第１特定部Ｓ１から故障フラグ（Ｆ２１、Ｆ３１、Ｆ４１）、第２特定部Ｓ２から故障フラグ（Ｆ１２、Ｆ３２、Ｆ４２）、第３特定部Ｓ３から故障フラグ（Ｆ１３、Ｆ２３、Ｆ４３）、第４特定部Ｓ４から故障フラグ（Ｆ１４、Ｆ２４、Ｆ３４）が出力される。

【0054】

すなわち、故障候補特定部４３からは、１番舵Ｒ１に対応する故障フラグ（Ｆ１２、Ｆ１３、Ｆ１４）、２番舵Ｒ２に対応する故障フラグ（Ｆ２１、Ｆ２３、Ｆ２４）、３番舵Ｒ３に対応する故障フラグ（Ｆ３１、Ｆ３２、Ｆ３４）、４番舵Ｒ４に対応する故障フラグ（Ｆ４１、Ｆ４２、Ｆ４３）が出力される。各フラグは、多数決部４４へ出力される。

【0055】

多数決部４４では、各操作手段に対応して異常操作量が特定された後に、特定された各異常操作量の推定に用いた指令値に対応する操作手段が等しい場合に、各異常操作量に対応する操作手段に対して故障が発生していると判定する。換言すると、多数決部４４は、故障候補特定部４３において各舵に対応して故障舵の候補が特定された後に、特定された故障舵が等しい場合に、該故障舵に異常が発生していると推定する。このように、各操作手段に対応して異常操作量が特定された後に、各異常操作量の推定において正常と仮定した操作手段を特定し、特定された各操作手段が等しい場合に、該操作手段に異常が発生していると診断することができる。

【0056】

例えば、第１特定部Ｓ１において２番舵が故障舵として特定され、第３特定部Ｓ３において２番舵が故障舵として特定され、第４特定部Ｓ４において２番舵が故障舵として特定された場合に、２番舵に故障が発生していると判定する。このため、多数決部４４は、第１ＡＮＤ部Ａ１と、第２ＡＮＤ部Ａ２と、第３ＡＮＤ部Ａ３と、第４ＡＮＤ部Ａ４とを備えている。第１ＡＮＤ部Ａ１には１番舵Ｒ１に対応する故障フラグ（Ｆ１２、Ｆ１３、Ｆ１４）が入力され、第２ＡＮＤ部Ａ２には２番舵Ｒ２に対応する故障フラグ（Ｆ２１、Ｆ２３、Ｆ２４）が入力され、第３ＡＮＤ部Ａ３には３番舵Ｒ３に対応する故障フラグ（Ｆ３１、Ｆ３２、Ｆ３４）が入力され、第４ＡＮＤ部Ａ４には４番舵Ｒ４に対応する故障フラグ（Ｆ４１、Ｆ４２、Ｆ４３）が入力される。

【0057】

第１ＡＮＤ部Ａ１は、１番舵Ｒ１に対応する故障フラグ（Ｆ１２、Ｆ１３、Ｆ１４）が入力され、すべての故障フラグが１である場合に、１番舵Ｒ１に故障が発生していると判定する。すなわち、第１ＡＮＤ部Ａ１は、Ｆ１２、Ｆ１３、Ｆ１４がすべて１である場合に、１番舵故障フラグＦ（Ｒ１）を１として出力する。

【0058】

第２ＡＮＤ部Ａ２は、２番舵Ｒ２に対応する故障フラグ（Ｆ２１、Ｆ２３、Ｆ２４）が入力され、すべての故障フラグが１である場合に、２番舵Ｒ２に故障が発生していると判定する。すなわち、第２ＡＮＤ部Ａ２は、Ｆ２１、Ｆ２３、Ｆ２４がすべて１である場合に、２番舵故障フラグＦ（Ｒ２）を１として出力する。

【0059】

第３ＡＮＤ部Ａ３は、３番舵Ｒ３に対応する故障フラグ（Ｆ３１、Ｆ３２、Ｆ３４）が入力され、すべての故障フラグが１である場合に、３番舵Ｒ３に故障が発生していると判定する。すなわち、第３ＡＮＤ部Ａ３は、Ｆ３１、Ｆ３２、Ｆ３４がすべて１である場合に、３番舵故障フラグＦ（Ｒ３）を１として出力する。

【0060】

第４ＡＮＤ部Ａ４は、４番舵Ｒ４に対応する故障フラグ（Ｆ４１、Ｆ４２、Ｆ４３）が入力され、すべての故障フラグが１である場合に、４番舵Ｒ４に故障が発生していると判定する。すなわち、第４ＡＮＤ部Ａ４は、Ｆ４１、Ｆ４２、Ｆ４３がすべて１である場合に、４番舵故障フラグＦ（Ｒ４）を１として出力する。

【0061】

故障候補特定部４３における第１特定部Ｓ１、第２特定部Ｓ２、第３特定部Ｓ３、及び第４特定部Ｓ４の各部では、１つの舵に対応する故障フラグが１とされる。このため、多数決部４４では、１番舵故障フラグＦ（Ｒ１）、２番舵故障フラグＦ（Ｒ２）、３番舵故障フラグＦ（Ｒ３）、４番舵故障フラグＦ（Ｒ４）のうちのいずれか１つが１となる（またはすべてのフラグが０となる場合もある）。このため、Ｘ舵のうち１つの舵の故障を特定することができる。

【0062】

なお、上記例では、多数決部４４において故障舵を特定することとしているが、ＡＮＤ処理（論理積処理）ではなくＯＲ処理（論理和処理）を行うこととしてもよい。また、故障候補特定部４３から出力された情報を分析して故障舵の特定を行うこととしてもよい。分析については統計的手法ともちいてもよいし、ＡＩ等の機械学習を利用することとしてもよい。

【0063】

操作手段に対して推定された各操作量のそれぞれに対応する各偏差が閾値未満である場合に、該操作手段に対して故障が発生していると判定することとしてもよい。具体的には、ある舵に対応して推定された舵角の各偏差が閾値未満である場合とは、各舵に故障が生じていない場合、または、該舵に故障が発生している場合のいずれかである。すなわち、Ｘ舵において異常が発生していることを前提として、特定の舵に対応して推定された舵角の各偏差が閾値未満である場合には、該特定の舵に故障が発生していると推定することもできる。なお、この場合には、目標軌道から外れている等の原因によっていずれかの舵に故障が発生していることを推定することができる。

【0064】

演算部（故障操作量演算部）４５は、操作手段に対して推定された各操作量に基づいて、故障した操作手段の操作量である故障操作量を演算する。具体的には、演算部４５は、故障舵として特定された舵に対して推定された各舵角に基づいて、故障舵角を推定する。例えば、各舵角に対して平均値を算出することによって、故障舵角を算出する。なお、算出方法については平均値に限定されず中央値等の値とすることとしてもよい。

【0065】

図７は、演算部４５における故障舵角算出処理の一例を示すフローである。例えば、図７のフローは、故障舵が特定された後に開始される。なお、図７では、１番舵Ｒ１が故障舵として特定された場合を例としている。故障舵として特定された舵に対して推定された舵角（ｄｒ１２、ｄｒ１３、ｄｒ１４）を取得し（Ｓ２０１）、各舵角の平均値を算出する（Ｓ２０２）。そして、算出した平均値を、故障舵の舵角として出力する（Ｓ２０３）。なお、舵角の平均値はＳ１０２でも算出しているため、Ｓ１０２で算出した平均値を使用することとしてもよい。

【0066】

演算部４５は、平均値を算出する際には、故障が発生していると判定された操作手段の指令値に基づいて推定された操作量を除いて、故障操作量を演算する。図７のフローでは、１番舵Ｒ１に故障が発生した場合に、正常である舵（２番舵Ｒ２、３番舵Ｒ３、４番舵Ｒ４）を正常と仮定してオブザーバにより推定した舵角（ｄｒ１２、ｄｒ１３、ｄｒ１４）に基づいて１番舵Ｒ１の故障舵角を算出しているため、より正確に故障舵角を算出することができる。

【0067】

（故障診断部による故障舵特定処理の効果）
次に、上述の故障診断部４０による故障舵特定処理による効果について図１１を参照して説明する。図１１では、４番舵Ｒ４に固着故障が発生した場合において、舵角推定を行う場合を例示している。このため、図１１では、縦軸を角度（舵角）、横軸を時間としている。時刻Ｔ１において４番舵Ｒ４に故障が発生したとする。そうすると、４番舵Ｒ４の指令値は舵角に反映されず、４番舵Ｒ４の舵角は、特定の舵角に固着する。故障が発生すると、故障診断部４０では故障舵特定処理が実行される。故障舵特定処理によって、図１１に示すように、推定舵角は故障舵角へより迅速に収束する。そして、より正確な故障舵角が推定される。

【0068】

（縮退指令部の構成）
縮退指令部５０は、舵角指令算出部３０における無効化部３７に指令を出力して、舵角指令算出部３０における制御系を縮退化する。具体的には、縮退指令部５０は、正常に制御可能な操作手段の数が制御量の数以上である場合には、各制御量のそれぞれに対応する目標値に基づいて、各操作手段に対する指令値を算出させ、正常に制御可能な操作手段の数が制御量の数未満である場合には、各指令値の算出に使用する制御量の数を縮退し、各目標値の少なくともいずれか１つに基づいて各操作手段に対する指令値を算出させる。なお、故障舵の数については、故障診断部４０の結果を用いる。

【0069】

冗長性のある水中航走体１において、舵に故障が発生した場合に、制御可能な正常な舵の数が、機体の制御量（姿勢に係る制御の対象）の数より少ない場合には、正常な舵だけで各目標値に対応した制御を行うことが困難となる。例えば、制御量をピッチ角θ、ヨー角ψ、及びロール角φとしており、機体に４つ設けられた舵のうち２つ故障した場合（正常な舵は２つ）、２つの舵でピッチ角θ、ヨー角ψ、及びロール角φを制御することは困難である。もし制御した場合には、各制御が干渉し合い目的の制御を行うことができない場合もある。このため、縮退指令部５０では、舵の故障数に応じて、制御系の縮退化を行う。

【0070】

縮退とは、舵角指令算出部３０における制御系統を縮退化することである。具体的には、舵角指令算出部３０は、各目標値に対応した制御系（深度（ピッチ角θ）に対応した制御系、ヨー角ψに対応した制御系、ロール角φに対応した制御系）を有しているため、制御系の制御量の数（次元数）を減らすことによって縮退を行う。例えば、ロール角φに対応した制御系は使用せずに、深度（ピッチ角θ）に対応した制御系及びヨー角ψに対応した制御系のみで指令値を算出する。換言すると、Ｘ舵の指令値算出に用いる目標値の数を減少させ、各目標値の少なくともいずれか１つに基づいてＸ舵に対する指令値を算出する。

【0071】

縮退については、各制御量に対して予め設定された優先順位に基づいて縮退を行う。本実施形態では、ロール角φの優先順位が低い場合について説明する。すなわち、制御量は、姿勢として機体のピッチ角θ、ヨー角ψ、及びロール角φであり、正常に制御可能な舵の数が２つである場合には、ピッチ角θ及びヨー角ψの目標値に基づいて、各舵に対する指令値を算出する。このように、正常に制御可能な舵の数と、使用する目標値の数が等しくなるように、縮退を行う。

【0072】

このため、図４に示すように、ロール制御器３５の出力側（分配部３２の入力側）に、無効化部３７が設けられている。ピッチ角θ及びヨー角ψを優先的に制御することによって、進行方向の制御を継続することが可能となる。このため、舵に故障が発生した場合であっても、目標軌道への航行を継続することが可能となる。無効化の方法については、優先順位の低い制御対象の目標値がＸ舵の指令値算出に反映されない方法であれば、上記に限定されない。例えば、対象の目標値の入力を遮断する等の方法でもよい。無効化する制御対象（優先順位）についても運行目的等によって適宜変更可能である。

【0073】

縮退指令部５０は、正常な舵の数が、制御量の数以上である場合に、舵角指令算出部３０において、各制御対象のそれぞれに対応する目標値に基づいて各舵に対する指令値を算出させる。正常な舵の数が制御対象の数以上である場合とは、例えば、制御量であるピッチ角θ、ヨー角ψ、及びロール角φに対して３つ以上の舵が正常である場合である。縮退指令部５０は、正常な舵の数が制御量の数以上である場合には、無効化部３７においてロール制御器３５の出力をそのまま分配部３２へ出力させる。すなわち、正常な舵の数が制御量の数以上である場合には、制御系の縮退を行うことなく、通常の制御を行うこととなる。

【0074】

縮退指令部５０は、正常に制御可能な操作手段の数が制御量の数未満である場合には、舵角指令算出部３０において、制御量の数を縮退し、各目標値の少なくともいずれか１つに基づいて各舵に対する指令値を算出させる。正常な舵の数が制御量の数未満である場合とは、例えば、制御量であるピッチ角θ、ヨー角ψ、及びロール角φに対して２つ以下の舵が正常である場合である。縮退指令部５０は、正常な舵の数が制御量の数未満である場合には、無効化部３７においてロール制御器３５の出力を分配部３２へ出力させず（無効化）、０（零）をロール舵の指令値として分配部３２へ出力させる。すなわち、正常な舵の数が制御量の数未満である場合には、制御系を縮退して制御を行うこととなる。正常に制御可能な舵の数と、使用する目標値の数が等しくなるように縮退を行うこととしてもよいし、使用する目標値の数を、正常に制御可能な舵の数より少なくすることとしてもよい。

【0075】

縮退が行われた場合には、分配部３２には横舵の指令値、縦舵の指令値、及びロール舵の指令値（零）が入力されるため、横舵の指令値及び縦舵の指令値に基づいて、各舵の指令値が算出される。本実施形態では、分配部３２を各仮想舵の指令値の加減算としているため、縮退対象の制御系の出力を０とすることで、無効化を行うことができる。

【0076】

このように縮退を行うことによって、ロール角目標値φ*がＸ舵の指令値の算出に反映されなくなる。このためロール角φに対する制御は無効化して、ピッチ角θ及びヨー角ψを目標値に追従するように制御を行うことが可能となる。すなわち、目標軌道への航行を継続することが可能となる。

【0077】

本実施形態では、ロール角φに対する優先順位を低く設定する場合について説明したが、縮退に関する優先順位については、運行の目的（運行優先事項）に応じて適宜設定可能である。例えば、ピッチ角θの優先順位を低く設定することとしてもよい。具体的には、制御量は、姿勢として機体のピッチ角θ、ヨー角ψ、及びロール角φであり、正常に制御可能な操作手段の数が２つである場合には、ヨー角ψ及びロール角φの目標値に基づいて、各操作手段に対する指令値を算出する。この場合には、ピッチ角θに対する制御は無効化されるものの、ヨー角ψ及びロール角φに対して制御を行うことができるため、機体をなるべく移動させずに、その場にとどまらせることができる。

【0078】

図８は、縮退指令部５０における縮退処理の一例を示すフローである。例えば、図８のフローは、所定の制御周期で繰り返し実行される。

【0079】

まず、故障が発生したか否かを判定する（Ｓ３０１）。Ｓ３０１は、故障診断部４０による診断結果に応じて行う。故障が発生していない場合（Ｓ３０１のＮＯ判定）には、縮退を行うことなく、処理を終了する。水中航走体１は、冗長性のあるシステムであるため、Ｓ３０１のＮＯ判定では、正常な舵の数が制御対象の数以上である場合となり、舵角指令算出部３０では、各制御対象のそれぞれに対応する目標値に基づいて各舵に対する指令値を算出する。

【0080】

故障が発生した場合（Ｓ３０１のＹＥＳ判定）には、正常に制御可能な舵の数が機体の姿勢に関する制御量の数未満であるか否かを判定する（Ｓ３０２）。正常に制御可能な舵の数が機体の姿勢に関する制御量の数未満でない場合（Ｓ３０２のＮＯ判定）には、正常な舵の数が制御量の数以上である場合であるため、縮退を行うことなく、処理を終了する。この場合には、舵角指令算出部３０では、各制御量のそれぞれに対応する目標値に基づいて各舵に対する指令値を算出する。

【0081】

正常に制御可能な舵の数が機体の姿勢に関する制御量の数未満である場合（Ｓ３０２のＹＥＳ判定）には、縮退を行う（Ｓ３０３）。具体的には、優先順位の低い制御量（例えばロール角φ）に対応した制御を無効化させるため、無効化部３７へ無効化指示を行う。このため、舵角指令算出部３０では、制御量の数が縮退され、各目標値の少なくともいずれか１つに基づいて各舵に対する指令値を算出する。なお、無効化部３７への指示により縮退を行う場合に限定されず、縮退方法によって、Ｓ３０３の具体的な縮退処理は異なる。

【0082】

このように縮退制御を行うことで、より確実に目的にあう運行を行うことが可能となる。

【0083】

（耐故障制御部の構成）
耐故障制御部６０は、故障舵の影響を非干渉化するように、各舵に対する指令値を算出する。例えば、Ｘ舵のうちの１つの舵が故障して一定の舵角で固着した場合には、該故障舵は固着した舵角に応じて機体に力を及ぼす。このような故障舵の及ぼす力は、機体の姿勢制御に影響を及ぼす。このため、耐故障制御部６０では、故障舵の影響を抑制するように、他の舵（正常に動く舵）を制御する。

【0084】

耐故障制御部６０は、図９に示すように、取得部６１と、演算部（指令値演算部）６２とを備えている。

【0085】

取得部６１は、機体の状態量を取得する。機体の状態量とは、水中航走体１における所定のパラメータにおける現在の値（現状値）である。取得部６１は、耐故障制御部６０における演算に用いるために、状態量を取り込む。状態量は、例えば、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒ、ピッチ角θ、ロール角φが入力される。なお、耐故障制御部６０で使用される状態量については、上記に限定されず、後述する状態方程式の構成に応じて設定される。例えば、状態量としては、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒ、ピッチ角θ、ヨー角ψ、及びロール角φのうち少なくともいずれか１つを用いることが可能である。

【0086】

機体の状態量については、例えば機体に設けられたセンサ等によって計測され、取得部６１へ出力される。機体のシミュレーションモデルを用いて、各状態量の値を推定することとしてもよい。

【0087】

取得部６１において取得された状態量は、演算部６２へ出力される。

【0088】

演算部（指令値演算部）６２は、機体の状態量を状態変数とし、操作手段の操作量を入力とした状態方程式において、故障が発生した操作手段である故障操作手段に関する情報に基づいて故障操作手段に関する項を外乱項として設定し、取得した状態量に基づいて故障操作手段以外の操作手段の操作量を演算する。故障操作手段に関する情報とは、故障操作手段の識別情報（故障舵特定情報）、及び故障操作手段の故障操作量（故障舵角）である。演算部６２は、状態方程式に基づいて、外乱項を打ち消すように故障操作手段以外の操作手段の操作量を演算する。具体的には、演算部６２は、状態方程式において、状態変数の微分項を０とし、故障操作手段以外の操作手段の操作量に対して解くことにより、演算を行う。すなわち、演算部６２では、状態方程式を操作量に対して解くことで、故障舵の影響を非干渉化するように、正常に制御可能な舵（正常舵）の指令値（解として得られる操作量）を演算する。

【0089】

本実施形態では、演算部６２は、舵角指令算出部３０において算出された各舵に対する指令値に対して、故障舵の影響を非干渉化するための補正値を演算する場合について説明する。なお、本実施形態における耐故障制御部６０では、故障が発生した操作手段である故障操作手段に関する情報として、故障診断部４０の出力である故障操作手段の識別情報（故障舵特定情報）、及び故障操作手段の故障操作量（故障舵角）を用いる場合について説明しているが、耐故障制御部６０では故障操作手段の特定及び故障操作量が入力されれば、故障診断部４０の出力を用いる場合に限定されない。また、縮退指令部５０は、耐故障制御部６０とともに制御装置２０に設けられることとしているが、縮退指令部５０のみを制御装置２０に設けることとしてもよい。

【0090】

水中航走体１の状態方程式は、以下の式（４）によって表される。なお、式（４）は、船体座標系における線形化状態方程式である。

【0091】

【数4】

【0092】

式（４）において、Δｕは速度ｕの変化量（微小増減分）、Δｖは、速度ｖの変化量、Δｗは、速度ｗの変化量、Δｐは角速度ｐの変化量、Δｑは角速度ｑの変化量、Δｒは角速度ｒの変化量である。ａ１１からａ６６は、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、及び角速度ｒのそれぞれに対応したＡ行列の各要素である。Δφはロール角φの変化量であり、Δθはピッチ角θの変化量である。ａ１８からａ６９は、ロール角φ、及びピッチ角θのそれぞれに対応したＡ行列の各要素である。Δｎは、機体において推進力を得るためのプロペラ２の回転数（スラスタ回転数）の変化量であり、Δδｒ１は、１番舵Ｒ１の舵角の変化量であり、Δδｒ２は、２番舵Ｒ２の舵角の変化量であり、Δδｒ３は、３番舵Ｒ３の舵角の変化量であり、Δδｒ４は、４番舵Ｒ４の舵角の変化量である。ｂ１１からｂ６５は、回転数、１番舵Ｒ１の舵角、２番舵Ｒ２の舵角、３番舵Ｒ３の舵角、及び４番舵Ｒ４の舵角のそれぞれに対応したＢ行列の各要素である。式（４）においてＡ行列及びＢ行列については、水中航走体１の特性に応じて予め設定されている。

【0093】

式（４）において、各舵の舵角（Δδｒ１、Δδｒ２、Δδｒ３、Δδｒ４）は変数としているが、舵が故障して舵角が固着した場合には、舵角は一定（固定値）となる。具体的には、１番舵Ｒ１が固着した場合、式（４）について、１番舵Ｒ１に関する項を外乱項（定数項）として変形すると、式（５）を得る。

【0094】

【数5】

【0095】

式（５）において、右辺の最右項は、１番舵Ｒ１が故障したことによる力を示す項であり、外乱項となる。外乱項については、故障舵が特定されており、該故障舵の舵角がわかれば、固定値として設定される。故障舵が特定及び該故障舵の舵角（すなわち故障操作手段に関する情報）については、故障診断部４０の出力が使用される。すなわち、外乱項において、Δδｒ１は固着角自体となる。固着故障の影響を外乱項として扱うため、舵の平衡状態（δｒ１=０ｄｅｇ）からの差をΔδｒ１としている。

【0096】

式（５）について、機体の姿勢（ロール角φ、ピッチ角θ、ヨー角ψ）に関して、加速度と対角項を０となるように変形すると、式（６）を得る。式（６）では、角速度ｐ、角速度ｑ、及び角速度ｒの影響がないように式（５）が変形されている。

【0097】

【数6】

【0098】

すなわち、式（６）は、外乱項と、正常舵（２番舵Ｒ２、３番舵Ｒ３、４番舵Ｒ４）の操作量の干渉項と各制御軸における状態量の干渉項の関係を示す式となっている。式（６）について、正常舵（２番舵Ｒ２、３番舵Ｒ３、４番舵Ｒ４）の操作量について解くことにより、外乱項や操作量と状態量の干渉項を打ち消すための正常舵（２番舵Ｒ２、３番舵Ｒ３、４番舵Ｒ４）の操作量を得ることができる。すなわち、式（６）から算出される操作量（Δδｒ２、Δδｒ３、Δδｒ４）は、舵角指令算出部３０において算出された各舵の指令値に対する補正値となる。

【0099】

状態方程式において、状態量には機体の前進速度（速度ｕ）が含まれており、前進速度にかかる係数は０に設定されていることが好ましい。前進速度にかかる係数は、操作手段の操作量に関する情報が影響している場合がある。このような場合において前進速度を加味して操作量を演算すると、操作量として正確な値が得られない可能性がある。このため、状態方程式において、前進速度にかかる係数を０とすることで、演算の不安定性を抑制することができる。具体的には、式（６）において、ａ４１、ａ５１、及びａ６１が０と設定される。

【0100】

状態方程式における各係数（Ａ行列及び／またはＢ行列の要素）のうち少なくともいずれか１つは、機体の状態量に関するパラメータの関数として表されることが好ましい。Ａ行列及び／またはＢ行列の要素は、水中航走体１の特性に応じて値が設定されるが、線形化は近似的に設定される。具体的には、特定の平衡点周りで線形近似が行われる。すなわち、線形近似した平衡点から離れた運転状態では、近似精度が低下する場合がある。このため、Ａ行列及び／またはＢ行列の要素を、機体の状態量に関するパラメータの関数として設定しておくことで、運転状態に合わせて適切にＡ行列やＢ行列を設定することが可能となる。なお、Ａ行列及び／またはＢ行列の要素のうち少なくともいずれか１つを機体の状態量に関するパラメータの関数として表すことが可能である。

【0101】

このため、耐故障制御部６０の出力は、舵角指令算出部３０の出力側で目標値に基づいて算出された各舵の指令値に対して加算処理される。このように、故障舵に関する外乱力を非干渉化するように設定した補正値によって正常舵の指令値を補正するため、正常舵の操作によって外乱力を無効化し、より安定的に航行を継続することが可能となる。

【0102】

なお、本実施形態では、演算部６２は、舵角指令算出部３０において算出された各舵に対する指令値に対して、故障舵の影響を非干渉化するための補正値を演算する場合について説明したが、耐故障制御部６０の機能を舵角指令算出部３０へ含めてもよい。すなわち、目標値を加味しつつ故障舵の影響を低減するような各舵の指令値を算出することとしてもよい。すなわち、式（６）へ変形せずに式（５）を解くことによって、補正値としてではなく、指令値を算出することも可能である。

【0103】

また、式（５）では、固着による故障が発生した場合を例としたが、変形による故障が発生した場合には、式（５）に代えて式（７）を用いることとしてもよい。

【0104】

【数7】

【0105】

変形では、舵角を操作することができるものの、指令値に一定の変形量が付加された舵角となる。式（７）では、１番舵Ｒ１の変形量（オフセット量）をδ１として、右辺の最右項に外乱項を示している。

【0106】

耐故障制御部６０の上記演算処理では、１番舵Ｒ１が故障した場合を例示して説明したが、他の舵についても同様に適応することが可能である。また、２枚故障（Ｘ舵のうち２つの舵が故障）の場合についても同様に適応することができる。

【0107】

図１０は、耐故障制御部６０における演算処理の一例を示すフローである。例えば、図１０のフローは、舵に故障が発生した場合に開始される。

【0108】

まず、機体の状態量を取得する（Ｓ４０１）。例えば、機体の状態量として、速度ｕ、速度ｖ、速度ｗ、角速度ｐ、角速度ｑ、角速度ｒ、ピッチ角θ、ロール角φが取得される。

【0109】

次に、故障舵に関する情報を取得する（Ｓ４０２）。例えば、故障舵特定情報及びその故障舵角である。故障舵に関する情報については、故障舵の特定及び故障時の舵角がわかれば、故障舵特定情報及びその故障舵角に限定されず用いることが可能である。

【0110】

そして、正常舵に対する補正値を算出する（Ｓ４０３）。具体的には、故障舵の影響を外乱として設定した状態方程式を解くことによって、故障舵の影響を低減するように、故障舵以外の舵の指令値の補正値が演算される。なお、Ｓ４０３においては、補正値を演算する場合に限定されず、状態方程式を解くことにより外乱を加味した舵の指令値を算出することとしてもよい。

【0111】

（耐故障制御部による非干渉化の効果）
次に、上述の耐故障制御部６０の非干渉化による効果について図１２を参照して説明する。図１２では、４番舵Ｒ４に固着故障が発生した場合における水中航走体１の移動軌跡を示している。このため、図１２では、絶対座標系において３次元直交座標系を示している。具体的には、ｚ軸方向を深度Ｚとし、ｘ軸方向及びｙ軸方向を位置Ｘ及び位置Ｙとして示している。

【0112】

水中航走体１は、直進方向を目標軌道として運転されており、点Ｐにおいて故障（４番舵Ｒ４の固着）が発生したとする。すると、非干渉化制御を行わない場合には、固着した故障舵の影響によって機体は目標軌道から大きくずれる。一方で、非干渉化制御を行った場合には、機体は直ちに目標軌道へ復帰して運行を継続することができる。

【0113】

このように、非干渉化制御を行うことによって、故障舵の影響を抑制して迅速に運行復帰を行うことが可能となる。

【0114】

なお、縮退化制御では、２故障の場合（正常に制御可能な舵の数が機体の姿勢に関する制御量の数よりも少なくなってしまった場合）でも、優先順位の低い制御対象に対する制御を省略して、優先順位の高い制御対象に対する制御を実行する。このため、縮退化制御では、より重篤な故障の場合でも制御不安定となることを抑制し、優先的な目的運行をより確実に行うことが可能となる。

【0115】

すなわち、縮退化制御と、非干渉化制御とによって、舵の故障に対してより効果的に運行継続を行うことが可能となる。

【0116】

以上説明したように、本実施形態に係る制御システム及び構造体、並びに制御方法、並びに制御プログラムによれば、水中航走体１の状態量を状態変数とし舵の操作量を入力とした状態方程式において、状態変数として取得した状態量を用い、故障舵に関する情報に基づいて故障舵に関する項を外乱項とすることで、故障舵以外の舵の操作量を演算することができる。そして、操作量は、故障が発生した故障舵に係る外乱項を考慮して演算される。すなわち、故障舵の影響を低減するように、故障舵以外の舵の操作量が演算される。例えば、舵に故障（例えば固着や変形）が発生し、水中航走体１の姿勢に影響を及ぼす可能性がある場合であっても、他の舵（故障が発生していない舵）によって、該影響を抑制することが可能となる。なお、故障舵に関する情報とは、例えば故障が発生した舵を特定するための情報や、故障が発生した舵の操作量（例えば固着した操作量）である。

【0117】

また、状態方程式において、外乱項を打ち消すように故障舵以外の舵の操作量を演算することによって、該故障舵による影響分をキャンセルするための舵（故障が発生していない）の操作量を演算することができる。例えば、演算した操作量を補正値として用いることにより、故障舵の影響を抑制するような各舵の操作量を演算することができる。

【0118】

また、状態方程式における係数を、水中航走体１の状態量に関するパラメータの関数とすることによって、水中航走体１の状態によって適切に係数の値を設定することができる。このため、より精度よく、状態方正式を用いた演算を行うことが可能となる。状態方程式において水中航走体１の前進速度にかかる係数を０と設定した。前進速度にかかる係数には、舵の操作量に関する情報が影響している場合がある。このような場合において前進速度を加味して操作量を演算すると、操作量として正確な値が得られない可能性がある。このため、状態方程式において、前進速度にかかる係数を０とすることで、より正確な演算を行うことが可能となる。

【0119】

〔第２実施形態〕
次に、本開示の第２実施形態に係る制御システム及び構造体、並びに制御方法、並びに制御プログラムについて説明する。
上述した第１実施形態では、単一故障によるオブザーバを使用する場合についてしていたが、本実施形態では、複数故障におけるオブザーバについて説明する。以下、本実施形態に係る制御システム及び構造体、並びに制御方法、並びに制御プログラムについて、第１実施形態と異なる点について主に説明する。

【0120】

上記の実施形態のように各オブザーバを用いて多数決ロジックにより１つの故障舵を特定した場合、１回の処理では１つの故障舵が特定される。そして、複数故障（例えば２つの舵が故障）が発生した場合には、特定処理毎に、異なる故障舵が特定されることとなる。具体的には、１番舵Ｒ１と２番舵Ｒ２が故障している場合には、ある特定処理では１番舵Ｒ１の故障が特定され別の特定処理では２番舵Ｒ２の故障が特定される。これは、使用する状態量の変化に応じて、多数決ロジックにおいて特定される故障舵が異なる場合があるからである。すなわち、特定された故障舵の情報を保有しておくことで、複数故障の場合における故障舵を特定することができる。

【0121】

このため、本実施形態における故障診断部４０は、２故障用のオブザーバを用いて故障舵角の推定を行う。図１３に示すように、故障診断部４０は、複数故障用推定部７１と、選択部７２とを更に備えている。本実施形態では、２故障の場合について説明するが複数故障の場合であれば同様に適応することができる。

【0122】

複数故障用推定部７１は、複数の操作手段に対して故障が発生していると判定された場合に、構造体の状態量と、故障が発生していると判定された操作手段以外の操作手段の指令値とに基づいて、故障が発生していると判定された操作手段に対する操作量を推定する。具体的には、複数故障用推定部７１は、２故障用オブザーバを用いて、故障舵角を推定する。

【0123】

２故障用オブザーバとは、２つの舵を正常と仮定してその他の舵における舵角を推定するオブザーバである。２故障用オブザーバによる舵角推定原理は、上記実施形態と同様である。例えば、１番舵Ｒ１及び２番舵Ｒ２を正常と仮定して３番舵Ｒ３及び４番舵Ｒ４の舵角を推定する２故障用オブザーバは、以下の式（８）となる。

【数8】

【0124】

式（８）は、式（１）において、１番舵Ｒ１及び２番舵Ｒ２を正常として変形した式である。式（８）によれば、１番舵Ｒ１及び２番舵Ｒ２を正常と仮定して（指令値を用いて）３番舵Ｒ３及び４番舵Ｒ４の舵角を推定することができる。

【0125】

複数故障用推定部７１は、図１３のように、各舵の２つ故障に対応する各２故障用オブザーバを備えている。具体的には、１番舵Ｒ１及び２番舵Ｒ２を正常と仮定して３番舵Ｒ３及び４番舵Ｒ４の舵角を推定する２故障用オブザーバ＃１２、１番舵Ｒ１及び３番舵Ｒ３を正常と仮定して１番舵Ｒ１及び４番舵Ｒ４の舵角を推定する２故障用オブザーバ＃１３、１番舵Ｒ１及び４番舵Ｒ４を正常と仮定して２番舵Ｒ２及び３番舵Ｒ３の舵角を推定する２故障用オブザーバ＃１４、２番舵Ｒ２及び３番舵Ｒ３を正常と仮定して１番舵Ｒ１及び４番舵Ｒ４の舵角を推定する２故障用オブザーバ＃２３、２番舵Ｒ２及び４番舵Ｒ４を正常と仮定して１番舵Ｒ１及び３番舵Ｒ３の舵角を推定する２故障用オブザーバ＃２４、及び３番舵Ｒ３及び４番舵Ｒ４を正常と仮定して１番舵Ｒ１及び２番舵Ｒ２の舵角を推定する２故障用オブザーバ＃３４を備えている。

【0126】

すなわち、故障舵に応じて、対応する２故障用オブザーバを用いて故障舵角を推定する。例えば、故障が発生したと判定された舵以外の舵を正常と仮定する２故障用オブザーバによって、２つの故障舵の推定舵角を算出する。

【0127】

例えば、判定部４２において、１番舵Ｒ１と２番舵Ｒ２に故障が発生していると判定された場合には、２故障用オブザーバ＃３４を用いて、３番舵Ｒ３及び４番舵Ｒ４を正常と仮定して１番舵Ｒ１及び２番舵Ｒ２の舵角を推定する。

【0128】

選択部７２は、２の舵に故障が判定された場合に、２故障用オブザーバを選択する。具体的には、選択部７２は、判定部４２からの出力を保有し、２つの異なる舵に故障が発生していると判定された場合に、対応する２故障用オブザーバを選択する。判定部４２では、１つの故障舵を特定しているため、２故障の場合には、処理毎に、異なる故障舵が特定されることとなる。このため選択部７２では、判定部４２からの出力を保有しておくことで、２つの異なる舵の故障を認識することができる。

【0129】

例えば、選択部７２において１番舵Ｒ１と２番舵Ｒ２の故障を認識した場合には、２故障用オブザーバ＃３４を選択する。

【0130】

選択部７２において２故障用オブザーバが選択されると、複数故障用推定部７１では、該２故障用オブザーバによって、２つの異なる故障舵の推定舵角が推定される。例えば、複数故障用推定部７１が選択された場合には、故障舵である１番舵Ｒ１と２番舵Ｒ２の舵角が推定される。選択部７２では、選択した２故障用オブザーバから出力された２つの故障舵の推定舵角を、各故障舵の故障舵角として出力する。このようにして、複数の故障舵の特定と、該故障舵のそれぞれの故障舵角が特定される。

【0131】

上記例では、２故障の場合を例として説明をしたが、複数故障の場合であれば同様に適応することができる。

【0132】

以上説明したように、本実施形態に係る制御システム及び構造体、並びに制御方法、並びに制御プログラムによれば、２つの操作手段に対して故障が発生していると判定された場合であっても、構造体の状態量と、操作手段のうち２つの操作手段の指令値とに基づいて、操作手段のうち他の操作手段に対する操作量を推定する複数故障用推定処理を、各操作手段に対応して行うことで、各操作手段に対応する各操作量を推定することができる。このため、推定された各操作手段にそれぞれ対応する各操作量に基づいて故障判定を行うことができる。

【0133】

本開示は、上述の実施形態のみに限定されるものではなく、発明の要旨を逸脱しない範囲において、種々変形実施が可能である。なお、各実施形態を組み合わせることも可能である。

【0134】

以上説明した各実施形態に記載の制御システム及び構造体、並びに制御方法、並びに制御プログラムは例えば以下のように把握される。
本開示に係る制御システムは、機体の姿勢に関する制御量（θ、ψ、φ）を、前記制御量の数よりも多い複数の操作手段（Ｒ）によって制御する構造体（１）に適用される制御システムであって、機体の状態量を取得する取得部（６１）と、機体の状態量を状態変数とし、前記操作手段（Ｒ）の操作量を入力とした状態方程式において、故障が発生した前記操作手段（Ｒ）である故障操作手段（Ｒ）に関する情報に基づいて前記故障操作手段（Ｒ）に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段（Ｒ）以外の前記操作手段（Ｒ）の前記操作量を演算する演算部（６２）と、を備える。

【0135】

本開示に係る制御システムによれば、機体の状態量を状態変数とし操作手段（Ｒ）の操作量を入力とした状態方程式において、状態変数として取得した状態量を用い、故障操作手段（Ｒ）に関する情報に基づいて故障操作手段（Ｒ）に関する項を外乱項とすることで、故障操作手段（Ｒ）以外の操作手段（Ｒ）の操作量を演算することができる。そして、操作量は、故障が発生した故障操作手段（Ｒ）に係る外乱項を考慮して演算される。すなわち、故障操作手段（Ｒ）の影響を低減するように、故障操作手段（Ｒ）以外の操作手段（Ｒ）の操作量が演算される。例えば、操作手段（Ｒ）に故障（例えば固着や変形）が発生し、機体の姿勢に影響を及ぼす可能性がある場合であっても、他の操作手段（Ｒ）（故障が発生していない操作手段（Ｒ））によって、該影響を抑制することが可能となる。なお、故障操作手段（Ｒ）に関する情報とは、例えば故障が発生した操作手段（Ｒ）を特定するための情報や、故障が発生した操作手段（Ｒ）の操作量（例えば固着した操作量）である。

【0136】

本開示に係る制御システムは、前記演算部（６２）は、前記状態方程式に基づいて、前記外乱項を打ち消すように前記故障操作手段（Ｒ）以外の前記操作手段（Ｒ）の前記操作量を演算することとしてもよい。

【0137】

本開示に係る制御システムによれば、状態方程式において、外乱項を打ち消すように故障操作手段（Ｒ）以外の操作手段（Ｒ）の操作量を演算することによって、該故障操作手段（Ｒ）による影響分をキャンセルするための操作手段（Ｒ）（故障が発生していない）の操作量を演算することができる。例えば、演算した操作量を補正値として用いることにより、故障操作手段（Ｒ）の影響を抑制するような各操作手段（Ｒ）の操作量を演算することができる。

【0138】

本開示に係る制御システムは、前記演算部（６２）は、前記状態方程式において、前記状態変数の微分項を０とし、前記故障操作手段（Ｒ）以外の前記操作手段（Ｒ）の前記操作量に対して解くことにより、演算を行うこととしてもよい。

【0139】

本開示に係る制御システムによれば、状態方程式における状態変数の微分項を０として、該状態方程式を、故障操作手段（Ｒ）以外の操作手段（Ｒ）の操作量に対して解くことによって、外乱項を打ち消すような故障操作手段（Ｒ）以外の操作手段（Ｒ）の操作量を演算することができる。

【0140】

本開示に係る制御システムは、前記状態方程式における各係数のうち少なくともいずれか１つは、機体の状態量に関するパラメータの関数として表されることとしてもよい。

【0141】

本開示に係る制御システムによれば、状態方程式における係数を、機体の状態量に関するパラメータの関数とすることによって、機体の状態によって適切に係数の値を設定することができる。このため、より精度よく、状態方正式を用いた演算を行うことが可能となる。

【0142】

本開示に係る制御システムは、前記状態方程式において、前記状態量には機体の前進速度が含まれており、前記前進速度にかかる係数は０に設定されていることとしてもよい。

【0143】

本開示に係る制御システムによれば、状態方程式において機体の前進速度にかかる係数を０と設定した。前進速度には、操作手段（Ｒ）の操作量に関する情報が影響している場合がある。このような場合において前進速度にかかる係数を加味して操作量を演算すると、操作量として正確な値が得られない可能性がある。このため、状態方程式において、前進速度にかかる係数を０とすることで、より正確な演算を行うことが可能となる。

【0144】

本開示に係る制御システムは、前記操作手段（Ｒ）は、アクチュエータにより駆動されることとしてもよい。

【0145】

本開示に係る制御システムによれば、操作手段（Ｒ）としてアクチュエータを対象とすることができる。

【0146】

本開示に係る構造体（１）は、機体の姿勢を制御する複数の操作手段（Ｒ）と、上記の制御システムとを備える。

【0147】

本開示に係る制御方法は、機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段（Ｒ）によって制御する構造体（１）に適用される制御方法であって、機体の状態量を取得する工程と、機体の状態量を状態変数とし、前記操作手段（Ｒ）の操作量を入力とした状態方程式において、故障が発生した前記操作手段（Ｒ）である故障操作手段（Ｒ）に関する情報に基づいて前記故障操作手段（Ｒ）に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段（Ｒ）以外の前記操作手段（Ｒ）の前記操作量を演算する工程と、を有する。

【0148】

本開示に係る制御プログラムは、機体の姿勢に関する制御量を、前記制御量の数よりも多い複数の操作手段（Ｒ）によって制御する構造体（１）に適用される制御プログラムであって、機体の状態量を取得する処理と、機体の状態量を状態変数とし、前記操作手段（Ｒ）の操作量を入力とした状態方程式において、故障が発生した前記操作手段（Ｒ）である故障操作手段（Ｒ）に関する情報に基づいて前記故障操作手段（Ｒ）に関する項を外乱項として設定し、取得した状態量に基づいて前記故障操作手段（Ｒ）以外の前記操作手段（Ｒ）の前記操作量を演算する処理と、をコンピュータに実行させる。

【符号の説明】

【0149】

１ ：水中航走体（構造体）
２ ：プロペラ
１１ ：ＣＰＵ
１２ ：ＲＯＭ
１３ ：ＲＡＭ
１４ ：ハードディスクドライブ
１５ ：通信部
１８ ：バス
２０ ：制御装置（制御システム）
３０ ：舵角指令算出部
３１ ：仮想舵角算出部
３２ ：分配部
３３ ：深度制御器
３４ ：方位制御器
３５ ：ロール制御器
３６ ：姿勢角制御器
３７ ：無効化部
４０ ：故障診断部（故障診断システム）
４１ ：推定部
４２ ：判定部
４５ ：演算部（故障操作量演算部）
５０ ：縮退指令部
６０ ：耐故障制御部
６１ ：取得部
６２ ：演算部（指令値演算部）
７１ ：複数故障用推定部
７２ ：選択部
Ａ１ ：第１ＡＮＤ部
Ａ２ ：第２ＡＮＤ部
Ａ３ ：第３ＡＮＤ部
Ａ４ ：第４ＡＮＤ部
Ｒ１～Ｒ４：舵（操作手段）
Ｓ１ ：第１特定部
Ｓ２ ：第２特定部
Ｓ３ ：第３特定部
Ｓ４ ：第４特定部
Ｚ ：深度
ｐ ：角速度
ｑ ：角速度
ｒ ：角速度
ｕ ：速度
ｖ ：速度
ｗ ：速度
θ ：ピッチ角
φ ：ロール角
ψ ：ヨー角

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】