JP2010143238A - 水中移動体の制御装置 - Google Patents

Abstract

【課題】水中移動体の位置および姿勢を精度良く制御する水中移動体の制御装置を提供することにある。
【解決手段】指令速度−指令推力変換手段２４は、コントローラ１３から入力した速度指令値を指令推力へと変換する。ＰＩＤ制御器２７は、変換された指令推力と、３軸周りの角速度と３軸方向の加速度から、ＸＹＺ方向の推力制御量を算出する。スラスタ推力配分手段２８は、ＸＹＺ方向の推力制御量を各スラスタが発生するスラスタ推力に変換する。スラスタ推力−モータ電圧変換手段２９は、スラスタ推力を各スラスタの駆動モータの指令電圧へと変換し、スラスタ２１，２２，２３に供給し、水中検査装置９を動作させる。初期キャリブレーション手段３０は、指令値−指令推力変換手段２４とスラスタ推力−モータ電圧変換手段２９で用いる変換ゲインを予め同定し、同定した変換ゲインを各手段２４，２９に設定する。
【選択図】図３

Description

本発明は、水中移動体の位置および姿勢を制御する制御装置に係り、特に、複数の推進器を備えた水中移動体を利用する場合に、その制御系の特性を使用前または使用中に自動的に把握して制御系へ反映するための水中移動体の制御装置に関する。

従来、水中移動体の位置および姿勢の制御は、ジャイロや加速度センサなどの慣性センサの信号をフィードバックし、制御量を算出している。例えば、加速度計、ジャイロセンサ、速度計で水中移動体の動作を計測し、具備している推進器の異常を検知することで、各推進器が発生する推力の配分量を調整し、ＰＩＤ制御器において制御量を生成する水中航走体が知られている（例えば、特許文献１参照）。

また、水中移動体の動特性モデルを予め用意し、指令値に対する実際の出力値とモデルから算出される理想的な出力値とを比較し、ＰＩＤ制御器の出力値に外力を打ち消すために必要な制御量を加えることで、未知外力がある中でも指令値に追従するための制御方式が知られている（例えば、特許文献２参照）。

特開２００８−１０７９４３号公報 特開平９−２３８３号公報

特許文献１で記載されたものでは、ジャイロや加速度センサなどの慣性センサの信号をフィードバックし、制御量を算出している。この方式では、ＰＩＤ制御器のＰＩＤゲインは固定値であり、さらに個々の推進器の特性が制御系で考慮されていない。従って、水中移動体に取り付けられているケーブルによる外力や流体抗力等の外乱存在下や、複数の推進器の特性が不均一で有る場合には、指令値への追従性に問題がある。

また、特許文献２で記載されたものでは、水中移動体の動特性モデルを予め用意し、センサフィードバックから求めた制御量を補正する方式が用いられているが、動特性モデルの構築に時間を要するため、モデル精度が低いと、制御性能が低下するという問題がある。

本発明の目的は、個々の推進器特性の違いや動作中に未知外乱が加わる環境において、水中移動体の位置および姿勢を精度良く制御する水中移動体の制御装置を提供することにある。

（１）上記目的を達成するために、本発明は、入力した動作指令値を指令推力に変換する指令値−指令推力変換手段と、該指令値−指令推力変換手段により変換された前記指令推力と、水中移動体に搭載されたセンサ信号の出力を用いて、動的にゲインを変更しながら推力制御量を生成するＰＩＤ制御手段と、該ＰＩＤ制御手段により生成された前記推力制御量を、スラスタ推力に変換するスラスタ推力配分手段と、該スラスタ推力配分手段により変換された前記スラスタ推力を、モータ電圧に変換するスラスタ推力−モータ電圧変換手段とを有し、該スラスタ推力−モータ電圧変換手段により変換された前記モータ電圧を推進機構に与えて、水中移動体を３次元移動する水中移動体の制御装置であって、前記指令値−指令推力変換手段と前記スラスタ推力−モータ電圧変換手段で用いる変換ゲインを予め同定し、同定した変換ゲインを前記指令値−指令推力変換手段と前記スラスタ推力−モータ電圧変換手段に設定する初期キャリブレーション手段を備えるようにしたものである。
かかる構成により、個々の推進器特性の違いや動作中に未知外乱が加わる環境において、水中移動体の位置および姿勢を精度良く制御し得るものとなる。

（２）上記（１）において、好ましくは、前記初期キャリブレーション手段は、前記水中移動体の各推進機構ごとに異なる指令値を入力し、その入力ごとの水中移動体の動作を計測する動作生成・計測手段と、該動作生成・計測手段の計測結果を基に動特性を解析する動特性解析手段と、該動特性解析手段の結果を基に、前記指令値−指令推力変換手段と前記スラスタ推力−モータ電圧変換手段で用いる前記変換ゲインを算出する変換ゲイン算出手段とを備えるようにしたものである。

