JP6951063B2 - 水中航走体および水中航走体の姿勢制御方法 - Google Patents

Description

本発明は、水中航走体および水中航走体の姿勢制御方法に関する。

波や海流などの外乱により外力を受ける環境下で艇体を目標位置に保持させる場合、スラスタを用いて艇体が受ける外力に対してバランスをとる必要がある。具体的には前後に推力を与えるスラスタおよび左右に推力を与えるスラスタを制御し、艇体を目標位置に保持させる。

ところで、海上では電力供給を受けることができない場合があり、艇体を目標位置に保持するにあたりスラスタの駆動などにより消費される電力量を抑制させることが望まれている。例えば、消費電力量を抑制し、目標位置に艇体を保持させる技術として、外乱の合力方向（以下、外力が及ぼされる方向）に船首を向けて船を目標位置に保持させる自動方位設定方法が提案されている（特許文献１）。特許文献１に係る自動方位設定方法では、船首を外力が及ぼされる方向に向けることで、目標位置へ艇体を保持するために必要となる消費電力量を抑制させることができる。

特開２０００−３０２０９８号公報

上述した特許文献１の自動方位設定方法では、艇体を目標位置に保持させる場合に、船首を外力が及ぼされる方向に向けて消費電力量の抑制を図ることができる。しかしながら、特許文献１に係る自動方位設定方法では、水平面上における船舶の位置および姿勢の制御について考慮されているが、ＡＵＶ（autonomous underwater vehicle）などの水中航走体のように艇体の左右方向のみならず上下方向からも外力が及ぼされる場合については考慮されていない。

本発明は、上述した問題点に鑑みてなされたものであり、その目的は、消費電力量を抑制しつつ、艇体を目標位置に保持させることができる水中航走体および水中航走体の姿勢制御方法を実現することにある。

本発明のある形態に係る水中航走体は、艇体の位置を示す位置情報を検知する測位装置と、前記艇体の姿勢を示す姿勢情報を検知する姿勢検知センサと、水中において前記艇体に対して前後方向、左右方向、および上下方向それぞれに推力を与え、該艇体の位置および姿勢を変更させるアクチュエータと、前記アクチュエータを制御する制御装置と、を備え、前記制御装置は、前記測位装置によって検知された前記位置情報に基づき前記艇体を目標位置に保持させるように、前記艇体における前後方向の制御力、左右方向の制御力、上下方向の制御力、および艇体をロール方向、ヨー方向、ピッチ方向それぞれに旋回させる旋回制御力を求めて前記アクチュエータを制御しており、目標位置に保持されている前記艇体に外力が及ぼされたとき、前記左右方向および上下方向の制御力の大きさがゼロとなるように、目標とする姿勢情報を更新し、前記姿勢検知センサによって検知された前記姿勢情報に基づき前記艇体を更新後の姿勢情報に応じた姿勢に変更させるように前記アクチュエータを制御する。
以上

ここで前後方向とは艇体の船尾から船首もしくは船首から船尾に向かう方向であり、左右方向とは艇体の左舷から右舷もしくは右舷から左舷に向かう方向である。また上下方向とは、艇体の底面から上面もしくは上面から底面に向かう方向である。

上記構成によると、目標位置に保持されている艇体に外力が及ぼされたとき、制御装置は、左右方向および上下方向それぞれの制御力の大きさがゼロとなるように、アクチュエータを制御して艇体の姿勢を変更させることができる。左右方向および上下方向それぞれの制御力の大きさがゼロとなる姿勢とは、外力が及ぼされる方向に船首を向けた姿勢である。一般的に艇体は前進するときに流体抵抗が小さくなるように設計されている。このため、外力が及ぼされる方向に船首を向けた姿勢は、艇体に作用する流体力を低減させた姿勢であるといえる。

このように、水中航走体は、艇体に外力が及ぼされた場合であっても、艇体に作用する流体力を低減させた姿勢とすることができるため、目標位置に艇体を保持させるために駆動させるアクチュエータによって消費される電力量を低減させることができる。

よって、本発明のある形態に係る水中航走体は、消費電力量を抑制しつつ、艇体を目標位置に保持させることができるという効果を奏する。

また、本発明のある形態に係る水中航走体は、上記した構成において、前記制御装置は、目標とする位置情報および姿勢情報と、前記測位装置によって検知された位置情報および前記姿勢検知センサによって検知された姿勢情報との偏差から前記前後方向の制御力、前記左右方向の制御力、前記上下方向の制御力、および前記ロール方向、ヨー方向、ピッチ方向それぞれでの旋回制御力を算出する制御力算出部を備え、目標位置に保持されている前記艇体に外力が及ぼされたとき、前記制御力算出部によって算出された前記左右方向および前記上下方向の制御力の大きさがゼロとなるように、目標とする姿勢情報のうちヨー角およびピッチ角の指令値を更新させるように構成されていてもよい。

上記構成によると制御装置がヨー角とピッチ角の指令値を更新することができるため、外力が艇体に及ぼされたとしても、艇体をヨー方向およびピッチ方向に旋回させ、左右方向および上下方向の制御力の大きさがゼロとなる姿勢、すなわち艇体に作用する流体力を低減させた姿勢とすることができる。

また、本発明のある形態に係る水中航走体は、上記した構成において、前記艇体に及ぼされる外力の方向として潮流到来方向を計測する流向計を備え、前記制御装置は、前記流向計によって計測された前記潮流到来方向に船首が向いた艇体の姿勢を、目標とする姿勢情報とし、前記ヨー角および前記ピッチ角それぞれの指令値を更新するように構成してもよい。

上記構成によると、流向計を備えているため、潮流の到来方向、すなわち艇体に及ぼされる外力の方向を把握することができる。このため、制御装置は、流向計による計測結果に基づき左右方向および上下方向の制御力の大きさがゼロとなる姿勢、すなわち艇体に作用する流体力を低減させた姿勢を把握し、該姿勢となるようにヨー角およびピッチ角の指令値をそれぞれ更新することができる。

