航走体の安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラム
【課題】舵駆動源が故障し、かつ、正常に動作する舵駆動源が残っている場合においても、水中航走体の姿勢を安定に保つことのできる安定ロバスト制御装置を提供する。
【解決手段】安定ロバスト制御装置１０は、舵駆動源７の故障を検知する故障検知手段１１と、水中航走体１の姿勢に関する情報を検知するセンサ８にて検出した検出値と目標値との偏差を算出する偏差算出手段１４と、水中航走体１の姿勢制御用パラメータとして、偏差に基づいて水中航走体１に付与する制御力および制御モーメントを算出する自動制御パラメータ算出手段１５と、制御力および制御モーメントを、配分則に基づいて、各舵駆動源７に対して配分する配分量を算出すると共に、舵駆動源７の故障が検知された場合に、制御力および制御モーメントを、故障が検知された舵駆動源７を除く残りの舵駆動源７に対して再配分する配分量算出手段１６とを備える。
【解決手段】安定ロバスト制御装置１０は、舵駆動源７の故障を検知する故障検知手段１１と、水中航走体１の姿勢に関する情報を検知するセンサ８にて検出した検出値と目標値との偏差を算出する偏差算出手段１４と、水中航走体１の姿勢制御用パラメータとして、偏差に基づいて水中航走体１に付与する制御力および制御モーメントを算出する自動制御パラメータ算出手段１５と、制御力および制御モーメントを、配分則に基づいて、各舵駆動源７に対して配分する配分量を算出すると共に、舵駆動源７の故障が検知された場合に、制御力および制御モーメントを、故障が検知された舵駆動源７を除く残りの舵駆動源７に対して再配分する配分量算出手段１６とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、水中、空中、陸上等で利用される航走体の姿勢を安定に保つ制御を行う安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラムに関する。
【背景技術】
【０００２】
従来、水中で利用される航走体（水中航走体）や、空中で利用される航走体（飛行体）の姿勢を安定に保つ技術が知られている（例えば、特許文献１〜特許文献３参照）。
特許文献１に開示された水中航走体は、後端部に設けられたスクリューを推進器として水中を航走し、スクリューの上下近傍および左右近傍に設けられた４つの舵を４つのサーボモータでそれぞれ駆動させることで姿勢が制御される。この水中航走体は、制御手段によって、本体の左右方向への傾きを補正するヨー制御と、上下方向への傾きを補正するピッチ制御と、本体のローリングを補正するロール制御とを行うものであり、ロール制御を優先して各サーボモータを個別に制御する。
【０００３】
特許文献２に開示された技術は、ＰＩ制御で生成した制御コマンドに基づいて、航走体の複数の推力発生器（スラスタ、舵、プロペラ等）に対して、燃料消費が最小となるような最適な推力配分を行うものである。
【０００４】
特許文献３に開示された技術は、所定の定位置に静止した飛行船が風に流されないように、ＰＩＤ制御で生成した制御コマンドに基づいて、飛行船の複数のスラスタに対する駆動指令信号を生成し、飛行船を定位置に復帰させて保持する制御を行うものである。
【特許文献１】特開平６−３４１８５２号公報（段落０００８〜００２２、図４）
【特許文献２】特開平２−１０６４９２号公報（第２頁右上１４行目〜第５頁右上３行目、第３図）
【特許文献３】特開平１−３１１１９９７号公報（第２頁左上１３行目〜右下１２行目、第２図）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来の技術では、複数の推力発生器をそれぞれ駆動する複数の推力発生器駆動源（例えば、アクチュエータ）が正常に動作することを前提としている。そのため、推力発生器駆動源が故障した場合に、航走体の周囲の流体（水、空気、燃料等）が航走体に及ぼす力やモーメントが大きく変化することとなる。その結果、航走体の姿勢を安定に保つことができないという問題がある。例えば、水中航走体が海中で目標に向かって移動しているときに、何らかの理由で１つの姿勢制御舵を駆動するアクチュエータが動作しなくなった場合には、正常に動作するアクチュエータが残っていたとしても、目標へ向かう軌道（航路）をはずれてしまい、目標へ到達することができなくなる。
【０００６】
本発明は、前記した問題に鑑み創案されたものであり、推力発生器駆動源が故障し、かつ、正常に動作する推力発生器駆動源が残っている場合においても、航走体の姿勢を安定に保つことのできる航走体の安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
前記課題を解決するため、本発明の請求項１に記載の航走体の安定ロバスト制御装置は、航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の安定ロバスト制御装置であって、前記推力発生器駆動源の故障を検知する故障検知手段と、前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段とを備えることを特徴とする。
【０００８】
かかる構成によれば、航走体の安定ロバスト制御装置は、推力発生器駆動源が故障していない場合には、各推力発生器駆動源に対して、制御力および制御モーメントを配分し、推力発生器駆動源が故障している場合には、故障しているものを除いた残りの推力発生器駆動源に対して、制御力および制御モーメントを再配分する。
【０００９】
また、請求項２に記載の航走体の安定ロバスト制御装置は、請求項１に記載の航走体の安定ロバスト制御装置において、前記航走体の姿勢に関する情報を検知するセンサにて検出した検出値と予め定められた目標値との偏差を算出する偏差算出手段と、前記航走体の姿勢制御用パラメータとして、前記偏差に基づいて前記航走体に付与する制御力および制御モーメントを算出する自動制御パラメータ算出手段とをさらに備えることを特徴とする。
【００１０】
かかる構成によれば、航走体の安定ロバスト制御装置は、センサ検出値と目標値との偏差に基づいて、航走体に付与する制御力および制御モーメントを自動的に算出することができる。ここで、自動制御パラメータ算出手段は、例えば、ＰＩＤ制御により制御力および制御モーメントを算出し、算出結果に対応した制御コマンドを配分量算出手段に出力する。
【００１１】
また、請求項３に記載の航走体の安定ロバスト制御装置は、請求項１に記載の航走体の安定ロバスト制御装置において、前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として前記制御力および制御モーメントを算出する手動制御パラメータ算出手段をさらに備え、前記配分量算出手段が、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする。
【００１２】
かかる構成によれば、航走体の安定ロバスト制御装置は、操作者によって入力された操作コマンドを変換則に基づいて変換し、その変換結果を、航走体に付与する制御力および制御モーメントとして算出することができる。ここで、手動制御パラメータ算出手段は、変換結果に対応した制御コマンドを配分量算出手段に出力する。
【００１３】
また、請求項４に記載の航走体の安定ロバスト制御装置は、請求項２に記載の航走体の安定ロバスト制御装置において、前記推力発生器駆動源に対する操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として制御力および制御モーメントを算出する手動制御パラメータ算出手段と、前記自動制御パラメータ算出手段で算出された制御力および制御モーメントと、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントとを加算する加算手段とをさらに備え、前記配分量算出手段が、前記加算手段で加算された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする。
【００１４】
かかる構成によれば、航走体の安定ロバスト制御装置は、操作者によって入力された操作コマンドに基づく手動制御の結果と、センサ検出値と目標値との偏差に基づく自動制御の結果とを加算した制御力および制御モーメントと配分則とに基づいて、配分量を算出する。したがって、操作コマンドを入力しない場合の制御と、入力する場合の制御とを行うことが可能となる。例えば、何も故障していない場合には、操作者が操作コマンドを入力しないようにして運用することが可能である。また、例えば、推力発生器駆動源が故障した場合に、一旦は、航走体の姿勢が崩れたり航路がそれたりする。このときに、操作コマンドを入力する運用を行うことで、自動制御の結果と手動制御の結果とが加算される。その結果、航走体が元の状態に復帰するまでに要する時間を短縮することが可能となる。さらに、例えば、センサが故障するなどの重大な故障が発生した場合に、操作者が操作コマンドを入力するように運用することが可能となるので、航走体の運用の信頼性を向上させることができる。
【００１５】
また、前記目的を達成するために、請求項５に記載の航走体の安定ロバスト制御システムは、請求項３または請求項４に記載の航走体の安定ロバスト制御装置と、推進手段と、複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、姿勢に関する情報を検知するセンサとを有する航走体と、前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置とを備えることを特徴とする。
【００１６】
かかる構成によれば、航走体の安定ロバスト制御システムは、操作コマンド入力装置を航走体に含むことで有人航走体を実現することが可能である。また、操作コマンド入力装置を航走体と別体とすることで無人航走体を実現することが可能である。
【００１７】
また、請求項６に記載の航走体の安定ロバスト制御システムは、請求項５に記載の航走体の安定ロバスト制御システムにおいて、前記操作コマンド入力装置から入力された操作コマンドを前記航走体に送信するコマンド送信装置と、前記送信された操作コマンドを受信するコマンド受信装置とをさらに備え、前記手動制御パラメータ算出手段が、前記受信された操作コマンドを前記変換則に基づいて変換することを特徴とする。
【００１８】
かかる構成によれば、航走体の安定ロバスト制御システムは、操作コマンド入力装置を航走体と別体とすることで無人航走体を実現することが可能である。ここで、操作コマンドを送受信する方法は、航走体が利用される外部の環境に応じて、音波を用いる方法、あるいは、無線または有線の通信による方法を含む。
【００１９】
また、前記目的を達成するために、請求項７に記載の航走体の安定ロバスト制御方法は、航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、安定ロバスト制御装置とを備えた航走体の安定ロバスト制御方法であって、前記安定ロバスト制御装置が、前記推力発生器駆動源の故障を検知する故障検知ステップと、前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出ステップとを有することを特徴とする。
【００２０】
かかる手順によれば、航走体の安定ロバスト制御装置は、推力発生器駆動源が故障していない場合には、各推力発生器駆動源に対して、制御力および制御モーメントを配分し、推力発生器駆動源が故障している場合には、故障しているものを除いた残りの推力発生器駆動源に対して、制御力および制御モーメントを再配分する。
【００２１】
また、請求項８に記載の航走体の安定ロバスト制御プログラムは、航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の姿勢を安定に保つために、コンピュータを、前記推力発生器駆動源の故障を検知する故障検知手段、前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段、として機能させることを特徴とする。
【００２２】
かかる構成によれば、安定ロバスト制御プログラムは、推力発生器駆動源が故障していない場合には、各推力発生器駆動源に対して、制御力および制御モーメントを配分し、推力発生器駆動源が故障している場合には、故障しているものを除いた残りの推力発生器駆動源に対して、制御力および制御モーメントを再配分する。
【発明の効果】
【００２３】
請求項１、請求項７または請求項８に記載の発明によれば、推力発生器駆動源が故障し、かつ、正常に動作する推力発生器駆動源が残っている場合においても、航走体の姿勢を安定に保つことができる。
【００２４】
請求項２に記載の発明によれば、航走体に付与する制御力および制御モーメントを精度よく算出することができる。
【００２５】
請求項３に記載の発明によれば、操作者が操作コマンドを入力することで、航走体に所望の運動をさせることが可能となる。
【００２６】
請求項４、請求項５または請求項６に記載の発明によれば、航走体の運用の幅を広げ、信頼性を向上させることが可能となる。
【発明を実施するための最良の形態】
【００２７】
以下、図面を参照して本発明の航走体の安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラムを実施するための最良の形態（以下「実施形態」という）について詳細に説明する。以下の実施形態では、航走体の一例として、水中で利用される水中航走体を用いて説明することとする。
【００２８】
（第１実施形態）
本発明の第１実施形態に係る水中航走体の安定ロバスト制御システムについて、図１および図２を参照しながら説明する。第１実施形態に係る水中航走体の安定ロバスト制御システムは、水中航走体で実現できる。
【００２９】
［安定ロバスト制御システムの構成］
図１は、第１実施形態の安定ロバスト制御システムの外観図であって、（ａ）は概略平面図、（ｂ）は概略左側面図、（ｃ）は概略背面図をそれぞれ示している。図２は、図１に示した安定ロバスト制御システムの構成図である。
【００３０】
水中航走体（安定ロバスト制御システム）１は、図１および図２に示すように、船体２と、推進器３と、姿勢制御舵４と、プロペラ駆動源５と、推進制御装置６と、舵駆動源７と、センサ８と、安定ロバスト制御装置１０とを主たる要素として備えている。
【００３１】
推進器３は、船体２に推進力を付与するものであり、プロペラ３ａを備え、船体２の後端部に配置されている。
【００３２】
姿勢制御舵４は、水中航走体（航走体）１の姿勢を安定に保つための制御を行うものである。この姿勢制御舵４は、断面視翼型を有する板状の部材であり、図１（ｃ）に示すように、船体２の後部から４５°，１３５°，２２５°，３１５°の方向に沿ってそれぞれ一枚ずつ、放射状（Ｘ字状）に合計四枚設けられている。また、姿勢制御舵４の後縁部には、舵駆動源７により駆動される補助翼（推力発生器）４ａが設けられている。本実施形態では、この補助翼４ａが水中航走体１の姿勢を安定に保つための制御を実質的に行うように構成した。なお、補助翼４ａを設けることなく、姿勢制御舵４そのものを舵駆動源７により駆動することで、水中航走体１の姿勢を安定に保つための制御を行うように構成してもよい。この場合には、姿勢制御舵４そのものが推力発生器として機能する。
【００３３】
プロペラ駆動源５は、推進制御装置６から出力される制御信号に基づいて、プロペラ３ａを正回転または逆回転させるように駆動するものである。プロペラ駆動源５は、例えば、バッテリー駆動の電動モータ等から構成される。
推進制御装置６は、所定のプログラムにしたがって、センサ８の出力に基づいて、目標地点に到達したか否かを判別し、水中航走体１を前進または後進させるために、プロペラ３ａを正回転または逆回転させる制御信号をプロペラ駆動源５に出力する。
【００３４】
舵駆動源（推力発生器駆動源）７は、安定ロバスト制御装置１０から出力される制御信号（舵角指令信号）に基づいて、姿勢制御舵４の補助翼４ａを駆動するものであり、例えば、バッテリー駆動のアクチュエータ等から構成される。この舵駆動源７は、補助翼４ａを、図１（ｃ）に示す船体２の後部から４５°，１３５°，２２５°，３１５°の方向とそれぞれ平行な軸線まわりに回動させる。安定ロバスト制御装置１０から出力される舵角指令信号は、姿勢制御舵４の４つの補助翼４ａをそれぞれ、適宜必要な方向に必要な角度だけ回動させるためのものである。これらの信号によって、水中航走体１が潜行（下降）、浮上（上昇）、進行方向左方に変針（左回頭）、および進行方向右方に変針（右回頭）することができる。
【００３５】
なお、図示は省略するが、水中航走体１内部には、姿勢角（ピッチ角）を調整したり、重力に対する浮力を大きく（または小さく）したりするために、例えば、バラスト水の注排水装置等の浮量調整装置や、重錘を移動させてその重心位置を変化させる重錘移動装置等が格納されている。
【００３６】
センサ８は、水中航走体１の運動情報を検知し、検知した運動情報を示すセンサ信号を安定ロバスト制御装置１０に出力するものであり、速度計８ａと、加速度計８ｂと、ジャイロセンサ８ｃとを備える。
速度計８ａは、例えば、統合航海システム（ＩＮＳ：Integrated Navigation System）を用いたものであり、水中航走体１の現在の位置（例えば、経度、緯度、深度）や水中航走体１の速度を計測するものである。
加速度計８ｂは、水中航走体１の加速度を計測するものである。
ジャイロセンサ８ｃは、水中航走体１の姿勢（ロール角、ピッチ角、ヨー角）を計測するものである。
【００３７】
本実施形態では、式（１）に示すように、（Ｘ，Ｙ，Ｚ）で、水中航走体１の現在の位置を表し、式（２）に示すように、（φ，θ，Ψ）で、水中航走体１の現在の姿勢として、ロール角、ピッチ角、ヨー角をそれぞれ表すこととする。
【００３８】
【数１】
【００３９】
安定ロバスト制御装置１０は、姿勢制御舵４の補助翼４ａを駆動する舵駆動源７に舵角指令信号を出力するものである。この安定ロバスト制御装置１０は、例えば、ＣＰＵ（Central Processing Unit）と、ＲＡＭ（Random Access Memory）と、ＲＯＭ（Read Only Memory）と、ＨＤＤ（Hard Disk Drive）と、入出力インタフェース等から構成され、図２に示すように、故障検知手段１１と、記憶手段１２と、目標値管理手段１３と、偏差算出手段１４と、自動制御パラメータ算出手段１５と、配分量算出手段１６とを備えている。
【００４０】
故障検知手段１１は、舵駆動源７の故障を検知するものである。この故障検知手段１１は、４つの舵駆動源７のうちのどれが故障したのかを示す信号を配分量算出手段１６に出力する。本実施形態では、舵駆動源７の識別変数を「ｉ（＝１，２，３，４）」で表し、故障を検知したときに、その舵駆動源を識別する識別変数ｉの値を出力するものとする。
【００４１】
また、故障を検知する方法は、公知の技術を利用することができる。本実施形態では、舵駆動源７からの出力信号の電流値（または電圧値）を測定して、この測定値が所定のしきい値よりも小さくなったか否かを判別し、所定のしきい値よりも小さくなった場合に、その出力信号を出力している舵駆動源７が故障したものとみなすこととした。例えば、センサ８として、舵駆動源７の回転角の変化を検出するタコジェネレータを備え、この検出値が、１秒間の変化角度が３°であることを目標値として、２．８°〜３．２°であれば正常と判別し、上限値以上または下限値以下であれば故障であるものとすることが可能である。あるいは、しきい値を設定する代わりに、目標値からの誤差が目標値の１０％以下である場合に正常であると判別するようにしてもよい。なお、故障検知手段１１は、故障したか否かを連続的に判別するようにしてもよいし、一定間隔（例えば、50msごと）で判別を繰り返すようにしてもよい。
【００４２】
記憶手段１２は、例えば、ＲＡＭと、ＲＯＭと、ＨＤＤとを備えている。本実施形態では、記憶手段１２は、目標値記憶部１２１と、配分則記憶部１２２とを備えている。これらは、例えば、ＨＤＤから構成される。
【００４３】
目標値記憶部１２１は、予め設定された目標値を記憶するものである。この設定された目標値は、水中航走体１の目標とする未来の目標地点（目標位置）と、水中航走体１の目標とする未来の姿勢（目標姿勢）とからなる。本実施形態では、式（３）に示すように、（Ｘ〜，Ｙ〜，Ｚ〜）で水中航走体１の目標位置を表す。ここで、記号「〜（チルダ）」は、それぞれ直前の文字の上に付される記号を示し、本明細書では、以下、記号「〜（チルダ）」を同様な意味で使用する。また、式（４）に示すように、（φ〜，θ〜，Ψ〜）で、水中航走体１の目標姿勢として、ロール角、ピッチ角、ヨー角をそれぞれ表すこととする。
【００４４】
【数２】
【００４５】
配分則記憶部１２２は、水中航走体１に付与する制御力および制御モーメントを、各舵駆動源７に、配分するためのルールである配分則を記憶するものである。本実施形態では、配分則は、例えば、線形計画法を利用するアルゴリズムによって構築されている。
【００４６】
目標値管理手段１３は、目標値記憶部１２１から目標値を読み出して、偏差算出手段１４に出力するものである。
偏差算出手段１４は、センサ８にて検出した検出値と、目標値記憶部１２１から読み出された目標値との偏差を式（５）および式（６）に基づいて算出するものであり、例えば、減算器から構成される。この偏差算出手段１４は、算出結果を示す信号を自動制御パラメータ算出手段１５に出力する。
【００４７】
【数３】
【００４８】
自動制御パラメータ算出手段１５は、センサ８の検出値と目標値との偏差を示す入力信号に基づいて、水中航走体１の姿勢制御用パラメータとして、水中航走体１の船体２に付与する制御力および制御モーメントを算出するものである。この自動制御パラメータ算出手段１５は、算出結果から制御力コマンドおよび制御モーメントコマンド（総称して制御コマンドという）を生成し、配分量算出手段１６に出力する。
【００４９】
本実施形態では、自動制御パラメータ算出手段１５は、ＰＩＤ制御に基づいて制御コマンドを生成する。具体的には、自動制御パラメータ算出手段１５は、力コマンド（ＦXC，ＦYC，ＦZC）のＸ成分、Ｙ成分およびＺ成分を、式（７）〜式（９）に基づいてそれぞれ算出する。ここで、制御力コマンド（ＦXC，ＦYC，ＦZC）には、制御力が記述されるので、式（７）〜式（９）は、制御力の各成分を示すこととなる。また、自動制御パラメータ算出手段１５は、制御モーメントコマンド（ＮXC，ＮYC，ＮZC）のＸ成分、Ｙ成分およびＺ成分を、式（１０）〜式（１２）に基づいてそれぞれ算出する。ここで、制御モーメントコマンド（ＮXC，ＮYC，ＮZC）には、制御モーメントが記述されるので、式（10）〜式（12）は、制御モーメントの各成分を示すこととなる。また、前記した式（７）〜式（12）において、ｅは、式（５）および式（６）に示した偏差、Ｐは比例ゲイン、Ｉは積分ゲイン、Ｄは微分ゲインを示している。
【００５０】
【数４】
【００５１】
配分量算出手段１６は、入力された制御コマンドで示される制御力および制御モーメントを、予め定められた配分則に基づいて、舵駆動源７に対して配分する配分量を算出し、演算結果を示す制御信号を各舵駆動源７に出力するものである。この配分量算出手段１６は、舵駆動源７の故障を示す信号が入力された場合には、制御力および制御モーメントを、配分則に基づいて、故障した舵駆動源７を除いた残りの各舵駆動源７に対して配分する配分量を算出し、算出結果を示す制御信号を残りの各舵駆動源７に出力する。つまり、制御力および制御モーメントが再配分されることとなる。
【００５２】
具体的には、配分量算出手段１６は、式（13）〜式（18）に示す等式条件と、式（19）〜式（21）に示す不等式条件との下で、式（22）に示すＪ1の値が最小となるように、各舵駆動源７に対して配分する配分量（ＦXi，ＦYi，ＦZi）を算出する。
【００５３】
【数５】
【００５４】
【数６】
【００５５】
【数７】
ただし、
Ｆi2＝ＦXi2＋ＦYi2＋ＦZi2 …式（23）
【００５６】
式（13）〜式（22）において、ｉは舵駆動源７の識別変数を示し、本実施形態では、ｉの取り得る値は、１，２，３，４である。また、式（13）〜式（22）において、ｎは舵駆動源７の個数を示し、本実施形態ではｎの値は４である。また、式（16）〜式（18）において、ｌXiは水中航走体１の重心から識別変数ｉの舵駆動源７までの距離に関するものであり、水中航走体１の重心を三次元座標空間の所定の原点に合致させたときに、重心と舵駆動源７とを結ぶ直線をＸ軸に投影したときのＸ軸上の距離を示し、同様にｌYiおよびｌZiは、Ｙ軸上の距離およびＺ軸上の距離をそれぞれ示す。
【００５７】
また、式（19）〜式（21）において、Ｆmiは、ｉ番目の舵駆動源７で発生される推力の大きさであり、この大きさ（絶対値）は、舵駆動源７の最大容量（最大移動量）で決定される（既知の値）。さらに、配分量算出手段１６は、舵駆動源７の故障を示す信号が入力された場合には、式（13）〜式（22）において、その入力信号で示される識別変数ｉの値についての力の成分や距離の値を「０」として計算する。
【００５８】
また、式（22）において、Ｊ1は、識別変数ｉの舵駆動源７によって、姿勢制御舵４の補助翼４ａを駆動するエネルギーに相当し、ｆ1は、式（23）で定義されたＦi2についての所定の評価関数を示す。つまり、配分量算出手段１６は、式（22）の左辺のＪ1の値が最小となるように、配分量（ＦXi，ＦYi，ＦZi）を変化させながら、式（22）の右辺のＦi2の値を求め、そのときの配分量（ＦXi，ＦYi，ＦZi）を求める。式（22）で示される最小値問題の方程式は、例えば、ラグランジェ未定乗数法等の線形計画法を用いて以下のようにして計算することが可能である。この方法として、例えば、特開平２−１０６４９２号公報（特許文献２）に開示された方法を利用することができる。具体的には、マトリックス形式で記述された式（24）〜式（27）に示す６元連立一次方程式を解けばよい。
【００５９】
Ａλ＝ｂ …式（24）
【００６０】
【数８】
【００６１】
【数９】
【００６２】
【数１０】
【００６３】
ここで、式（24）に示すＡは、式（25）に示すように６×６の正方行列である。また、式（24）に示すλは、ラグランジェ乗数であり、式（26）に示すように１行６列の行列である。また、式（24）に示すｂは、前記した式（13）〜式（18）に示した制御コマンドであり、式（26）に示すように１行６列の行列である。なお、式（27）では、行列中において、制御モーメントコマンドを、前記した式（18）に示すＮZC、式（17）に示すＮYC、式（16）に示すＮXCの順序に配置することとした。式（24）〜式（27）に示す６元連立一次方程式を解くことによって、ラグランジェ乗数λ1〜λ6の値を求めることができる。一方、前記した式（22）の左辺のＪ1の値が最小となるときに、識別変数ｉの舵駆動源７が発生する推力のＸ成分、Ｙ成分およびＺ成分は、式（28）〜式（30）で表すことができる。したがって、式（28）〜式（30）の各右辺に含まれるラグランジェ乗数λ1〜λ6に対して、６元連立一次方程式を解くことによって求めた値を代入することによって、配分量（ＦXi，ＦYi，ＦZi）を決定することができる。
【００６４】
【数１１】
【００６５】
ここで、決定した推力の配分量（ＦXi，ＦYi，ＦZi）と、舵駆動源７の移動量との対応関係式（または対応関係テーブル）は、予め求められて記憶手段１２に記憶されており、配分量算出手段１６は、当該対応関係式（または対応関係テーブル）に基づいて、舵駆動源７の移動量を求め、求めた移動量を示す制御信号を該当する舵駆動源７に出力する。なお、決定した推力の配分量（ＦXi，ＦYi，ＦZi）の値をそのまま舵駆動源７の移動量として出力するようにしてもよい。
【００６６】
なお、故障検知手段 １１、目標値管理手段１３、偏差算出手段１４、自動制御パラメータ算出手段１５および配分量算出手段１６は、ＣＰＵが記憶手段１２のＨＤＤ等に格納された所定のプログラムをＲＡＭに展開して実行することにより実現することができる。
【００６７】
［安定ロバスト制御システムの動作］
図２に示した安定ロバスト制御システム（水中航走体）１の動作について、安定ロバスト制御装置１０の動作を中心に図３を参照（適宜図２参照）して説明する。図３は、図２に示した安定ロバスト制御システムの動作を示すフローチャートである。ここでは、水中航走体１が目標地点へ移動するときに、移動中の姿勢を制御する場合を想定している。また、以下のステップＳ１およびステップＳ２は、推進制御装置６の処理を示し、ステップＳ３ないしステップＳ７は、安定ロバスト制御装置１０の処理を示している。
【００６８】
まず、安定ロバスト制御システム１は、推進制御装置６によって、プロペラ３ａを回転させる制御信号をプロペラ駆動源５に出力し、プロペラ駆動源５によって、プロペラ３ａを駆動する（ステップＳ１）。そして、推進制御装置６は、目標地点に到達したか否かを判別する（ステップＳ２）。目標地点に到達した場合（ステップＳ２：Ｙｅｓ）、安定ロバスト制御システム１は、処理を終了する。一方、目標地点に到達していない場合（ステップＳ２：Ｎｏ）、安定ロバスト制御システム１では、推進制御装置６が、引き続きプロペラ３ａを駆動し続ける。
【００６９】
また、安定ロバスト制御装置１０が、偏差算出手段１４によって、姿勢制御に関する目標値とセンサ検出値との偏差を算出し（ステップＳ３）、自動制御パラメータ算出手段１５によって、目標値とセンサ検出値との偏差に基づいて、水中航走体１に付与する制御力および制御モーメントを算出する（ステップＳ４）。続いて、安定ロバスト制御装置１０は、故障検知手段１１によって、舵駆動源７が故障したか否かを判別する（ステップＳ５）。舵駆動源７が故障している場合（ステップＳ５：Ｙｅｓ）、安定ロバスト制御装置１０は、配分量算出手段１６によって、故障中の舵駆動源７を制御対象から除外し（ステップＳ６）、その上で、制御力および制御モーメントを配分則に基づいて残りの舵駆動源７に対して配分する（ステップＳ７）。各舵駆動源７は、各配分量に対応した移動量で作動する。これにより、姿勢制御舵４の補助翼４ａがそれぞれ回動する。その結果、水中航走体１の姿勢を制御することができる。
【００７０】
一方、ステップＳ５において、舵駆動源７が正常である場合（ステップＳ５：Ｎｏ）、安定ロバスト制御装置１０は、配分量算出手段１６によって、ステップＳ６をスキップしてステップＳ７に進む。この場合には、配分量算出手段１６は、制御力および制御モーメントを配分則に基づいてすべての舵駆動源７に対して配分する。ステップＳ７に続いて、安定ロバスト制御システム１は、ステップＳ２に戻る。
【００７１】
なお、安定ロバスト制御装置１０は、一般的なコンピュータに、前記した各ステップを実行させる安定ロバスト制御プログラムを実行することで実現することもできる。このプログラムは、通信回線を介して配布することも可能であるし、ＣＤ−ＲＯＭ等の記録媒体に書き込んで配布することも可能である。
【００７２】
第１実施形態に係る安定ロバスト制御システム１によれば、水中航走体１の舵駆動源７に故障が発生し、正常に動作する舵駆動源７が残っている場合においても、水中航走体１の姿勢を安定に保つことができる。また、残っている舵駆動源７を、その時点で最小のエネルギーで駆動させることができる。その結果、水中航走体１の信頼性を向上させることができる。
【００７３】
（第２実施形態）
［安定ロバスト制御システムの構成］
図４は、第２実施形態の安定ロバスト制御システムの構成図である。安定ロバスト制御システム１Ａは、図４に示すように、水中航走体１００と、監視装置２００とを備えている。水中航走体１００は、コマンド受信装置２０を備え、安定ロバスト制御装置１０Ａの機能が異なる点を除いて、図２に示した水中航走体１と同一の構成なので、同一の構成には同一の符号を付し、説明を省略する。
【００７４】
コマンド受信装置２０は、監視装置２００から音波によって送られる操作コマンドを受信し、安定ロバスト制御装置１０Ａに出力するものである。
安定ロバスト制御装置１０Ａは、記憶手段１２に配分則記憶部１２２と、変換則記憶部１２３とを備えると共に、手動制御パラメータ算出手段１７を備えている。
【００７５】
変換則記憶部１２３は、コマンド受信装置２０で受信する操作コマンドを、水中航走体１００に付与する制御力および制御モーメントに変換するための変換則を記憶するものであり、例えば、メモリやハードディスク等から構成される。
【００７６】
手動制御パラメータ算出手段１７は、コマンド受信装置２０で受信する操作コマンドを、変換則記憶部１２３に記憶された変換則に基づいて変換し、変換結果として水中航走体１００に付与する制御力および制御モーメントを算出するものである。この手動制御パラメータ算出手段１７は、制御力および制御モーメントに対応する制御コマンドを配分量算出手段１６に出力する。本実施形態では、手動制御パラメータ算出手段１７は、操作コマンドから、入力時に入力された角度と方向とを検出し、検出した角度を力の大きさに変換すると共に、検出した方向を力の向きに変換する。
【００７７】
監視装置２００は、水中航走体１００の現在位置を監視し、操作コマンドを水中航走体１００に送信することで、水中航走体１００を遠隔操作するものである。この監視装置２００は、例えば、ＣＰＵと、ＲＡＭと、ＲＯＭと、ＨＤＤと、入出力インタフェース等から構成され、図４に示すように、航走体検知装置２０１と、表示装置２０２と、コマンド入力装置２０３と、コマンド送信装置２０４とを備えている。
【００７８】
航走体検知装置２０１は、音波の反響を測定することにより、水中航走体１００の位置を測定するものであり、例えば、アクティブソナーやパッシブソナーから構成される。
表示装置２０２は、水中航走体１００の位置を表示するものであり、例えば、ＣＲＴ（Cathode Ray Tube）、液晶ディスプレイ（ＬＣＤ：Liquid Crystal Display）、ＰＤＰ（Plasma Display Panel）、ＥＬ（Electronic Luminescence）等から構成される。
【００７９】
コマンド入力装置（操作コマンド入力装置）２０３は、水中航走体１００の姿勢制御用パラメータに対応した操作コマンドを入力するものであり、例えば、コンピュータ一般に用いられるマウスやキーボード、レバーやボタンを組み合わせた遠隔操縦装置、ジョイスティック等から構成される。本実施形態では、コマンド入力装置２０３は、ジョイスティックで構成されることとした。
コマンド送信装置２０４は、コマンド入力装置２０３から入力された操作コマンドを音波によって水中航走体１００に送信するものである。
【００８０】
次に、コマンド入力装置２０３による操作コマンドの入力方法について図５を参照して説明する。図５は、図４に示したコマンド入力装置の説明図である。コマンド入力装置２０３は、図５（ａ）に示すように、直立したジョイスティックレバーを倒したときに、倒れたジョイスティックレバーの向く方向と鉛直方向との間の角度によって、制御力の大きさを表す。例えば、図５（ａ）に示すように、「７０°」の位置まで倒した場合の制御力の大きさは、「３５°」の位置まで倒した場合の制御力の大きさの２倍となる。
【００８１】
また、コマンド入力装置２０３は、図５（ｂ）に示すように、直立したジョイスティックレバーを倒したときに、倒れたジョイスティックレバーの向く方向を水平面に投影した場合の方向と水平面内の基準方向との間の角度によって、制御力の向きを表す。例えば、図５（ｂ）に示すようにジョイスティックレバーを倒した場合には、進行方向右を基準としたときに、進行方向右から前方に向かって「４０°」の方向を指すこととなる。
【００８２】
図５に示したジョイスティック（コマンド入力装置２０３）は、水平面内において、前進、後退、左回頭、および右回頭を指定するためのものである。そこで、水中航走体１が下降および上昇するために、同様なジョイスティック（コマンド入力装置２０３）を設けてある。なお、入力する力の大きさと向きとを切り替えるモード切替手段を設けることで、１つのジョイスティック（コマンド入力装置２０３）によって、前進、後退、左回頭、右回頭、下降および上昇を指定するようにしてもよい。
【００８３】
さらに、水中航走体１の姿勢を制御するために制御モーメントを入力するためのジョイスティックを備えてもよい。例えば、図５に示したジョイスティック（コマンド入力装置２０３）を用いた場合には、直立したジョイスティックレバーを倒したときに、倒れたジョイスティックレバーの向く方向と鉛直方向との間の角度によってピッチ角を表し、倒れたジョイスティックレバーの向く方向を水平面に投影した場合の方向と水平面内の基準方向との間の角度によって、ヨー角を表す。なお、ジョイスティックレバーを逆方向に倒したときには角度の値の正負を逆転させる。また、ロール角を指定するために、同様なジョイスティック（コマンド入力装置２０３）を別に設けるか、１つのジョイスティックをモード切替できるように構成する。
【００８４】
［安定ロバスト制御システムの動作］
図４に示した安定ロバスト制御システム１Ａの動作について、安定ロバスト制御装置１０Ａの動作を中心に図６を参照（適宜図４参照）して説明する。図６は、図４に示した安定ロバスト制御システムの動作を示すフローチャートである。以下のステップＳ２１およびステップＳ２２は、推進制御装置６の処理を示し、図３のフローチャートに示したステップＳ１およびステップＳ２の各処理と同様なので説明を省略する。また、以下のステップＳ２３〜ステップＳ２７は、安定ロバスト制御装置１０Ａの処理を示している。
【００８５】
ステップＳ２３において、安定ロバスト制御装置１０Ａは、手動制御パラメータ算出手段１７によって、操作コマンドを受信したか否かを判別する（ステップＳ２３）。操作コマンドを受信していない場合（ステップＳ２３：Ｎｏ）、安定ロバスト制御システム１Ａは、ステップＳ２２に戻る。すなわち、目標地点に到達したか否かを判別する。一方、操作コマンドを受信した場合（ステップＳ２３：Ｙｅｓ）、安定ロバスト制御装置１０Ａは、手動制御パラメータ算出手段１７によって、変換則に基づいて、水中航走体１００に付与する制御力および制御モーメントを算出する（ステップＳ２４）。以下のステップＳ２５〜ステップＳ２７の各処理は、図３のフローチャートに示したステップＳ５〜ステップＳ７の各処理と同様なので説明を省略する。なお、ステップＳ２３〜ステップＳ２７の一連の処理を手動制御という。
【００８６】
第２実施形態に係る安定ロバスト制御システム１Ａによれば、操作者によって入力された操作コマンドを変換則に基づいて変換し、その変換結果を、水中航走体１００に付与する制御力および制御モーメントとして算出することができる。したがって、水中航走体１００を監視できる場合に、水中航走体１００を操作者の所望の姿勢になるように安定的に制御することができる。
【００８７】
（第３実施形態）
［安定ロバスト制御システムの構成］
図７は、第３実施形態の安定ロバスト制御システムの構成図である。安定ロバスト制御システム１Ｂは、水中航走体１００Ｂと監視装置２００（図７では図示せず、図６参照）とを備えている。水中航走体１００Ｂは、安定ロバスト制御装置１０Ｂの機能が異なる点を除いて、図４に示した水中航走体１００と同一の構成なので、同一の構成には同一の符号を付し、説明を省略する。
【００８８】
安定ロバスト制御装置１０Ｂは、故障検知手段１１Ｂと、記憶手段１２と、目標値管理手段１３と、偏差算出手段１４と、自動制御パラメータ算出手段１５と、配分量算出手段１６と、手動制御パラメータ算出手段１７と、切替手段１８と、加算手段１９とを備えている。ここで、故障検知手段１１Ｂ、切替手段１８および加算手段１９以外の構成は、図２または図４と同一の構成なので同一の符号を付して説明を省略する。
【００８９】
故障検知手段１１Ｂは、舵駆動源７の故障を検知すると共に、センサ８の故障を検知するものである。この故障検知手段１１Ｂは、センサ８の故障のレベルを判定し、判定結果に応じた信号を切替手段１８に出力する。
【００９０】
本実施形態では、故障検知手段１１Ｂは、通常、切替手段１８にオン信号を出力する。そして、故障検知手段１１Ｂは、レベル１の故障であると判別した場合には、切替手段１８にオフ信号を出力する。ここで、レベル１の故障とは、水中航走体１００Ｂの動きを安定化させるための制御を、自動制御から手動制御に切り替える必要のある重大な故障である。例えば、センサ８が完全に機能しなくなるような故障を起こした場合を示す。
【００９１】
また、故障検知手段１１Ｂは、レベル２の故障であると判別した場合には、切替手段１８にオン信号を出力し続けると共に、検知した故障情報を、配分量算出手段１６に出力する。ここで、レベル２の故障とは、水中航走体１００Ｂの動きを安定化させるための制御として、手動制御に完全に切り替える必要のない軽微な故障である。例えば、センサ８が機能はするが、設定どおりの性能を果たさなくなった場合や、舵駆動源７が故障した場合を示す。
【００９２】
切替手段１８は、故障検知手段１１Ｂからのオン信号によって、自動制御パラメータ算出手段１５から配分量算出手段１６へ向かう入力信号を通過させるようにスイッチを切り替えるものである。また、切替手段１８は、故障検知手段１１Ｂからのオフ信号によって、自動制御パラメータ算出手段１５から配分量算出手段１６へ向かう入力信号を遮断させるようにスイッチを切り替える。
【００９３】
加算手段１９は、切替手段１８を介して自動制御パラメータ算出手段１５から入力する信号と、手動制御パラメータ算出手段１７から入力する信号とを加算して配分量算出手段１６に出力するものである。
【００９４】
前記した構成によって、安定ロバスト制御装置１０Ｂは、故障検知手段１１Ｂによる故障レベルの判別結果に基づいて、手動制御パラメータ算出手段１７で制御コマンドを生成するか、あるいは、自動制御パラメータ算出手段１５と手動制御パラメータ算出手段１７とを併用して制御コマンドを生成することとなる。このうち、自動制御パラメータ算出手段１５と手動制御パラメータ算出手段１７とを併用して制御コマンドを生成するようにした場合には、コマンド受信装置２０が操作コマンドを受信しなければ、手動制御パラメータ算出手段１７の生成する制御コマンドが実質的に「０」になり、加算手段１９の出力信号は、実質的に、自動制御パラメータ算出手段１５の生成する制御コマンドと等しくなってしまう。
【００９５】
つまり、安定ロバスト制御装置１０Ｂが、自動制御パラメータ算出手段１５と手動制御パラメータ算出手段１７とを併用して制御コマンドを生成するようにしていたとしても、故障が同じ条件のときに、以下に示すように、複数通りの運用が考えられる。
【００９６】
例えば、舵駆動源７が正常であり、かつ、センサ８が軽微な故障をした場合を想定する。この場合には、自動制御のみで対応し、故障の影響が甚大になってきたときにだけ、監視装置２００から操作コマンドを送信するような運用が可能である（これを実質的自動制御という）。
【００９７】
また、センサ８が正常であり、かつ、舵駆動源７が故障した場合には、操作コマンドを全く送信せずに自動制御で対応することが可能である（これを完全自動制御という）。完全自動制御は、第１実施形態の場合に相当する。また、この場合には、実質的自動制御で運用することも可能である。あるいは、操作コマンドを常に送信することが可能である（これを手動併用制御という）。特に、手動併用制御の運用を行う場合には、水中航走体１００Ｂが元の状態に復帰するまでに要する時間を大幅に短縮することが可能となる。
【００９８】
このような複数通りの運用ができる理由は、本実施形態では、監視装置２００の操作者が、監視装置２００の表示装置２０２（図４参照）に表示された水中航走体１００Ｂの運動軌跡に基づいて、センサ８の故障が重大であるか、あるいは、軽微であるかの最終的な判断をするからである。なお、監視装置２００において、表示装置２０２（図４参照）に表示された水中航走体１００Ｂの運動軌跡と、目標とする運動軌跡との偏差に基づいて、運用のレベル（完全自動制御、実質自動制御、手動併用制御）を自動的に判別するように構成してもよい。そして、判別した運用のレベルに応じた照明、ブザー、アナウンス等の報知手段によって、操作者に対して、操作コマンドの送信が必要か否かを報知するようにしてもよい。
【００９９】
［安定ロバスト制御システムの動作］
図７に示した安定ロバスト制御システム１Ｂの動作について、安定ロバスト制御装置１０Ｂの動作を中心に図８を参照（適宜図７参照）して説明する。図８は、図７に示した安定ロバスト制御システムの動作を示すフローチャートである。以下のステップＳ４１、Ｓ４２およびＳ５１は、推進制御装置６の処理を示し、このうち、ステップＳ４１、Ｓ４２は、図３のフローチャートに示したステップＳ１、Ｓ２の各処理と同様なので説明を省略する。また、以下のステップＳ４３〜ステップＳ５０は、安定ロバスト制御装置１０Ｂの処理を示している。このうち、ステップＳ４３、Ｓ４４、および、ステップＳ４６〜Ｓ４８の各処理は、図３のフローチャートに示したステップＳ３〜ステップＳ７の各処理と同様なので説明を省略する。
【０１００】
安定ロバスト制御装置１０Ｂは、ステップＳ４４で制御力および制御モーメントを算出した後、ステップＳ４５において、故障検知手段１１Ｂによって、センサ８の故障のレベルを判別する。センサ８が故障していないか、または、故障レベルがレベル２の場合（ステップＳ４５：Ｎｏ）、ステップＳ４６〜Ｓ４８を実行する。一方、センサ８の故障レベルがレベル１である場合（ステップＳ４５：Ｙｅｓ）、故障検知手段１１Ｂは、切替手段１８にオフ信号を出力する。これによって、切替手段１８は、自動制御パラメータ算出手段１５から配分量算出手段１６へ向かう入力信号を遮断させるようにスイッチを切り替える。すなわち、手動に切り替える（ステップＳ４９）。
【０１０１】
そして、安定ロバスト制御装置１０Ｂは、手動制御パラメータ算出手段１７および配分量算出手段１６によって、手動制御を行う（ステップＳ５０）。このステップＳ５０の処理は、図６のフローチャートに示したステップＳ２３〜ステップＳ２７の一連の処理を指している。ステップＳ５０に続いて、安定ロバスト制御システム１Ｂは、推進制御装置６によって、目標地点に到達したか否かを判別する（ステップＳ５１）。目標地点に到達した場合（ステップＳ５１：Ｙｅｓ）、安定ロバスト制御システム１Ｂは、処理を終了する。目標地点に到達していない場合（ステップＳ５２：Ｎｏ）、安定ロバスト制御システム１Ｂでは、安定ロバスト制御装置１０ＢがステップＳ５０に戻る。
【０１０２】
第３実施形態に係る安定ロバスト制御システム１Ｂによれば、操作者によって入力された操作コマンドに基づく手動制御の結果と、センサ８の検出値と目標値との偏差に基づく自動制御の結果とを加算した制御力および制御モーメントと配分則とに基づいて、配分量を算出する。したがって、操作コマンドを入力しない場合の制御と、入力する場合の制御とを行うことが可能となる。つまり運用の幅が広がり、水中航走体１００Ｂの運用の信頼性を向上させることができる。
【０１０３】
（第４実施形態）
第４実施形態に係る水中航走体の安定ロバスト制御システムは、その構成および動作を、前記した第１実施形態と同様なものとすることができる。ただし、前記した第１実施形態では、配分量算出手段１６は、式（22）で示される最小値問題の方程式を、ラグランジェ未定乗数法を用いて計算するものとしたが、本実施形態では、ペナルティ法等の非線形計画法を用いて計算するものとする。したがって、構成を示す構成図および動作を示すフローチャートを省略し、必要な場合は図１ないし図３を参照して、配分量算出手段１６が計算に利用する非線形計画法のアルゴリズムを説明する。
【０１０４】
まず、式（31）で示すＪ2と、式（32）で示すＪ3を導入する。
【０１０５】
【数１２】
【０１０６】
【数１３】
【０１０７】
Ｊ2は、正常なすべての舵駆動源７によって、姿勢制御舵４の補助翼４ａを駆動するエネルギーに相当し、ｆ2は、ｘについてのコスト関数（第１評価関数）を示す。ここで、ｘは、水中航走体１の重心を始点とする制御力のベクトルを示す変数であり、その絶対値（ベクトルの距離）が最小となるものを探すための指標となるものである。
また、Ｊ3は、Ｊ2を変換したものであり、Ｆ（ｘ,ｒ,ｔ）は、ｘ，ｒ，ｔについてのトータルコスト関数（第２評価関数）を示す。ｒとｔは、それぞれ可変パラメータである。
式（31）および式（32）において、ｉは舵駆動源７の識別変数を示し、ｉの取り得る値は、１〜４である。また、ｎは舵駆動源７の個数を示し、本実施形態では４である。
【０１０８】
式（32）において、Ｈj（ｘ）は等式条件の数値である。なお、ｊ＝１，２，…，ｐである。本実施形態では、ｐ＝３である。そのため、Ｈj（ｘ）は、具体的には、式（33）〜式（35）で表される。これらの式（33）〜式（35）は、前記した式（13）〜式（15）に対応している。式（33）〜式（35）の左辺を示すＨj（ｘ）は、それぞれ、計算をする上で便宜上おいたものであり、数値としては０（零）となる。
また、式（32）において、Ｇk（ｘ）は不等式条件の数値である。なお、ｋ＝１，２，…，ｑであり、本実施形態では、ｑ＝３である。同様に、Ｇk（ｘ）は、具体的には、式（36）〜式（38）で表される。これらの式（36）〜式（38）は、前記した式（16）〜式（18）に対応している。式（36）〜式（38）の左辺を示すＧk（ｘ）は、それぞれ、計算をする上で便宜上おいたものであり、数値としては０（零）となる。
【０１０９】
【数１４】
【０１１０】
式（32）の最右辺の第２項は、等式条件（Ｈj（ｘ））のペナルティ関数であり、式（39）のように展開できる。なお、式（39）では一般形で記述している。ここでは、力の３成分に対応してｐ＝３となる。
【０１１１】
【数１５】
【０１１２】
式（32）の最右辺の第３項は、不等式条件（Ｇk（ｘ））のペナルティ関数であり、式（40）のように展開できる。なお、式（40）では一般形で記述している。ここでは、モーメントを３軸まわりに展開してｑ＝３となる。
【０１１３】
【数１６】
【０１１４】
本実施形態では、配分量算出手段１６は、式（22）のＪ1の代わりに、式（32）で示したトータルコスト関数Ｆ（ｘ,ｒ,ｔ）の値が最小となるように、配分量（ＦXi，ＦYi，ＦZi）を変化させながら、制御力に相当する式（32）の最右辺の値を求め、そのときの配分量（ＦXi，ＦYi，ＦZi）をそれぞれ求める。そして、配分量算出手段１６は、式（32）で示される最小値問題の方程式を、例えば、公知の準ニュートン法を用いて計算することが可能である。
【０１１５】
第４実施形態に係る安定ロバスト制御システムによれば、配分量算出手段１６が舵駆動源７に対する制御力の配分量を算出する際に用いるコスト関数の段階で、故障中の舵駆動源７を制御対象から排除することができるので、舵駆動源７が正常な場合に必要な計算処理から、舵駆動源７が故障した場合に必要な計算処理へ切り替えるために必要な変更が、線形計画法を用いた場合よりも、容易になる。
【０１１６】
以上、本発明の各実施形態について説明したが、本発明はこれらに限定されるものではなく、その趣旨を変えない範囲で実施することができる。例えば、故障検知手段１１（１１Ｂ）は、舵駆動源７（およびセンサ８）の故障を検知するものとしたが、その他の構成要素の故障を検知するようにしてもよい。また、故障レベルの判定は、２段階（正常、故障）または３段階（正常、レベル１、レベル２）としたが、４段階以上であってもよい。
また、監視装置２００は、水中航走体１００（１００Ｂ）と別体であるものとしたが、監視装置を水中航走体に設けて有人水中航走体を構成するようにしてもよい。
【０１１７】
各実施形態では、姿勢制御舵４をＸ字状に配置したが、独立駆動できる複数舵であれば、十字状あるいはＹ字状などの配置としてもよい。また、姿勢制御舵４の枚数は、２枚以上あればよい。また、姿勢制御舵４の後端部に配置された補助翼４ａが回動するものとしたが、姿勢制御舵４全体が回動するように構成してもよい。
【０１１８】
また、推力発生器の一例として姿勢制御舵４を用いて説明したが、これに限定されるものではなく、推力発生器は、例えば、スラスタやプロペラであってもよい。また、推力発生器駆動源の一例として舵駆動源７を用いて説明したが、推力発生器駆動源は、例えば、モータ等のスラスタ駆動源やプロペラ駆動源であってもよい。また、航走体として水中で利用される水中航走体を用いて説明したが、航走体は、空中で利用される飛行体（航空機、飛行船、人工衛星等）や陸上で僅かに浮上して走行可能なホバークラフト（hovercraft）でもよい。
【図面の簡単な説明】
【０１１９】
【図１】第１実施形態の安定ロバスト制御システムの外観図であって、（ａ）は概略平面図、（ｂ）は概略左側面図、（ｃ）は概略背面図を示している。
【図２】図１に示した安定ロバスト制御システムの構成図である。
【図３】図２に示した安定ロバスト制御システムの動作を示すフローチャートである。
【図４】第２実施形態の安定ロバスト制御システムの構成図である。
【図５】図４に示したコマンド入力装置の説明図である。
【図６】図４に示した安定ロバスト制御システムの動作を示すフローチャートである。
【図７】第３実施形態の安定ロバスト制御システムの構成図である。
【図８】図７に示した安定ロバスト制御システムの動作を示すフローチャートである。
【符号の説明】
【０１２０】
１（１Ａ，１Ｂ） 安定ロバスト制御システム
２ 船体
３ 推進器
３ａ プロペラ
４ 姿勢制御舵
４ａ 補助翼（推力発生器）
５ プロペラ駆動源
６ 推進制御装置
７ 舵駆動源（推力発生器駆動源）
８ センサ
８ａ 速度計
８ｂ 加速度計
８ｃ ジャイロセンサ
１０（１０Ａ，１０Ｂ） 安定ロバスト制御装置
１１（１１Ｂ） 故障検知手段
１２ 記憶手段
１２１ 目標値記憶部
１２２ 配分則記憶部
１２３ 変換則記憶部
１３ 目標値管理手段
１４ 偏差算出手段
１５ 自動制御パラメータ算出手段
１６ 配分量算出手段
１７ 手動制御パラメータ算出手段
１８ 切替手段
１９ 加算手段
２０ コマンド受信装置
１００（１００Ｂ） 水中航走体（航走体）
２００ 監視装置
２０１ 航走体検知装置
２０２ 表示装置
２０３ コマンド入力装置（操作コマンド入力装置）
２０４ コマンド送信装置
【技術分野】
【０００１】
本発明は、水中、空中、陸上等で利用される航走体の姿勢を安定に保つ制御を行う安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラムに関する。
【背景技術】
【０００２】
従来、水中で利用される航走体（水中航走体）や、空中で利用される航走体（飛行体）の姿勢を安定に保つ技術が知られている（例えば、特許文献１〜特許文献３参照）。
特許文献１に開示された水中航走体は、後端部に設けられたスクリューを推進器として水中を航走し、スクリューの上下近傍および左右近傍に設けられた４つの舵を４つのサーボモータでそれぞれ駆動させることで姿勢が制御される。この水中航走体は、制御手段によって、本体の左右方向への傾きを補正するヨー制御と、上下方向への傾きを補正するピッチ制御と、本体のローリングを補正するロール制御とを行うものであり、ロール制御を優先して各サーボモータを個別に制御する。
【０００３】
特許文献２に開示された技術は、ＰＩ制御で生成した制御コマンドに基づいて、航走体の複数の推力発生器（スラスタ、舵、プロペラ等）に対して、燃料消費が最小となるような最適な推力配分を行うものである。
【０００４】
特許文献３に開示された技術は、所定の定位置に静止した飛行船が風に流されないように、ＰＩＤ制御で生成した制御コマンドに基づいて、飛行船の複数のスラスタに対する駆動指令信号を生成し、飛行船を定位置に復帰させて保持する制御を行うものである。
【特許文献１】特開平６−３４１８５２号公報（段落０００８〜００２２、図４）
【特許文献２】特開平２−１０６４９２号公報（第２頁右上１４行目〜第５頁右上３行目、第３図）
【特許文献３】特開平１−３１１１９９７号公報（第２頁左上１３行目〜右下１２行目、第２図）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来の技術では、複数の推力発生器をそれぞれ駆動する複数の推力発生器駆動源（例えば、アクチュエータ）が正常に動作することを前提としている。そのため、推力発生器駆動源が故障した場合に、航走体の周囲の流体（水、空気、燃料等）が航走体に及ぼす力やモーメントが大きく変化することとなる。その結果、航走体の姿勢を安定に保つことができないという問題がある。例えば、水中航走体が海中で目標に向かって移動しているときに、何らかの理由で１つの姿勢制御舵を駆動するアクチュエータが動作しなくなった場合には、正常に動作するアクチュエータが残っていたとしても、目標へ向かう軌道（航路）をはずれてしまい、目標へ到達することができなくなる。
【０００６】
本発明は、前記した問題に鑑み創案されたものであり、推力発生器駆動源が故障し、かつ、正常に動作する推力発生器駆動源が残っている場合においても、航走体の姿勢を安定に保つことのできる航走体の安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
前記課題を解決するため、本発明の請求項１に記載の航走体の安定ロバスト制御装置は、航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の安定ロバスト制御装置であって、前記推力発生器駆動源の故障を検知する故障検知手段と、前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段とを備えることを特徴とする。
【０００８】
かかる構成によれば、航走体の安定ロバスト制御装置は、推力発生器駆動源が故障していない場合には、各推力発生器駆動源に対して、制御力および制御モーメントを配分し、推力発生器駆動源が故障している場合には、故障しているものを除いた残りの推力発生器駆動源に対して、制御力および制御モーメントを再配分する。
【０００９】
また、請求項２に記載の航走体の安定ロバスト制御装置は、請求項１に記載の航走体の安定ロバスト制御装置において、前記航走体の姿勢に関する情報を検知するセンサにて検出した検出値と予め定められた目標値との偏差を算出する偏差算出手段と、前記航走体の姿勢制御用パラメータとして、前記偏差に基づいて前記航走体に付与する制御力および制御モーメントを算出する自動制御パラメータ算出手段とをさらに備えることを特徴とする。
【００１０】
かかる構成によれば、航走体の安定ロバスト制御装置は、センサ検出値と目標値との偏差に基づいて、航走体に付与する制御力および制御モーメントを自動的に算出することができる。ここで、自動制御パラメータ算出手段は、例えば、ＰＩＤ制御により制御力および制御モーメントを算出し、算出結果に対応した制御コマンドを配分量算出手段に出力する。
【００１１】
また、請求項３に記載の航走体の安定ロバスト制御装置は、請求項１に記載の航走体の安定ロバスト制御装置において、前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として前記制御力および制御モーメントを算出する手動制御パラメータ算出手段をさらに備え、前記配分量算出手段が、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする。
【００１２】
かかる構成によれば、航走体の安定ロバスト制御装置は、操作者によって入力された操作コマンドを変換則に基づいて変換し、その変換結果を、航走体に付与する制御力および制御モーメントとして算出することができる。ここで、手動制御パラメータ算出手段は、変換結果に対応した制御コマンドを配分量算出手段に出力する。
【００１３】
また、請求項４に記載の航走体の安定ロバスト制御装置は、請求項２に記載の航走体の安定ロバスト制御装置において、前記推力発生器駆動源に対する操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として制御力および制御モーメントを算出する手動制御パラメータ算出手段と、前記自動制御パラメータ算出手段で算出された制御力および制御モーメントと、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントとを加算する加算手段とをさらに備え、前記配分量算出手段が、前記加算手段で加算された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする。
【００１４】
かかる構成によれば、航走体の安定ロバスト制御装置は、操作者によって入力された操作コマンドに基づく手動制御の結果と、センサ検出値と目標値との偏差に基づく自動制御の結果とを加算した制御力および制御モーメントと配分則とに基づいて、配分量を算出する。したがって、操作コマンドを入力しない場合の制御と、入力する場合の制御とを行うことが可能となる。例えば、何も故障していない場合には、操作者が操作コマンドを入力しないようにして運用することが可能である。また、例えば、推力発生器駆動源が故障した場合に、一旦は、航走体の姿勢が崩れたり航路がそれたりする。このときに、操作コマンドを入力する運用を行うことで、自動制御の結果と手動制御の結果とが加算される。その結果、航走体が元の状態に復帰するまでに要する時間を短縮することが可能となる。さらに、例えば、センサが故障するなどの重大な故障が発生した場合に、操作者が操作コマンドを入力するように運用することが可能となるので、航走体の運用の信頼性を向上させることができる。
【００１５】
また、前記目的を達成するために、請求項５に記載の航走体の安定ロバスト制御システムは、請求項３または請求項４に記載の航走体の安定ロバスト制御装置と、推進手段と、複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、姿勢に関する情報を検知するセンサとを有する航走体と、前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置とを備えることを特徴とする。
【００１６】
かかる構成によれば、航走体の安定ロバスト制御システムは、操作コマンド入力装置を航走体に含むことで有人航走体を実現することが可能である。また、操作コマンド入力装置を航走体と別体とすることで無人航走体を実現することが可能である。
【００１７】
また、請求項６に記載の航走体の安定ロバスト制御システムは、請求項５に記載の航走体の安定ロバスト制御システムにおいて、前記操作コマンド入力装置から入力された操作コマンドを前記航走体に送信するコマンド送信装置と、前記送信された操作コマンドを受信するコマンド受信装置とをさらに備え、前記手動制御パラメータ算出手段が、前記受信された操作コマンドを前記変換則に基づいて変換することを特徴とする。
【００１８】
かかる構成によれば、航走体の安定ロバスト制御システムは、操作コマンド入力装置を航走体と別体とすることで無人航走体を実現することが可能である。ここで、操作コマンドを送受信する方法は、航走体が利用される外部の環境に応じて、音波を用いる方法、あるいは、無線または有線の通信による方法を含む。
【００１９】
また、前記目的を達成するために、請求項７に記載の航走体の安定ロバスト制御方法は、航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、安定ロバスト制御装置とを備えた航走体の安定ロバスト制御方法であって、前記安定ロバスト制御装置が、前記推力発生器駆動源の故障を検知する故障検知ステップと、前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出ステップとを有することを特徴とする。
【００２０】
かかる手順によれば、航走体の安定ロバスト制御装置は、推力発生器駆動源が故障していない場合には、各推力発生器駆動源に対して、制御力および制御モーメントを配分し、推力発生器駆動源が故障している場合には、故障しているものを除いた残りの推力発生器駆動源に対して、制御力および制御モーメントを再配分する。
【００２１】
また、請求項８に記載の航走体の安定ロバスト制御プログラムは、航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の姿勢を安定に保つために、コンピュータを、前記推力発生器駆動源の故障を検知する故障検知手段、前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段、として機能させることを特徴とする。
【００２２】
かかる構成によれば、安定ロバスト制御プログラムは、推力発生器駆動源が故障していない場合には、各推力発生器駆動源に対して、制御力および制御モーメントを配分し、推力発生器駆動源が故障している場合には、故障しているものを除いた残りの推力発生器駆動源に対して、制御力および制御モーメントを再配分する。
【発明の効果】
【００２３】
請求項１、請求項７または請求項８に記載の発明によれば、推力発生器駆動源が故障し、かつ、正常に動作する推力発生器駆動源が残っている場合においても、航走体の姿勢を安定に保つことができる。
【００２４】
請求項２に記載の発明によれば、航走体に付与する制御力および制御モーメントを精度よく算出することができる。
【００２５】
請求項３に記載の発明によれば、操作者が操作コマンドを入力することで、航走体に所望の運動をさせることが可能となる。
【００２６】
請求項４、請求項５または請求項６に記載の発明によれば、航走体の運用の幅を広げ、信頼性を向上させることが可能となる。
【発明を実施するための最良の形態】
【００２７】
以下、図面を参照して本発明の航走体の安定ロバスト制御装置、安定ロバスト制御システム、その方法およびプログラムを実施するための最良の形態（以下「実施形態」という）について詳細に説明する。以下の実施形態では、航走体の一例として、水中で利用される水中航走体を用いて説明することとする。
【００２８】
（第１実施形態）
本発明の第１実施形態に係る水中航走体の安定ロバスト制御システムについて、図１および図２を参照しながら説明する。第１実施形態に係る水中航走体の安定ロバスト制御システムは、水中航走体で実現できる。
【００２９】
［安定ロバスト制御システムの構成］
図１は、第１実施形態の安定ロバスト制御システムの外観図であって、（ａ）は概略平面図、（ｂ）は概略左側面図、（ｃ）は概略背面図をそれぞれ示している。図２は、図１に示した安定ロバスト制御システムの構成図である。
【００３０】
水中航走体（安定ロバスト制御システム）１は、図１および図２に示すように、船体２と、推進器３と、姿勢制御舵４と、プロペラ駆動源５と、推進制御装置６と、舵駆動源７と、センサ８と、安定ロバスト制御装置１０とを主たる要素として備えている。
【００３１】
推進器３は、船体２に推進力を付与するものであり、プロペラ３ａを備え、船体２の後端部に配置されている。
【００３２】
姿勢制御舵４は、水中航走体（航走体）１の姿勢を安定に保つための制御を行うものである。この姿勢制御舵４は、断面視翼型を有する板状の部材であり、図１（ｃ）に示すように、船体２の後部から４５°，１３５°，２２５°，３１５°の方向に沿ってそれぞれ一枚ずつ、放射状（Ｘ字状）に合計四枚設けられている。また、姿勢制御舵４の後縁部には、舵駆動源７により駆動される補助翼（推力発生器）４ａが設けられている。本実施形態では、この補助翼４ａが水中航走体１の姿勢を安定に保つための制御を実質的に行うように構成した。なお、補助翼４ａを設けることなく、姿勢制御舵４そのものを舵駆動源７により駆動することで、水中航走体１の姿勢を安定に保つための制御を行うように構成してもよい。この場合には、姿勢制御舵４そのものが推力発生器として機能する。
【００３３】
プロペラ駆動源５は、推進制御装置６から出力される制御信号に基づいて、プロペラ３ａを正回転または逆回転させるように駆動するものである。プロペラ駆動源５は、例えば、バッテリー駆動の電動モータ等から構成される。
推進制御装置６は、所定のプログラムにしたがって、センサ８の出力に基づいて、目標地点に到達したか否かを判別し、水中航走体１を前進または後進させるために、プロペラ３ａを正回転または逆回転させる制御信号をプロペラ駆動源５に出力する。
【００３４】
舵駆動源（推力発生器駆動源）７は、安定ロバスト制御装置１０から出力される制御信号（舵角指令信号）に基づいて、姿勢制御舵４の補助翼４ａを駆動するものであり、例えば、バッテリー駆動のアクチュエータ等から構成される。この舵駆動源７は、補助翼４ａを、図１（ｃ）に示す船体２の後部から４５°，１３５°，２２５°，３１５°の方向とそれぞれ平行な軸線まわりに回動させる。安定ロバスト制御装置１０から出力される舵角指令信号は、姿勢制御舵４の４つの補助翼４ａをそれぞれ、適宜必要な方向に必要な角度だけ回動させるためのものである。これらの信号によって、水中航走体１が潜行（下降）、浮上（上昇）、進行方向左方に変針（左回頭）、および進行方向右方に変針（右回頭）することができる。
【００３５】
なお、図示は省略するが、水中航走体１内部には、姿勢角（ピッチ角）を調整したり、重力に対する浮力を大きく（または小さく）したりするために、例えば、バラスト水の注排水装置等の浮量調整装置や、重錘を移動させてその重心位置を変化させる重錘移動装置等が格納されている。
【００３６】
センサ８は、水中航走体１の運動情報を検知し、検知した運動情報を示すセンサ信号を安定ロバスト制御装置１０に出力するものであり、速度計８ａと、加速度計８ｂと、ジャイロセンサ８ｃとを備える。
速度計８ａは、例えば、統合航海システム（ＩＮＳ：Integrated Navigation System）を用いたものであり、水中航走体１の現在の位置（例えば、経度、緯度、深度）や水中航走体１の速度を計測するものである。
加速度計８ｂは、水中航走体１の加速度を計測するものである。
ジャイロセンサ８ｃは、水中航走体１の姿勢（ロール角、ピッチ角、ヨー角）を計測するものである。
【００３７】
本実施形態では、式（１）に示すように、（Ｘ，Ｙ，Ｚ）で、水中航走体１の現在の位置を表し、式（２）に示すように、（φ，θ，Ψ）で、水中航走体１の現在の姿勢として、ロール角、ピッチ角、ヨー角をそれぞれ表すこととする。
【００３８】
【数１】
【００３９】
安定ロバスト制御装置１０は、姿勢制御舵４の補助翼４ａを駆動する舵駆動源７に舵角指令信号を出力するものである。この安定ロバスト制御装置１０は、例えば、ＣＰＵ（Central Processing Unit）と、ＲＡＭ（Random Access Memory）と、ＲＯＭ（Read Only Memory）と、ＨＤＤ（Hard Disk Drive）と、入出力インタフェース等から構成され、図２に示すように、故障検知手段１１と、記憶手段１２と、目標値管理手段１３と、偏差算出手段１４と、自動制御パラメータ算出手段１５と、配分量算出手段１６とを備えている。
【００４０】
故障検知手段１１は、舵駆動源７の故障を検知するものである。この故障検知手段１１は、４つの舵駆動源７のうちのどれが故障したのかを示す信号を配分量算出手段１６に出力する。本実施形態では、舵駆動源７の識別変数を「ｉ（＝１，２，３，４）」で表し、故障を検知したときに、その舵駆動源を識別する識別変数ｉの値を出力するものとする。
【００４１】
また、故障を検知する方法は、公知の技術を利用することができる。本実施形態では、舵駆動源７からの出力信号の電流値（または電圧値）を測定して、この測定値が所定のしきい値よりも小さくなったか否かを判別し、所定のしきい値よりも小さくなった場合に、その出力信号を出力している舵駆動源７が故障したものとみなすこととした。例えば、センサ８として、舵駆動源７の回転角の変化を検出するタコジェネレータを備え、この検出値が、１秒間の変化角度が３°であることを目標値として、２．８°〜３．２°であれば正常と判別し、上限値以上または下限値以下であれば故障であるものとすることが可能である。あるいは、しきい値を設定する代わりに、目標値からの誤差が目標値の１０％以下である場合に正常であると判別するようにしてもよい。なお、故障検知手段１１は、故障したか否かを連続的に判別するようにしてもよいし、一定間隔（例えば、50msごと）で判別を繰り返すようにしてもよい。
【００４２】
記憶手段１２は、例えば、ＲＡＭと、ＲＯＭと、ＨＤＤとを備えている。本実施形態では、記憶手段１２は、目標値記憶部１２１と、配分則記憶部１２２とを備えている。これらは、例えば、ＨＤＤから構成される。
【００４３】
目標値記憶部１２１は、予め設定された目標値を記憶するものである。この設定された目標値は、水中航走体１の目標とする未来の目標地点（目標位置）と、水中航走体１の目標とする未来の姿勢（目標姿勢）とからなる。本実施形態では、式（３）に示すように、（Ｘ〜，Ｙ〜，Ｚ〜）で水中航走体１の目標位置を表す。ここで、記号「〜（チルダ）」は、それぞれ直前の文字の上に付される記号を示し、本明細書では、以下、記号「〜（チルダ）」を同様な意味で使用する。また、式（４）に示すように、（φ〜，θ〜，Ψ〜）で、水中航走体１の目標姿勢として、ロール角、ピッチ角、ヨー角をそれぞれ表すこととする。
【００４４】
【数２】
【００４５】
配分則記憶部１２２は、水中航走体１に付与する制御力および制御モーメントを、各舵駆動源７に、配分するためのルールである配分則を記憶するものである。本実施形態では、配分則は、例えば、線形計画法を利用するアルゴリズムによって構築されている。
【００４６】
目標値管理手段１３は、目標値記憶部１２１から目標値を読み出して、偏差算出手段１４に出力するものである。
偏差算出手段１４は、センサ８にて検出した検出値と、目標値記憶部１２１から読み出された目標値との偏差を式（５）および式（６）に基づいて算出するものであり、例えば、減算器から構成される。この偏差算出手段１４は、算出結果を示す信号を自動制御パラメータ算出手段１５に出力する。
【００４７】
【数３】
【００４８】
自動制御パラメータ算出手段１５は、センサ８の検出値と目標値との偏差を示す入力信号に基づいて、水中航走体１の姿勢制御用パラメータとして、水中航走体１の船体２に付与する制御力および制御モーメントを算出するものである。この自動制御パラメータ算出手段１５は、算出結果から制御力コマンドおよび制御モーメントコマンド（総称して制御コマンドという）を生成し、配分量算出手段１６に出力する。
【００４９】
本実施形態では、自動制御パラメータ算出手段１５は、ＰＩＤ制御に基づいて制御コマンドを生成する。具体的には、自動制御パラメータ算出手段１５は、力コマンド（ＦXC，ＦYC，ＦZC）のＸ成分、Ｙ成分およびＺ成分を、式（７）〜式（９）に基づいてそれぞれ算出する。ここで、制御力コマンド（ＦXC，ＦYC，ＦZC）には、制御力が記述されるので、式（７）〜式（９）は、制御力の各成分を示すこととなる。また、自動制御パラメータ算出手段１５は、制御モーメントコマンド（ＮXC，ＮYC，ＮZC）のＸ成分、Ｙ成分およびＺ成分を、式（１０）〜式（１２）に基づいてそれぞれ算出する。ここで、制御モーメントコマンド（ＮXC，ＮYC，ＮZC）には、制御モーメントが記述されるので、式（10）〜式（12）は、制御モーメントの各成分を示すこととなる。また、前記した式（７）〜式（12）において、ｅは、式（５）および式（６）に示した偏差、Ｐは比例ゲイン、Ｉは積分ゲイン、Ｄは微分ゲインを示している。
【００５０】
【数４】
【００５１】
配分量算出手段１６は、入力された制御コマンドで示される制御力および制御モーメントを、予め定められた配分則に基づいて、舵駆動源７に対して配分する配分量を算出し、演算結果を示す制御信号を各舵駆動源７に出力するものである。この配分量算出手段１６は、舵駆動源７の故障を示す信号が入力された場合には、制御力および制御モーメントを、配分則に基づいて、故障した舵駆動源７を除いた残りの各舵駆動源７に対して配分する配分量を算出し、算出結果を示す制御信号を残りの各舵駆動源７に出力する。つまり、制御力および制御モーメントが再配分されることとなる。
【００５２】
具体的には、配分量算出手段１６は、式（13）〜式（18）に示す等式条件と、式（19）〜式（21）に示す不等式条件との下で、式（22）に示すＪ1の値が最小となるように、各舵駆動源７に対して配分する配分量（ＦXi，ＦYi，ＦZi）を算出する。
【００５３】
【数５】
【００５４】
【数６】
【００５５】
【数７】
ただし、
Ｆi2＝ＦXi2＋ＦYi2＋ＦZi2 …式（23）
【００５６】
式（13）〜式（22）において、ｉは舵駆動源７の識別変数を示し、本実施形態では、ｉの取り得る値は、１，２，３，４である。また、式（13）〜式（22）において、ｎは舵駆動源７の個数を示し、本実施形態ではｎの値は４である。また、式（16）〜式（18）において、ｌXiは水中航走体１の重心から識別変数ｉの舵駆動源７までの距離に関するものであり、水中航走体１の重心を三次元座標空間の所定の原点に合致させたときに、重心と舵駆動源７とを結ぶ直線をＸ軸に投影したときのＸ軸上の距離を示し、同様にｌYiおよびｌZiは、Ｙ軸上の距離およびＺ軸上の距離をそれぞれ示す。
【００５７】
また、式（19）〜式（21）において、Ｆmiは、ｉ番目の舵駆動源７で発生される推力の大きさであり、この大きさ（絶対値）は、舵駆動源７の最大容量（最大移動量）で決定される（既知の値）。さらに、配分量算出手段１６は、舵駆動源７の故障を示す信号が入力された場合には、式（13）〜式（22）において、その入力信号で示される識別変数ｉの値についての力の成分や距離の値を「０」として計算する。
【００５８】
また、式（22）において、Ｊ1は、識別変数ｉの舵駆動源７によって、姿勢制御舵４の補助翼４ａを駆動するエネルギーに相当し、ｆ1は、式（23）で定義されたＦi2についての所定の評価関数を示す。つまり、配分量算出手段１６は、式（22）の左辺のＪ1の値が最小となるように、配分量（ＦXi，ＦYi，ＦZi）を変化させながら、式（22）の右辺のＦi2の値を求め、そのときの配分量（ＦXi，ＦYi，ＦZi）を求める。式（22）で示される最小値問題の方程式は、例えば、ラグランジェ未定乗数法等の線形計画法を用いて以下のようにして計算することが可能である。この方法として、例えば、特開平２−１０６４９２号公報（特許文献２）に開示された方法を利用することができる。具体的には、マトリックス形式で記述された式（24）〜式（27）に示す６元連立一次方程式を解けばよい。
【００５９】
Ａλ＝ｂ …式（24）
【００６０】
【数８】
【００６１】
【数９】
【００６２】
【数１０】
【００６３】
ここで、式（24）に示すＡは、式（25）に示すように６×６の正方行列である。また、式（24）に示すλは、ラグランジェ乗数であり、式（26）に示すように１行６列の行列である。また、式（24）に示すｂは、前記した式（13）〜式（18）に示した制御コマンドであり、式（26）に示すように１行６列の行列である。なお、式（27）では、行列中において、制御モーメントコマンドを、前記した式（18）に示すＮZC、式（17）に示すＮYC、式（16）に示すＮXCの順序に配置することとした。式（24）〜式（27）に示す６元連立一次方程式を解くことによって、ラグランジェ乗数λ1〜λ6の値を求めることができる。一方、前記した式（22）の左辺のＪ1の値が最小となるときに、識別変数ｉの舵駆動源７が発生する推力のＸ成分、Ｙ成分およびＺ成分は、式（28）〜式（30）で表すことができる。したがって、式（28）〜式（30）の各右辺に含まれるラグランジェ乗数λ1〜λ6に対して、６元連立一次方程式を解くことによって求めた値を代入することによって、配分量（ＦXi，ＦYi，ＦZi）を決定することができる。
【００６４】
【数１１】
【００６５】
ここで、決定した推力の配分量（ＦXi，ＦYi，ＦZi）と、舵駆動源７の移動量との対応関係式（または対応関係テーブル）は、予め求められて記憶手段１２に記憶されており、配分量算出手段１６は、当該対応関係式（または対応関係テーブル）に基づいて、舵駆動源７の移動量を求め、求めた移動量を示す制御信号を該当する舵駆動源７に出力する。なお、決定した推力の配分量（ＦXi，ＦYi，ＦZi）の値をそのまま舵駆動源７の移動量として出力するようにしてもよい。
【００６６】
なお、故障検知手段 １１、目標値管理手段１３、偏差算出手段１４、自動制御パラメータ算出手段１５および配分量算出手段１６は、ＣＰＵが記憶手段１２のＨＤＤ等に格納された所定のプログラムをＲＡＭに展開して実行することにより実現することができる。
【００６７】
［安定ロバスト制御システムの動作］
図２に示した安定ロバスト制御システム（水中航走体）１の動作について、安定ロバスト制御装置１０の動作を中心に図３を参照（適宜図２参照）して説明する。図３は、図２に示した安定ロバスト制御システムの動作を示すフローチャートである。ここでは、水中航走体１が目標地点へ移動するときに、移動中の姿勢を制御する場合を想定している。また、以下のステップＳ１およびステップＳ２は、推進制御装置６の処理を示し、ステップＳ３ないしステップＳ７は、安定ロバスト制御装置１０の処理を示している。
【００６８】
まず、安定ロバスト制御システム１は、推進制御装置６によって、プロペラ３ａを回転させる制御信号をプロペラ駆動源５に出力し、プロペラ駆動源５によって、プロペラ３ａを駆動する（ステップＳ１）。そして、推進制御装置６は、目標地点に到達したか否かを判別する（ステップＳ２）。目標地点に到達した場合（ステップＳ２：Ｙｅｓ）、安定ロバスト制御システム１は、処理を終了する。一方、目標地点に到達していない場合（ステップＳ２：Ｎｏ）、安定ロバスト制御システム１では、推進制御装置６が、引き続きプロペラ３ａを駆動し続ける。
【００６９】
また、安定ロバスト制御装置１０が、偏差算出手段１４によって、姿勢制御に関する目標値とセンサ検出値との偏差を算出し（ステップＳ３）、自動制御パラメータ算出手段１５によって、目標値とセンサ検出値との偏差に基づいて、水中航走体１に付与する制御力および制御モーメントを算出する（ステップＳ４）。続いて、安定ロバスト制御装置１０は、故障検知手段１１によって、舵駆動源７が故障したか否かを判別する（ステップＳ５）。舵駆動源７が故障している場合（ステップＳ５：Ｙｅｓ）、安定ロバスト制御装置１０は、配分量算出手段１６によって、故障中の舵駆動源７を制御対象から除外し（ステップＳ６）、その上で、制御力および制御モーメントを配分則に基づいて残りの舵駆動源７に対して配分する（ステップＳ７）。各舵駆動源７は、各配分量に対応した移動量で作動する。これにより、姿勢制御舵４の補助翼４ａがそれぞれ回動する。その結果、水中航走体１の姿勢を制御することができる。
【００７０】
一方、ステップＳ５において、舵駆動源７が正常である場合（ステップＳ５：Ｎｏ）、安定ロバスト制御装置１０は、配分量算出手段１６によって、ステップＳ６をスキップしてステップＳ７に進む。この場合には、配分量算出手段１６は、制御力および制御モーメントを配分則に基づいてすべての舵駆動源７に対して配分する。ステップＳ７に続いて、安定ロバスト制御システム１は、ステップＳ２に戻る。
【００７１】
なお、安定ロバスト制御装置１０は、一般的なコンピュータに、前記した各ステップを実行させる安定ロバスト制御プログラムを実行することで実現することもできる。このプログラムは、通信回線を介して配布することも可能であるし、ＣＤ−ＲＯＭ等の記録媒体に書き込んで配布することも可能である。
【００７２】
第１実施形態に係る安定ロバスト制御システム１によれば、水中航走体１の舵駆動源７に故障が発生し、正常に動作する舵駆動源７が残っている場合においても、水中航走体１の姿勢を安定に保つことができる。また、残っている舵駆動源７を、その時点で最小のエネルギーで駆動させることができる。その結果、水中航走体１の信頼性を向上させることができる。
【００７３】
（第２実施形態）
［安定ロバスト制御システムの構成］
図４は、第２実施形態の安定ロバスト制御システムの構成図である。安定ロバスト制御システム１Ａは、図４に示すように、水中航走体１００と、監視装置２００とを備えている。水中航走体１００は、コマンド受信装置２０を備え、安定ロバスト制御装置１０Ａの機能が異なる点を除いて、図２に示した水中航走体１と同一の構成なので、同一の構成には同一の符号を付し、説明を省略する。
【００７４】
コマンド受信装置２０は、監視装置２００から音波によって送られる操作コマンドを受信し、安定ロバスト制御装置１０Ａに出力するものである。
安定ロバスト制御装置１０Ａは、記憶手段１２に配分則記憶部１２２と、変換則記憶部１２３とを備えると共に、手動制御パラメータ算出手段１７を備えている。
【００７５】
変換則記憶部１２３は、コマンド受信装置２０で受信する操作コマンドを、水中航走体１００に付与する制御力および制御モーメントに変換するための変換則を記憶するものであり、例えば、メモリやハードディスク等から構成される。
【００７６】
手動制御パラメータ算出手段１７は、コマンド受信装置２０で受信する操作コマンドを、変換則記憶部１２３に記憶された変換則に基づいて変換し、変換結果として水中航走体１００に付与する制御力および制御モーメントを算出するものである。この手動制御パラメータ算出手段１７は、制御力および制御モーメントに対応する制御コマンドを配分量算出手段１６に出力する。本実施形態では、手動制御パラメータ算出手段１７は、操作コマンドから、入力時に入力された角度と方向とを検出し、検出した角度を力の大きさに変換すると共に、検出した方向を力の向きに変換する。
【００７７】
監視装置２００は、水中航走体１００の現在位置を監視し、操作コマンドを水中航走体１００に送信することで、水中航走体１００を遠隔操作するものである。この監視装置２００は、例えば、ＣＰＵと、ＲＡＭと、ＲＯＭと、ＨＤＤと、入出力インタフェース等から構成され、図４に示すように、航走体検知装置２０１と、表示装置２０２と、コマンド入力装置２０３と、コマンド送信装置２０４とを備えている。
【００７８】
航走体検知装置２０１は、音波の反響を測定することにより、水中航走体１００の位置を測定するものであり、例えば、アクティブソナーやパッシブソナーから構成される。
表示装置２０２は、水中航走体１００の位置を表示するものであり、例えば、ＣＲＴ（Cathode Ray Tube）、液晶ディスプレイ（ＬＣＤ：Liquid Crystal Display）、ＰＤＰ（Plasma Display Panel）、ＥＬ（Electronic Luminescence）等から構成される。
【００７９】
コマンド入力装置（操作コマンド入力装置）２０３は、水中航走体１００の姿勢制御用パラメータに対応した操作コマンドを入力するものであり、例えば、コンピュータ一般に用いられるマウスやキーボード、レバーやボタンを組み合わせた遠隔操縦装置、ジョイスティック等から構成される。本実施形態では、コマンド入力装置２０３は、ジョイスティックで構成されることとした。
コマンド送信装置２０４は、コマンド入力装置２０３から入力された操作コマンドを音波によって水中航走体１００に送信するものである。
【００８０】
次に、コマンド入力装置２０３による操作コマンドの入力方法について図５を参照して説明する。図５は、図４に示したコマンド入力装置の説明図である。コマンド入力装置２０３は、図５（ａ）に示すように、直立したジョイスティックレバーを倒したときに、倒れたジョイスティックレバーの向く方向と鉛直方向との間の角度によって、制御力の大きさを表す。例えば、図５（ａ）に示すように、「７０°」の位置まで倒した場合の制御力の大きさは、「３５°」の位置まで倒した場合の制御力の大きさの２倍となる。
【００８１】
また、コマンド入力装置２０３は、図５（ｂ）に示すように、直立したジョイスティックレバーを倒したときに、倒れたジョイスティックレバーの向く方向を水平面に投影した場合の方向と水平面内の基準方向との間の角度によって、制御力の向きを表す。例えば、図５（ｂ）に示すようにジョイスティックレバーを倒した場合には、進行方向右を基準としたときに、進行方向右から前方に向かって「４０°」の方向を指すこととなる。
【００８２】
図５に示したジョイスティック（コマンド入力装置２０３）は、水平面内において、前進、後退、左回頭、および右回頭を指定するためのものである。そこで、水中航走体１が下降および上昇するために、同様なジョイスティック（コマンド入力装置２０３）を設けてある。なお、入力する力の大きさと向きとを切り替えるモード切替手段を設けることで、１つのジョイスティック（コマンド入力装置２０３）によって、前進、後退、左回頭、右回頭、下降および上昇を指定するようにしてもよい。
【００８３】
さらに、水中航走体１の姿勢を制御するために制御モーメントを入力するためのジョイスティックを備えてもよい。例えば、図５に示したジョイスティック（コマンド入力装置２０３）を用いた場合には、直立したジョイスティックレバーを倒したときに、倒れたジョイスティックレバーの向く方向と鉛直方向との間の角度によってピッチ角を表し、倒れたジョイスティックレバーの向く方向を水平面に投影した場合の方向と水平面内の基準方向との間の角度によって、ヨー角を表す。なお、ジョイスティックレバーを逆方向に倒したときには角度の値の正負を逆転させる。また、ロール角を指定するために、同様なジョイスティック（コマンド入力装置２０３）を別に設けるか、１つのジョイスティックをモード切替できるように構成する。
【００８４】
［安定ロバスト制御システムの動作］
図４に示した安定ロバスト制御システム１Ａの動作について、安定ロバスト制御装置１０Ａの動作を中心に図６を参照（適宜図４参照）して説明する。図６は、図４に示した安定ロバスト制御システムの動作を示すフローチャートである。以下のステップＳ２１およびステップＳ２２は、推進制御装置６の処理を示し、図３のフローチャートに示したステップＳ１およびステップＳ２の各処理と同様なので説明を省略する。また、以下のステップＳ２３〜ステップＳ２７は、安定ロバスト制御装置１０Ａの処理を示している。
【００８５】
ステップＳ２３において、安定ロバスト制御装置１０Ａは、手動制御パラメータ算出手段１７によって、操作コマンドを受信したか否かを判別する（ステップＳ２３）。操作コマンドを受信していない場合（ステップＳ２３：Ｎｏ）、安定ロバスト制御システム１Ａは、ステップＳ２２に戻る。すなわち、目標地点に到達したか否かを判別する。一方、操作コマンドを受信した場合（ステップＳ２３：Ｙｅｓ）、安定ロバスト制御装置１０Ａは、手動制御パラメータ算出手段１７によって、変換則に基づいて、水中航走体１００に付与する制御力および制御モーメントを算出する（ステップＳ２４）。以下のステップＳ２５〜ステップＳ２７の各処理は、図３のフローチャートに示したステップＳ５〜ステップＳ７の各処理と同様なので説明を省略する。なお、ステップＳ２３〜ステップＳ２７の一連の処理を手動制御という。
【００８６】
第２実施形態に係る安定ロバスト制御システム１Ａによれば、操作者によって入力された操作コマンドを変換則に基づいて変換し、その変換結果を、水中航走体１００に付与する制御力および制御モーメントとして算出することができる。したがって、水中航走体１００を監視できる場合に、水中航走体１００を操作者の所望の姿勢になるように安定的に制御することができる。
【００８７】
（第３実施形態）
［安定ロバスト制御システムの構成］
図７は、第３実施形態の安定ロバスト制御システムの構成図である。安定ロバスト制御システム１Ｂは、水中航走体１００Ｂと監視装置２００（図７では図示せず、図６参照）とを備えている。水中航走体１００Ｂは、安定ロバスト制御装置１０Ｂの機能が異なる点を除いて、図４に示した水中航走体１００と同一の構成なので、同一の構成には同一の符号を付し、説明を省略する。
【００８８】
安定ロバスト制御装置１０Ｂは、故障検知手段１１Ｂと、記憶手段１２と、目標値管理手段１３と、偏差算出手段１４と、自動制御パラメータ算出手段１５と、配分量算出手段１６と、手動制御パラメータ算出手段１７と、切替手段１８と、加算手段１９とを備えている。ここで、故障検知手段１１Ｂ、切替手段１８および加算手段１９以外の構成は、図２または図４と同一の構成なので同一の符号を付して説明を省略する。
【００８９】
故障検知手段１１Ｂは、舵駆動源７の故障を検知すると共に、センサ８の故障を検知するものである。この故障検知手段１１Ｂは、センサ８の故障のレベルを判定し、判定結果に応じた信号を切替手段１８に出力する。
【００９０】
本実施形態では、故障検知手段１１Ｂは、通常、切替手段１８にオン信号を出力する。そして、故障検知手段１１Ｂは、レベル１の故障であると判別した場合には、切替手段１８にオフ信号を出力する。ここで、レベル１の故障とは、水中航走体１００Ｂの動きを安定化させるための制御を、自動制御から手動制御に切り替える必要のある重大な故障である。例えば、センサ８が完全に機能しなくなるような故障を起こした場合を示す。
【００９１】
また、故障検知手段１１Ｂは、レベル２の故障であると判別した場合には、切替手段１８にオン信号を出力し続けると共に、検知した故障情報を、配分量算出手段１６に出力する。ここで、レベル２の故障とは、水中航走体１００Ｂの動きを安定化させるための制御として、手動制御に完全に切り替える必要のない軽微な故障である。例えば、センサ８が機能はするが、設定どおりの性能を果たさなくなった場合や、舵駆動源７が故障した場合を示す。
【００９２】
切替手段１８は、故障検知手段１１Ｂからのオン信号によって、自動制御パラメータ算出手段１５から配分量算出手段１６へ向かう入力信号を通過させるようにスイッチを切り替えるものである。また、切替手段１８は、故障検知手段１１Ｂからのオフ信号によって、自動制御パラメータ算出手段１５から配分量算出手段１６へ向かう入力信号を遮断させるようにスイッチを切り替える。
【００９３】
加算手段１９は、切替手段１８を介して自動制御パラメータ算出手段１５から入力する信号と、手動制御パラメータ算出手段１７から入力する信号とを加算して配分量算出手段１６に出力するものである。
【００９４】
前記した構成によって、安定ロバスト制御装置１０Ｂは、故障検知手段１１Ｂによる故障レベルの判別結果に基づいて、手動制御パラメータ算出手段１７で制御コマンドを生成するか、あるいは、自動制御パラメータ算出手段１５と手動制御パラメータ算出手段１７とを併用して制御コマンドを生成することとなる。このうち、自動制御パラメータ算出手段１５と手動制御パラメータ算出手段１７とを併用して制御コマンドを生成するようにした場合には、コマンド受信装置２０が操作コマンドを受信しなければ、手動制御パラメータ算出手段１７の生成する制御コマンドが実質的に「０」になり、加算手段１９の出力信号は、実質的に、自動制御パラメータ算出手段１５の生成する制御コマンドと等しくなってしまう。
【００９５】
つまり、安定ロバスト制御装置１０Ｂが、自動制御パラメータ算出手段１５と手動制御パラメータ算出手段１７とを併用して制御コマンドを生成するようにしていたとしても、故障が同じ条件のときに、以下に示すように、複数通りの運用が考えられる。
【００９６】
例えば、舵駆動源７が正常であり、かつ、センサ８が軽微な故障をした場合を想定する。この場合には、自動制御のみで対応し、故障の影響が甚大になってきたときにだけ、監視装置２００から操作コマンドを送信するような運用が可能である（これを実質的自動制御という）。
【００９７】
また、センサ８が正常であり、かつ、舵駆動源７が故障した場合には、操作コマンドを全く送信せずに自動制御で対応することが可能である（これを完全自動制御という）。完全自動制御は、第１実施形態の場合に相当する。また、この場合には、実質的自動制御で運用することも可能である。あるいは、操作コマンドを常に送信することが可能である（これを手動併用制御という）。特に、手動併用制御の運用を行う場合には、水中航走体１００Ｂが元の状態に復帰するまでに要する時間を大幅に短縮することが可能となる。
【００９８】
このような複数通りの運用ができる理由は、本実施形態では、監視装置２００の操作者が、監視装置２００の表示装置２０２（図４参照）に表示された水中航走体１００Ｂの運動軌跡に基づいて、センサ８の故障が重大であるか、あるいは、軽微であるかの最終的な判断をするからである。なお、監視装置２００において、表示装置２０２（図４参照）に表示された水中航走体１００Ｂの運動軌跡と、目標とする運動軌跡との偏差に基づいて、運用のレベル（完全自動制御、実質自動制御、手動併用制御）を自動的に判別するように構成してもよい。そして、判別した運用のレベルに応じた照明、ブザー、アナウンス等の報知手段によって、操作者に対して、操作コマンドの送信が必要か否かを報知するようにしてもよい。
【００９９】
［安定ロバスト制御システムの動作］
図７に示した安定ロバスト制御システム１Ｂの動作について、安定ロバスト制御装置１０Ｂの動作を中心に図８を参照（適宜図７参照）して説明する。図８は、図７に示した安定ロバスト制御システムの動作を示すフローチャートである。以下のステップＳ４１、Ｓ４２およびＳ５１は、推進制御装置６の処理を示し、このうち、ステップＳ４１、Ｓ４２は、図３のフローチャートに示したステップＳ１、Ｓ２の各処理と同様なので説明を省略する。また、以下のステップＳ４３〜ステップＳ５０は、安定ロバスト制御装置１０Ｂの処理を示している。このうち、ステップＳ４３、Ｓ４４、および、ステップＳ４６〜Ｓ４８の各処理は、図３のフローチャートに示したステップＳ３〜ステップＳ７の各処理と同様なので説明を省略する。
【０１００】
安定ロバスト制御装置１０Ｂは、ステップＳ４４で制御力および制御モーメントを算出した後、ステップＳ４５において、故障検知手段１１Ｂによって、センサ８の故障のレベルを判別する。センサ８が故障していないか、または、故障レベルがレベル２の場合（ステップＳ４５：Ｎｏ）、ステップＳ４６〜Ｓ４８を実行する。一方、センサ８の故障レベルがレベル１である場合（ステップＳ４５：Ｙｅｓ）、故障検知手段１１Ｂは、切替手段１８にオフ信号を出力する。これによって、切替手段１８は、自動制御パラメータ算出手段１５から配分量算出手段１６へ向かう入力信号を遮断させるようにスイッチを切り替える。すなわち、手動に切り替える（ステップＳ４９）。
【０１０１】
そして、安定ロバスト制御装置１０Ｂは、手動制御パラメータ算出手段１７および配分量算出手段１６によって、手動制御を行う（ステップＳ５０）。このステップＳ５０の処理は、図６のフローチャートに示したステップＳ２３〜ステップＳ２７の一連の処理を指している。ステップＳ５０に続いて、安定ロバスト制御システム１Ｂは、推進制御装置６によって、目標地点に到達したか否かを判別する（ステップＳ５１）。目標地点に到達した場合（ステップＳ５１：Ｙｅｓ）、安定ロバスト制御システム１Ｂは、処理を終了する。目標地点に到達していない場合（ステップＳ５２：Ｎｏ）、安定ロバスト制御システム１Ｂでは、安定ロバスト制御装置１０ＢがステップＳ５０に戻る。
【０１０２】
第３実施形態に係る安定ロバスト制御システム１Ｂによれば、操作者によって入力された操作コマンドに基づく手動制御の結果と、センサ８の検出値と目標値との偏差に基づく自動制御の結果とを加算した制御力および制御モーメントと配分則とに基づいて、配分量を算出する。したがって、操作コマンドを入力しない場合の制御と、入力する場合の制御とを行うことが可能となる。つまり運用の幅が広がり、水中航走体１００Ｂの運用の信頼性を向上させることができる。
【０１０３】
（第４実施形態）
第４実施形態に係る水中航走体の安定ロバスト制御システムは、その構成および動作を、前記した第１実施形態と同様なものとすることができる。ただし、前記した第１実施形態では、配分量算出手段１６は、式（22）で示される最小値問題の方程式を、ラグランジェ未定乗数法を用いて計算するものとしたが、本実施形態では、ペナルティ法等の非線形計画法を用いて計算するものとする。したがって、構成を示す構成図および動作を示すフローチャートを省略し、必要な場合は図１ないし図３を参照して、配分量算出手段１６が計算に利用する非線形計画法のアルゴリズムを説明する。
【０１０４】
まず、式（31）で示すＪ2と、式（32）で示すＪ3を導入する。
【０１０５】
【数１２】
【０１０６】
【数１３】
【０１０７】
Ｊ2は、正常なすべての舵駆動源７によって、姿勢制御舵４の補助翼４ａを駆動するエネルギーに相当し、ｆ2は、ｘについてのコスト関数（第１評価関数）を示す。ここで、ｘは、水中航走体１の重心を始点とする制御力のベクトルを示す変数であり、その絶対値（ベクトルの距離）が最小となるものを探すための指標となるものである。
また、Ｊ3は、Ｊ2を変換したものであり、Ｆ（ｘ,ｒ,ｔ）は、ｘ，ｒ，ｔについてのトータルコスト関数（第２評価関数）を示す。ｒとｔは、それぞれ可変パラメータである。
式（31）および式（32）において、ｉは舵駆動源７の識別変数を示し、ｉの取り得る値は、１〜４である。また、ｎは舵駆動源７の個数を示し、本実施形態では４である。
【０１０８】
式（32）において、Ｈj（ｘ）は等式条件の数値である。なお、ｊ＝１，２，…，ｐである。本実施形態では、ｐ＝３である。そのため、Ｈj（ｘ）は、具体的には、式（33）〜式（35）で表される。これらの式（33）〜式（35）は、前記した式（13）〜式（15）に対応している。式（33）〜式（35）の左辺を示すＨj（ｘ）は、それぞれ、計算をする上で便宜上おいたものであり、数値としては０（零）となる。
また、式（32）において、Ｇk（ｘ）は不等式条件の数値である。なお、ｋ＝１，２，…，ｑであり、本実施形態では、ｑ＝３である。同様に、Ｇk（ｘ）は、具体的には、式（36）〜式（38）で表される。これらの式（36）〜式（38）は、前記した式（16）〜式（18）に対応している。式（36）〜式（38）の左辺を示すＧk（ｘ）は、それぞれ、計算をする上で便宜上おいたものであり、数値としては０（零）となる。
【０１０９】
【数１４】
【０１１０】
式（32）の最右辺の第２項は、等式条件（Ｈj（ｘ））のペナルティ関数であり、式（39）のように展開できる。なお、式（39）では一般形で記述している。ここでは、力の３成分に対応してｐ＝３となる。
【０１１１】
【数１５】
【０１１２】
式（32）の最右辺の第３項は、不等式条件（Ｇk（ｘ））のペナルティ関数であり、式（40）のように展開できる。なお、式（40）では一般形で記述している。ここでは、モーメントを３軸まわりに展開してｑ＝３となる。
【０１１３】
【数１６】
【０１１４】
本実施形態では、配分量算出手段１６は、式（22）のＪ1の代わりに、式（32）で示したトータルコスト関数Ｆ（ｘ,ｒ,ｔ）の値が最小となるように、配分量（ＦXi，ＦYi，ＦZi）を変化させながら、制御力に相当する式（32）の最右辺の値を求め、そのときの配分量（ＦXi，ＦYi，ＦZi）をそれぞれ求める。そして、配分量算出手段１６は、式（32）で示される最小値問題の方程式を、例えば、公知の準ニュートン法を用いて計算することが可能である。
【０１１５】
第４実施形態に係る安定ロバスト制御システムによれば、配分量算出手段１６が舵駆動源７に対する制御力の配分量を算出する際に用いるコスト関数の段階で、故障中の舵駆動源７を制御対象から排除することができるので、舵駆動源７が正常な場合に必要な計算処理から、舵駆動源７が故障した場合に必要な計算処理へ切り替えるために必要な変更が、線形計画法を用いた場合よりも、容易になる。
【０１１６】
以上、本発明の各実施形態について説明したが、本発明はこれらに限定されるものではなく、その趣旨を変えない範囲で実施することができる。例えば、故障検知手段１１（１１Ｂ）は、舵駆動源７（およびセンサ８）の故障を検知するものとしたが、その他の構成要素の故障を検知するようにしてもよい。また、故障レベルの判定は、２段階（正常、故障）または３段階（正常、レベル１、レベル２）としたが、４段階以上であってもよい。
また、監視装置２００は、水中航走体１００（１００Ｂ）と別体であるものとしたが、監視装置を水中航走体に設けて有人水中航走体を構成するようにしてもよい。
【０１１７】
各実施形態では、姿勢制御舵４をＸ字状に配置したが、独立駆動できる複数舵であれば、十字状あるいはＹ字状などの配置としてもよい。また、姿勢制御舵４の枚数は、２枚以上あればよい。また、姿勢制御舵４の後端部に配置された補助翼４ａが回動するものとしたが、姿勢制御舵４全体が回動するように構成してもよい。
【０１１８】
また、推力発生器の一例として姿勢制御舵４を用いて説明したが、これに限定されるものではなく、推力発生器は、例えば、スラスタやプロペラであってもよい。また、推力発生器駆動源の一例として舵駆動源７を用いて説明したが、推力発生器駆動源は、例えば、モータ等のスラスタ駆動源やプロペラ駆動源であってもよい。また、航走体として水中で利用される水中航走体を用いて説明したが、航走体は、空中で利用される飛行体（航空機、飛行船、人工衛星等）や陸上で僅かに浮上して走行可能なホバークラフト（hovercraft）でもよい。
【図面の簡単な説明】
【０１１９】
【図１】第１実施形態の安定ロバスト制御システムの外観図であって、（ａ）は概略平面図、（ｂ）は概略左側面図、（ｃ）は概略背面図を示している。
【図２】図１に示した安定ロバスト制御システムの構成図である。
【図３】図２に示した安定ロバスト制御システムの動作を示すフローチャートである。
【図４】第２実施形態の安定ロバスト制御システムの構成図である。
【図５】図４に示したコマンド入力装置の説明図である。
【図６】図４に示した安定ロバスト制御システムの動作を示すフローチャートである。
【図７】第３実施形態の安定ロバスト制御システムの構成図である。
【図８】図７に示した安定ロバスト制御システムの動作を示すフローチャートである。
【符号の説明】
【０１２０】
１（１Ａ，１Ｂ） 安定ロバスト制御システム
２ 船体
３ 推進器
３ａ プロペラ
４ 姿勢制御舵
４ａ 補助翼（推力発生器）
５ プロペラ駆動源
６ 推進制御装置
７ 舵駆動源（推力発生器駆動源）
８ センサ
８ａ 速度計
８ｂ 加速度計
８ｃ ジャイロセンサ
１０（１０Ａ，１０Ｂ） 安定ロバスト制御装置
１１（１１Ｂ） 故障検知手段
１２ 記憶手段
１２１ 目標値記憶部
１２２ 配分則記憶部
１２３ 変換則記憶部
１３ 目標値管理手段
１４ 偏差算出手段
１５ 自動制御パラメータ算出手段
１６ 配分量算出手段
１７ 手動制御パラメータ算出手段
１８ 切替手段
１９ 加算手段
２０ コマンド受信装置
１００（１００Ｂ） 水中航走体（航走体）
２００ 監視装置
２０１ 航走体検知装置
２０２ 表示装置
２０３ コマンド入力装置（操作コマンド入力装置）
２０４ コマンド送信装置
【特許請求の範囲】
【請求項１】
航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の安定ロバスト制御装置であって、
前記推力発生器駆動源の故障を検知する故障検知手段と、
前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段とを備えることを特徴とする航走体の安定ロバスト制御装置。
【請求項２】
前記航走体の姿勢に関する情報を検知するセンサにて検出した検出値と予め定められた目標値との偏差を算出する偏差算出手段と、
前記航走体の姿勢制御用パラメータとして、前記偏差に基づいて前記航走体に付与する制御力および制御モーメントを算出する自動制御パラメータ算出手段とをさらに備えることを特徴とする請求項１に記載の航走体の安定ロバスト制御装置。
【請求項３】
前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として前記制御力および制御モーメントを算出する手動制御パラメータ算出手段をさらに備え、
前記配分量算出手段は、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする請求項１に記載の航走体の安定ロバスト制御装置。
【請求項４】
前記推力発生器駆動源に対する操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として制御力および制御モーメントを算出する手動制御パラメータ算出手段と、
前記自動制御パラメータ算出手段で算出された制御力および制御モーメントと、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントとを加算する加算手段とをさらに備え、
前記配分量算出手段は、前記加算手段で加算された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする請求項２に記載の航走体の安定ロバスト制御装置。
【請求項５】
請求項３または請求項４に記載の航走体の安定ロバスト制御装置と、推進手段と、複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、姿勢に関する情報を検知するセンサとを有する航走体と、
前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置とを備えることを特徴とする航走体の安定ロバスト制御システム。
【請求項６】
前記操作コマンド入力装置から入力された操作コマンドを前記航走体に送信するコマンド送信装置と、
前記送信された操作コマンドを受信するコマンド受信装置とをさらに備え、
前記手動制御パラメータ算出手段は、前記受信された操作コマンドを前記変換則に基づいて変換することを特徴とする請求項５に記載の航走体の安定ロバスト制御システム。
【請求項７】
航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、安定ロバスト制御装置とを備えた航走体の安定ロバスト制御方法であって、
前記安定ロバスト制御装置は、
前記推力発生器駆動源の故障を検知する故障検知ステップと、
前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出ステップとを有することを特徴とする航走体の安定ロバスト制御方法。
【請求項８】
航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の姿勢を安定に保つために、コンピュータを、
前記推力発生器駆動源の故障を検知する故障検知手段、
前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段、
として機能させることを特徴とする航走体の安定ロバスト制御プログラム。
【請求項１】
航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の安定ロバスト制御装置であって、
前記推力発生器駆動源の故障を検知する故障検知手段と、
前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段とを備えることを特徴とする航走体の安定ロバスト制御装置。
【請求項２】
前記航走体の姿勢に関する情報を検知するセンサにて検出した検出値と予め定められた目標値との偏差を算出する偏差算出手段と、
前記航走体の姿勢制御用パラメータとして、前記偏差に基づいて前記航走体に付与する制御力および制御モーメントを算出する自動制御パラメータ算出手段とをさらに備えることを特徴とする請求項１に記載の航走体の安定ロバスト制御装置。
【請求項３】
前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として前記制御力および制御モーメントを算出する手動制御パラメータ算出手段をさらに備え、
前記配分量算出手段は、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする請求項１に記載の航走体の安定ロバスト制御装置。
【請求項４】
前記推力発生器駆動源に対する操作コマンドを入力する操作コマンド入力装置から入力された操作コマンドを予め定められた変換則に基づいて変換し、変換結果として制御力および制御モーメントを算出する手動制御パラメータ算出手段と、
前記自動制御パラメータ算出手段で算出された制御力および制御モーメントと、前記手動制御パラメータ算出手段で算出された制御力および制御モーメントとを加算する加算手段とをさらに備え、
前記配分量算出手段は、前記加算手段で加算された制御力および制御モーメントと前記配分則とに基づいて、前記配分量を算出することを特徴とする請求項２に記載の航走体の安定ロバスト制御装置。
【請求項５】
請求項３または請求項４に記載の航走体の安定ロバスト制御装置と、推進手段と、複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、姿勢に関する情報を検知するセンサとを有する航走体と、
前記航走体の姿勢制御用パラメータに対応した操作コマンドを入力する操作コマンド入力装置とを備えることを特徴とする航走体の安定ロバスト制御システム。
【請求項６】
前記操作コマンド入力装置から入力された操作コマンドを前記航走体に送信するコマンド送信装置と、
前記送信された操作コマンドを受信するコマンド受信装置とをさらに備え、
前記手動制御パラメータ算出手段は、前記受信された操作コマンドを前記変換則に基づいて変換することを特徴とする請求項５に記載の航走体の安定ロバスト制御システム。
【請求項７】
航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源と、安定ロバスト制御装置とを備えた航走体の安定ロバスト制御方法であって、
前記安定ロバスト制御装置は、
前記推力発生器駆動源の故障を検知する故障検知ステップと、
前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出ステップとを有することを特徴とする航走体の安定ロバスト制御方法。
【請求項８】
航走体の姿勢を安定に保つための複数の推力発生器と、前記推力発生器を駆動する複数の推力発生器駆動源とを備えた航走体の姿勢を安定に保つために、コンピュータを、
前記推力発生器駆動源の故障を検知する故障検知手段、
前記航走体の姿勢制御用パラメータとして予め算出された、前記航走体に付与する制御力および制御モーメントを、予め定められた配分則に基づいて、前記各推力発生器駆動源に対して配分する配分量を算出すると共に、前記推力発生器駆動源の故障が検知された場合に、前記予め算出された制御力および制御モーメントを、前記故障が検知された推力発生器駆動源を除く残りの推力発生器駆動源に対して再配分する配分量算出手段、
として機能させることを特徴とする航走体の安定ロバスト制御プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２００８−１０７９４３（Ｐ２００８−１０７９４３Ａ）
【公開日】平成２０年５月８日（２００８．５．８）
【国際特許分類】
【出願番号】特願２００６−２８８３５５（Ｐ２００６−２８８３５５）
【出願日】平成１８年１０月２４日（２００６．１０．２４）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．ＩＮＳ
【出願人】（５０４１９４８７８）独立行政法人海洋研究開発機構 (110)
【Ｆターム（参考）】
【公開日】平成２０年５月８日（２００８．５．８）
【国際特許分類】
【出願日】平成１８年１０月２４日（２００６．１０．２４）
【公序良俗違反の表示】
（特許庁注：以下のものは登録商標）
１．ＩＮＳ
【出願人】（５０４１９４８７８）独立行政法人海洋研究開発機構 (110)
【Ｆターム（参考）】
[ Back to top ]