（３）上記（１）において、好ましくは、前記動作生成・計測手段は、動作を計測する手段として、水中移動体の３軸周りの角速度を検知する角速度検知手段と、３軸方向の加速度を検知する加速度検知手段とを備えるようにしたものである。

（４）上記（１）において、好ましくは、前記ＰＩＤ制御器は、前記指令推力及び前記センサ信号を取得する指令推力・センサ信号取得手段と、前記指令推力と前記センサ信号を比較し、応答性，オーバーシュート量，定常偏差を含む制御特性量を算出し、この算出結果を基にＰＩＤゲインを決定し、そのゲインを制御系へ反映するゲイン更新手段とを備えるようにしたものである。

（５）上記（４）において、好ましくは、前記ゲイン更新手段は、前記制御特性解析手段の結果を一定時間蓄積しておき、それらデータを用いて、ＰＩＤゲインの増減を行うようにしたものである。

本発明によれば、個々の推進器特性の違いや動作中に未知外乱が加わる環境において、水中移動体の位置および姿勢を精度良く制御し得るものとなる。

以下、図１〜図１２を用いて、本発明の第１の実施形態による水中検査装置の制御装置の構成及び動作について説明する。
最初に、図１を用いて、原子炉内検査作業時における本実施形態による水中検査装置の機器配置について説明する。
図１は、原子炉内検査作業時における本発明の第１の実施形態による水中検査装置の機器配置の説明図である。

原子炉１の内部には、シュラウド２、上部格子板３、炉心支持板４、シュラウドサポート５等の構造物があり、ＰＬＲ配管６をはじめとする配管が接続されている。また、原子炉１の上部には、作業スペースであるオペレーションフロア７があり、同じく上方には燃料交換装置８がある。

原子炉内の目視検査を行う場合、水中検査装置９を投入し、検査員１４は、燃料交換装置８の上から、表示装置１２で水中検査装置９の現在位置を確認しながら、コントローラ１３により水中検査装置９の操作を行う。ここで、表示装置１２およびコントローラ１３は、制御装置１１と接続されている。水中検査装置９は、ケーブル１０を介して、制御装置１１と接続されている。

次に、図２を用いて、本実施形態による水中検査装置の構成について説明する。
図２は、本発明の第１の実施形態による水中検査装置の構成図である。

ここで、水中検査装置９に固定された座標系を、鉛直下向きがＺ軸正方向である右手座標系と定義する。水中検査装置９には、姿勢を検出するために慣性センサ部１８が設置されている。慣性センサ部１８の中には、３軸周りの角速度を検出する３軸ジャイロ１９と、３軸方向の加速度を検出する３軸加速度センサ２０が内蔵される。また、水中検査装置９には、画像取込部１６を設置し、目視検査用カメラ１５を内蔵している。さらに、３次元的な移動を可能とするため、各軸方向にそれぞれ、前後移動スラスタ２１、左右移動スラスタ２２、昇降移動スラスタ２３を備えている。

なお、慣性センサ部１８、画像取込部１６から出力される信号は、水中検査装置側信号伝送部１７及びケーブル１０を介して、制御装置１１に伝送される。スラスタ２１，２２，２３の駆動信号は、ケーブル１０を介して制御装置１１から伝送される。

次に、図３を用いて、本実施形態による水中検査装置の制御装置の構成について説明する。
図３は、本発明の第１の実施形態による水中検査装置の制御装置の構成を示すブロック図である。

制御装置１１は、指令速度−指令推力変換手段２４と、角速度算出手段２５と、加速度算出手段２６と、ＰＩＤ制御器２７と、スラスタ推力配分手段２８と、スラスタ推力−モータ電圧変換手段２９と、初期キャリブレーション手段３０とを備えている。

コントローラ１３が出力する速度指令値は、指令速度−指令推力変換手段２４に入力する。速度指令値は、検査員１４によって入力される。指令速度−指令推力変換手段２４は、速度指令値を指令推力に変換する。角速度算出手段２５は、３軸ジャイロ１９によって検知された水中検査装置９の角速度の出力信号を用いて、３軸周りの角速度を算出する。加速度算出手段２６は、加速度センサ２０によって検知された水中検査装置９の加速度の出力信号を用いて、３軸方向の加速度を算出する。

ＰＩＤ制御器２７は、変換された指令推力と角速度と加速度データを用いて、ＸＹＺ方向の推力制御量を算出する。スラスタ推力配分手段２８は、ＸＹＺ方向の推力制御量を各スラスタが発生する推力（スラスタ推力）に変換する。最後に、スラスタ推力−モータ電圧変換手段２９は、スラスタ推力を各スラスタの駆動モータの指令電圧へと変換し、スラスタ２１，２２，２３スラスタ２３へ供給し、水中検査装置９を動作させる。

なお、指令速度−指令推力変換手段２４とスラスタ推力−モータ電圧変換手段２９において使用する各種換算ゲインは、初期キャリブレーション手段３０によって求められる。