また、本発明のある形態に係る水中航走体は、上記した構成において、前記制御装置は、更新された前記ピッチ角の指令値と前記姿勢検知センサによって検知された姿勢情報におけるピッチ角の値との偏差から求められるピッチ方向への旋回制御力の変化量に制限を加える第１変化率リミッタと、更新された前記ヨー角の指令値と前記姿勢検知センサによって検知された姿勢情報におけるヨー角の値との偏差から求められるヨー方向への旋回制御力の変化量に制限を加える第２変化率リミッタと、を備え、前記第１変化率リミッタの変化量および前記第２変化率リミッタの変化量それぞれの設定を変更し、ヨー角、ピッチ角の順にそれぞれの指令値を更新する、あるいはヨー角の指令値の更新速度に対してピッチ角の指令値の更新速度を小さくするように構成されていてもよい。

上記構成によると、第１変化率リミッタおよび第２変化率リミッタを制御装置が備えるため、ピッチ方向およびヨー方向への旋回制御力の変化量が大きくなり、艇体がピッチ方向およびヨー方向に大きく旋回し急激に姿勢を変更させることを防ぐことができる。

また、第１変化率リミッタおよび第２変化率リミッタそれぞれの設定を変更し、ヨー角、ピッチ角の順にそれぞれの指令値を更新する、あるいはヨー角の方がピッチ角よりも指令値の更新速度を大きくすることができる。このため、ピッチ方向よりもヨー方向への回頭を優先して行うことができる。それ故、例えば、艇体の後方の斜め上から及ぼされた外力に対処するために姿勢変更する場面において、ピッチ角９０度を超えて回頭し艇体の上面と底面とが逆転した特異姿勢となることを防ぐことができる。

また、本発明のある形態に係る水中航走体は、上記した構成において、前記制御装置は、前記左右方向の制御力の値を積分し、目標とするヨー角の指令値を求めるヨー角指令値算出部と、前記上下方向の制御力の値を積分し、目標とするピッチ角の指令値を算出するピッチ角指令値算出部と、を備え、前記左右方向および前記上下方向の制御力の大きさがゼロとなるまで、前記ヨー角指令値算出部によって算出された指令値と前記ピッチ角指令値算出部によって算出された指令値によって、前記ヨー角および前記ピッチ角それぞれの指令値を更新するように構成されていてもよい。

上記構成によると、制御装置はヨー角指令値算出部およびピッチ角指令値算出部を備えるため、左右方向および上下方向の制御力の大きさがゼロとなる姿勢に姿勢変更させるようにヨー角およびピッチ角の指令値を更新することができる。

したがって、流向計を備えていなくても、左右方向および上下方向の制御力の大きさがゼロとなる姿勢、すなわち艇体に作用する流体力を低減させた姿勢に艇体を姿勢変更させることができる。

また、本発明のある形態に係る水中航走体は、上記した構成において、前記ヨー角指令値算出部は、前記左右方向の制御力の値にゲインを乗じて積分した値から目標とするヨー角の指令値を求めており、前記ピッチ角指令値算出部は、前記上下方向の制御力の値にゲインを乗じて積分した値から目標とするピッチ角の指令値を求めており、前記ヨー角指令値算出部において前記左右方向の制御力の値に乗じる前記ゲインおよび前記ピッチ角指令値算出部において前記上下方向の制御力の値に乗じる前記ゲインそれぞれの値を変更し、ヨー角、ピッチ角の順にそれぞれの指令値を更新する、あるいはヨー角の指令値の更新速度に対してピッチ角の指令値の更新速度を小さくするように構成されていてもよい。

上記構成によると、前記ヨー角指令値算出部は左右方向の制御力の値にゲインを乗じて積分した値から、ならびに前記ピッチ角指令値算出部は前記上下方向の制御力の値にゲインを乗じて積分した値から、それぞれ目標とするヨー角およびピッチ角の指令値を求める構成である。このため、それぞれに乗じるゲインの値の設定を変更することで、ヨー角、ピッチ角の順にそれぞれの指令値を更新する、あるいはヨー角の方がピッチ角よりも指令値の更新速度を大きくすることができる。

したがって、ピッチ方向よりもヨー方向への回頭を優先して行うことができる。それ故、例えば、艇体の後方の斜め上から及ぼされた外力に対処するために姿勢変更する場面において、ピッチ角９０度を超えて回頭し艇体の上面と底面とが逆転した特異姿勢となることを防ぐことができる。

また、本発明のある形態に係る水中航走体は、上記した構成において、前記アクチュエータは、前記艇体の重心位置を変更するように、艇体内において前後方向に移動する重心位置変更部と、を備える構成であってもよい。

上記構成によると重心位置変更部を備えるため、艇体の重心位置を前後方向において変更させることできる。このため、艇体のピッチングにおける回転方向を容易に決定させることができ、ピッチ方向での旋回制御をより容易とすることができる。

本発明のある形態に係る水中航走体の姿勢制御方法は、艇体の位置を示す位置情報を検知する測位装置と、前記艇体の姿勢を示す姿勢情報を検知する姿勢検知センサと、水中において前記艇体に対して前後方向、左右方向、および上下方向それぞれに推力を与え、該艇体の位置および姿勢を変更させるアクチュエータと、前記アクチュエータを制御する制御装置と、を備えた水中航走体の姿勢制御方法であって、前記制御装置が、前記測位装置によって検知された前記位置情報に基づき前記艇体を目標位置に保持させるように、前記艇体における前後方向の制御力、左右方向の制御力、上下方向の制御力、および艇体をロール方向、ヨー方向、ピッチ方向それぞれに旋回させる旋回制御力を求めて前記アクチュエータを制御するステップと、目標位置に保持されている前記艇体に外力が及ぼされたとき、前記左右方向および上下方向の制御力の大きさがゼロとなるように、目標とする姿勢情報を更新し、前記姿勢検知センサによって検知された前記姿勢情報に基づき前記艇体を更新後の姿勢情報に応じた姿勢に変更させるように前記アクチュエータを制御するステップと、を含む。

