(57)【特許請求の範囲】
【請求項１】
水上又は水中を航走する航走体であって、
前記航走体は、
現在の速度、位置又は姿勢を検出し、検出値を出力するセンサー部と、
制御信号に応じ、位置又は姿勢に変化を与える駆動部と、
前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて前記制御信号を生成し、前記駆動部に対し前記制御信号を出力する制御部とを有し、
前記制御部は、
前記センサー部から所定のサンプル時間間隔で入力される前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、
速度にかかる前記検出値が大きくなると前記サンプル個数を減少させ、速度にかかる前記検出値が小さくなると前記サンプル個数を増大させるよう、前記サンプル個数の数を変化させる
航走体。
【請求項２】
水上又は水中を航走する航走体であって、
前記航走体は、
現在の速度、位置又は姿勢を検出し、検出値を出力するセンサー部と、
制御信号に応じ、位置又は姿勢に変化を与える駆動部と、
前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて前記制御信号を生成し、前記駆動部に対し前記制御信号を出力する制御部とを有し、
前記制御部は、
前記センサー部から所定のサンプル時間間隔で入力する前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、
速度にかかる前記検出値が大きくなると前記サンプル時間間隔を短くするよう前記センサー部を制御し、速度にかかる前記検出値が低くなると前記サンプル時間間隔を長くするよう前記センサー部を制御する
航走体。
【請求項３】
前記制御部は、
ＰＩＤ（Ｐｒｏｐｏｒｔｉｏｎａｌ  Ｉｎｔｅｇｒａｌ  Ｄｅｒｉｖａｔｉｖｅ）制御方式における制御ゲインパラメータを、速度にかかる前記検出値に基づいて算出するとともに、
前記センサー部から入力される前記検出値、前記設定値及び前記制御ゲインパラメータに基づいて、前記ＰＩＤ制御方式により前記制御信号を生成する
請求項１又は２記載の航走体。
【請求項４】
水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から検出値を入力する入力部と、
前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する制御部とを有し、
前記制御部は、
前記センサー部から所定のサンプル時間間隔で入力する前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、
前記速度にかかる前記検出値が高くなると前記サンプル個数を減少させ、前記速度にかかる前記検出値が低くなると前記サンプル個数を増大させるよう、前記サンプル個数の数を変化させる
操縦装置。
【請求項５】
水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から検出値を入力する入力部と、
前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する制御部とを有し、
前記制御部は、
前記センサー部から所定のサンプル時間間隔で入力する前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、
速度にかかる前記検出値が大きくなると前記サンプル時間間隔を短くするよう前記センサー部を制御し、速度にかかる前記検出値が低くなると前記サンプル時間間隔を長くするよう前記センサー部を制御する
操縦装置。
【請求項６】
前記制御部は、
ＰＩＤ（Ｐｒｏｐｏｒｔｉｏｎａｌ  Ｉｎｔｅｇｒａｌ  Ｄｅｒｉｖａｔｉｖｅ）制御方式における制御ゲインパラメータを、速度にかかる前記検出値に基づいて算出するとともに、
前記センサー部から入力される前記検出値、前記設定値及び前記制御ゲインパラメータに基づいて、前記ＰＩＤ制御方式により前記制御信号を生成する
請求項４又は５記載の操縦装置。
【請求項７】
水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から入力される検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する操縦制御方法であって、
前記センサー部から所定のサンプル時間間隔で入力される前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、
前記速度にかかる前記検出値が高くなると前記サンプル個数を減少させ、前記速度にかかる前記検出値が低くなると前記サンプル個数を増大させるよう、前記サンプル個数の数を変化させる
操縦制御方法。
【請求項８】
水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から入力される検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する操縦制御方法であって、
前記センサー部から所定のサンプル時間間隔で入力される前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、
速度にかかる前記検出値が大きくなると前記サンプル時間間隔を短くするよう前記センサー部を制御し、速度にかかる前記検出値が低くなると前記サンプル時間間隔を長くするよう前記センサー部を制御する
操縦制御方法。
【請求項９】
前記制御信号は、前記センサー部から入力される前記検出値、速度にかかる前記検出値に基づいて算出される制御ゲインパラメータ、及び前記設定値に基づいて、ＰＩＤ（Ｐｒｏｐｏｒｔｉｏｎａｌ  Ｉｎｔｅｇｒａｌ  Ｄｅｒｉｖａｔｉｖｅ）制御方式により生成される
請求項７又は８記載の操縦制御方法。
【請求項１０】
請求項７乃至９いずれか１項記載の方法を、コンピュータに実行させるための
プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、航走体、操縦装置、操縦制御方法及びプログラムに関し、特に水上及び水中の航走体の針路、姿勢角及び深度を変更するため舵を制御する技術に関する。
【背景技術】
【０００２】
従来より、航走体の速度等を検出し、これらの検出結果に基づいて駆動装置を自動制御する航走体の操縦装置が知られている。
【０００３】
例えば、特許文献１は、図６に示すような船舶自動操縦装置を開示している。この船舶自動操縦装置１００のＰＩＤ制御手段１０９は、設定器１０１から設定された設定コースφ０と実コース検出器１０２が検出した実コースφとの偏差ｅに応じて、舵指令β０を発生する。ＰＩＤ制御手段１０９はさらに、ゲイン計算手段１０５が出力する制御ゲインを舵指令βに反映させる。ゲイン計算手段１０５は、舵特性メモリ１０６が記憶している速度に応じた舵の発生力特性、速度検出器１０３からの速度信号Ｅ、偏差信号ｅ及び外力推定手段１０７が出力する船にかかる外力の推定値に基づいて、制御ゲインの計算を行う。外力推定手段１０７は、運動特性メモリ１０８が記憶している船舶の運動特性をもとに速度検出器１０３が検出した速度信号ｕ、姿勢検出器１０４が検出した船の方位角信号ψ、ピッチ角信号θ、ロール角信号φ及びアクチュエータ１１０からの実舵角信号βを観測値とし、カルマンフィルタにより船の外力を推定計算する。
【０００４】
また、特許文献２は、アクセル操作量を所定のサンプリングタイムでサンプリングし、現時点から過去の所定数のサンプル値を平均し、その平均値に基づいて走行用電動機を駆動する電気走行車を開示している。この電気走行車は、サンプル値が小さくなると、サンプリングタイムを変更して長くし、又はサンプル数を変更して多くすることにより、走行用電動機の応答性を緩和する構成を備えている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開平０４−１３５９９９号公報
【特許文献２】特開平０５−１６８１０８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
一般に、検出器が出力する信号にはノイズが含まれることがあり、そのノイズは当該信号を用いるその後の計算において、誤差の発生要因となり得る。特許文献１記載の船舶自動操縦装置４００においても、実コース検出器４０２、速度検出器４０３、姿勢検出器４０４の各検出器の出力値には、ノイズ等が含まれ得、このノイズにより、ＰＩＤ制御手段４０９が計算する舵指令β０の値にも誤差が生じ得る。
【０００７】
このノイズの影響を抑制する手法の一つとして、信号の平滑化が知られている。例えば、制御器が、検出器からの一定のサンプル時間間隔で入力される信号のうち、一番新しく入力された信号から順にある一定のサンプル個数の信号を選択し、これらの信号の平均値、すなわち移動平均値を求めて、この移動平均値を後の計算に用いる。
【０００８】
しかしながら、この手法では、サンプル個数が多いほど、制御による遅延に伴う影響が大きくなり、制御器からの出力波形が発散し、アクチュエータが安定しない。これは速度が高い場合に特に問題になる。サンプル個数を少なくすれば、当該問題は解消するが、ノイズを除去しきれず、そのノイズがアクチュエータの動作に悪影響を与える。これは、遅延がさほど問題にならない低速の場合において新たな問題となる。
【０００９】
特許文献２記載の電気走行車は、このような信号の平滑化をアクセル操作について適用した例であり、アクセル操作量に対する電動機の応答性を、速度に応じて制御することを可能としている。しかしながら、特許文献２記載の構成は、単に設定器（特許文献２におけるアクセルに相当）の出力を平滑化するものであって、設定器だけでなく、種々の検出器からの出力に基づいてアクチュエータを制御しなければならない航走体に対しては、直ちに適用できないという問題があった。
【００１０】
本発明は、このような問題点を解決するためになされたものであり、ノイズの影響を抑制するとともに、外乱作用時においても安定的な航走が可能となる航走体、操縦装置、操縦制御方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１１】
本発明に係る航走体は、水上又は水中を航走する航走体であって、前記航走体は、現在の速度、位置又は姿勢を検出し、検出値を出力するセンサー部と、制御信号に応じ、位置又は姿勢に変化を与える駆動部と、前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて前記制御信号を生成し、前記駆動部に対し前記制御信号を出力する制御部とを有し、前記制御部は、前記センサー部から所定のサンプル時間間隔で入力される前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、速度にかかる前記検出値が大きくなると前記サンプル個数を減少させ、速度にかかる前記検出値が小さくなると前記サンプル個数を増大させるよう、前記サンプル個数の数を変化させるものである。
【００１２】
本発明に係る航走体は、水上又は水中を航走する航走体であって、前記航走体は、現在の速度、位置又は姿勢を検出し、検出値を出力するセンサー部と、制御信号に応じ、位置又は姿勢に変化を与える駆動部と、前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて前記制御信号を生成し、前記駆動部に対し前記制御信号を出力する制御部とを有し、前記制御部は、前記センサー部から所定のサンプル時間間隔で入力する前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、速度にかかる前記検出値が大きくなると前記サンプル時間間隔を短くするよう前記センサー部を制御し、速度にかかる前記検出値が低くなると前記サンプル時間間隔を長くするよう前記センサー部を制御するものである。
【００１３】
本発明に係る操縦装置は、水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から検出値を入力する入力部と、前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する制御部とを有し、前記制御部は、前記センサー部から所定のサンプル時間間隔で入力する前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、前記速度にかかる前記検出値が高くなると前記サンプル個数を減少させ、前記速度にかかる前記検出値が低くなると前記サンプル個数を増大させるよう、前記サンプル個数の数を変化させるものである。
【００１４】
本発明に係る操縦装置は、水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から検出値を入力する入力部と、前記センサー部から入力される前記検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する制御部とを有し、前記制御部は、前記センサー部から所定のサンプル時間間隔で入力する前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、速度にかかる前記検出値が大きくなると前記サンプル時間間隔を短くするよう前記センサー部を制御し、速度にかかる前記検出値が低くなると前記サンプル時間間隔を長くするよう前記センサー部を制御するものである。
【００１５】
本発明に係る操縦制御方法は、水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から入力される検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する操縦制御方法であって、前記センサー部から所定のサンプル時間間隔で入力される前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、前記速度にかかる前記検出値が高くなると前記サンプル個数を減少させ、前記速度にかかる前記検出値が低くなると前記サンプル個数を増大させるよう、前記サンプル個数の数を変化させるものである。
【００１６】
本発明に係る操縦制御方法は、水上又は水中を航走する航走体の現在の速度、位置又は姿勢を検出するセンサー部から入力される検出値と、目標となる位置又は姿勢を示す設定値とに基づいて制御信号を生成し、前記航走体の位置又は姿勢に変化を与える駆動部に対し、前記制御信号を出力する操縦制御方法であって、前記センサー部から所定のサンプル時間間隔で入力される前記検出値のうち、最新の所定のサンプル個数の前記検出値の平均値である移動平均値を、前記制御信号の生成に利用するとともに、速度にかかる前記検出値が大きくなると前記サンプル時間間隔を短くするよう前記センサー部を制御し、速度にかかる前記検出値が低くなると前記サンプル時間間隔を長くするよう前記センサー部を制御するものである。
【００１７】
本発明に係るプログラムは、上述の操縦制御方法を、コンピュータに実行させるためのプログラムである。
【発明の効果】
【００１８】
本発明により、ノイズの影響を抑制するとともに、外乱作用時においても安定的な航走が可能となる航走体、操縦装置、操縦制御方法及びプログラムを提供することができる。
【図面の簡単な説明】
【００１９】
【図１】実施の形態１にかかる水中航走体の構成を示す図である。
【図２】実施の形態１にかかる水中航走体の処理を示す図である。
【図３】実施の形態１にかかる水中航走体の処理を示す図である。
【図４】実施の形態２にかかる水中航走体の構成を示す図である。
【図５】実施の形態２にかかる水中航走体の処理を示す図である。
【図６】従来の水中航走体の構成を示す図である。
【発明を実施するための形態】
【００２０】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。
【００２１】
＜実施の形態１＞
本実施の形態は、本発明を水中航走体の鉛直方向の運動制御へ適用したものである。
【００２２】
図１に、本実施の形態１における水中航走体１の構成を示す。
【００２３】
航走体としての水中航走体１は、センサー部としての位置検出器２及び速度検出器３、駆動部としての舵アクチュエータ５、制御部としての設定器４及び操縦装置６を有している。
【００２４】
位置検出器２は、水中航走体１の運動に伴う位置を検出するセンサーである。本実施の形態では、位置検出器２は、水面と水中航走体１の間の距離である深度を計測する深度計であり、所定のサンプル時間間隔で深度を出力する。
【００２５】
速度検出器３は、水中航走体１の進行方向の速度を計測するセンサーである。速度検出器３は、計測した速度をサンプル時間間隔で出力する。
【００２６】
設定器４は、水中航走体の垂直方向の位置である深度の目標値を設定するための入力手段である。
【００２７】
舵アクチュエータ５は、水中航走体１の垂直方向の位置を変える舵を駆動するためのアクチュエータである。舵アクチュエータ５は、後述の操縦装置６が出力する舵の命令値に応じて動作する。
【００２８】
操縦装置６は、位置検出器２から検出された水中航走体１の深度と、設定器４による深度の目標設定値との差を示す誤差信号に基づき、舵アクチュエータ５に対して出力すべき命令値を演算し、出力を行う。これにより、舵アクチュエータ５が動作し、水中航走体１の深度に変化が与えられ、再びその深度が位置検出器２によって検出される。このように、操縦装置６はフィードバックによる制御系を構成している。
【００２９】
操縦装置６は、典型的には図示しない記憶手段に格納された各種プログラムに基づいて各種制御を実行する機能を有する制御手段であり、中央演算処理装置（ＣＰＵ）、読出専用メモリ（ＲＯＭ）、ランダムアクセスメモリ（ＲＡＭ）、入出力ポート（Ｉ／Ｏ）等により実現される。
【００３０】
操縦装置６は、移動平均処理部７、移動平均処理部８、制御係数演算部９及びＰＩＤ制御部１０を有している。
【００３１】
移動平均処理部７及び移動平均処理部８は、それぞれ位置検出器２及び速度検出器３から所定のサンプル時間間隔で入力される信号（検出値）から、最新の所定のサンプル個数の信号を選択し、これらの選択された信号の平均値である移動平均値を出力することにより、信号の平滑化を行う演算処理手段である。
【００３２】
制御係数演算部９は、移動平均処理部８から入力される速度に基づいて、移動平均処理部７において移動平均値を求める際に用いるべきサンプル個数Ｎａ、及びＰＩＤ制御器１０における制御ゲイン係数である比例ゲイン係数Ｋｐ、積分ゲイン係数Ｋｉ並びに微分ゲイン係数Ｋｄをそれぞれ算出する。
【００３３】
ＰＩＤ制御部１０は、移動平均処理部７から入力される深度と、設定器４から入力される目標深度（設定値）との差である、誤差信号ｅを入力信号とする。ＰＩＤ制御部１０は、この入力信号ｅから、このｅに対し時間積分したｅｉ、ｅに対し時間微分したｅｄを求める。ＰＩＤ制御部１０は、信号ｅに対して比例ゲインＫｐ、信号ｅｉに対し積分ゲイン係数Ｋｉ、信号ｅｄに対し微分ゲイン係数Ｋｄを乗じ、これらの総和を舵アクチュエータ５に対する出力とする。
【００３４】
次に、図１を用いて、水中航走体１における信号の流れについて説明する。
【００３５】
速度検出器３は、水中航走体１の進行方向速度を検出し、その検出値をサンプル時間間隔で操縦装置６に出力する。速度検出器３の検出値は、新しいものから順にｗ１、ｗ２、ｗ３、・・・・・、ｗｊとする。
【００３６】
操縦装置６の移動平均処理部８は、速度検出器３から入力される上記検出値のうち、新しいものから順にサンプル個数ｊの検出値を選択し、これらの選択された検出値の平均値すなわち移動平均値を演算する。この移動平均値を、速度Ｖとする。Ｖは以下の数１により求められる。
【００３７】
【数１】
0005888007.tif 000002
【００３８】
制御係数演算部９は、移動平均処理部８から入力される平滑化された速度Ｖに基づいて、移動平均処理部７が演算に利用するサンプル個数と、ＰＩＤ制御部１０が演算に利用する制御ゲイン係数とを算出する。
【００３９】
図２に、進行速度Ｖと、移動平均処理部７が移動平均値を演算する際に利用するサンプル個数との関係を表す関数ｆ（Ｖ）を示す。
【００４０】
ここで、水中航走体１の高速走行時における進行速度Ｖをｕ、ｕよりも低速走行時における進行速度Ｖをｖとする。また、速度ｕのときのサンプル個数をｍ、速度ｖのときのサンプル個数をｎとする。
【００４１】
関数ｆ（Ｖ）は単調減少関数であり、高速で走行する際にはサンプル個数は少なく、低速で走行する際にはサンプル個数は多くなる。サンプル個数が少なくなれば、移動平均処理部７及び移動平均処理部８は、より高速に移動平均値を出力できるため、制御における遅延はより少なくなる。一方、サンプル値が多くなれば、検出値にノイズが混入したとしても、そのノイズが移動平均値の算出に与える影響を抑制することができる。このように、サンプル個数の多寡に応じて、ＰＩＤ制御部１０の出力値の性質は異なるものとなるが、本実施の形態では、このサンプル個数を速度に応じて変化させることにより、ＰＩＤ制御部１０は速度に応じて適切に舵アクチュエータ５を制御することが可能となっている。
【００４２】
図３に、進行速度Ｖと、ＰＩＤ制御部１０におけるＰＩＤ制御に関係するゲイン係数Ｋｐ、Ｋｄ及びＫｉとの関係を表す関数Ｆｐ（Ｖ）、Ｆｄ（Ｖ）及びＦｉ（Ｖ）を示す。
ゲイン係数Ｋｐは、ＰＩＤ制御部１０への入力信号に対して直接の係数となる比例ゲイン係数である。ゲイン係数Ｋｄは、入力信号の時間微分に対しての係数すなわち微分ゲイン係数である。ゲイン係数Ｋｉは、入力信号の時間積分に対しての係数すなわち積分ゲイン係数である。
【００４３】
本実施の形態における関数Ｆｐ（Ｖ）、Ｆｄ（Ｖ）及びＦｉ（Ｖ）は、図３に示すようにいずれも単調減少関数とする。これらの関数によれば、水中航走体１の速度ｕでの高速走行時の制御ゲイン係数として、比例ゲイン係数Ｋｐｖ、積分ゲイン係数Ｋｉｕ、微分ゲイン係数Ｋｄｕが得られる。また、より低速な速度ｖでの走行時の制御ゲイン係数としては、比例ゲイン係数Ｋｐｖ、積分ゲイン係数Ｋｉｖ、微分ゲイン係数Ｋｄｖが得られる。
【００４４】
他方、水中航走体１の位置検出器２においては、深度の検出値ｄが所定のサンプル時間間隔で操縦装置６に出力される。位置検出器２の検出値は、新しいものから順にｄ１、ｄ２、ｄ３、・・・・・ｄｉとする。
【００４５】
操縦装置６の移動平均処理部７は、位置検出器２から入力される上記検出値のうち、新しいものから順に、関数ｆ（Ｖ）により定められるサンプル個数の検出値を選択し、これらの選択された検出値の平均値すなわち移動平均値を演算する。この移動平均値を、深度Ｄとする。
【００４６】
例えば高速ｕでの走行時においては、関数ｆ（Ｖ）によりサンプル個数はｍとなるから、深度の移動平均値Ｄｕは、以下の数２により求められる。
【００４７】
【数２】
0005888007.tif 000003
【００４８】
また、より低速な速度ｖでの走行時においては、関数ｆ（Ｖ）によりサンプル個数はｎとなるから、深度の移動平均値Ｄｕは、以下の数３により求められる。
【００４９】
【数３】
0005888007.tif 000004
【００５０】
制御係数演算部９は、移動平均処理部８から入力される進行速度Ｖに基づいて、移動平均処理部７が演算に利用するサンプル個数と、ＰＩＤ制御部１０が演算に利用する制御ゲイン係数とを算出する。
【００５１】
ＰＩＤ制御部１０は、移動平均処理部７から入力された平滑化された深度Ｄと、設定器４で設定された目標深度である設定値Ｄ０と差である誤差信号ｅとに基づいて、時間積分したｅｉ、時間微分したｅｄを求める。さらに、ＰＩＤ制御部１０は、ｅ、ｅｉ及びｅｄそれぞれについて、制御係数演算部９により水中航走体１の進行速度Ｖに応じて算出された比例ゲイン係数Ｋｐ、積分ゲイン係数Ｋｉ、微分ゲイン係数Ｋｄを乗じ、これらの総和を舵アクチュエータ５に対して出力する。これに応じ、舵アクチュエータ５が、水中航走体１の鉛直方向の運動を制御する。
【００５２】
ここで、これらの制御ゲイン係数としては、図３に示すように、水中航走体１の進行速度が高速ｕのときは比例ゲイン係数Ｋｐｖ、積分ゲイン係数Ｋｉｕ、微分ゲイン係数Ｋｄｕが適用され、低速ｖのときは比例ゲイン係数Ｋｐｖ、積分ゲイン係数Ｋｉｖ、微分ゲイン係数Ｋｄｖが適用される。
【００５３】
本実施の形態においては、制御係数演算部９が、移動平均処理部８から入力される速度Ｖに応じて最適なサンプル個数及び制御ゲイン係数とを演算し、ＰＩＤ制御部１０が、これらのサンプル個数及び制御ゲイン係数に基づいて水中航走体１の鉛直方向の運動を制御する。よって、水中航走体１は、制御における遅延及びノイズの影響を適切に抑制することができる。
【００５４】
＜実施の形態２＞
図４を用いて、本発明の実施の形態２にかかる水中航走体１の構成について説明する。
【００５５】
本実施の形態にかかる水中航走体１の構成要素及びそれらの機能は、実施の形態１にかかるものと同様である。すなわち、水中航走体１は、センサー部としての位置検出器２及び速度検出器３、駆動部としての舵アクチュエータ５、制御部としての設定器４及び操縦装置６を有している。
【００５６】
次に、図４を用いて、本実施の形態にかかる水中航走体１における信号の流れについて説明する。
【００５７】
速度検出器３は、水中航走体１の進行方向速度を検出し、その検出値をサンプル時間間隔で操縦装置６に出力する。速度検出器３の検出値は、新しいものから順にｗ１、ｗ２、ｗ３、・・・・・、ｗｊとする。
【００５８】
操縦装置６の移動平均処理部８は、速度検出器３から入力される上記検出値のうち、新しいものから順にサンプル個数ｊの検出値を選択し、これらの選択された検出値の平均値すなわち移動平均値を演算する。この移動平均値を、速度Ｖとする。Ｖは上述の数１により求められる。
【００５９】
制御係数演算部９は、移動平均処理部８から入力される平滑化された速度Ｖに基づいて、位置検出器２が出力し移動平均処理部７に入力される深度信号のサンプル時間間隔τと、ＰＩＤ制御部１０が演算に利用する制御ゲイン係数とを算出する。
【００６０】
図５に、進行速度Ｖと、サンプル時間間隔τとの関係を表す関数ｇ（Ｖ）を示す。
【００６１】
水中航走体１の進行速度Ｖとしてｖが得られたとき、制御係数演算部９は、関数ｇ（Ｖ）に基づき、サンプル時間間隔ｔを算出する。
【００６２】
関数ｇ（Ｖ）は単調減少関数であり、高速で走行する際にはサンプル時間間隔は短く、低速で走行する際にはサンプル時間間隔は長くなる。サンプル時間間隔が短くなれば、移動平均処理部７及び移動平均処理部８は、所定数のサンプル値の移動平均値をより高速に出力できるため、制御における遅延はより少なくなる。一方、サンプル時間間隔が長くなれば、検出値に対するノイズの影響を抑制することができる。このように、サンプル時間間隔の長短に応じて、ＰＩＤ制御部１０の出力値の性質は異なるものとなるが、本実施の形態では、このサンプル時間間隔を速度に応じて変化させることにより、ＰＩＤ制御部１０は速度に応じて適切に舵アクチュエータ５を制御することが可能となっている。
【００６３】
同時に、制御係数演算部９は、ＰＩＤ制御部１０におけるＰＩＤ制御に関係するゲイン係数Ｋｐ、Ｋｄ及びＫｉを求める。これらの制御ゲイン係数の算出方法は、実施の形態１と同様である。すなわち、進行速度ｖに応じて、比例ゲイン係数Ｋｐｖ、積分ゲイン係数Ｋｉｖ、微分ゲイン係数Ｋｄｖが算出される。
【００６４】
他方、水中航走体１の位置検出器２においては、深度の検出値ｄが操縦装置６に出力される。ここで、深度ｄは、速度ｖに応じて算出されるサンプル時間間隔ｔ１，ｔ２、ｔ３、・・・・ｔｉで出力される。また、深度ｄは、新しいものから順にｄ１、ｄ２、ｄ３、・・・・ｄｉとして出力される。
【００６５】
操縦装置６の移動平均処理部７は、位置検出器２からサンプル時間間隔ｔ１、ｔ２、ｔ３、・・・・ｔｉごとに入力される深度信号ｄ１、ｄ２、ｄ３、・・・・ｄｉのうち、新しいものから順に、サンプル個数ｎの検出値を選択し、これらの選択された検出値の平均値すなわち移動平均値を演算する。この移動平均値を、深度Ｄとする。深度Ｄは、以下の数４により求められる。
【００６６】
【数４】
0005888007.tif 000005
【００６７】
なお、本実施の形態では、サンプル時間間隔が異なる場合であっても全ての検出値を同じ重みとして扱い、単純移動平均の演算を行っているが、加重移動平均又は指数移動平均の演算を行うことも可能である。
【００６８】
例えば、サンプル時間間隔τに応じて検出値に対して重み付けを行い、τが大きな検出値に対しては大きな係数を、τが小さな検出値に対しては小さな係数を掛けたうえで平均を求めることが考えられる。また、より新しい検出値には大きい重みをつけ、過去のデータは軽く扱うことも可能である。
【００６９】
ＰＩＤ制御部１０は、移動平均処理部７から入力された平滑化された深度Ｄと、設定器４で設定された目標深度である設定値Ｄ０と差である誤差信号ｅとに基づいて、時間積分したｅｉ、時間微分したｅｄを求める。さらに、ＰＩＤ制御部１０は、ｅ、ｅｉ及びｅｄそれぞれについて、制御係数演算部９により水中航走体１の進行速度Ｖに応じて算出された比例ゲイン係数Ｋｐ、積分ゲイン係数Ｋｉ、微分ゲイン係数Ｋｄを乗じ、これらの総和を舵アクチュエータ５に対して出力する。これに応じ、舵アクチュエータ５が、水中航走体１の鉛直方向の運動を制御する。
【００７０】
本実施の形態においては、制御係数演算部９が、移動平均処理部８から入力される速度Ｖに応じて最適なサンプル時間間隔及び制御ゲイン係数とを演算し、位置検出器２及び移動平均処理部７がこのサンプル時間間隔により深度Ｄを演算し、ＰＩＤ制御部１０が、この深度Ｄ及び制御ゲイン係数に基づいて水中航走体１の鉛直方向の運動を制御する。よって、水中航走体１は、制御における遅延及びノイズの影響を適切に抑制することができる。
【００７１】
＜その他の実施の形態＞
なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【００７２】
例えば、実施の形態１又は実施の形態２において、移動平均処理部８は、一定のサンプル個数による移動平均値の演算処理を行うこととしている。しかしながら、移動平均処理部７と同様に、速度に応じて最適なサンプル個数を求め、そのサンプル個数による移動平均値の演算を行うことも可能である。
【００７３】
また、上述の形態においては、速度検出器３が検出した速度（ｗ１、ｗ２、ｗ３、・・・・・、ｗｊ）に基づき、移動平均処理部８が速度の移動平均値を算出し、制御係数演算部９がこの速度の移動平均値に応じて最適なサンプル個数及び制御ゲイン係数とを演算することとしている。しかしながら、移動平均処理部７又は８が、位置検出器８が検出した位置、例えば深度（ｄ１、ｄ２、ｄ３、・・・・・ｄｉ）に基づき、位置の移動平均値を算出し、制御係数演算部９がこの位置の移動平均値に応じて最適なサンプル個数及び制御ゲイン係数とを演算することとしてもよい。このように、位置（主に深度）に応じてサンプル個数及び制御ゲイン係数を変化させることで、波浪等の影響を軽減できるという効果が生じる。
【００７４】
また、上述の実施の形態では、本発明を水中航走体１の鉛直方向の深度制御に適用した例を示したが、本発明はこれに限定されず、姿勢角、針路等に対しても適用が可能であり、さらに水上の船舶等の針路に対しても適用が可能である。
【００７５】
また、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ  ｃｏｍｐｕｔｅｒ  ｒｅａｄａｂｌｅ  ｍｅｄｉｕｍ）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（ｔａｎｇｉｂｌｅ  ｓｔｏｒａｇｅ  ｍｅｄｉｕｍ）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Ｐｒｏｇｒａｍｍａｂｌｅ  ＲＯＭ）、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ  ＰＲＯＭ）、フラッシュＲＯＭ、ＲＡＭ（ｒａｎｄｏｍ  ａｃｃｅｓｓ  ｍｅｍｏｒｙ））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（ｔｒａｎｓｉｔｏｒｙ  ｃｏｍｐｕｔｅｒ  ｒｅａｄａｂｌｅ  ｍｅｄｉｕｍ）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００７６】
１  航走体
２  位置検出器
３  速度検出器
４  設定器
５  舵アクチュエータ
６  操縦装置
７  移動平均処理部
８  移動平均処理部
９  制御係数演算部
１０  ＰＩＤ制御部
１００  船舶自動操縦装置
１０１  設定器
１０２  実コース検出器
１０３  速度検出器
１０４  姿勢検出器
１０５  ゲイン計算手段
１０６  舵特性メモリ
１０７  外力推定手段
１０８  運動特性メモリ
１１０  アクチュエータ
【図１】
0005888007.tif 000006
【図２】
0005888007.tif 000007
【図３】
0005888007.tif 000008
【図４】
0005888007.tif 000009
【図５】
0005888007.tif 000010
【図６】
0005888007.tif 000011