次に、図４を用いて、本実施形態による水中検査装置の制御装置に用いる初期キャリブレーション手段３０の構成について説明する。
図４は、本発明の第１の実施形態による水中検査装置の制御装置に用いる初期キャリブレーション手段の構成を示すブロック図である。

初期キャリブレーション手段３０は、動作生成・計測手段３０Ａと、動特性解析手段３０Ｂと、変換ゲイン算出手段３０Ｃとを備えている。動作生成・計測手段３０Ａは、水中検査装置９のスラスタ２１からスラスタ２３にそれぞれモータ電圧を入力し、その際の水中検査装置９の動作を内蔵している３軸ジャイロ１９および３軸加速度センサ２０で計測するものであり、その処理内容については、図８を用いて後述する。動特性解析手段３０Ｂは、動作生成・計測手段３０Ａにより計測した角速度と加速度、指令電圧をデータファイルとして保存するものであり、その処理内容については、図９を用いて後述する。変換ゲイン算出手段３０Ｃは、計測したデータから、指令速度と指令推力、スラスタ推力とモータ電圧の関係を表す近似式を導出するものであり、その処理内容については、図１０を用いて後述する。

次に、図５を用いて、本実施形態による水中検査装置の制御装置に用いるＰＩＤ制御器２７の構成について説明する。
図５は、本発明の第１の実施形態による水中検査装置の制御装置に用いるＰＩＤ制御器の構成を示すブロック図である。

ＰＩＤ制御器２７は、指令推力・加速度取得手段２７Ａと、ＰＩＤゲイン更新手段２７Ｂとを備えている。指令推力・加速度取得手段２７Ａは、指令速度−指令推力変換手段２４において算出された指令推力と、加速度算出手段２５において算出された３軸方向の加速度を取得するものであり、その処理内容については、図１１を用いて後述する。ＰＩＤゲイン更新手段２７Ｂは、最適なＰＩＤゲインを設定するものであり、その処理内容については、図１２，図１３を用いて後述する。

次に、図６〜図１３を用いて、本実施形態による水中検査装置の制御内容について説明する。
図６〜図１３は、本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。

最初に、図６に示すように、原子炉内検査を開始する前に、ステップＳ１００において、初期キャリブレーション手段３０は、初期キャリブレーション処理を実行する。初期キャリブレーション処理は、図３に示した指令速度−指令推力変換手段２４とスラスタ推力−モータ電圧変換手段２９において使用する各種換算ゲインを算出するものであり、その詳細については、図７〜図９を用いて後述する。

次に、ステップＳ２００において、水中検査装置１１は、ステップＳ１００で算出された各種換算ゲインを制御系へ反映し、水中検査装置制御処理を実行し、検査を開始する。なお、水中検査装置制御処理の詳細については、図１０〜図１２を用いて後述する。

最後に、ステップＳ３００において、水中検査装置１１は、検査終了信号の入力の有無を判定し、信号が入力されると、水中検査装置制御処理を終了する。

次に、図７〜図１０を用いて、本実施形態による水中検査装置における初期キャリブレーション処理の内容について説明する。
最初に、図７を用いて、図６のステップＳ１００の初期キャリブレーション処理の全体の流れについて説明する。

ステップＳ１１０の動作生成・計測処理において、動作生成・計測手段３０Ａは、水中検査装置９のスラスタ２１，２２，スラスタ２３にそれぞれモータ電圧を入力し、その際の水中検査装置９の動作を内蔵している３軸ジャイロ１９および３軸加速度センサ２０で計測する。

次に、ステップＳ１２０の動特性解析処理において、動特性解析手段３０Ｂは、計測した角速度と加速度、指令電圧をデータファイルとして保存する。

最後に、ステップＳ１３０の変換ゲイン算出処理において、変換ゲイン算出手段３０Ｃは、計測したデータから、指令速度と指令推力、スラスタ推力とモータ電圧の関係を表す近似式を導出し、処理を終了する。

次に、図８を用いて、図７のステップＳ１１０の動作生成・計測処理の内容について詳述する。

ステップＳ１１２のモータ指令電圧入力処理において、動作生成・計測手段３０Ａ各モータへ予め設定した指令電圧を順番に入力する。ここで、入力する指令電圧は、使用するモータの仕様から決定するが、本例では−１０Ｖから＋１０Ｖの指令値入力が可能であるものを用い、０Ｖから正負にそれぞれ２．５Ｖずつ、指令電圧を変化させる。そして、各指令電圧ごとに水中検査装置９の動作を計測する。

次にステップＳ１１４の速度・加速度算出処理において、動作生成・計測手段３０Ａは、速度と加速度を算出する。加速度A（Ax, Ay, Az）は、加速度センサ２０から出力される加速度信号から、高域通過フィルタを用いて重力加速度成分を除去し、加速度センサ固有のスペックとして示されている加速度換算係数を乗じて算出する。また、速度V（Vx, Vy, Vz）は、算出した加速度を積分し、算出する。

最後に、ステップＳ１１６において、動作生成・計測手段３０Ａは、それぞれの指令電圧ごとに、サンプリング時間、加速度A、速度Vを保存し、処理を終了する。