上記方法によると、目標位置に保持されている艇体に外力が及ぼされたとき、左右方向および上下方向それぞれの制御力の大きさがゼロとなるように、アクチュエータを制御して艇体の姿勢を変更させることができる。左右方向および上下方向それぞれの制御力の大きさがゼロとなる姿勢とは、外力が及ぼされる方向に船首を向け、艇体に作用する流体力を低減させた姿勢である。

このように、艇体に外力が及ぼされた場合であっても、艇体に作用する流体力を低減させた姿勢とすることができるため、目標位置に艇体を保持させるために駆動させるアクチュエータによって消費される電力量を低減させることができる。

よって、本発明のある形態に係る水中航走体の姿勢制御方法は、消費電力量を抑制しつつ、艇体を目標位置に保持させることができるという効果を奏する。

本発明は以上に説明したように構成され、本発明に係る水中航走体および水中航走体の姿勢制御方法は、消費電力量を抑制しつつ、艇体を目標位置に保持させることができるという効果を奏する。

本発明の第１実施形態に係る水中航走体が備えるアクチュエータの一例を示す図であり、同図（Ａ）は水中航走体の上面（平面）図であり、同図（Ｂ）は水中航走体の側面図である。 本発明の第１実施形態に係る水中航走体における目標位置保持に関する構成の一例を示すブロック図である。 図２に示す水中航走体の水平面における姿勢の一例を示す図であり、同図（Ａ）は、外乱発生時における水中航走体の姿勢の一例を示し、同図（Ｂ）は、外乱発生に伴い変更させた水中航走体の姿勢の一例を示す。 図２に示す水中航走体の鉛直方向における姿勢の一例を示す図であり、同図（Ａ）は、外乱発生時における水中航走体の姿勢の一例を示し、同図（Ｂ）は、外乱発生に伴い変更させた水中航走体の姿勢の一例を示す。 第１実施形態に係る水中航走体における、外乱発生時の姿勢制御に関する構成を示すブロック図である。 第１実施形態に係る水中航走体において船首の方向が真上前方から真上後方へと移動した状態の一例を示す図である。 第２実施形態に係る水中航走体の、外乱発生時における姿勢制御に関する構成を示すブロック図である。 水中航走体の変形例に係る構成の一例を模式的に示す図である。

以下、本発明の実施の形態を、図面を参照しながら説明する。明細書中では、本発明に係る水中航走体１としてＡＵＶ等の潜水艇を例に挙げて説明するが、これに限定されるものではなく、水中において目標位置を保持し、作業等を行うものであればよい。

［第１実施形態］
図１（Ａ），（Ｂ）は、本発明の第１実施形態に係る水中航走体１が備えるアクチュエータ３の一例を示す図であり、同図（Ａ）は水中航走体１の上面（平面）図であり、同図（Ｂ）は水中航走体１の側面図である。なお、図１では、説明の便宜上、水中航走体１が有するアクチュエータ３の配置に関してのみ図示している。

図１（Ａ）、（Ｂ）に示すように水中航走体１は、艇体２の左右の側面および前後の側面の面積よりも上面および下面の面積の方が大きくなる略直方体形状をしている。水中航走体１は、アクチュエータ３として、艇体２を前後方向に移動させるための２つの主推進器３１ａ，３１ｂ、艇体２を上下方向に移動させるための４つの垂直スラスタ３２ａ，３２ｂ，３２ｃ，３２ｄ、艇体２を左右方向に移動させるための２つの水平スラスタ３３ａ，３３ｂをそれぞれ備えている。なお、主推進器３１ａ，３１ｂそれぞれを区別して説明をする必要が無い場合は単に主推進器３１と称する。また、垂直スラスタ３２ａ，３２ｂ，３２ｃ，３２ｄそれぞれを区別して説明する必要が無い場合は単に垂直スラスタ３２と称する。また、水平スラスタ３３ａ，３３ｂそれぞれを区別して説明する必要が無い場合は単に水平スラスタ３３と称する。

第１実施形態に係る水中航走体１では、図１（Ａ）に示すように、艇体２の前後に延びる軸線にプロペラの回転軸が沿うように２つの主推進器３１ａ，３１ｂが設けられている。また、艇体２の左右に延びる軸線にプロペラの回転軸が沿うように２つの水平スラスタ３３ａ，３３ｂが設けられ、艇体２の上下に延びる軸線にプロペラの回転軸が沿うように４つの垂直スラスタ３２ａ，３２ｂ，３２ｃ，３２ｄがそれぞれ設けられている。

そして、水中航走体１は、２つの主推進器３１ａ，３１ｂによって艇体２を前後方向に移動させることができる。また、２つの水平スラスタ３３ａ，３３ｂによって艇体２を左右方向に移動させるとともに、２つの水平スラスタ３３ａ，３３ｂそれぞれの出力を調整することで艇体２のヨー方向の回転運動を制御することができる。また、水中航走体１は、４つの垂直スラスタ３２ａ，３２ｂ，３２ｃ，３２ｄによって上下方向に艇体２を移動させるとともに、４つの垂直スラスタ３２ａ，３２ｂ，３２ｃ，３２ｄそれぞれの出力を調整することで艇体２のピッチ方向の回転運動を制御したり、ロール方向での回転運動を制御したりすることができる。

なお、図１（Ａ）、（Ｂ）に示すように水中航走体１の艇体２は、略直方体形状をしているがこれに限定されるものではなく水中航走体１によって行う作業目的等により適宜選択される。また、第１実施形態に係る水中航走体１は、上記したように、アクチュエータ３として、２つの主推進器３１ａ，３１ｂ、４つの垂直スラスタ３２ａ，３２ｂ，３２ｃ，３２ｄ、２つの水平スラスタ３３ａ，３３ｂを備える構成であるが、備えるアクチュエータ３の個数および種類はこれらに限定されるものではない。

