(57)【特許請求の範囲】
【請求項１】
３次元空間内の自由方向に航行する航行体の３次元航行に用いられる目標の航行制御パラメータであって、目標深度、目標ロール角、目標ピッチ角、目標方位角についての目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行うフィードバック制御部と、
前記航行体の進行方向の進行速度と前記進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得する速度取得部と、
前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正項であって前記進行速度と前記横流れ速度の積に比例する補正項を含む、前記航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、前記航行制御パラメータに基づいて算出した値、前記進行速度、前記横流れ速度を含む状態変数と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出するフィードフォワード制御部と、
前記フィードフォワード制御値を用いて前記フィードバック制御の結果を補正する制御補正部と、
を備えることを特徴とすることを特徴とする航行体制御装置。
【請求項２】
前記干渉補償演算式は、前記航行体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記航行体の全運転範囲において適応可能なように変化させた線形化状態方程式を、前記航行体が所望の運動を行うために必要な航行制御量に関するパラメータについて逆に解くことにより導出された演算式であり、
前記フィードフォワード制御部は、前記干渉補償演算式に、前記状態変数を代入することにより干渉補償舵角を算出し、
前記制御補正部は、前記フィードバック制御の結果である舵角指令を前記干渉補償舵角とに基づいて補正する
ことを特徴とする請求項１に記載の航行体制御装置。
【請求項３】
前記航行体は水中を航行する航行体であり、カルマンフィルタを用いて前記横流れ速度を推定する横流れ速度推定部と、
を備えることを特徴とする請求項１に記載の航行体制御装置。
【請求項４】
請求項１から請求項３のいずれか一項に記載の航行体制御装置を備えた航行体。
【請求項５】
３次元空間内の自由方向に航行する航行体の３次元航行に用いられる目標の航行制御パラメータであって、目標深度、目標ロール角、目標ピッチ角、目標方位角についての目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行い、
前記航行体の進行方向の進行速度と前記進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得し、
前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正項であって前記進行速度と前記横流れ速度の積に比例する補正項を含む、前記航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、前記航行制御パラメータに基づいて算出した値、前記進行速度、前記横流れ速度を含む状態変数と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出し、
前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正係数であって前記進行速度と前記横流れ速度の積に比例する補正係数を含む、前記航行体の任意の平衡点近傍における線形化状態方程式と、前記航行制御パラメータに基づいて算出した状態変数と、干渉補償演算式と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出し、
前記フィードフォワード制御値を用いて前記フィードバック制御の結果を補正する
ことを特徴とすることを特徴とする航行体制御方法。
【請求項６】
航行体制御装置のコンピュータを、
３次元空間内の自由方向に航行する航行体の３次元航行に用いられる目標の航行制御パラメータであって、目標深度、目標ロール角、目標ピッチ角、目標方位角についての目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行うフィードバック制御手段、
前記航行体の進行方向の進行速度と前記進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得する速度取得手段、
前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正項であって前記進行速度と前記横流れ速度の積に比例する補正項を含む、前記航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、前記航行制御パラメータに基づいて算出した値、前記進行速度、前記横流れ速度を含む状態変数と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出するフィードフォワード制御手段、
前記フィードフォワード制御値を用いて前記フィードバック制御の結果を補正する制御補正手段、
として機能させることを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、航行体制御装置、航行体、航行体制御方法、プログラムに関する。
【背景技術】
【０００２】
水中等の３次元空間内を運転者の制御に基づいて自由方向に浮遊して航行する航行体は、進行方向の変更制御が行われると進行方向の慣性力に基づいて、進行方向を変更した航行体に横方向の力が加わり横流れ方向の速度が発生する。またその際、航行体には、進行方向の変更前ベクトルと変更後ベクトルを含んで形成される平面に垂直で、航行体の重心を通る軸を中心として、進行方向の変更による旋回方向と同方向に回転力（モーメント）が生じる。これら横流方向の力や重心を通る軸を中心とする回転力は、進行方向の速度や重心の位置によって変動する。なお、関連する技術として、横加速度に応じて、ハンドルに付与する反トルクを制御し、操作感覚の向上を目的とする技術が特許文献１に開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第４９３８２７１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、上述のような航行体の重心を通る軸を中心に回転する際の回転力は進行方向のオーバシュート（船首の方向が目標方位方向を超えて回転し目標方位方向がずれる等の現象）を生じる。
【０００５】
そこでこの発明は、上述の課題を解決することのできる航行体制御装置、航行体、航行体制御方法、プログラムを提供することを目的としている。
【課題を解決するための手段】
【０００６】
上記目的を達成するために、本発明は、３次元空間内の自由方向に航行する航行体の３次元航行に用いられる目標の航行制御パラメータであって、目標深度、目標ロール角、目標ピッチ角、目標方位角についての目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行うフィードバック制御部と、前記航行体の進行方向の進行速度と前記進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得する速度取得部と、前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正項であって前記進行速度と前記横流れ速度の積に比例する補正項を含む、前記航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、前記航行制御パラメータに基づいて算出した値、前記進行速度、前記横流れ速度を含む状態変数と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出するフィードフォワード制御部と、前記フィードフォワード制御値を用いて前記フィードバック制御の結果を補正する制御補正部と、を備えることを特徴とすることを特徴とする航行体制御装置である。
【０００７】
また本発明は、前記干渉補償演算式は、前記航行体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、前記航行体の全運転範囲において適応可能なように変化させた線形化状態方程式を、前記航行体が所望の運動を行うために必要な航行制御量に関するパラメータについて逆に解くことにより導出された演算式であり、前記フィードフォワード制御部は、前記干渉補償演算式に、前記状態変数を代入することにより干渉補償舵角を算出し、前記制御補正部は、前記フィードバック制御の結果である舵角指令を前記干渉補償舵角とに基づいて補正することを特徴とする。
【０００８】
また本発明は、前記航行体は水中を航行する航行体であり、カルマンフィルタを用いて前記横流れ速度を推定する横流れ速度推定部と、を備えることを特徴とする。
【０００９】
また本発明は、上述の航行体制御装置を備えた航行体である。
【００１０】
また本発明は、３次元空間内の自由方向に航行する航行体の３次元航行に用いられる目標の航行制御パラメータであって、目標深度、目標ロール角、目標ピッチ角、目標方位角についての目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行い、前記航行体の進行方向の進行速度と前記進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得し、前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正項であって前記進行速度と前記横流れ速度の積に比例する補正項を含む、前記航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、前記航行制御パラメータに基づいて算出した値、前記進行速度、前記横流れ速度を含む状態変数と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出し、前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正係数であって前記進行速度と前記横流れ速度の積に比例する補正係数を含む、前記航行体の任意の平衡点近傍における線形化状態方程式と、前記航行制御パラメータに基づいて算出した状態変数と、干渉補償演算式と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出し、前記フィードフォワード制御値を用いて前記フィードバック制御の結果を補正することを特徴とすることを特徴とする航行体制御方法である。
【００１１】
また本発明は、航行体制御装置のコンピュータを、３次元空間内の自由方向に航行する航行体の３次元航行に用いられる目標の航行制御パラメータであって、目標深度、目標ロール角、目標ピッチ角、目標方位角についての目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行うフィードバック制御手段、前記航行体の進行方向の進行速度と前記進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得する速度取得手段、前記進行方向の変更に伴う前記航行制御パラメータへの干渉を補正する補正項であって前記進行速度と前記横流れ速度の積に比例する補正項を含む、前記航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、前記航行制御パラメータに基づいて算出した値、前記進行速度、前記横流れ速度を含む状態変数と、を用いて前記フィードバック制御の結果を補正するフィードフォワード制御値を算出するフィードフォワード制御手段、前記フィードフォワード制御値を用いて前記フィードバック制御の結果を補正する制御補正手段、として機能させることを特徴とするプログラムである。
【発明の効果】
【００１２】
本発明によれば、航行体の方位方向の変更に基づく横流れに対する干渉項を適用した上述の線形化状態方程式を用いてフィードフォワード制御を行うことにより、ロール角φ、方位角ψの相互干渉を抑制することができ、航行体の船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。
【図面の簡単な説明】
【００１３】
【図１】本発明の一実施形態による航行体の概略構成を示す図である。
【図２】本発明の一実施形態による航行体の後方から見たときの舵の配置について模式的に示した図である。
【図３】本発明の一実施形態による航行体の運動の自由度について説明するための図である。
【図４】本発明の一実施形態による航行体制御装置の機能ブロックを示す第１の図である。
【図５】本発明の一実施形態による航行体制御装置の機能ブロックを示す第２の図である。
【図６】本発明の一実施形態による航行体制御装置の機能ブロックを示す第３の図である。
【図７】本発明の一実施形態による航行体制御装置の進行方向制御時におけるシミュレーション結果を示す第１の図である。
【図８】本発明の一実施形態による航行体制御装置の進行方向制御時におけるシミュレーション結果を示す第２の図である。
【発明を実施するための形態】
【００１４】
以下、本発明の一実施形態による航行体及び航行体制御装置、航行体制御方法を図面を参照して説明する。
図１は本発明の一実施形態による航行体の概略構成を示す図である。
図２は本発明の一実施形態による航行体の後方から見たときの舵の配置について模式的に示した図である。
図３は航行体の運動の自由度について説明するための図である。
【００１５】
図１、図２において符号１は航行体の船体を示している。また図１，２に示すように航行体には複数の舵３ａ〜３ｅが設けられている。航行体は、舵３ａ〜３ｄを、主に航行体のピッチ角θ、ロール角φ、方位角ψに関する制御に用いる。また航行体は、舵３ｅを、主に深度に関する制御に用いる。
【００１６】
図３に示すように、航行体において、その船体１は、絶対座標系（地球座標系）における船尾軸線（以下「ｘ軸」という）、左右軸線（以下「ｙ軸」という）、上下軸線（以下「ｚ軸」という）からなる互いに直行する３つの直線軸に基づいて、航行体制御装置によって制御される。
【００１７】
図３に示すように、本実施形態では、船体座標系における上記ｘ軸方向、ｙ軸方向、ｚ軸方向のプラス側への進行速度をそれぞれｕ，ｖ，ｗとし、それぞれの軸周りの角速度と角加速度をｐ、ｄｐ／ｄｔ、ｑ，ｄｑ／ｄｔ、ｒ，ｄｒ／ｄｔと定義する。なお航行体制御装置１０が目標とするそれぞれの軸周りの各速度と各加速度の指令値をｐ＊（目標ロール角速度），ｄｐ＊／ｄｔ（目標ロール角加速度），ｑ＊（目標ピッチ角速度），ｄｑ＊／ｄｔ（目標ピッチ角加速度），ｒ＊（目標方位角速度＝目標ヨー角速度），ｄｒ＊／ｄｔ（目標方位角加速度＝目標ヨー角加速度）と定義する。各記号に付される＊印は指令値であることを示している。更に、本実施形態においては、ｘ軸周りの回転角度をロール角φ、ｙ軸周りの回転角度をピッチ角θ、ｚ軸周りの回転角度（方位角度）をヨー角ψと定義する。
航行体制御装置は、後述する処理によって舵３ａ〜３ｅを制御することにより、航行体の絶対座標系における姿勢や位置、進行方向（方位角）、進行速度、深度を制御する。
なお航行体は水中を走行する水中走行体や、空中を浮遊する空中浮遊体である。
【００１８】
図４は本実施形態による航行体制御装置の機能ブロックを示す第１の図である。
以下に本発明の一実施形態に係る航行体の制御方法について図を参照して説明する。
図４において符号１０は航行体制御装置を示している。当該航行体制御装置１０は航行体の船体に設けられ、舵３ａ〜３ｅや推進装置（エンジン、プロペラ等を含む装置）による推進速力を制御する。
図４で示すように、航行体制御装置１０は、航行制御パラメータ取得部１１、フィードバック制御部１２、速度取得部１３、フィードフォワード制御部１４、制御補正部１５を少なくとも備えている。これら各制御部は、航行体制御装置１０のコンピュータのＣＰＵ（Central Processing Unit）がメモリやハードディスクドライブ等の記憶部に記録されているプログラムを読み込んで実行することにより航行体制御装置１０に備わる各機能に対応する。
航行体制御装置における制御補正部１５からの出力は船体運動制御装置３０へ出力される。船体運動制御装置３０は、上述した舵３ａ〜３ｅや推進装置である。
【００１９】
航行制御パラメータ取得部１１は、絶対座標系（地球座標系）内（３次元空間内）の自由方向の自由方向に航行する航行体の３次元航行に用いられる航行制御パラメータであって、深度、ロール角、ピッチ角、方位角についての航行制御パラメータを順次取得する。
フィードバック制御部１２は、目標の航行制御パラメータと該目標の航行制御パラメータに対応する実測値とに基づいてフィードバック制御を行う。
速度取得部１３は、航行体の進行方向の進行速度と進行方向の変更に伴う航行体の横流れ方向の横流れ速度とを取得する。
フィードフォワード制御部１４は、進行方向の変更に伴う航行制御パラメータへの干渉を補正する補正項であって進行速度ｕと横流れ速度ｖの積に比例する補正項を含む、航行体の任意の平衡点近傍における線形化状態方程式を用いて導出した干渉補償演算式と、航行制御パラメータに基づいて算出した値、進行速度ｕ、横流れ速度ｖを含む状態変数と、を用いてフィードバック制御の結果を補正するフィードフォワード制御値を算出する。
制御補正部１５は、フィードフォワード制御値を用いてフィードバック制御の結果を補正する。
【００２０】
なお、干渉補償演算式は、航行体の任意の平衡点近傍における線形化状態方程式において、係数行列を平衡点の関数として表して、航行体の全運転範囲において適応可能なように変化させた線形化状態方程式を用いて導出する。具体的には干渉補償演算式は、この線形化状態方程式を、航行体が所望の運動を行うために必要な航行制御量に関するパラメータについて逆に解くことにより導出する。
フィードフォワード制御部１４は、干渉補償演算式に、状態変数を代入することにより干渉補償舵角を算出し、制御補正部１５は、フィードバック制御の結果である舵角指令を前記干渉補償舵角とに基づいて補正する。
【００２１】
図５は本実施形態による航行体制御装置の機能ブロックを示す第２の図である。
図６は本実施形態による航行体制御装置の機能ブロックを示す第３の図である。
図５で示す機能ブロック図は、図４の機能ブロック図における航行制御パラメータ取得部１１、フィードバック制御部１２、制御補正部１５を詳細に説明している。
図６で示す機能ブロック図は、図４の機能ブロック図における速度取得部１３、フィードフォワード制御部１４をさらに詳細に説明している。
図５で示すように、航行制御パラメータ取得部１１は、目標深度算出部１１１、目標ロール角算出部１１２、目標ピッチ角算出部１１３、目標方位角算出部１１４を備えている。
なお、図中のｗ＊，ｐ＊，ｑ＊，ｒ＊の上に付されているドット（・）は、微分値ｄ／ｄｔであることを示している。
【００２２】
目標深度算出部１１１は、取得した設定深度ｚｓｅｔに基づいて、目標深度ｚ＊を算出する。当該目標深度ｚ＊を図６で示す微分器９２ａが微分演算して目標深度方向速度ｗ＊を算出する。また目標深度方向速度ｗ＊を図６で示す微分器９２ｂが微分演算して目標深度方向加速度ｄｗ＊／ｄｔを算出する。微分器９２ａ，９２ｂの機能を目標深度算出部１１１が有しており、当該目標深度算出部１１１が目標深度方向速度ｗ＊、目標深度方向加速度ｄｗ＊／ｄｔを算出するようにしてもよい。
目標ロール角算出部１１２は、取得した設定ロール角φｓｅｔに基づいて、目標ロール角φ＊を算出する。当該目標ロール角φ＊を図６で示す微分器９２ｃが微分演算して目標ロール角速度ｐ＊を算出する。また目標ロール角速度ｐ＊を図６で示す微分器９２ｄが微分演算して目標ロール角加速度ｄｐ＊／ｄｔを算出する。微分器９２ｃ，９２ｄの機能を目標ロール角算出部１１２が有しており、当該目標ロール角算出部１１２が目標ロール角速度ｐ＊、目標ロール角加速度ｄｐ＊／ｄｔを算出するようにしてもよい。
目標ピッチ角算出部１１３は、取得した設定ピッチ角θｓｅｔに基づいて、目標ピッチ角θ＊を算出する。当該目標ピッチ角θ＊を図６で示す微分器９２ｅが微分演算して目標ピッチ角速度ｑ＊を算出する。また目標ピッチ角速度ｑ＊を図６で示す微分器９２ｆが微分演算して目標ピッチ角加速度ｄｑ＊／ｄｔを算出する。微分器９２ｅ，９２ｆの機能を目標ピッチ角算出部１１３が有しており、当該目標ピッチ角算出部１１３が、目標ピッチ角速度ｑ＊、目標ピッチ角加速度ｄｑ＊／ｄｔを算出するようにしてもよい。
目標方位角算出部１１４は、取得した設定方位角ψｓｅｔに基づいて、目標方位角ψ＊を算出する。当該目標方位角ψ＊を図６で示す微分器９２ｇが微分演算して目標方位角速度ｒ＊を算出する。また目標方位角速度ｒ＊を図６で示す微分器９２ｈが微分演算して目標方位角加速度ｄｒ＊／ｄｔを算出する。微分器９２ｇ，９２ｈの機能を目標方位角算出部１１４が有しており、当該目標方位角算出部１１４が、目標方位角速度ｒ＊、目標方位角加速度ｄｒ＊／ｄｔを算出するようにしてもよい。
【００２３】
図５で示すように、フィードバック制御部１２は、差分演算部１２１、１２２、１２３、１２４を備えている。またフィードバック制御部１２は、フィードバック制御器（以下、ＦＢ制御器）１２５、１２６、１２７、１２８を備えている。
差分演算部１２１は、目標深度ｚ＊と、前回の実測値を基準とする実測値の深度ｚとの差分を算出する。
差分演算部１２２は、目標ロール角φ＊と、前回の実測値を基準とする今回の実測値のロール角φとの差分を算出する。
差分演算部１２３は、目標ピッチ角θ＊と、前回の実測値を基準とする実測値のピッチ角θとの差分を算出する。
差分演算部１２４は、目標方位角ψ＊と、前回の実測値を基準とする実測値の方位角ψとの差分を算出する。
ＦＢ制御器１２５は、差分演算部１２１の出力に基づいてＰＩＤ制御を行うことにより深度干渉補償舵角のフィードバック制御値（以下、深度フィードバック舵角δｂＦＢ）を算出する。
ＦＢ制御器１２６は、差分演算部１２２の出力に基づいてＰＩＤ制御を行うことによりロール角干渉補償舵角のフィードバック制御値（以下、ロール角フィードバック舵角δｒｒＦＢ）を算出する。
ＦＢ制御器１２７は、差分演算部１２３の出力に基づいてＰＩＤ制御を行うことによりピッチ角干渉補償舵角のフィードバック制御値（以下、ピッチ角フィードバック舵角δｒｃＦＢ）を算出する。
ＦＢ制御器１２８は、差分演算部１２４の出力に基づいてＰＩＤ制御を行うことにより方位角干渉補償舵角のフィードバック制御値（以下、方位角フィードバック舵角δｒｌＦＢ）を算出する。
【００２４】
図６で示すように、フィードフォワード制御部１４は、係数行列算出部１４１、干渉補償舵角算出部１４２、座標変換部１４３を備えている。
係数行列算出部１４１は、速度取得部１３から入力した航行体の進行方向の進行速度ｕ、航行体の横流れ速度ｖ、目標深度方向速度ｗ＊、目標ロール角速度ｐ＊、目標ロール角φ＊、目標ピッチ角速度ｑ＊、目標ピッチ角θ＊、目標方位角速度ｒ＊の値を順次取得し、それらの平衡状態の値に基づいて、Ａ（ｎ）行列、Ｂ（ｎ）行列の２つの行列を算出する。当該２つの行列の詳細については後述する。なおＡ（ｎ）行列、Ａ（ｎ）行列の各要素はある平衡点Ｘ￣，Ｕ￣における関数として表現される。
【００２５】
干渉補償舵角算出部１４２は、係数行列算出部１４１からＡ（ｎ）行列、Ｂ（ｎ）行列の情報を取得する。
また干渉補償舵角算出部１４２は、微分器９２ａ，９２ｂからそれぞれ目標深度方向速度ｗ＊、目標深度方向加速度ｄｗ＊／ｄｔを取得する。
また干渉補償舵角算出部１４２は、目標ロール角算出部１１２から目標ロール角φ＊を、微分器９２ｃ，９２ｄからそれぞれ目標ロール角速度ｐ＊、目標ロール角加速度ｄｐ＊／ｄｔを取得する。
また干渉補償舵角算出部１４２は、目標ピッチ角算出部１１３から目標ピッチ角θ＊を、微分器９２ｅ，９２ｆからそれぞれ目標ピッチ角速度ｑ＊、目標ピッチ角加速度ｄｑ＊／ｄｔを取得する。
また干渉補償舵角算出部１４２は、微分器９２ｇ，９２ｈからそれぞれ目標方位角速度ｒ＊、目標方位角加速度ｄｒ＊／ｄｔを取得する。
【００２６】
そして干渉補償舵角算出部１４２は、取得した各値や行列の情報を用いて、舵角の干渉補償値となるフィードフォワード制御値である干渉補償舵角δｂＦＦ＊（深度干渉補償舵角）を算出する。
また干渉補償舵角算出部１４２は、取得した各値や行列の情報を用いて、舵角の干渉補償値となるフィードフォワード制御値である干渉補償舵角δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊、を算出する。
なお干渉とは、深度、ロール角、ピッチ角、方位角の何れか一つを制御しようとすると、他の値に影響を与えることを言う。例えば、船尾軸線を中心とする楕円状の航行体が水中を航行し、その航行体の進行方向（方位角）を変更した場合、上述したように横流れ速度ｖが発生する。この時、当該航行体の船尾軸線を中心とする面の縁に図１のような凸形状が存在するような場合には、横流れ速度ｖに基づく当該凸形状に作用する水の反作用によって航行体が船尾軸線を回転軸として回転する。このような回転を干渉という。
【００２７】
座標変換部１４３は、干渉補償舵角算出部１４２によって得られた干渉補償舵角δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊を用いて、ロール角、ピッチ角、方位角のそれぞれに対応する干渉補償舵角（ロール角干渉補償舵角δｒｒＦＦ＊、ピッチ角干渉補償舵角δｒｃＦＦ＊、方位角干渉補償舵角δｒｌＦＦ＊）へ変換する。これは、幾何学的な舵の配置に基づき、４つの舵がロール、ピッチ、方位へ作用する舵力に変換するものである。
【００２８】
図５で示すように、制御補正部１５は、深度舵角指令算出部１５１、ロール角舵角指令算出部１５２、ピッチ角舵角指令算出部１５３、方位角舵角指令算出部１５４、座標変換部１５５を備えている。
深度舵角指令算出部１５１は、深度フィードバック舵角δｂＦＢと深度干渉補償舵角δｂＦＦ＊とを加算して深度舵角指令δｂ＊の値を算出して船体運動制御装置３０へ出力する。
ロール角舵角指令算出部１５２は、ロール角フィードバック舵角δｒｒＦＢとロール角干渉補償舵角δｒｒＦＦ＊とを加算してロール角舵角指令δｒｒ＊の値を算出し、座標変換部１５５へ出力する。
ピッチ角舵角指令算出部１５３は、ピッチ角フィードバック舵角δｒｃＦＢとピッチ角干渉補償舵角δｒｃＦＦ＊とを加算してピッチ角舵角指令δｒｃ＊の値を算出し、座標変換部１５５へ出力する。
方位角舵角指令算出部１５４は、方位角フィードバック舵角δｒｌＦＢと方位角干渉補償舵角δｒｌＦＦ＊とを加算して方位角舵角指令δｒｌ＊の値を算出し、座標変換部１５５へ出力する。
座標変換部１５５は、取得したロール角舵角指令δｒｒ＊、ピッチ角舵角指令δｒｃ＊、方位角舵角指令δｒｌ＊に基づいて、座標変換処理を行い、各舵３ａ〜３ｄへの舵制御指令δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊を算出する。これにより、舵３ｅが深度舵角指令δｂ＊に基づいて、舵３ａが舵制御指令δｒ１＊に基づいて、舵３ｂが舵制御指令δｒ２＊に基づいて、舵３ｃが舵制御指令δｒ３＊に基づいて、舵３ｄが舵制御指令δｒ４＊に基づいて、それぞれ駆動制御されることにより、深度ｚ、ロール角φ、ピッチ角θ、方位角ψ間の相互干渉を抑制しながら、船体姿勢に追従させることが可能となる。
【００２９】
船体運動制御装置３０は、深度舵角指令δｂ＊と、舵制御指令δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊に基づいて、舵３ａ〜３ｅを制御する。
なお船体運動制御装置３０の制御の結果、深度ｚ、ロール角φ、ピッチ角θ、方位角ψが変化する。これら深度ｚ、ロール角φ、ピッチ角θ、方位角ψは、航行体に備わる深度計（又は高度計）や、ジャイロセンサ、角速度センサなどのセンサによって計測され、その値を航行体制御装置が取得してフィードバック制御等に用いる。
【００３０】
次に、本実施形態に係る航行体制御装置の処理の詳細について説明する。
まず目標深度算出部１１１は、上位装置（例えば運転者の操作を出力する装置）から離散的な値である設定深度ｚｓｅｔを取得し、この値に対して所定の応答モデルを用いて、連続的な目標深度ｚ＊を算出する。応答モデルの一例としては、以下の式（１）に示すような、２次のローパスフィルタが挙げられる。
【００３１】
【数１】
0006380951.tif 000002
【００３２】
式（１）においてξは減衰率［−］、ωｎは応答周波数［ｒａｄ／ｓ］であり、以下の式（２）で表される。例えば、深度制御系においては、オーバシュートなしてとして、ξ＝１と設定される。
【００３３】
【数２】
0006380951.tif 000003
【００３４】
式（２）において、Ｔｓは制定時間［ｓｅｃ］であり、運転条件に応じて任意に設定される。そして、目標深度算出部１１１は、目標深度ｚ＊を差分演算部１２１、係数行列算出部１４１、干渉補償舵角算出部１４２へ出力する。
【００３５】
目標ロール角算出部１１２は、上位装置から離散的な値である設定ロール角φｓｅｔを取得し、この値に対して所定の応答モデルを用いて、連続的な目標ロール角φ＊を算出する。なお目標ロール角算出部１１２の目標ロール角φ＊の算出処理は、上述した目標深度算出部１１１における目標深度ｚ＊の算出処理と同様であるため、詳細な説明は省略する。そして、目標ロール角算出部１１２は、目標ロール角φ＊を差分演算部１２２、係数行列算出部１４１、干渉補償舵角算出部１４２へ出力する。
目標ピッチ角算出部１１３は、上位装置から離散的な値である設定ピッチ角θｓｅｔを取得し、この値に対して所定の応答モデルを用いて、連続的な目標ピッチ角θ＊を算出する。なお目標ピッチ角算出部１１３の目標ピッチ角θ＊の算出処理は、上述した目標深度算出部１１１における目標深度ｚ＊の算出処理と同様であるため、詳細な説明は省略する。そして、目標ピッチ角算出部１１３は、目標ピッチ角θ＊を差分演算部１２３、係数行列算出部１４１、干渉補償舵角算出部１４２へ出力する。
目標方位角算出部１１４は、上位装置から離散的な値である設定方位角ψｓｅｔを取得し、この値に対して所定の応答モデルを用いて、連続的な目標方位角ψ＊を算出する。なお目標方位角算出部１１４の目標方位角ψ＊の算出処理は、上述した目標深度算出部１１１における目標深度ｚ＊の算出処理と同様であるため、詳細な説明は省略する。そして、目標方位角算出部１１４は、目標方位角ψ＊を差分演算部１２４、係数行列算出部１４１、干渉補償舵角算出部１４２へ出力する。
【００３６】
係数行列算出部１４１および干渉補償舵角算出部１４２を備えたフィードフォワード制御部１４では、線形化された船体１の状態方程式から逆問題解析によって導出された干渉補償演算式を用いて、航行体の深度ｚ、ロール角φ、ピッチ角θ、方位角ψそれぞれの相互干渉を抑制するための干渉補償値となる深度干渉補償舵角δｂＦＦ＊、ロール角干渉補償舵角δｒｒＦＦ＊、ピッチ角干渉補償舵角δｒｃＦＦ＊、方位角干渉補償舵角δｒｌＦＦ＊を算出する。
【００３７】
ここで、干渉補償演算式の導出について説明する。一般的に、船体１の運動は非線形であるが、船体１の平衡点近傍（平衡点まわりの微小空間）では線形化できる。例えば船体１の任意の平衡点近傍における線形化状態方程式は、以下の式（３）、式（４）で表される。ここで、平衡点については、目標値において船体１が平衡状態となると仮定し、船体運動の目標値を平衡点としている。
【００３８】
【数３】
0006380951.tif 000004
【００３９】
式（３）から式（６）において、ｕ、ｖ、ｗ、ｐ、ｑ、ｒ、φ、θ、ψは、上述したｕ＊、ｖ＊、ｗ＊、ｐ＊、ｑ＊、ｒ＊、φ＊、θ＊、ψ＊に対応し、ｎは推進力を得るために航行体に備わるプロペラの回転数［ｒｐｓ］、δｂは舵３ｅの舵角、換言すると深度に関する舵角［ｒａｄ］、δｒ１、δｒ２、δｒ３、δｒ４は、上述したδｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊に対応し、それぞれ舵３ａ、舵３ｂ、舵３ｃ、舵３ｄの舵角である。
【００４０】
続いて、任意の操作量Ｕに対して、平衡状態での状態量が、あるＸに落ち着く平衡点（ｄｘ／ｄｔ＝０）が存在すると仮定すると、以下の式（７）が得られる。
【００４１】
ｆ（Ｘ＊，Ｕ＊）＝０      （７）
【００４２】
この状態からの微小変動量をΔＸ，ΔＵとすると、以下の式（８）が得られる。
【００４３】
【数４】
0006380951.tif 000005
【００４４】
上記式（８）を平衡点の近傍でテーラー展開し、１次近似とすると、以下の式（９）で表される平衡点周りの状態方程式が得られる。
【００４５】
【数５】
0006380951.tif 000006
【００４６】
ここで、速度ｕ、ｖ、は制御量ではないこと、回転数ｎは速度ｕにのみ影響を与える操作量であること、方位角ψは、船体に復元力として作用しないことから、本発明の目的とは無関係であるため、省略してモデルリダクションすることができる。ただし、任意の推進速力ｕで航行体が航行しているときに方位変換を行い、横流れ速度ｖが発生した際に作用する干渉項（補正項）をＦ・ｕ・ｖとして定義する。Ｆは航行体の船体１の特性で定まる係数である。つまり任意の速力ｕで航行体が航行しているときに方位変換を行い、横流れ速度ｖが発生した際に作用する干渉力は、推進速力ｕと横流れ速力ｖの積に比例する。式（９）ではこの干渉項を考慮した状態方程式である。式（９）において、Ａ（ｎ）行列は以下の式（１０）、行列Ｂは以下の式（１１）で与えられる。
【００４７】
【数６】
0006380951.tif 000007
【００４８】
【数７】
0006380951.tif 000008
【００４９】
上述の式（１０）、（１１）と、この干渉項Ｆ・ｕ・ｖを考慮すると、線形化状態方程式は、以下の式（１２）で表される。
【００５０】
【数８】
0006380951.tif 000009
【００５１】
式（１２）において、Δｗはｚ軸方向の速度の微小変化［ｍ／ｓ］である。Δｐは目標ロール角速度の微小変化［ｒａｄ／ｓ］である。Δｑは目標ピッチ角速度の微小変化［ｒａｄ／ｓ］である。Δｒはヨー角速度の微小変化［ｒａｄ／ｓ］である。Δφはロール角の微小変化［ｒａｄ］である。Δθはピッチ角の微小変化［ｒａｄ］である。Δδｂは舵３ｅの微小変化、換言すると、深度に関する舵角指令の微小変化［ｒａｄ］である。Δδｒｒ、Δδｒｃ、Δδｒｌはそれぞれロール舵、ピッチ舵、方位（ヨー）舵の微小変化、換言すると、ロール角φ、ピッチ角θ、方位角（ヨー角）ψに関する舵角指令の微小変化［ｒａｄ］である。
【００５２】
上記Ａ（ｎ）行列、Ｂ（ｎ）行列の各要素ａ１１〜ａ６６、ｂ１１〜ｂ６５は、ある平衡点Ｘ＊、Ｕ＊における関数または定数として表現される。例えば、要素ａ１４、ａ１５、ａ１６は、以下の式（１３）〜式（１５）で表される。なお、その他の各要素についても上記の式（３）〜（９）から当然のごとく導出される。
【００５３】
【数９】
0006380951.tif 000010
【００５４】
式（１３）から式（１５）において、係数Ｋは、船体固有の運動特性によって決定される定数である。係数Ｋの添え字については、最初の文字が運動方程式の軸を表し、２番目以降の文字が状態変数を示している。例えば、Ｋｕｐｒｒは、「ｕ」が航行体の船体１の進行速度ｕが示す方向の軸（ｘ軸）を表し、「ｐｒｒ（ｐｒ２）」が状態変数を示している。
【００５５】
次に、上記線形化状態方程式を逆問題解析し、干渉を打ち消すための舵角指令について解く。すなわち、任意の状態目標値に制御したいときの操作量は、上記式（９）を逆問題解析して、以下の式（１６）で演算できる。
【００５６】
【数１０】
0006380951.tif 000011
【００５７】
また、ΔＵは操作量の指令値の微小変化量、ΔＸ＊は状態量の微小変化量である。上記式（１６）に対して、深度、ロール角、ピッチ角、方位角に関するパラメータのそれぞれに、各目標値に基づく値を代入して連立方程式を作成し、この連立方程式を各舵角指令の微小変化Δδｂ、Δδｒ１、Δδｒ２、Δδｒ３、Δδｒ４について解くことにより、相互間の干渉を低減させるための干渉補償舵角指令が得られる。
例えば、方位角ψのみを変化させ、深度ｚ、ロール角φ、ピッチ角θについては現在値を維持したい場合には、方位角ψに関するパラメータには目標となる方位角ψに基づく値を代入し、方位角ψ以外のパラメータには全て０を設定する。そして、その時の連立方程式を各舵角指令の微小変化Δδｂ、Δδｒ１、Δδｒ２、Δδｒ３、Δδｒ４について解くことにより、方位角ψの変化による他のパラメータへの影響（干渉）を回避するための舵角指令を得ることができる。
【００５８】
ここで、本実施形態では、上記式（１６）に示した微小変化量を絶対値に置き換えた逆解析演算式を干渉補償演算式として用いて、干渉補償舵角を演算する。すなわち、本実施形態に係る干渉補償演算式は、以下の式（１７）で表される。
このように、微小変化量でなく絶対値に置き換えることにより、微小変化量を用いる場合に比べて構成を簡素化することができ、演算処理の負荷を軽減することができる。なお、式（１７）において、Ｕ＊は操作量の指令値、Ｘ＊は状態量の目標値、ｄＸ＊／ｄｔ（式中はＸ＊の上にドット付き）は微分状態量の目標値を示している。
【００５９】
【数１１】
0006380951.tif 000012
【００６０】
係数行列算出部１４１には、ｘ軸方向の進行速度ｕ、横流れ方向（ｙ軸方向）の横流れ速度ｖが入力される。進行速度ｕ、横流れ速度ｖは、上位装置から取得する値である。上位装置は進行速度を計測する速度センサや、横流れ速度を計測する速度センサである。または上位装置は、横流れ速度ｖを、進行速度やその他のパラメータ（航行体の深度方向速度、ロール方向角速度、ピッチ方向角速度、方位方向角速度、ロール角、ピッチ角、前記進行方向速度）を用いて、カルマンフィルタによる内部推定値として算出する装置であってもよい。また航行体制御装置１０に備わる横流れ速度推定部が、同様の処理により横流れ速度ｖを算出するようにしてもよい。
【００６１】
係数行列算出部１４１は、取得した進行速度ｕに基づいてその平衡状態の値を示すｕ￣を算出する。
係数行列算出部１４１は、取得した横流れ速度ｖに基づいてその平衡状態の値を示すｖ￣を算出する。
係数行列算出部１４１は、微分器９２ａにおいて算出された目標深度方向速度ｗ＊を取得してその値に基づいてその平衡状態の値を示すｗ￣を算出する。
係数行列算出部１４１は同様に、微分器９２ｃ目標ロール角算出部１１２において算出された目標ロール角速度ｐ＊に基づいてその平衡状態の値を示すｐ￣を算出する。
係数行列算出部１４１は同様に、目標ロール角算出部１１２において算出された目標ロール角φ＊に基づいてその平衡状態の値を示すφ￣を算出する。
係数行列算出部１４１は同様に、微分器９２ｅにおいて算出された目標ピッチ角速度ｑ＊に基づいてその平衡状態の値を示すｑ￣を算出する。
係数行列算出部１４１は同様に、目標ピッチ角算出部１１３において算出された目標ピッチ角θ＊に基づいてその平衡状態の値を示すθ￣を算出する。
係数行列算出部１４１は同様に、微分器９２９２ｇにおいて算出された目標方位角速度ｒ＊に基づいてその平衡状態の値を示すｒ￣を算出する。
【００６２】
係数行列算出部１４１は、上記式（１３）〜式（１５）に例示されるように、係数行列Ａ（上述のＡ（ｎ）行列）、Ｂ（上述のＢ（ｎ）行列）の各要素を算出するための関数（一部は、定数やゼロである）をそれぞれ保有しており、これらの関数に、上記平衡状態の値を示すｕ￣，ｖ￣，ｗ￣，ｐ￣，φ￣，ｑ￣，θ￣，ｒ￣を代入することにより、現在の平衡点における係数行列Ａ（ｎ），Ｂ（ｎ）を算出する。
【００６３】
なお、上記の線形化状態方程式における干渉項中の干渉係数（補正係数）Ｆは、以下の式（１８）により表される。
【００６４】
【数１２】
0006380951.tif 000013
【００６５】
当該干渉係数Ｆは、主に進行方向とロール角φの回転方向に作用するため、式（１８）のようにロール角φの回転方向についての係数ｆｐと、進行方向についての係数ｆｒとにより設定される。
【００６６】
他方、干渉補償舵角算出部１４２は、係数行列算出部１４１の算出した係数行列Ａ（ｎ），Ｂ（ｎ）を取得する。また干渉補償舵角算出部１４２は、微分器９２ａ，９２ｂそれぞれから目標深度方向速度ｗ＊、目標深度方向加速度ｄｗ＊／ｄｔを取得する。
また干渉補償舵角算出部１４２は、目標ロール角算出部１１２から目標ロール角φ＊を、微分器９２ｃ，９２ｄそれぞれから目標ロール角速度ｐ＊、目標ロール角加速度ｄｐ＊／ｄｔを取得する。
また干渉補償舵角算出部１４２は、目標ピッチ角算出部１１３から目標ピッチ角θ＊を、微分器９２ｅ，９２ｆそれぞれから目標ピッチ角速度ｑ＊、目標ピッチ角加速度ｄｑ＊／ｄｔを取得する。
また干渉補償舵角算出部１４２は、微分器９２ｇ，９２ｈそれぞれから目標方位角速度ｒ＊、目標方位角加速度ｄｒ＊／ｄｔを取得する。
【００６７】
干渉補償舵角算出部１４２は、干渉補償演算式（例えば、上述した式（１７））をメモリ等に記憶しており、これらの式の情報を読み込んで、当該式に取得した各値を代入して演算することで、深度干渉補償舵角δｂＦＦ＊、ロール角干渉補償舵角δｒｒＦＦ＊、ピッチ角干渉補償舵角δｒｃＦＦ＊、方位角干渉補償舵角δｒｌＦＦ＊を算出する。
【００６８】
次に、上述したように、座標変換部１４３が、干渉補償舵角算出部１４２によって得られた干渉補償舵角δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊を、干渉補償舵角（ロール角干渉補償舵角δｒｒＦＦ＊、ピッチ角干渉補償舵角δｒｃＦＦ＊、方位角干渉補償舵角δｒｌＦＦ＊）へ変換する。
【００６９】
そして、深度舵角指令算出部１５１は、深度フィードバック舵角δｂＦＢと深度干渉補償舵角δｂＦＦ＊とを加算して深度舵角指令δｂ＊の値を算出して船体運動制御装置３０へ出力する。
またロール角舵角指令算出部１５２は、ロール角フィードバック舵角δｒｒＦＢとロール角干渉補償舵角δｒｒＦＦ＊とを加算してロール角舵角指令δｒｒ＊の値を算出し、座標変換部１５５へ出力する。
またピッチ角舵角指令算出部１５３は、ピッチ角フィードバック舵角δｒｃＦＢとピッチ角干渉補償舵角δｒｃＦＦ＊とを加算してピッチ角舵角指令δｒｃ＊の値を算出し、座標変換部１５５へ出力する。
また方位角舵角指令算出部１５４は、方位角フィードバック舵角δｒｌＦＢと方位角干渉補償舵角δｒｌＦＦ＊とを加算して方位角舵角指令δｌ＊の値を算出し、座標変換部１５５へ出力する。
【００７０】
座標変換部１５５は、取得したロール角舵角指令δｒｒ＊、ピッチ角舵角指令δｒｃ＊、方位角舵角指令δｌ＊に基づいて、座標変換処理を行い、各舵３ａ〜３ｄへの舵制御指令δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊を算出し、船体運動制御装置３０へ出力する。
【００７１】
船体運動制御装置３０は、深度舵角指令δｂ＊と、舵制御指令δｒ１＊、δｒ２＊、δｒ３＊、δｒ４＊に基づいて、主に舵３ａ〜３ｅを制御する。
船体運動制御装置３０の制御の結果、深度ｚ、ロール角φ、ピッチ角θ、方位角ψが変化し、これら深度ｚ、ロール角φ、ピッチ角θ、方位角ψをセンサが計測して、それぞれの値を、差分演算部１２１、１２２、１２３、１２４へ出力する。
【００７２】
図７は航行体制御装置の進行方向制御時におけるシミュレーション結果を示す第１の図である。
図８は航行体制御装置の進行方向制御時におけるシミュレーション結果を示す第２の図である。
図７は、上述の線形化状態方程式で示した干渉項を当該線形化状態方程式に適用せずにフィードフォワード制御した場合のシミュレーション結果である。また図８は上述の線形化状態方程式を用いてフィードフォワード制御した場合のシミュレーション結果である。図８で示すシミュレーション結果によれば、図７で示すシミュレーション結果と比較して、方位角およびロール角について実測値が目標値に精度よく追従していることがわかる。
【００７３】
以上、本実施形態に係る航行体制御装置１０によれば、深度ｚ、ロール角φ、ピッチ角θ、及び方位角ψの相互干渉を抑制することが可能となる。したがって、航行体の船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。
そして、本実施形態に係る航行体制御装置１０によれば、上述した干渉項を適用した上述の線形化状態方程式を用いてフィードフォワード制御を行うことにより、さらにロール角φ、方位角ψの相互干渉を抑制することができ、航行体の船体制御の精度を向上させることができ、舵駆動系の動力消費を低減させることが可能となる。
【００７４】
なお上述の航行体制御装置１０は内部に、コンピュータシステムを有している。そして、上述した各処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって、上記処理が行われる。ここでコンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、半導体メモリ等をいう。
【００７５】
また、上記プログラムは、前述した機能の一部を実現するためのものであっても良い。さらに、前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であっても良い。
【符号の説明】
【００７６】
１・・・船体（航行体）
３ａ，３ｂ，３ｃ，３ｄ，３ｅ・・・舵
１０・・・航行体制御装置
１１・・・航行制御パラメータ取得部
１２・・・フィードバック制御部
１３・・・速度取得部
１４・・・フィードフォワード制御部
１５・・・制御補正部１５
３０・・・船体運動制御装置
１１１・・・目標深度算出部
１１２・・・目標ロール角算出部
１１３・・・目標ピッチ角算出部
１１４・・・目標方位角算出部
１２１，１２２，１２３，１２４・・・差分演算部
１２５、１２６、１２７、１２８・・・ＦＢ制御器
１５１・・・深度舵角指令算出部
１５２・・・ロール角舵角指令算出部
１５３・・・ピッチ角舵角指令算出部
１５４・・・方位角舵角指令算出部
１５５・・・座標変換部
９２ａ〜９２ｈ・・・微分器
１４１・・・係数行列算出部
１４２・・・干渉補償舵角算出部
１４３・・・座標変換部
【図１】
0006380951.tif 000014
【図２】
0006380951.tif 000015
【図３】
0006380951.tif 000016
【図４】
0006380951.tif 000017
【図５】
0006380951.tif 000018
【図６】
0006380951.tif 000019
【図７】
0006380951.tif 000020
【図８】
0006380951.tif 000021