次に、図９を用いて、図７のステップＳ１２０の動特性解析処理の内容について詳述する。

ステップＳ１２２において、動特性解析手段３０Ｂは、ステップＳ１１０の動作生成・計測処理で保存したデータファイルを読込む。

次に、ステップＳ１２４において、動特性解析手段３０Ｂは、スラスタの進行方向と一致する軸方向の加速度と速度をデータファイルから取得し、加速度については、ある閾値以下になったサンプリング時間の一つ前の時間での加速度を、速度については、等速になった後の一定時間内のサンプリングデータの平均値を、指令電圧で計測された動特性として算出する。この処理を、スラスタ２１、スラスタ２２、スラスタ２３の順に、指令電圧−１０Ｖ〜＋１０Ｖでの動特性をそれぞれ算出する。

そして、ステップＳ１２６において、動特性解析手段３０Ｂは、各指令電圧での代表加速度と平均速度を保存し、処理を終了する。

次に、図１０を用いて、図７のステップＳ１３０の変換ゲイン算出処理の内容について詳述する。

ステップＳ１３２において、変換ゲイン算出手段３０Ｃは、ステップＳ１２０の動特性解析処理において保存した解析結果を読込む。

次に、ステップＳ１３４のスラスタ推力−モータ電圧変換ゲイン算出処理において、変換ゲイン算出手段３０Ｃは、読込んだ解析結果から指令電圧と加速度データを取得し、以下の式（１）を用いて、加速度データAをスラスタ推力Fmへと変換し、指令電圧Vconとスラスタ推力Fmから最小二乗法を用いて、近似曲線を算出する。

ここで、Ｍは水中検査装置９の水中での付加質量であり、予め同定試験により導出したものである。

次に、ステップＳ１３６の指令速度−指令推力変換ゲイン算出処理において、変換ゲイン算出手段３０Ｃは、読込んだ解析結果から速度Vを取得し、それとスラスタ推力Fmから最小二乗法を用いて、近似曲線を算出する。

最後に、ステップＳ１３８において、変換ゲイン算出手段３０Ｃは、ステップＳ１３４，Ｓ１３６の処理で算出した近似曲線の係数を変換ゲインとして保存し、処理を終了する。なお、保存した変換ゲインは、水中検査装置制御開始前に、図３のブロック図内における指令速度−指令推力変換手段２４、スラスタ推力−モータ電圧変換手段２９へ供給し、反映する。

次に、図１１〜図１３を用いて、本実施形態による水中検査装置における水中検査装置制御処理（図６のステップＳ２００）の内容について説明する。
最初に、図１１を用いて、図６のステップＳ２００の水中検査装置制御処理の全体の流れについて説明する。

ステップＳ２１０において、ＰＩＤ制御器２７は、図３の指令速度−指令推力変換手段２４において算出された指令推力と、図３の加速度算出手段２５において算出された３軸方向の加速度を取得する。

そして、ステップＳ２２０のＰＩＤゲイン更新処理において、ＰＩＤ制御器２７は、最適なＰＩＤゲインを設定する。ＰＩＤ更新処理の詳細処理内容については、図１２，図１３を用いて後述する。

次に、ステップＳ２３０の推力制御量算出処理において、ＰＩＤ制御器２７は、指令推力Frefと加速度Aから、以下の式（２）を用いて、推力制御量Fconを算出する。

式（２）のKp, Ki, Kdが、ＰＩＤゲイン更新処理により算出されるゲインである。

次に、ステップＳ２４０のスラスタ推力算出処理において、スラスタ推力配分手段２８は、各スラスタが発生すべき推力Fmを算出する。なお、本例では、図２に示したように、水中検査装置９のスラスタ配置はＸＹＺの各軸方向に１つずつ設置しているので、FconとFmは等しいものとなる。

さらに、ステップＳ２５０のモータ電圧算出処理において、スラスタ推力−モータ電圧変換手段２９は、スラスタ推力Fmをモータ指令電圧Vconへ変換する。変換には、初期キャリブレーション処理により導出したスラスタ推力−モータ電圧変換ゲインを反映させた以下の式（３）を用いる。

ここで、a, b, cは最小二乗法で算出された近似曲線の係数である。なお、本例では、近似曲線を2次多項式としたが、近似式と実データとが一致していない場合は、任意の次数に変更し、最小二乗法を実行すれば良い。

最後に、ステップＳ２６０において、スラスタ推力−モータ電圧変換手段２９は、モータ指令電圧Vconをスラスタ２１からスラスタ２３へ供給し、処理を終了する。

次に、図１２を用いて、図１１のステップＳ２２０のＰＩＤゲイン更新処理の内容を詳述する。

ステップＳ２２１の発生推力算出処理において、ＰＩＤ制御器２７は、加速度Aを用いて、水中検査装置９が発生している推力Fcurを算出する。