例えば、２つの主推進器３１ａ，３１ｂそれぞれのプロペラの回転軸を、水中航走体１の前後方向に延びる中心線（不図示）に対して約４５度の角度をなして、後方側の左右方向にそれぞれに延伸するように設けた構成とし、これら主推進器３１ａ，３１ｂによって艇体２の前後、左右の移動および艇体２のヨー方向における回転運動を制御させる構成としてもよい。

すなわち、艇体２を前後、左右、上下に移動させたり、艇体２をロール方向、ヨー方向、ピッチ方向にそれぞれ回転させ艇体２の姿勢を変更させたりすることができるようになっていればよく、備えるアクチュエータ３の個数および種類は任意である。

（目標位置に保持する構成と制御フロー）
次に、上記したアクチュエータ３を用いて艇体２を目標位置に保持するための構成について図２を参照して説明する。図２は、本発明の第１実施形態に係る水中航走体１における目標位置保持に関する構成の一例を示すブロック図である。図２では、説明の便宜上、艇体２の位置を示す位置情報であるｘ、ｙ、ｚ座標それぞれの指令値の流れをまとめて１つの矢印で示している。また艇体２の姿勢を示す姿勢情報であるロール角、ピッチ角、ヨー角それぞれの指令値の流れもまとめて１つの矢印で示している。

図２に示すように、水中航走体１は、上記したアクチュエータ３に加え、ジャイロセンサ８、測位装置９、および制御装置５０をさらに備えている。

ジャイロセンサ８は、本発明の姿勢検知センサの一例であり、艇体２の姿勢を示す姿勢情報を検出する。測位装置９は、艇体２の位置を示す位置情報を検出する。なお、測位装置９としては、超音波を使用し、基準点として設定した母船または海底の所定の位置から艇体２の相対位置を測定する公知の音響測位装置を利用することができる。

また、制御装置５０は、水中航走体１の各種制御を行うものであり、第１比較部４、第２比較部５、制御力算出部６、および推力配分装置７を備えてなる構成である。第１比較部４は、目標値とするｘ、ｙ、ｚ座標の指令値と、測定されたｘｙｚ座標の指令値との偏差を求めるものである。なお、水中航走体１は、ｘ、ｙ、ｚ座標の指令値それぞれについて第１比較部４ａ，４ｂ，４ｃをそれぞれ備えているが（後述の図７参照）、特にこれらを区別する必要がないときは、単に第１比較部４と称する。また第２比較部５は、目標値とするロール角、ピッチ角、ヨー角の指令値と、測定されたロール角、ピッチ角、ヨー角の指令値との偏差を求めるものである。水中航走体１は、ロール角、ピッチ角、ヨー角それぞれについて第２比較部５ａ，５ｂ，５ｃをそれぞれ備えているが（後述の図７参照）、特にこれらを区別する必要がないときは、単に第２比較部５と称する。

制御力算出部６は、艇体２を保持させる目標位置および姿勢と、実際の艇体２の位置および姿勢との偏差から、水中航走体１における前後方向の制御力である前後制御力、左右方向の制御力である左右制御力、上下方向の制御力である上下制御力、ロール方向の旋回制御力であるロール旋回制御力、ピッチ方向の旋回制御力であるピッチ旋回制御力、ヨー方向の旋回制御力であるヨー旋回制御力それぞれを演算する。

推力配分装置７は、制御力算出部６の演算結果に基づき、各アクチュエータ３に配分する推力の計算を行う。そして、推力配分装置７は、求めた推力から各アクチュエータ３の操作量を算出し、算出した操作量に応じた指令値をアクチュエータ３に出力する。より具体的には、推力配分装置７は、アクチュエータ３を構成する主推進器３１、水平スラスタ３３、垂直スラスタ３２それぞれのプロペラ（不図示）のピッチ角、回転数等を演算しその指令値を出力する。

また、上記した構成を有する水中航走体１は、以下の制御フローにより目標位置に艇体２を保持させることができる。すなわち、まず、操船者が、目標値として、艇体２を保持させる位置を地球固定座標系ｘ、ｙ、ｚ軸の値で示した指令値、ならびに水中航走体１の姿勢を規定するロール角、ピッチ角、ヨー角の指令値を、該水中航走体１に入力する。そして、第１比較部４において測位装置９から得られた艇体２の実際の位置を示すｘ、ｙ、ｚ軸それぞれの値と、目標値となるｘ、ｙ、ｚ軸それぞれの値との偏差を求め、制御力算出部６に入力する。また、第２比較部５において、ジャイロセンサ８から得られた艇体２の実際の姿勢を示すロール角、ピッチ角、ヨー角と、目標値となるロール角、ピッチ角、ヨー角それぞれの値との偏差を求め、制御力算出部６に入力する。

制御力算出部６は、水中航走体１における前後制御力、左右制御力、上下制御力、ロール旋回制御力、ピッチ旋回制御力、ヨー旋回制御力それぞれを演算し、その演算結果から求めた各指令値を推力配分装置７に入力する。推力配分装置７は、入力された各指令値から各アクチュエータ３に配分する推力の計算を行う。そして、推力配分装置７は、求めた推力から各アクチュエータ３の操作量を算出し、操作量を示す指令値を各アクチュエータ３に出力する。以上の制御フローを実施することで、第１実施形態に係る水中航走体１は、艇体２を目標位置に保持させることができる。

ところで、第１実施形態に係る水中航走体１は、艇体２を目標位置に保持させた状態で外乱が生じると、消費電力量の抑制を図るため、外乱により及ぼされる外力の方向に船首を向け、艇体に作用する流体力を低減させた姿勢をとるように構成されている。例えば、図３（Ａ），（Ｂ）および図４（Ａ），（Ｂ）に示すように艇体２に対して、該艇体２の左斜め前でかつ、上方から外力が及ぼされた場合、前後制御力を前方に、左右制御力を左方に、上下制御力を上方にそれぞれ及ぼし、艇体２が目標位置に保持されるようにバランスをとる。そして、水中航走体１は、外力が及ぼされる方向に船首を向け、左右制御力および上下制御力それぞれの大きさがゼロとなる姿勢、換言すると、艇体に作用する流体力を低減させた姿勢とする。