次に、ステップＳ２２３の外力推定処理において、ＰＩＤ制御器２７は、指令推力Frefと発生推力Fcurの各軸方向成分を比較し、水中検査装置９に加わっている外力Fext（流体抵抗、ケーブル外乱等）を算出し、ステップＳ２２５において、それを保存する。なお、本例において外力データを保存する際には、最新の算出結果以前の１００ステップ分のデータを保存することとする。

そして、ステップＳ２２７のＰＩＤゲイン判定処理において、ＰＩＤ制御器２７は、保存されている外力データFextを読込み、最適なＰＩＤゲインを決定・出力し、処理を終了する。なお、ステップＳ２２７のＰＩＤゲイン判定処理の詳細については、図１３を用いて後述する。

次に、図１３を用いて、図１２のステップＳ２２７のＰＩＤゲイン判定処理の内容を詳述する。

ステップＳ２２７Ａにおいて、ＰＩＤ制御器２７は、図１２のステップＳ２２５において保存した外力データFextを取得する。

そして、ステップＳ２２７Ｂ，Ｓ２２７Ｃにおいて、ＰＩＤ制御器２７は、PゲインKpとIゲインKiを外力データに応じて調整する。ここで、PゲインKpについては、ステップＳ２２７Ｂにおいて、ある閾値以下であれば減少、以上であれば増加させる。IゲインKiについては、ステップＳ２２７Ｃにおいて、外力データの平均値がある閾値以上であれば増加、以下であれば減少させる。これにより、オーバーシュート量と定常偏差を小さくすることができ、外力の大きさに応じて最適な推力制御量を生成することが可能となる。

最後に、ステップＳ２２７Ｄにおいて、ＰＩＤ制御器２７は、ＰＩＤゲインを保存し、処理を終了する。なお、DゲインKdは、制御系の応答時間を決定するゲインであるが、水中では移動体の動作応答が遅いため、本実施例ではゲインの増減は行わず固定値とする。ただし、動作の応答性が遅い場合は、指令推力Frefと発生推力Fcurの間で応答時間を算出し、その時間幅Δtを基にゲインの増減をすればよい。

以上説明した本実施形態によれば、水中検査装置９を動作させる際に、初期キャリブレーション機能と動的ゲイン変更機能を備えた制御装置を用いることで、検査開始前に各スラスタの制御ゲイン調整を迅速に実施でき、かつ移動中のビークルの直進性および操作性を向上できる。

以下、図１４〜図２０を用いて、本発明の第２の実施形態による水中検査装置の制御装置の構成及び動作について説明する。
本実施形態は、第１の実施形態と比較し、水中検査装置の構成が異なるが、それ以外の構成は同一である。すなわち、同一符号は同一部分を示している。そこで、以下、異なる点を主として説明する。

最初に、図１４を用いて、原子炉内検査作業時における本実施形態による水中検査装置の機器配置について説明する。
図１４は、原子炉内検査作業時における本発明の第２の実施形態による水中検査装置の機器配置の説明図である。

原子炉１の内部には、シュラウド２、上部格子板３、炉心支持板４、シュラウドサポート５等の構造物があり、ＰＬＲ配管６をはじめとする配管が接続されている。また、原子炉１の上部には、作業スペースであるオペレーションフロア７があり、同じく上方には燃料交換装置８がある。

原子炉内の目視検査を行う場合、水中検査装置９Ａに小型検査装置９Ｂを収納し、検査箇所付近まで投入し、検査付近到着後、水中検査装置９Ａより小型検査装置９Ｂを分離し、検査を開始する。検査員１４は、燃料交換装置８上から表示装置１２で水中検査装置９Ａと小型検査装置９Ｂの現在位置を確認しながら、コントローラ１３により操作を行う。ここで、表示装置１２およびコントローラ１３は、制御装置１１と接続され、水中検査装置９Ａは、一次ケーブル１０を介して、制御装置１１と接続され、水中検査装置９Ａと小型検査装置９Ｂは二次ケーブル１０Ｂを介して接続される。

なお、本実施形態では、水中検査装置９Ａが、初期キャリブレーション機能と動的ゲイン変更機能を備えている。そして、本実施形態による水中検査装置の制御装置の構成は、図３に示したものと同様である。

次に、図１５及び図１６を用いて、本実施形態による水中検査装置の構成について説明する。
図１５及び図１６は、本発明の第２の実施形態による水中検査装置の構成図である。

図１５は、水中検査装置９Ａの外観図であり、図１５(a)は前方、図１５(b)は後方から見た図である。

水中検査装置９Ａは、本体上部９３、本体推進部９４、本体下部９５の３つの部位で構成する。

ここで、本体下部９５には、水中検査装置９Ａの位置および姿勢を検知するための、３軸ジャイロ１０５と、３軸加速度センサ１０６とを搭載する。また、水中検査装置９Ａと制御装置１１を接続する一次ケーブル１０は、水中検査装置９Ａの直上においてサポートケーブル９７と接続し、水中検査装置９Ａのサポートケーブル取付部９６において支持する。なお、サポートケーブル９７と一次ケーブル１０の接続部は、圧着治具（図示せず）を用いて密着させる。一次ケーブル１０は、一次ケーブル接続部９８から、水中検査装置９Ａ本体に引き込む。

また、水中検査装置９Ａの上部には照明９９を設置し、本体上部９３には、カメラ１００を設置する。さらに、図１６で詳述するが、本体上部９３から本体推進部９４に掛けて、上下方向に、昇降スラスタダクト１０１ａ，１０１ｂを左右に設置し、また、本体推進部９４には、並進スラスタダクト１０２ａ，１０２ｂ，１０２ｃを３つ設置する。さらに、本体推進部９４から本体下部９５にかけて、小型検査装置９Ｂ、および小型検査装置の収納部１０３及び収納ガード１０４を設けている。

図１６は、水中検査装置９Ａの断面図であり、図１６(a)は中心軸を通る垂直断面、図１６(b)は並進スラスタダクト１０２ａ，１０２ｂ，１０２ｃを通る水平断面を示している。

昇降スラスタダクト１０１ａ，１０１ｂ内には、昇降スラスタ１０７ａ，１０７ｂを設置してあり、それらを駆動する推進用モータ１０８は、本体下部９５内に設置する。本体推進部９４の中央には、小型検査装置９Ｂと接続する二次ケーブル１０Ｂのハンドリングをするウインチ１０９を搭載する。また、本体下部９５の中央は、下部空洞１１０を設けている。

本体推進部９４には、３つの並進スラスタダクト１０２ａ，１０２ｂ，１０２ｃを設け、その中に、それぞれ並進スラスタ１１１ａ，１１１ｂ，１１１ｃを取り付ける。ここで、３つの並進スラスタ１１１ａ，１１１ｂ，１１１ｃの配置は、各々が１２０度を越えないように配置する。本例では、制御の簡便化のために、１２０度の均等配置とする。

次に、図１７〜図２０を用いて、本実施形態による水中検査装置の制御内容について説明する。
図１７〜図２０は、本発明の第２の実施形態による水中検査装置の制御内容を示すフローチャートである。

最初に、図１７に示すように、原子炉内検査を開始する前に、ステップＳ１００Ａにおいて、初期キャリブレーション手段３０は、初期キャリブレーション処理を実行する。初期キャリブレーション処理の詳細については、図１８〜図２０を用いて後述する。

次に、ステップＳ２００において、水中検査装置１１は、ステップＳ１００で算出された各種換算ゲインを制御系へ反映し、水中検査装置制御処理を実行し、検査を開始する。水中検査装置制御処理の詳細は、図１０〜図１２にて説明したものと同様である。

最後に、ステップＳ３００において、水中検査装置１１は、検査終了信号の入力の有無を判定し、信号が入力されると、水中検査装置制御処理を終了する。

次に、図１８〜図２０を用いて、初期キャリブレーション処理の処理内容について説明する。

図１８は、初期キャリブレーション処理の全体の流れを示す図である。

ステップＳ１３０Ａのスラスタ推力−モータ電圧変換ゲイン算出処理において、初期キャリブレーション手段３０は、スラスタ推力とモータ電圧の近似式を導出し、その係数を変換ゲインとして算出する。なお、ステップＳ１３０Ａの詳細は、図１９を用いて後述する。

次に、ステップＳ１３０Ｂの指令速度−指令推力変換ゲイン算出処理（ステップ１１４）において、初期キャリブレーション手段３０は、指令速度と指令推力の近似式を導出し、その係数を変換ゲインとして算出する。

図１９を用いて、図１８のステップＳ１３０Ａのスラスタ推力−モータ電圧変換ゲイン算出処理の内容について詳述する。

ステップＳ１１２のモータ指令電圧入力処理において、初期キャリブレーション手段３０は、モータに予め設定した指令電圧を順番に入力する。ここで、入力する指令電圧は、使用するモータの仕様から決定するが、本例では−１０Ｖから＋１０Ｖの指令値入力が可能であるものを用い、０Ｖから正負にそれぞれ２．５Ｖずつ、指令電圧を変化させる。

そして、ステップＳ１１４Ａの角速度計測・算出処理において、初期キャリブレーション手段３０は、各指令電圧ごとに水中検査装置９Ａの動作を３軸ジャイロ１０５を用いて計測し、その出力信号から角速度Ω（ωx, ωy, ωz）を算出する。

本例では、３軸ジャイロ１０５として静電浮上型ジャイロを用いるが、一般に、静電浮上型ジャイロは正の電圧値を出力するセンサであり、基準電圧と称する一定の電圧値からの増減値が、センサにかかる角速度に比例する性質を持っている。ここで、基準電圧は、通常、ジャイロ固有のスペックとして一定値が示されているが、本例では、角速度信号を入力しない場合の電圧値を予め計測し、それを平均化したものを基準電圧とする。そして、出力信号から基準電圧を減じ、それにジャイロ固有のスペックとして示されている電圧-角速度換算係数を乗じて、角速度を算出する。