なお、図３は図２に示す水中航走体１の水平面における姿勢の一例を示す図であり、同図（Ａ）は、外乱発生時における水中航走体１の姿勢の一例を示し、同図（Ｂ）は、外乱発生に伴い変更させた水中航走体１の姿勢の一例を示す。図４は図２に示す水中航走体１の鉛直方向における姿勢の一例を示す図であり、同図（Ａ）は、外乱発生時における水中航走体１の姿勢の一例を示し、同図（Ｂ）は、外乱発生に伴い変更させた水中航走体１の姿勢の一例を示す。

（流向計の計測結果を利用した姿勢制御）
以下において、第１実施形態に係る水中航走体１における外乱発生時の艇体２の姿勢制御について図５を参照して説明する。図５は、第１実施形態に係る水中航走体１における、外乱発生時の姿勢制御に関する構成を示すブロック図である。なお、図５では、艇体２の姿勢制御をより具体的に説明するためにロール角、ピッチ角、ヨー角の指令値の流れをそれぞれ別個の矢印線により示している。

外乱発生時の艇体２の姿勢制御に関する構成として、図５に示すように第１実施形態に係る水中航走体１は、流向計１１を備え、制御装置５０が第１変化率リミッタ１２および第２変化率リミッタ１３をさらに有している。

流向計１１は、潮流到来方向を計測する装置であり、第１変化率リミッタ１２および第２変化率リミッタ１３は、例えば、求められた指令値の１秒あたりの変化量を制限するものである。水中航走体１では、第１変化率リミッタ１２は、ピッチ角の指令値の１秒当たりの変化量を制限し、第２変化率リミッタ１３は、ヨー角の指令値の１秒当たりの変化量を制限する。

水中航走体１は、上記したように、目標値として与えられたｘ，ｙ，ｚ座標の指令値（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）およびロール角、ピッチ角、ヨー角の指令値（φ_ｔ，θ_ｔ，Ψ_ｔ）と、測定された艇体２の位置を示すｘ，ｙ，ｚ座標の指令値（ｘ，ｙ，ｚ）および艇体２の姿勢を示すロール角、ピッチ角、ヨー角の指令値（φ，θ，Ψ）との偏差に基づき、制御力算出部６が水中航走体１における前後制御力、左右制御力、上下制御力、ロール旋回制御力、ピッチ旋回制御力、ヨー旋回制御力それぞれを演算する構成となっている。そして、推力配分装置７が、制御力算出部６の演算結果に基づき、各アクチュエータ３に配分する推力を求め、求めた推力から各アクチュエータ３の操作量を算出し、艇体２を目標位置に保持するようにアクチュエータ３の制御を行う構成である。

このような構成において、外力が艇体２に及ぼされた場合、水中航走体１は、外力に対してピッチ方向およびヨー方向でのみ旋回し、姿勢変更させる。このため、ピッチ角およびヨー角の指令値についてのみ、流向計１１から得られた潮流方向の情報を用いて更新しており、以下では、ピッチ角およびヨー角の指令値の更新についてのみ説明するものとする。

水中航走体１では、まず、外力が及ぼされる方向（潮流到来方向）を流向計１１により計測する。そして、目標値として与えられたピッチ角、ヨー角の指令値（θ_ｔ，Ψ_ｔ）を、流向計１１により計測された潮流到来方向に対して船首を向けた姿勢となるときのピッチ角およびヨー角の値（θ_ｃ、Ψ_ｃ）に更新する。そして、更新されたピッチ角およびヨー角の値（θ_ｃ、Ψ_ｃ）と、ジャイロセンサ８で測定したピッチ角の指令値θおよびヨー角の指令値Ψとの偏差（θ_ｃ−θ、Ψ_ｃ−Ψ）を第２比較部５ｂおよび第２比較部５ｃそれぞれにおいて求める。そして、第２比較部５ｂにおいて求めたピッチ角の指令値の偏差に対して第１変化率リミッタ１２によって変化率リミットを適用させた値を制御力算出部６に入力する。同様に第２比較部５ｃにおいて求めたヨー角の指令値の偏差に対して第２変化率リミッタ１３によって変化率リミットを適用させた値を制御力算出部６に入力する。

なお、外乱の発生に伴う艇体２の姿勢変更を急激に行なうことを防止するために、水中航走体１は、第１変化率リミッタ１２および第２変化率リミッタ１３を備えている。しかしながら、例えば、外乱発生に伴う姿勢の変更が小さいような環境下で水中航走体１を用いる場合は、これらの部材は必ずしも必要はない。

制御力算出部６は、入力されたピッチ角の指令値の偏差に対して変化率リミットを適用させた値からピッチ旋回制御力を演算する。また、制御力算出部６は、入力されたヨー角の指令値の偏差に対して変化率リミットを適用させた値からヨー旋回制御力を演算する。そして、制御力算出部６は、演算結果から求めた各旋回制御力の指令値を推力配分装置７に入力する。

推力配分装置７は、入力された各旋回制御力の指令値に基づき、艇体２をピッチ方向およびヨー方向それぞれに旋回させるように、アクチュエータ３の操作量を算出し、算出した操作量に応じた指令値を各アクチュエータ３に出力する。以上の制御フローを、艇体２に及ぼされる外力の方向に船首が向くまで行う。このようにして第１実施形態に係る水中航走体１は、所定の位置に保持されたまま、船首を外力の方向に徐々に向けるように姿勢を変更することができる。