次に、ステップＳ１３３Ａのスラスタ推力算出処理において、初期キャリブレーション手段３０は、ステップＳ１１４Ａで一定時間計測した角速度データを用いて、等角速度になった後の一定区間内のサンプリングデータの平均値を、指令電圧で計測された角速度として算出し、その角速度Ωを以下の式（４）を用いてスラスタ推力Fmへ変換する。

ここで、Ｎはモーメント、ｒは水中検査装置９ＡのＸＹ平面中心からスラスタまでの距離、Ｉは慣性モーメント、mは水中検査装置９Ａの水中での付加質量（予め同定試験により導出）である。

次に、ステップ１３４Ａの変換ゲイン算出処理（ステップ１２０）において、初期キャリブレーション手段３０は、指令電圧Vconとスラスタ推力Fmを用いて、最小二乗法により近似曲線を算出する。

最後に、ステップＳ１３８において、初期キャリブレーション手段３０は、近似式の係数を変換ゲインとして保存し、処理を終了する。

算出された変換ゲインは、図３のスラスタ推力−モータ電圧変換手段２９に供給し、反映する。なお、ステップＳ１１２からステップＳ１３４Ａは各スラスタ毎に繰り返し、全てのスラスタについて変換ゲインを算出した後、ステップＳ１３８に進むものとする。

次に、図２０を用いて、図１８の指令速度−指令推力変換ゲイン算出処理の内容について詳述する。

ステップＳ１１３の指令速度入力処理において、初期キャリブレーション手段３０は、予め設定した指令速度を順番に入力し、ＸＹＺ方向に動作させる。ここで、入力する指令速度は、使用する水中検査装置９Ａの最大移動速度から決定するが、本実施例では６０ｍｍ／ｓとし、０ｍｍ／ｓから正負にそれぞれ１５ｍｍ／ｓずつ、指令速度を変化させる。

そして、ステップＳ１１４Ｂの加速度計測・算出処理において、初期キャリブレーション手段３０は、各指令速度ごとに水中検査装置９Ａの動作を３軸加速度１０６を用いて計測し、加速度を算出する。加速度A（Ax, Ay, Az）は、加速度センサ１０６から出力される加速度信号から、高域通過フィルタを用いて重力加速度成分を除去し、加速度センサ固有のスペックとして示されている加速度換算係数を乗じて算出する。

次に、ステップＳ１３３Ｂの軸方向推力算出処理において、初期キャリブレーション手段３０は、ステップＳ１１４Ｂで一定時間計測した加速度データを用いて、ある閾値以下になったサンプリング時間の一つ前の時間での加速度を、指令速度で計測された加速度として算出し、その加速度を以下の式（５）を用いて軸方向推力Fへ変換する。

ここで、Ｍは水中検査装置９Ａの水中での付加質量であり、予め同定試験により導出するものである。

さらに、ステップＳ１３６Ａの変換ゲイン算出処理において、初期キャリブレーション手段３０は、指令速度と軸方向推力Ｆｍを用いて、最小二乗法により近似曲線を算出する。

最後に、ステップＳ１３８において、初期キャリブレーション手段３０は、近似式の係数を変換ゲインとして保存し、処理を終了する。算出された変換ゲインは、図３の指令速度−指令推力変換手段２４に供給し、反映する。なお、ステップＳ１１３からステップＳ１３６Ａは各スラスタ毎に繰り返し、全てのスラスタについて変換ゲインを算出した後、ステップＳ１３８に進むものとする。

以上説明した本実施形態によれば、水中検査装置９を動作させる際に、初期キャリブレーション機能と動的ゲイン変更機能を備えた制御装置を用いることで、検査開始前に各スラスタの制御ゲイン調整を迅速に実施でき、かつ移動中のビークルの直進性および操作性を向上できる。

また、本実施形態によれば、水中検査装置の推進器配置が各軸方向に独立でない場合においても、初期キャリブレーションにより各スラスタの制御ゲイン調整を自動で実施できる。

なお、本発明は、原子炉内の検査に用いる検査装置のみでなく、水中で使用する移動体に広く適用できるものであり、特に複数の推進器を持つ移動体の制御パラメータ調整時間の短縮および移動性能向上に適している。

原子炉内検査作業時における本発明の第１の実施形態による水中検査装置の機器配置の説明図である。 本発明の第１の実施形態による水中検査装置の構成図である。 本発明の第１の実施形態による水中検査装置の制御装置の構成を示すブロック図である。 本発明の第１の実施形態による水中検査装置の制御装置に用いる初期キャリブレーション手段の構成を示すブロック図である。 本発明の第１の実施形態による水中検査装置の制御装置に用いるＰＩＤ制御器の構成を示すブロック図である。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第１の実施形態による水中検査装置の制御内容を示すフローチャートである。 原子炉内検査作業時における本発明の第２の実施形態による水中検査装置の機器配置の説明図である。 本発明の第２の実施形態による水中検査装置の構成図である。 本発明の第２の実施形態による水中検査装置の構成図である。 本発明の第２の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第２の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第２の実施形態による水中検査装置の制御内容を示すフローチャートである。 本発明の第２の実施形態による水中検査装置の制御内容を示すフローチャートである。

符号の説明

１…原子炉
２…シュラウド
３…上部格子板
４…炉心支持板
５…シュラウドサポート
６…ＰＬＲ配管
７…オペレーションフロア
８…燃料交換装置
９，９Ａ…水中検査装置
９Ｂ…小型検査装置
１０…ケーブル
１０Ｂ…２次ケーブル
１１…制御装置
１２…表示装置
１３…コントローラ
１４…検査員
１５…目視検査用カメラ
１６…画像取込部
１７…検査装置側信号伝送部
１８…慣性センサ部
１９…３軸ジャイロ
２０…３軸加速度センサ
２１…前後移動スラスタ
２２…左右移動スラスタ
２３…昇降移動スラスタ
２４…指令速度−指令推力変換手段
２５…角速度算出手段
２６…加速度算出手段
２７…ＰＩＤ制御器
２７Ａ…指令推力・加速度取得手段
２７Ｂ…ＰＩＤゲイン更新手段
２８…スラスタ推力配分手段
２９…スラスタ推力−モータ電圧変換手段
３０…初期キャリブレーション手段
３０Ａ…動作生成・計測手段
３０Ｂ…動特性解析手段
３０Ｃ…変換ゲイン算出手段
９３…本体上部
９４…本体推進部
９５…本体下部
９６…サポートケーブル取付部
９７…サポートケーブル
９８…一次ケーブル接続部
９９…照明
１００…カメラ
１０１ａ，１０１ｂ…昇降スラスタダクト
１０２ａ，１０２ｂ，１０２ｃ…並進スラスタダクト
１０３…小型検査装置収納部
１０４…小型検査装置収納ガード
１０５…３軸ジャイロ
１０６…３軸加速度センサ
１０７ａ，１０７ｂ…昇降スラスタ
１０８…推進用モータ
１０９…ウインチ
１１０…下部空洞
１１１ａ，１１１ｂ，１１１ｃ…並進スラスタ

Claims (5)

1. 入力した動作指令値を指令推力に変換する指令値−指令推力変換手段と、
   該指令値−指令推力変換手段により変換された前記指令推力と、水中移動体に搭載されたセンサ信号の出力を用いて、動的にゲインを変更しながら推力制御量を生成するＰＩＤ制御手段と、
   該ＰＩＤ制御手段により生成された前記推力制御量を、スラスタ推力に変換するスラスタ推力配分手段と、
   該スラスタ推力配分手段により変換された前記スラスタ推力を、モータ電圧に変換するスラスタ推力−モータ電圧変換手段とを有し、
   該スラスタ推力−モータ電圧変換手段により変換された前記モータ電圧を推進機構に与えて、水中移動体を３次元移動する水中移動体の制御装置であって、
   前記指令値−指令推力変換手段と前記スラスタ推力−モータ電圧変換手段で用いる変換ゲインを予め同定し、同定した変換ゲインを前記指令値−指令推力変換手段と前記スラスタ推力−モータ電圧変換手段に設定する初期キャリブレーション手段を備えることを特徴とする水中移動体の制御装置。
2. 請求項１記載の水中移動体の制御装置において、
   前記初期キャリブレーション手段は、
   前記水中移動体の各推進機構ごとに異なる指令値を入力し、その入力ごとの水中移動体の動作を計測する動作生成・計測手段と、
   該動作生成・計測手段の計測結果を基に動特性を解析する動特性解析手段と、
   該動特性解析手段の結果を基に、前記指令値−指令推力変換手段と前記スラスタ推力−モータ電圧変換手段で用いる前記変換ゲインを算出する変換ゲイン算出手段とを備えることを特徴とする水中移動体の制御装置。
3. 請求項１記載の水中移動体の制御装置において、
   前記動作生成・計測手段は、動作を計測する手段として、水中移動体の３軸周りの角速度を検知する角速度検知手段と、３軸方向の加速度を検知する加速度検知手段とを備えることを特徴とする水中移動体の制御装置。
4. 請求項１記載の水中移動体の制御装置において、
   前記ＰＩＤ制御器は、
   前記指令推力及び前記センサ信号を取得する指令推力・センサ信号取得手段と、
   前記指令推力と前記センサ信号を比較し、応答性，オーバーシュート量，定常偏差を含む制御特性量を算出し、この算出結果を基にＰＩＤゲインを決定し、そのゲインを制御系へ反映するゲイン更新手段とを備えることを特徴とする水中移動体の制御装置。
5. 請求項４記載の水中移動体の制御装置において、
   前記ゲイン更新手段は、前記制御特性解析手段の結果を一定時間蓄積しておき、それらデータを用いて、ＰＩＤゲインの増減を行うことを特徴とする水中移動体の制御装置。

Images