ところで、艇体２の後方の斜め上から外力が及ぼされると、図６に示すように、艇体２は、前方の斜め上に船首が向けられていた状態から、該艇体２のピッチ角が９０度を超えてそのまま後方へ船首が向けられるように移動する場合も想定できる。図６は、第１実施形態に係る水中航走体１において船首の方向が真上前方から真上後方へと移動した状態の一例を示す図である。図６では、水平面をｘｙ平面とし鉛直方向をｚ軸方向として表している。

この場合、水中航走体１の艇体２は、上下が反対となる特異姿勢となってしまい水中航走体１の制御または水中航走体１による所定の作業を行うにあたり好ましくない状態となる。また、図６において真上前方に船首が向けられている時の姿勢（例えば（ピッチ，ヨー）＝（８９，０）度）から、ピッチ方向に２度、船首が回頭し真上後方に向けられたとした場合、回頭後の姿勢は、（ピッチ，ヨー）＝（８９，１８０）度のように表される。このように、姿勢を示す方位角において、ヨーが０度からいきなり１８０度に不連続に変化してしまい制御の不安定化を招くといった問題を生じさせてしまう場合もある。

そこで、水中航走体１は、図６に示すような艇体２の姿勢変更を防止するために、ヨー方向の姿勢を優先的に変更させた後、ピッチ方向の姿勢を変更するように構成してもよい。

具体的には、水中航走体１は、外力が艇体２に及ぼされると、当初は、第１変化率リミッタ１２においてピッチ方向の変化量をゼロに設定し、まずはヨー方向についてのみ回頭を実施する。ヨー方向の回頭を行った後、今度は第２変化率リミッタ１３においてヨー方向の変化量をゼロに設定するとともに、第１変化率リミッタにおいてゼロに設定されている変化量を初期値に戻し、ピッチ方向の回頭を行う。

あるいは、第１実施形態に係る水中航走体１は、図６に示すような艇体２の姿勢変更を防止するために、ヨー角に比べてピッチ角の方が目標値への更新速度が小さくなるように構成してもよい。具体的には、水中航走体１は、第１変化率リミッタ１２の変化量の方が、第２変化率リミッタ１３の変化量よりも小さくなるようにそれぞれ変化率を設定する。

以上のように、第１実施形態に係る水中航走体１は、艇体２の回頭を、ピッチ方向よりもヨー方向を優先させる構成とすることができるため、艇体２が特異姿勢となることを防止するとともに、特異姿勢前後において制御が不安定な状態となることを回避できる。

［第２実施形態］
（流向計を備えない構成における姿勢制御）
第２実施形態に係る水中航走体１０として、水中航走体１０が流向計１１を備えていない場合における、外乱発生時の姿勢制御について、図７を参照して説明する。図７は、第２実施形態に係る水中航走体１０の、外乱発生時における姿勢制御に関する構成を示すブロック図である。図７に示すように、第２実施形態に係る水中航走体１０は、第１実施形態に係る水中航走体１の構成と比較して、流向計１１を備えていない点で異なる。また、制御装置５０が第１変化率リミッタ１２および第２変化率リミッタ１３を備えていない一方で、ヨー角指令値算出部２１およびピッチ角指令値算出部２２を備えている点で異なる。なお、それ以外では第２実施形態に係る水中航走体１０は、第１実施形態に係る水中航走体１と同様の構成であるため、同じ部材には同じ符号を付し、その説明は省略する。

詳細は後述するがヨー角指令値算出部２１は、目標値とするヨー角指令値を算出するものであり、制御力算出部６から出力される左右制御力の指令値を積分する積分器を含む。また、ピッチ角指令値算出部２２は、目標値とするピッチ角指令値を算出するものであり、制御力算出部６から出力される上下制御力の指令値を積分する積分器を含む。

水中航走体１０では、流向計１１を備えていないため、直接的に潮流到来方向（外力が及ぼされる方向）を把握することができない。そこで、水中航走体１０は、外力が艇体２に及ぼされた際に、艇体２を所定の位置に保持させるために働く左右制御力からヨー角の指令値を、上下制御力からピッチ角の指令値をそれぞれ算出するように構成されている。

例えば、図３（Ａ）、（Ｂ）および図４（Ａ）、（Ｂ）に示すように、左斜め前でかつ前方の斜め上から外力が及ぼされる場合、外力の方向と船首とが向かいあう位置、換言すると、図３（Ｂ）および図４（Ｂ）に示すように左右制御力がゼロでかつ、上下制御力がゼロとなる位置に回頭させ停止させるように制御する。例えば、外力が図３（Ａ）に示すように、艇体２の左舷方向から及ぼされるとき、艇体２を目標位置に保持するために左右制御力は左方向に働き、逆に外力が右舷方向から及ぼされるときは、左右制御力は右方向に働く。また、外力が図４（Ａ）に示すように、艇体２の上方向から及ぼされるとき、上下制御力は上方向に働き、逆に外力が下方から及ぼされるときは、上下制御力は下方向に働く。そこで、左右制御力および上下制御力が働く方向から回頭方向を定め、左右制御力および上下制御力がゼロとなる姿勢にあるときの船首の方向を外力が及ぼされる方向としてピッチ角およびヨー角の指令値を更新していく。

具体的には第２実施形態に係る水中航走体１０は、以下の制御フローにより、目標位置に保持された艇体２の姿勢制御を行う。まず、水中航走体１０は、第１実施形態に係る水中航走体１と同様に、目標値として与えられたｘ，ｙ，ｚ座標の指令値（ｘ_ｔ，ｙ_ｔ，ｚ_ｔ）およびロール角、ピッチ角、ヨー角の指令値（φ_ｔ，θ_ｔ，Ψ_ｔ）と、測定された艇体２の位置を示すｘ，ｙ，ｚ座標の指令値（ｘ，ｙ，ｚ）および艇体２の姿勢を示すロール角、ピッチ角、ヨー角の指令値（φ，θ，Ψ）との偏差に基づき、制御力算出部６が水中航走体１０における前後制御力、左右制御力、上下制御力、ロール旋回制御力、ピッチ旋回制御力、ヨー旋回制御力それぞれを求める構成となっている。そして、推力配分装置７が、制御力算出部６の演算結果に基づき、各アクチュエータ３に配分する推力を求め、求めた推力から各アクチュエータ３の操作量を算出し、艇体２を目標位置に保持するようにアクチュエータ３の制御を行っている。このような構成において、外力が艇体２に及ぼされた場合、第２実施形態に係る水中航走体１０は、以下のようにして艇体２の姿勢を変更させる。

すなわち、水中航走体１０は、外乱発生時において艇体２を目標位置に保持するために働く左右方向の制御力の指令値をヨー角指令値算出部２１に、上下方向の制御力の指令値をピッチ角指令値算出部２２にそれぞれ入力する。ヨー角指令値算出部２１は、入力された左右制御力の指令値にゲインを乗じて積分した値からヨー角指令値Ψ_ｒを求め、目標値であるヨー角指令値Ψ_ｔをこの求めたヨー角指令値Ψ_ｒに更新する。

一方、ピッチ角指令値算出部２２は、入力された上下制御力の指令値にゲインを乗じて積分した値からピッチ角指令値θ_ｒを求め、目標値であるピッチ角指令値θ_ｔをこの求めたピッチ角指令値θ_ｒに更新する。このように、ヨー角指令値算出部２１は、左右制御力の指令値を積分した値から目標とするヨー角の方位を定める。また、ピッチ角指令値算出部２２は、上下制御力の指令値を積分した値から目標とするピッチ角の方位を定める。

更新された目標値であるヨー角指令値Ψ_ｒは、第２比較部５ｃにおいてジャイロセンサ８により測定された実際のヨー角指令値Ψと比較され、その偏差が制御力算出部６に入力される。また、更新されたピッチ角指令値θ_ｒは、第２比較部５ｂにおいてジャイロセンサ８により測定された実際のピッチ角指令値θと比較され、その偏差が制御力算出部６に入力される。

制御力算出部６は、上記したピッチ角指令値の偏差からピッチ旋回制御力を、上記したヨー角指令値の偏差からヨー旋回制御力を演算する。そして、制御力算出部６は、演算結果から求めた各旋回制御力の指令値を推力配分装置７に入力する。推力配分装置７は、入力された各旋回制御力の指令値に基づき、艇体２をピッチ方向およびヨー方向それぞれに旋回させるための、アクチュエータ３の操作量を算出し、算出した操作量の指令値を各アクチュエータ３に出力する。ヨー角指令値およびピッチ角指令値それぞれの目標値の更新は、左右制御力および上下制御力がゼロになるまで行われる。このようにして第２実施形態に係る水中航走体１０は、所定の位置に艇体２が保持されたまま、船首を外力の方向に向けるように姿勢を変更することができる。

また、第２実施形態に係る水中航走体１０においても、第１実施形態に係る水中航走体１と同様に、艇体２のピッチ角が９０度を超えて移動し、特異姿勢となる場合が想定される。そこで、水中航走体１０では、このような特異姿勢となることを回避させるために、以下のように構成されていてもよい。

すなわち、水中航走体１０は、外力が及ぼされると、当初は、ピッチ角指令値算出部２２において、上下制御力の指令値に乗じるゲインの値をゼロに設定し、まずはヨー方向についてのみ回頭を実施する。ヨー方向の回頭を行った後、ヨー角指令値算出部２１において、左右制御力の指令値に乗じるゲインの値をゼロに設定するとともに、ピッチ角指令値算出部２２においてゼロに設定されているゲインの値を初期値に戻し、ピッチ方向の回頭を行う。

あるいは、水中航走体１０は、艇体２が特異姿勢となることを回避させるために、ヨー角に比べてピッチ角の方が目標値の更新速度を小さくするように構成してもよい。具体的には、水中航走体１０は、ヨー角指令値算出部２１において左右制御力の指令値に乗じるゲインの値に比べて、ピッチ角指令値算出部２２において上下制御力の指令値に乗じるゲインの値の方が小さくなるように設定する。

以上のように、第２実施形態に係る水中航走体１０は、艇体２の回頭を、ピッチ方向よりもヨー方向を優先させる構成であるため、艇体２が特異姿勢となることを防止するとともに、特異姿勢前後において制御が不安定な状態となることを回避できる。

（変形例）
上記した第１実施形態に係る水中航走体１、および第２実施形態に係る水中航走体１０は、複数の垂直スラスタ３２を動作させて艇体２のピッチングの回転方向を制御する構成であった。しかしながら、図８に示すようにアクチュエータ３として、さらに前後方向に移動可能な重心位置変更部３０を備え、艇体２の重心位置を変更させ、該艇体２の上下方向の傾き、つまりピッチングの回転方向を制御する構成としてもよい。図８は、水中航走体１，１０の変形例に係る構成の一例を模式的に示す図である。図８では、水中航走体１，１０を前後方向に鉛直に切り出したときの断面構造を模式的に示している。

重心位置変更部３０は、鉛等の金属から構成された錘であってもよいし、空気タンクであってもよい。すなわち、重心位置変更部３０は、艇体２内において前後方向に移動することにより水中航走体１，１０の前後方向において重心位置を変更できるものであればよい。このように重心位置変更部３０を備える場合、水中航走体１，１０は、艇体２のピッチングにおける回転方向を該重心位置変更部３０の移動により決定させることができ、垂直スラスタ３２によるピッチ方向での旋回制御をより容易とすることができる。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施の形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

本発明は、水中において艇体を目標位置に保持して作業を行ったり、作業を行う前段階で艇体を目標位置に保持したりする必要があるＡＵＶ等の水中航走体全般に有用である。

１ 水中航走体
２ 艇体
３ アクチュエータ
４ 第１比較部
４ａ 第１比較部
４ｂ 第１比較部
４ｃ 第１比較部
５ 第２比較部
５ａ 第２比較部
５ｂ 第２比較部
５ｃ 第２比較部
６ 制御力算出部
７ 推力配分装置
８ ジャイロセンサ
９ 測位装置
１０ 水中航走体
１１ 流向計
１２ 第１変化率リミッタ
１３ 第２変化率リミッタ
２１ ヨー角指令値算出部
２２ ピッチ角指令値算出部
３０ 重心位置変更部
５０ 制御装置

Claims (8)

1. 艇体の位置を示す位置情報を検知する測位装置と、
   前記艇体の姿勢を示す姿勢情報を検知する姿勢検知センサと、
   水中において前記艇体に対して前後方向、左右方向、および上下方向それぞれに推力を与え、該艇体の位置および姿勢を変更させるアクチュエータと、
   前記アクチュエータを制御する制御装置と、を備え、
   前記制御装置は、
   前記測位装置によって検知された前記位置情報に基づき前記艇体を目標位置に保持させるように、前記艇体における前後方向の制御力、左右方向の制御力、上下方向の制御力、および艇体をロール方向、ヨー方向、ピッチ方向それぞれに旋回させる旋回制御力を求めて前記アクチュエータを制御しており、
   目標位置に保持されている前記艇体に外力が及ぼされたとき、前記左右方向および上下方向の制御力の大きさがゼロとなるように、目標とする姿勢情報を更新し、前記姿勢検知センサによって検知された前記姿勢情報に基づき前記艇体を更新後の姿勢情報に応じた姿勢に変更させるように前記アクチュエータを制御する水中航走体。
2. 前記制御装置は、目標とする位置情報および姿勢情報と、前記測位装置によって検知された位置情報および前記姿勢検知センサによって検知された姿勢情報との偏差から前記前後方向の制御力、前記左右方向の制御力、前記上下方向の制御力、および前記ロール方向、ヨー方向、ピッチ方向それぞれでの旋回制御力を算出する制御力算出部を備え、
   目標位置に保持されている前記艇体に外力が及ぼされたとき、前記制御力算出部によって算出された前記左右方向および前記上下方向の制御力の大きさがゼロとなるように、目標とする姿勢情報のうちヨー角およびピッチ角の指令値を更新させる請求項１に記載の水中航走体。
3. 前記艇体に及ぼされる外力の方向として潮流到来方向を計測する流向計を備え、
   前記制御装置は、前記流向計によって計測された前記潮流到来方向に船首が向いた艇体の姿勢を、目標とする姿勢情報とし、前記ヨー角および前記ピッチ角それぞれの指令値を更新する請求項２に記載の水中航走体。
4. 前記制御装置は、
   更新された前記ピッチ角の指令値と前記姿勢検知センサによって検知された姿勢情報におけるピッチ角の値との偏差から求められるピッチ方向への旋回制御力の変化量に制限を加える第１変化率リミッタと、
   更新された前記ヨー角の指令値と前記姿勢検知センサによって検知された姿勢情報におけるヨー角の値との偏差から求められるヨー方向への旋回制御力の変化量に制限を加える第２変化率リミッタと、を備え、
   前記第１変化率リミッタの変化量および前記第２変化率リミッタの変化量それぞれの設定を変更し、ヨー角、ピッチ角の順にそれぞれの指令値を更新する、あるいはヨー角の指令値の更新速度に対してピッチ角の指令値の更新速度を小さくする請求項３に記載の水中航走体。
5. 前記制御装置は、
   前記左右方向の制御力の値を積分し、目標とするヨー角の指令値を求めるヨー角指令値算出部と、
   前記上下方向の制御力の値を積分し、目標とするピッチ角の指令値を算出するピッチ角指令値算出部と、を備え、
   前記左右方向および前記上下方向の制御力の大きさがゼロとなるまで、前記ヨー角指令値算出部によって算出された指令値と前記ピッチ角指令値算出部によって算出された指令
   値によって、前記ヨー角および前記ピッチ角それぞれの指令値を更新する請求項２に記載の水中航走体。
6. 前記ヨー角指令値算出部は、前記左右方向の制御力の値にゲインを乗じて積分した値から目標とするヨー角の指令値を求めており、
   前記ピッチ角指令値算出部は、前記上下方向の制御力の値にゲインを乗じて積分した値から目標とするピッチ角の指令値を求めており、
   前記ヨー角指令値算出部において前記左右方向の制御力の値に乗じる前記ゲインおよび前記ピッチ角指令値算出部において前記上下方向の制御力の値に乗じる前記ゲインそれぞれの値を変更し、ヨー角、ピッチ角の順にそれぞれの指令値を更新する、あるいはヨー角の指令値の更新速度に対してピッチ角の指令値の更新速度を小さくする請求項５に記載の水中航走体。
7. 前記アクチュエータは、
   前記艇体の重心位置を変更するように、艇体内において前後方向に移動する重心位置変更部を備える請求項１から６のいずれか１項に記載の水中航走体。
8. 艇体の位置を示す位置情報を検知する測位装置と、
   前記艇体の姿勢を示す姿勢情報を検知する姿勢検知センサと、
   水中において前記艇体に対して前後方向、左右方向、および上下方向それぞれに推力を与え、該艇体の位置および姿勢を変更させるアクチュエータと、
   前記アクチュエータを制御する制御装置と、を備えた水中航走体の姿勢制御方法であって、
   前記制御装置が、
   前記測位装置によって検知された前記位置情報に基づき前記艇体を目標位置に保持させるように、前記艇体における前後方向の制御力、左右方向の制御力、上下方向の制御力、および艇体をロール方向、ヨー方向、ピッチ方向それぞれに旋回させる旋回制御力を求めて前記アクチュエータを制御するステップと、
   目標位置に保持されている前記艇体に外力が及ぼされたとき、前記左右方向および上下方向の制御力の大きさがゼロとなるように、目標とする姿勢情報を更新し、前記姿勢検知センサによって検知された前記姿勢情報に基づき前記艇体を更新後の姿勢情報に応じた姿勢に変更させるように前記アクチュエータを制御するステップと、を含む、水中航走体の姿勢制御方法。

Images