(57)【要約】　　　（修正有）【課題】所与の制約条件内でパワーを最小にしつつ、所与の目標点に沿うよう最適な移動経路で自動操縦可能な移動体の操縦方法及び装置を提供する。【解決手段】目標点ＷＰｉ＋１を設定し、移動体５特有の非線形の条件も考慮された、前記目標点ＷＰｉ＋１に到達するための移動体の状態量及び推力に関する評価関数を最小化する推力値をリアルタイムで算出し、該算出した推力値に基づいて移動体５を移動させるものである。【選択図】図８
2004042884.tif 000002
【特許請求の範囲】
【請求項１】
目標点を設定し、移動体特有の非線形の条件も考慮された、前記目標点に到達するための移動体の状態量及び推力に関する評価関数を最小化又は最大化する推力値をリアルタイムで算出し、該算出した推力値に基づいて移動体を移動させる、移動体の操縦方法。
【請求項２】
前記評価関数を現在時刻から未来の所定時刻に渡って最小化又は最大化する推力値をリアルタイムで算出する、請求項１記載の移動体の操縦方法。
【請求項３】
目標点を設定し、前記目標点に到達するための移動体の推力に関する評価関数を最小化又は最大化する推力値をリアルタイムで算出し、該算出した推力値に基づいて移動体を移動させる移動体の操縦方法であって、
前記移動体の予定移動経路上に１以上の経由点を設定し、該設定された経由点を順次切り換えて前記目標点として設定する、移動体の操縦方法。
【請求項４】
前記評価関数を現在時刻から未来の所定時刻に渡って最小化又は最大化する推力値をリアルタイムで算出する、請求項３記載の移動体の操縦方法。
【請求項５】
前記評価関数が状態量及び前記推力と各項に対する重みとを含み、該重みを前記状態量及び推力が取り得る値を制限する拘束条件として設定し、該設定した拘束条件に制約された推力値を算出する、請求項１乃至４のいずれかに記載の移動体の操縦方法。
【請求項６】
前記予定移動経路が始点及び終点の２つの経由点のみを有しかつ該２つの経由点が同一点であるように設定する、請求項３又は４記載の移動体の操縦方法。
【請求項７】
前記予定移動経路上を前記移動体に先行するように移動する仮想点からなるガイドポイントを設定し、前記ガイドポイントが現在の目標点である経由点に到達すると、前記目標点を次の経由点に切り替える、請求項３又は４記載の移動体の操縦方法。
【請求項８】
前記評価関数が状態量及び推力と各項に対する重みとを含み、前記経由点で区画される前記予定移動経路の区間毎に前記重みを前記状態量及び推力が取り得る値を制限する拘束条件として設定し、前記経由点に対応する前記拘束条件に制約された推力値を算出する、請求項３又は４記載の移動体の操縦方法。
【請求項９】
前記重みを定数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１０】
前記重みを位置の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１１】
前記重みを前記移動体の位置及び姿勢の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１２】
前記重みを前記目標点における状態量からの偏差の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１３】
前記重みを速度の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１４】
前記重みを時間の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１５】
前記重みをイベント信号の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１６】
前記重みを障害物に関する情報の関数として設定する、請求項５又は８記載の移動体の操縦方法。
【請求項１７】
イベントを検知すると、前記目標点を現在位置以外の点と現在位置との間で切り換えて設定する、請求項１又は３記載の移動体の操縦方法。
【請求項１８】
前記評価関数が変数として状態量及び前記推力を含み、前記移動体の状態を非線形又は線形の状態観測器を用いて推定する、請求項１乃至４のいずれかに記載の移動体の操縦方法。
【請求項１９】
前記目標点及び評価関数を含む所要の事項をヒューマンインタフェースを用いて設定する、請求項１乃至４のいずれかに記載の移動体の操縦方法。
【請求項２０】
目標点を設定する手段と、移動体特有の非線形の条件も考慮された、前記目標点に到達するための移動体の状態量及び推力に関する評価関数を最小化又は最大化する推力値をリアルタイムで算出する演算手段と、該算出した推力値に基づいて移動体を移動させるアクチュエータとを備えた、移動体の操縦装置。
【請求項２１】
前記演算手段が前記評価関数を現在時刻から未来の所定時刻に渡って最小化又は最大化する推力値をリアルタイムで算出する、請求項２０記載の移動体の操縦装置。
【請求項２２】
目標点を設定する手段と、前記目標点に到達するための移動体の推力に関する評価関数を最小化又は最大化する推力値をリアルタイムで算出する演算手段と、該算出した推力値に基づいて移動体を移動させるアクチュエータとを備えた移動体の操縦装置であって、
前記移動体の予定移動経路と該予定移動経路上の１以上の経由点とを設定する手段を有し、該設定される経由点を順次切り換えて前記目標点として設定する、移動体の操縦装置。
【請求項２３】
前記演算手段が前記評価関数を現在時刻から未来の所定時刻に渡って最小化又は最大化する推力値をリアルタイムで算出する、請求項２２記載の移動体の操縦装置。
【請求項２４】
前記評価関数が状態量及び前記推力と各項に対する重みとを含み、該重みを前記状態量及び推力が取り得る値を制限する拘束条件として設定する手段を有し、前記演算手段が該設定した拘束条件に制約された推力値を算出する、請求項２０乃至２３のいずれかに記載の移動体の操縦装置。
【請求項２５】
前記予定移動経路が始点及び終点の２つの経由点のみを有しかつ該２つの経由点が同一点であるように設定された、請求項２２又は２３記載の移動体の操縦装置。
【請求項２６】
前記予定移動経路上を前記移動体に先行するように移動する仮想点からなるガイドポイントを設定する手段を有し、前記ガイドポイントが現在の目標点である経由点に到達すると、前記目標点を次の経由点に切り替える、請求項２２又は２３記載の移動体の操縦装置。
【請求項２７】
前記評価関数が状態量及び推力と各項に対する重みとを含み、前記経由点で区画される前記予定移動経路の区間毎に前記重みを前記状態量及び推力が取り得る値を制限する拘束条件として設定する手段を有し、前記演算手段が前記経由点に対応する前記拘束条件に制約された推力値を算出する、請求項２２又は２３記載の移動体の操縦装置。
【請求項２８】
前記重みが定数として設定された、請求項２４又は２５記載の移動体の操縦装置。
【請求項２９】
前記重みが位置の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３０】
前記重みが前記移動体の位置及び姿勢の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３１】
前記重みが前記目標点における状態量からの偏差の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３２】
前記重みが速度の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３３】
前記重みが時間の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３４】
前記重みがイベント信号の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３５】
前記重みが障害物に関する情報の関数として設定された、請求項２４又は２７記載の移動体の操縦装置。
【請求項３６】
イベントを検知する手段を有し、該イベントを検知すると前記目標点を現在位置以外の点と現在位置との間で切り換えて設定する、請求項２０又は２２記載の移動体の操縦装置。
【請求項３７】
前記評価関数が変数として状態量及び前記推力を含み、前記移動体の状態を推定するための非線形又は線形の状態観測器を備えた、請求項２０乃至２３のいずれかに記載の移動体の操縦装置。
【請求項３８】
前記目標点及び評価関数を含む所要の事項を設定するためのヒューマンインタフェースを備えた、請求項２０乃至２３のいずれかに記載の移動体の操縦装置。
【請求項３９】
前記移動体が船である、請求項１乃至１９のいずれかに記載の移動体の操縦方法。
【請求項４０】
前記移動体が水中航行船である、請求項１乃至１９のいずれかに記載の移動体の操縦方法。
【請求項４１】
前記移動体が航空機である、請求項１乃至１９のいずれかに記載の移動体の操縦方法。
【請求項４２】
前記移動体が船である、請求項２０乃至３８のいずれかに記載の移動体の操縦装置。
【請求項４３】
前記移動体が水中航行船である、請求項２０乃至３８のいずれかに記載の移動体の操縦装置。
【請求項４４】
前記移動体が航空機である、請求項２０乃至３８のいずれかに記載の移動体の操縦装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、移動体の操縦方法及び装置に関し、主に、移動体を予定移動経路に沿って自動的に移動させるものに関する。
【０００２】
【従来の技術及び発明が解決しようとする課題】
移動体の自動操縦方法として、例えば、自動操船方法がある。従来の自動操船方法として、例えば以下のものがある（特許文献１参照）。この自動操船方法では、計画航路上に現在位置から所定の距離だけ離れた点に目標点をおき、その目標点から航路偏差及び方位偏差を算出し、その算出した偏差から複数個の推進器（以下、アクチュエータという）が出力すべき推力を算出する。また、ルートは複数の線分と一定半径の円弧とで構成される連続な計画航路である。
【０００３】
この自動操船方法では、目標点を時々刻々算出する必要があり、また、与えられた経由点から事前に連続なルートを算出する必要がある。さらに、この自動操船方法では、目標値からの偏差をゼロにするように船体を制御するため、風や潮流などの外乱が作用しても必要なパワーを出力して偏差をゼロにするように航行する。従って、過大なパワーを消費する場合がある。
【０００４】
また、他の従来の自動操船方法として、以下のものがある（たとえば特許文献２参照）。この自動操船方法では、現航路から新航路への変更の際に、現航路上でのコースずれに関係なく一定のターンレートでもって旋回し、旋回終了時点で新航路へ航路変更する。しかし、この自動操船方法では、航路への航路キープ制御時と航路変更時とでは制御手法が異なり、航路変更時とそうでないときとで制御手法を切り換える必要がある。また、航路変更時には一定のターンレートでの旋回（フィードフォワード制御）となるため、航路からの偏差が大きくなる可能性がある。
【０００５】
さらに、移動体の位置制御方法として、以下のものがある（たとえば非特許文献１参照）。この移動体の位置制御方法では、非線形の最適フィードバック制御によってホバークラフトを位置制御しているが、予定移動経路上への拘束を考慮しながら移動させることについては開示されていない。
【０００６】
また、同じ移動体の位置制御方法として、以下のものがある（たとえば非特許文献２参照）。この移動体の位置制御方法では、障害物を避けて移動する方法として、評価関数内に移動体の位置に関するペナルティー関数を設ける手法を採用している。この手法では、予め障害物が分かっている場合には有効であるが、関数が時不変であるため、障害物が突然出現したり、障害物が移動したりする場合などには対応できない。また、ペナルティー関数は現在時刻から所定の時間Ｔだけ未来の状態のみを評価しているため、障害物の回避が必ずしも最適な移動経路で行われているとはいえない。
【０００７】
本発明は、上記のような課題を解決するためになされたもので、所与の制約条件内でパワーを最小にしつつ、所与の目標点に沿うよう最適な移動経路で自動操縦可能な移動体の操縦方法及び装置を提供することを目的としている。
【０００８】
【特許文献１】
特開平２００１−２８７６９７号公報
【特許文献２】
特開平０７−２４２１９９号公報
【非特許文献１】
第４４回自動制御連合講演会，Ｎｏ．０１−２５３，１１６〜１１９頁
【非特許文献２】
計測と制御　第３６巻　第１１号　（１９９７年１１月号）の７７６〜７８３頁
【０００９】
【課題を解決するための手段】
上記課題を解決するために、本発明に係る移動体の操縦方法及び装置は、目標点を設定し、移動体特有の非線形の条件も考慮された、前記目標点に到達するための移動体の状態量及び推力に関する評価関数を最小化又は最大化する推力値をリアルタイムで算出し、該算出した推力値に基づいて移動体を移動させる（請求項１，２０）。かかる構成とすると、従来例のように事前に移動経路を計算しておく必要がなく、実際の位置制御において、目標点までの最適なルートをリアルタイムで計算しつつ移動体を制御するので、外乱が作用しても所与の条件内でパワーを最小にしつつ最適なルートを自動的に移動することができる。
【００１０】
また、本発明に係る移動体の操縦方法及び装置は、目標点を設定し、前記目標点に到達するための移動体の推力に関する評価関数を最小化又は最大化する推力値をリアルタイムで算出し、該算出した推力値に基づいて移動体を移動させる移動体の操縦方法及び装置であって、前記移動体の予定移動経路上に１以上の経由点を設定し、該設定された経由点を順次切り換えて前記目標点として設定する（請求項３，２２）。かかる構成とすると、ルートに必要以上に拘束されない最適な移動が可能となるので、ルート上を忠実に辿る場合に比べてパワーを低減することができる。
【００１１】
また、前記評価関数を現在時刻から未来の所定時刻に渡って最小化又は最大化する推力値をリアルタイムで算出してもよい（請求項２，４，２１，２３）。かかる構成とすると、現在時刻から未来の所定時刻に渡る評価関数の変化の仕方を反映するようにその最小化又は最大化演算を行うので、最適なルートでの移動を必要最小限の演算時間で実現できる。
【００１２】
また、前記評価関数が状態量及び前記推力と各項に対する重みとを含み、該重みを前記状態量及び推力が取り得る値を制限する拘束条件として設定し、該設定した拘束条件に制約された推力値を算出してもよい（請求項５，２４）。かかる構成とすると、重みを調整することにより、移動体の所望の応答及び性能を最小限の推力で得ることができる。
【００１３】
また、前記予定移動経路が始点及び終点の２つの経由点のみを有しかつ該２つの経由点が同一点であるように設定してもよい（請求項６，２５）。かかる構成とすると、同じアルゴリズムでルートトラッキングと定点保持が可能となるので、制御方法を簡素化することができる。
【００１４】
また、前記予定移動経路上を前記移動体に先行するように移動する仮想点からなるガイドポイントを設定し、前記ガイドポイントが現在の目標点である経由点に到達すると、前記目標点を次の経由点に切り替えてもよい（請求項７，２６）。かかる構成とすると、移動体及びアクチュエータの動特性に起因する遅れを補償することができる。
【００１５】
また、前記評価関数が状態量及び推力と各項に対する重みとを含み、前記経由点で区画される前記予定移動経路の区間毎に前記重みを前記状態量及び推力が取り得る値を制限する拘束条件として設定し、前記経由点に対応する前記拘束条件に制約された推力値を算出してもよい（請求項８，２７）。かかる構成とすると、重みを調整することにより、移動体の所望の応答及び性能を最小限の推力で得ることができる。
【００１６】
また、前記重みを定数として設定してもよい（請求項９，２８）。
【００１７】
また、前記重みを位置の関数として設定してもよい（請求項１０，２９）。
【００１８】
また、前記重みを前記移動体の位置及び姿勢の関数として設定してもよい（請求項１１，３０）。
【００１９】
また、前記重みを前記目標点における状態量からの偏差の関数として設定してもよい（請求項１２，３１）。
【００２０】
また、前記重みを速度の関数として設定してもよい（請求項１３，３２）。かかる構成とすると、重みを適宜設定することにより、例えば、移動体の最大速度を設定することができる。
【００２１】
また、前記重みを時間の関数として設定してもよい（請求項１４，３３）。かかる構成とすると、例えば、移動体の速度に対する重みを時間に対し適宜変化するよう設定することにより、移動体を自動的に停止又は発進させることができる。
【００２２】
また、前記重みをイベント信号の関数として設定してもよい（請求項１５，３４）。かかる構成とすると、例えば、移動体の速度に対する重みをイベント信号に対し適宜変化するよう設定することにより、イベント信号に応じて移動体の速度を変化させることができ、イベント信号による停止又は発進も可能である。
【００２３】
また、前記重みを障害物に関する情報の関数として設定してもよい（請求項１６，３５）。かかる構成とすると、ルートからの偏差に対する重みを、障害物に関する情報に応じて適宜変化するよう設定することにより、移動体に障害物を自動的に回避させることができる。
【００２４】
また、イベントを検知すると前記目標点を現在位置以外の点と現在位置との間で切り換えて設定するようにしてもよい（請求項１７，３６）。かかる構成とすると、操作ボタンを押す等してイベントを発生させることにより、移動体を停止又は発進させることができる。
【００２５】
また、前記評価関数が変数として状態量及び前記推力を含み、前記移動体の状態を非線形又は線形の状態観測器を用いて推定してもよい（請求項１８，３７）。かかる構成とすると、直接検出することができない状態量を推定して、適切に最適化制御を遂行することが可能になる。
【００２６】
また、前記目標点及び評価関数を含む所要の事項をヒューマンインタフェースを用いて設定してもよい（請求項１９，３８）。かかる構成とすると、所要の事項を簡単に設定することができる。
【００２７】
また、前記移動体が船であってもよい（請求項３９，４２）。
【００２８】
また、前記移動体が水中航行船であってもよい（請求項４０，４３）。
【００２９】
また、前記移動体が航空機であってもよい（請求項４１，４４）。
【００３０】
【発明の実施の形態】
以下、本発明の実施の形態を図面を参照しながら説明する。
実施の形態１
図１は本発明の実施の形態１に係る移動体の操縦装置の制御系統の構成を示すブロック図、図２は図１の制御装置の構成を示すブロック図、図３は個別アクチュエータの配置を示す模式図である。
【００３１】
図１において、本実施の形態では移動体の操縦装置として操船装置１を例示している。従って、本実施の形態では移動体は船体（船）５である。操船装置１は、ルート情報入力装置２、制御装置３、アクチュエータ４、位置計測装置５、及び方位角計測装置７を備えている。
【００３２】
位置計測装置６は、例えばＧＰＳ（ｇｌｏｂａｌ　ｐｏｓｉｔｉｏｎｉｎｇ　ｓｙｓｔｅｍ）で構成され、船体５の位置を計測してこれを位置情報として出力する。方位角計測装置７は、例えばジャイロコンパスで構成され、船体５の方位を計測してこれを方位角情報として出力する。ルート情報入力装置２は、コンピュータに接続されたディスプレイ、マウス、キーボード等からなるＨＭＩ（ｈｕｍａｎ　ｉｎｔｅｒｆａｃｅ）で構成され、船の予定移動経路（予定航路：以下、ルートという）、ルートへの拘束度合い等のルート情報を制御装置３に入力してそれを設定するためのものである。制御装置３は、コンピュータで構成され、ルート情報入力装置から入力されるルート情報と、位置計測装置６及び方位角計測装置７からそれぞれ入力される位置情報及び方位角情報とに基づき、船体５に対して非線形の最適化フィードバック制御を行う。具体的には、その制御のための動作指令を出力する。アクチュエータ４は、制御装置３から出力される動作指令に従って推力を出力して船体５を移動（航行）させる。アクチュエータ４は、本実施の形態では、図３に示すように、船体５にそれぞれ配設された個別アクチュエータ、すなわち、１つのスラスタ２１と、２つのプロペラ２２，２３と、２つの舵２４，２５とで構成され、これにより、船体５を任意の二次元方向へ移動させかつ船体５を任意に旋回させることが可能になっている。
【００３３】
次に、制御装置３について詳しく説明する。図２において、制御装置３では、所定のソフトウエアによってルート最適化演算部８、推力配分部９、及びオブザーバ１０が実現されている。ルート最適化演算部８は、後述する評価関数を最小化するアルゴリズムを用いてルート最適化の演算を行い推力指令を出力する。推力配分部９は、ルート最適化演算部８から出力された推力指令を図３に示す個別アクチュエータ２１〜２５に最適に配分し、これを動作指令として出力する。オブザーバ１０は、ルート最適化演算に用いる評価関数の状態量をルート最適化演算部８に入力する。オブザーバ１０は、その基本的構成が周知であるのでここでは簡単に説明する。オブザーバ１０では、船体５及びアクチュエータ４の数式モデルに推力配分部９から出力される動作指令が制御入力として入力され、前記数式モデルの出力が船体５の位置情報及び方位角情報（位置計測装置６及び方位角計測装置７の出力）と比較され、それらの差分が前記数式モデルの所定箇所に入力される。そして、前記数式モデルの適宜な箇所から状態量（ｘ，ｙ，Ψ，ｕ，ｖ，ｒ）が取り出され、これが船体５の状態量の推定値としてルート最適化演算部８に入力される。
【００３４】
なお、本実施の形態においては、ルート最適化演算部８と推力配分部９とが分離されているが、ルート最適化演算部８において出力する推力指令が各アクチュエータの推力指令になるように構成してもよい。
【００３５】
次に、ルート最適化演算部８で遂行されるルート最適化演算について詳しく説明する。図４はルートの設定方法を示す模式図、図５は状態量の定義を示すグラフ、図６は評価関数の重みの設定例を示す模式図であって、（ａ）はルートの状態を示す図、（ｂ）は重みの変化を示す図、図７は拘束レベルの定義を例示する表である。
【００３６】
まず、ルートの定義について説明する。
【００３７】
図４に示すように、本実施の形態では、仮想の平面上に複数の経由点（以下、ウエイポイントという）ＷＰを想定し、その複数のウエイポイントＷＰ間を直線で結んだものをルート１０１として定義する。そして、このようにルート１０１上に複数想定されたウエイポイントＷＰを順次切り換えて目標点としながら最適化制御を行う。
【００３８】
具体的には、このルート１０１をグローバル座標系で定義する一方、ルート１０１のウエイポイントＷＰで区分された区間（以下、単に区間という）１０２毎にローカル座標系ｘｉ−ｙｉを定義する。このローカル座標系ｘｉ−ｙｉは任意に定義できるが、ここでは、ウエイポイントＷＰｉを原点とし、ｘｉ軸がウエイポイントＷＰｉから前方に延びる区間１０２ｉに一致するように定義される。そして、船体５が次のウエイポイントＷＰに到達するまではそのウエイポイントＷＰに対応するローカル座標系で制御を行う。つまり、船体５の位置及び方位角をウエイポイントＷＰ毎にそれに対応するローカル座標系に変換する。そして、船体５が次のウエイポイントＷＰｉ＋１に到達するまでは、評価関数の終端条件を、現在のウエイポイントＷＰｉに対応するローカル座標系ｘｉ−ｙｉ上の次のウエイポイントＷＰｉ＋１における状態とし、区間１０２毎に評価関数の重みを定義する。そして、船体５が次のウエイポイントＷＰｉ＋１に到達した時点で座標系をそのウエイポイントＷＰｉ＋１に対応するローカル座標系ｘｉ＋１−ｙｉ＋１に切り換えるとともに評価関数の重みをその区間１０２ｉ＋１に切り換える。
【００３９】
ところで、以上の説明では、理解し易くするために、船体５がウエイポイントＷＰへ到達すると座標系及び評価関数の重みを切り換える（すなわち目標点を切り換える）と述べたが、船体５はルート１０１に案内されるように移動するのであってルート１０１上を忠実に移動する訳ではない。そのため、船体５は必ずしもウエイポイントＷＰを通過しないので、船体５のウエイポイントＷＰ到達に代えて、目標点を切り換える目安を定義する必要がある。そこで、本実施の形態では、ルート１０１上を船体５に対応して移動するガイドポイントＧＰを想定し、このガイドポイントＧＰがウエイポイントＷＰに到達したときに目標点を切り換えることとしている。
【００４０】
ガイドポイントＧＰは、船体５の中心のローカル座標系のｘｉ軸上の射影点Ｐｍｘｉより距離Ｄだけ前方に位置し、該射影点Ｐｍｘと同じ速度で移動するように設定される。この距離Ｄは、例えば、航行速度、ルート１０１からの横ずれ量、方位角のずれ量、目標点とするウエイポイントＷＰまでの距離、設定された評価関数の重みの値等の関数とされるが、これに限定されるものではない。
【００４１】
このようにガイドポイントＧＰを船体５に先行するように設定すると、船体５及びアクチュエータ４の動特性に起因する遅れを補償することができる。
【００４２】
次に、ルート最適化演算のアルゴリズムを説明する。
【００４３】
このアルゴリズムでは、以下の（１）式のような船体の状態方程式に対し、各時刻ｔにおいて［ｔ，ｔ＋Ｔ］に渡る（時刻ｔからＴだけ未来まで最適化する）以下の（２）式のような評価関数を最小にする最適値問題を解き、その最適制御の初期値ｕ（ｔ）のみを実際の制御入力として与える。
【００４４】
【数１】
2004042884.tif 000003
【００４５】
ここで、ｘ（ｔ）：状態ベクトル、ｕ（ｔ）：制御入力ベクトル、ｐ（ｔ）：既知の時変パラメータである。
【００４６】
【数２】
2004042884.tif 000004
【００４７】
ここで、ｘｒｅｆ：目標状態（目標点とするＷＰにおける位置及び状態）、Ｓ：終端状態に対する重み、Ｑ：状態量に対する重み、Ｒ：制御入力に対する重みである。
【００４８】
つまり、この評価関数において、Φ、Ｌに適当な関数を選び、Ｓ、Ｑ、Ｒに適当な値を設定し、評価関数を最小にするｕ（ｔ）を求め、これを船体に作用させる推力の指令値とし、アクチュエータの動作指令値を算出する。そして、この演算をリアルタイムで遂行する。この評価関数において、第１項は現在時刻から所定時刻だけ未来の状態を評価する項であり、第２項は状態量及び制御入力（推力）を現在時刻から未来の所定時刻に渡って評価する項である。
【００４９】
そして、この評価関数の重みＳ、Ｑ、Ｒは可変であり、進入禁止区域の情報、目標点とするウエイポイントＷＰまでの距離、ユーザの指定などにより大きさを逐次変更できる。
【００５０】
本実施の形態では、（２）式の評価関数を、好ましい具体例として、以下の（３）式のように定義した。
【００５１】
【数３】
2004042884.tif 000005
【００５２】
ここで、ｘ１＝ｘ，ｘ２＝ｕ，ｘ３＝ｙ，ｘ４＝ｖ，ｘ５＝Ψ，ｘ６＝ｒ，ｘ７＝Ｘ，ｘ８＝Ｙ，ｘ９＝Ｎ
ｕ１＝Ｘｒ，ｕ２＝Ｙｒ，ｕ３＝Ｎｒ
であり、下記の運動方程式に従う。
【００５３】
【数４】
2004042884.tif 000006
【００５４】
ここで、（１０）〜（１２）式は図２の推力配分部９及びアクチュエータ４の動特性の近似式である。ここでは、この動特性を一次遅れ系で近似しているが、これには限定されず、任意の方程式を採用することができる。
【００５５】
また、図４及び図５に示すように、（３）〜（１２）式において各パラメータは以下の内容を表している。
【００５６】
ｘ：船体の中心のｘ座標、ｙ：船体の中心のｙ座標、Ψ：船体の方位角、ｕ：船体の前後方向（長手方向）の速度、ｖ：船体の左右方向（幅方向）の速度、ｒ：船体の角速度、Ｘ：船体の前後方向の推力、Ｙ：船体の左右方向の推力、Ｎ：船体の中心周りのモーメント、Ｘｒ：船体の前後方向の推力の指令値、Ｙｒ：船体の左右方向の推力の指令値、Ｎｒ：船体の中心周りのモーメントの指令値、Ｍ：船の質量、ｍｘ：前後方向の付加質量、ｍｙ左右方向の付加質量、Ｉｚｚ：慣性モーメント、Ｊｚｚ付加慣性モーメント、ｘＧ：船体の中心から重心までの距離、ＸＨ：船体の前後方向の流体力、ＹＨ：船体の左右方向の流体力、ＮＨ：船体の中心周りの流体力、Ｋｘ，Ｋｙ，Ｋｎ：入力に対する出力の振幅比を表す係数、Ｔｘ，Ｔｙ，Ｔｎ：一次遅れ系の時定数を表す係数。
【００５７】
前述したように、（３）式の評価関数の重みＳｆｉ，ｑｉ，ｒｉは可変であり、進入禁止区域の情報、目標点とするウエイポイントＷＰまでの距離、ルートへの拘束度合いなどにより大きさを変更する。
【００５８】
例えば、図６（ａ）に示すように、進入禁止区域などの船体が進入して欲しくない領域２０１がルート１０１に沿って存在する場合には、図６（ｂ）に示すように、ｙに対する重みｑ３を領域２０１に対応するように設定する。すなわち、ローカル座標のｙｉ軸方向において、重みｑ３が、領域２０１が存在しない範囲では一定値Ｖ０を取り、領域２０１が存在する範囲では値が急激に増加するように設定する。
【００５９】
この場合、偏差（目標点における状態量と現在地における状態量との差）の状態を考慮して、ルート１０１を基準とした場合における重み一定の領域Ａ１と重み増加の領域Ａ２とを設定する。また、重みは離散値を取ることができないので、連続になるように設定する。重み一定の領域Ａ１は、目標点とするウエイポイントＷＰまでの距離などに応じて変更する。重み一定の領域Ａ１の長さにより、ルート１０１への拘束度合いを調整する。
【００６０】
この拘束度合いは連続的に変化するように設定できるが、本実施の形態では、例えば、図７に示すように、重み一定の領域Ａ１の長さに比例して拘束レベルを”Ｎｏｎ”，”Ｌｏｏｓｅ”，”Ｍｉｄｄｌｅ”，”Ｔｉｇｈｔ”，”Ｓｔｒｉｃｔ”の５つのレベルに区分している。
【００６１】
重みの下限値Ｖ０は、目標点とするウエイポイントＷＰまでの距離などに応じて変更する。重み増加の領域Ａ２は、船体の右舷側及び左舷側の双方に設定することができる。ルートに対する拘束度合いは船体の方位角や速度などの他の状態量に対してもその重みを変化させることにより、船体の位置に対する場合と同様に設定することができる。
【００６２】
次に、以上のよう構成された操船装置１の動作（操船方法）を説明する。図８は船体の制御状態を示す図であって、（ａ）は本実施の形態を示す図、（ｂ）は従来例を示す図である。
【００６３】
図１〜図４において、まず、ルート情報入力装置２からウエイポイントＷＰ、目標点における状態量、ガイドポイントの先行距離Ｄ、各区間１０２における重み（ルートへの拘束度合いを含む）等を制御装置３に入力してこれらを設定する。すると、ルート最適化演算部８は（３）式の評価関数を最小にするような船体への２方向の力（推力指令Ｘｒ，Ｙｒ）及びモーメント（推力指令Ｎｒ）をリアルタイムで算出し、これを推力配分部９がアクチュエータ４の個別アクチュエータ２１〜２５に配分し、この配分した推力指令値を動作指令としてアクチュエータ４に出力する。すると、個別アクチュエータ２１〜２５が動作指令に基づいて推力を出力し、その出力された推力に応じて船体５が移動する。この間、船体５の位置及び方位角を位置計測装置６及び方位角計測装置７がそれぞれ計測し、オブザーバ１０がこの計測した位置及び方位角と推力配分部９から出力される動作指令に基づいて現在の状態量をルート最適演算部８に入力する。その結果、図８（ａ）に示すように、船体５は現在位置から目標点となるウエイポイントＷＰまで、最適なルートを辿って移動するように制御される。そして、図４に示すガイドポイントＧＰが目標点とするウエイポイントＷＰに到達すると、ルート最適化演算部８が目標点を次のウエイポイントＷＰに切り換えて船体５を同様に制御する。この際、区間１０２毎に設定された評価関数の重みに応じて、船体５のルート１０１への拘束度合いが変化する。
【００６４】
この本実施の形態による制御を従来例と比較すると、従来例では、図８（ｂ）に示すように、目標値からの偏差をゼロにするように船体を制御するため、風や潮流などの外乱が作用しても必要なパワーを出力して偏差をゼロにするように船が航行し、従って、過大なパワーを消費する場合があるのに対し、本実施の形態では、目標点までの最適なルートをリアルタイムで計算しつつ船体を制御するので、設定された拘束度合いの範囲内であれば、外乱が作用しても所与の条件内でパワーを最小にしつつ最適なルートを航行することができる。
【００６５】
次に、船体５のルート１０１への拘束度合いをシミュレーションによる実施例に従って説明する。
（実施例１）
図９は実施例１に係るシミュレーション結果を示す図である。図９において、横軸はｘ座標を縦軸はｙ座標を示す。太線は、それぞれ、ウエイポイントＷＰ０，ＷＰ１における船体５の目標位置及び目標方位角を示す。船体５は、ウエイポイントＷＰ０からウエイポイントＷＰ１に移動しながら（位置を変化させながら）その方位角をウエイポイントＷＰ０におけ方位角からウエイポイントＷＰ１における方位角に近付けるように変化させている。
【００６６】
また、一般に、（１）式や（３）式のように各項に重みを与えた評価関数を用いて最適化制御する場合には、重みを大きくすると、それに応じて評価関数が大きくなるため、船体の状態が重みの増加する領域には近づかなくなる。本実施例では、船体５の位置及び方位角に対して重みが増加するよう設定されているが、それらの拘束レベルが”Ｌｏｏｓｅ”（小：図７参照）に設定されており、その重みによるルートへの拘束度合いが小さい。従って、船体５はルート１０１から比較的離れた経路を辿ってウエイポイントＷＰ０からウエイポイントＷＰ１まで移動している。
（実施例２）
図１０は実施例２に係るシミュレーション結果を示す図である。図１０において、横軸はｘ座標を縦軸はｙ座標を示す。本実施例では、船体の位置に対する拘束レベルが”Ｓｔｒｉｃｔ”（超大：図７参照）、船体の方位角に対する拘束レベルが”Ｌｏｏｓｅ”（小）に設定されている。従って、船体５はほぼルート１０１上を通ってウエイポイントＷＰ０からウエイポイントＷＰ１まで移動している。
（実施例３）
図１１は実施例３に係るシミュレーション結果を示す図である。図１１において、横軸はｘ座標を縦軸はｙ座標を示す。本実施例では、船体の位置に対する拘束レベルが”Ｍｉｄｄｌｅ”（中：図７参照）、船体の方位角に対する拘束レベルが”Ｌｏｏｓｅ”（小）に設定されている。従って、船体５はルート１０１からやや離れた経路を辿ってウエイポイントＷＰ０からウエイポイントＷＰ１まで移動している。
（実施例４）
図１２は実施例４に係るシミュレーション結果を示す図である。図１２において、横軸はｘ座標を縦軸はｙ座標を示す。本実施例では、船体の位置及び方位角に対する拘束レベルが共に”Ｌｏｏｓｅ”（小）から”Ｓｔｒｉｃｔ”（超大）に変化するように設定されている。従って、船体５はウエイポイントＷＰ０からウエイポイントＷＰ１に至る途中でほぼルート１０１上に位置するとともにその方位角がウエイポイントＷＰ１における目標方位角に近い方位角にまで変化し、そこからウエイポイントＷＰ１までは、ほぼルート１０１上を通りかつわずかに方位角を変化させるようにして移動している。
【００６７】
以上の実施例１〜４から明らかなように、本実施の形態による制御では、船体５の位置及び方位角の重みを適宜設定することにより、船体５を所望の経路及び姿勢でウエイポイントＷＰ間を移動させることができる。なお、これらの重みはオペレータの感覚的判断により容易に設定できる。
【００６８】
次に、自動着桟を、シミュレーションによる実施例に従って説明する。
（実施例５）
図１３は実施例５に係るシミュレーションにおける船のルートを示す模式図、図１４は実施例５に係るシミュレーション結果を示す図である。図１４において、横軸はｘ座標を縦軸はｙ座標を示す。
【００６９】
図１３に示すように、本実施例に係るシミュレーションでは、中央に開口部３０２ａを有するように港の防波堤３０２が位置し、その開口部３０２ａから見て右奥に桟橋３０１が位置するように設定されている。そして、防波堤３０２の開口部３０２ａのほぼ中央部に第１のウエイポイントＷＰ０が設定され、その開口部３０２ａの正面でかつ桟橋３０１の左方に位置するように第２のウエイポイントＷＰ１が設定され、そこから桟橋３０１の先端部を迂回するように第３〜第５のウエイポイントＷＰ２〜ＷＰ４が設定され、第５のウエイポイントＷＰ４から直進して桟橋３０１に接岸する位置に第６のウエイポイントＷＰ５が設定されている。船体５の方位角（姿勢）は、第１〜第３のウエイポイントＷＰ０〜ＷＰ２では入港時の方位角であり、第４〜第６のウエイポイントＷＰ３〜ＷＰ５では入港時と逆の方位角であるように設定されている。
【００７０】
そして、本実施例では、船体５の位置に対する拘束レベルが全ての区間１０２において”Ｌｏｏｓｅ”（小）に設定され、船体５の方位角に対する拘束レベルが、第５のウエイポイントＷＰ４と第６のウエイポイントＷＰ５との間の区間１０２において”Ｌｏｏｓｅ”に、その他の区間１０２において”Ｎｏｎ”（無：図７参照）に設定されている。その結果、船体５は第３のウエイポイントＷＰ２と第４のウエイポイントＷＰ３との間においてルート１０１から離れて桟橋３０１に接近し、また、接岸の際に船体５が大きく旋回して桟橋３０１に衝突した。従って、好ましくない結果が得られた。
（実施例６）
図１５は実施例６に係るシミュレーション結果を示す図である。図１５において、横軸はｘ座標を縦軸はｙ座標を示す。本実施例では、船体５の位置及び方位角に対する拘束レベルが共に全ての区間１０２において”Ｌｏｏｓｅ”（小）に設定されている。その結果、船体５が桟橋３０１に衝突することは解消されたが、船体５が第３のウエイポイントＷＰ２と第４のウエイポイントＷＰ３との間を大きく旋回しながら移動するため、船首が防波堤３０２に接近し過ぎて危険な結果となった。
（実施例７）
図１６は実施例７に係るシミュレーション結果を示す図である。図１６において、横軸はｘ座標を縦軸はｙ座標を示す。本実施例では、船体５の位置に対する拘束レベルは全ての区間１０２において”Ｓｔｒｉｃｔ”（超大）に設定されている。そして、船体５の方位角に対する拘束レベルは、第１のウエイポイントＷＰ０と第２のウエイポイントＷＰ１との間の区間１０２において”Ｌｏｏｓｅ”（小）に設定され、第２のウエイポイントＷＰ１と第５のウエイポイントＷＰ４との間の各区間１０２において”Ｌｏｏｓｅ”（小）から”Ｓｔｒｉｃｔ”（超大）に変化するように設定され、第５のウエイポイントＷＰ４と第６のウエイポイントＷＰ５との間の区間１０２において”Ｓｔｒｉｃｔ”（超大）に設定されている。その結果、船体５が第３のウエイポイントＷＰ２と第４のウエイポイントＷＰ３との間との間においてほぼルート１０１上を移動しかつ船体５が第４のウエイポイントＷＰ３の近傍で旋回するため、船首が防波堤３０２に接近する危険性が解消され、良好な結果が得られた。
【００７１】
以上の実施例５〜７から明らかなように、本実施の形態による制御では、ルートを適宜設定し、船体５の位置及び方位角の重みを適宜選択することにより、船体５を自動的に適切に着桟させることができる。
実施の形態２
本発明の実施の形態２は好ましいＨＭＩを例示したものである。
【００７２】
図１７は本実施の形態に係るＨＭＩによるルート設定画面を示す模式図であって、（ａ）は進入禁止区域の設定を示す図、（ｂ）はルートを途中まで設定した状態を示す図、（ｃ）はルートの設定が完了した状態を示す図である。
【００７３】
本実施の形態に係るＨＭＩは、実施の形態１で述べたように、制御装置３（コンピュータ：図１参照）に接続されたディスプレイ等の表示装置と、マウスやキーボード等の入力装置によって実現される。また、一般的に、表示装置の画面に所定の画面を表示しながら入力装置により所定の情報を入力して所望の事項をコンピュータに設定することは周知であるので、以下に述べるルート及びルートへの拘束度合いの設定は設定画面のみを示し、そのソフトウエアの説明は省略する。
【００７４】
図１７（ａ）に示すように、本実施の形態では、まず、表示装置にルート設定画面を表示し、その設定画面上でマウス等を操作して、障害物３０１，３０２等の進入禁止区域を手動で設定する。なお、ＧＰＳ情報やＭＡＰ（地図）の情報を制御装置３に入力して自動的に進入禁止区域を設定するようにしてもよい。
【００７５】
次に、図１７（ｂ）及び図１７（ｃ）に示すように、マウス操作によりウエイポイントＷＰ０〜ＷＰ５を指定する。すると、ウエイポイントＷＰ０〜ＷＰ５の位置に基づいてその位置情報が制御装置３に転送され記憶される（設定される）。そして、各ウエイポイントＷＰ０〜ＷＰ５が順次直線で結ばれ、それが画面上に表示される。これにより、ルートが設定される。
【００７６】
次に、ルートへの拘束度合いの設定を説明する。図１８はルートへの拘束度合いの設定を示す模式図である。
【００７７】
図１８に示すように、ルート設定が完了した画面４０１（図１７（ｃ）に示す画面）において、所望の区間１０２をクリックすると、その区間におけるルートへの拘束度合いの設定する画面４０２が表示される。この画面４０２では、船体の位置に対する拘束度合いを設定するスライドバー（（４０３ａ，４０３ｂ），（４０４ａ，４０４ｂ））が区間１０２の始点側及び終点側のウエイポイントＷＰｉ，ＷＰｉ＋１の側方に表示され、船体の方位角に対する拘束度合いを設定する回転バー（（４０５，４０６），（４０７ａ，４０７ｂ），（４０８ａ，４０８ｂ））が区間１０２の始点側及び終点側のウエイポイントＷＰｉ，ＷＰｉ＋１上に表示され、さらに、船体の速度制限（速度の最大値）を設定するスライドバー４１１が画面の適宜な箇所に表示される。始点側及び終点側のスライドバー４０３ａ，４０３ｂを所望の位置にそれぞれ位置させると、双方のスライドバー４０３ａ，４０３ｂの位置を結ぶように直線４０３ｃが表示される。このスライドバー４０３ａ，４０３ｂ及び直線４０３ｃの位置が、左舷方向におけるルート１０１からの偏差（ｙ方向の偏差）に対し重みが増加し始める位置を表している。従って、これにより、船体の左舷側におけるｙ方向位置に対する拘束度合いが設定される。また、始点側及び終点側のスライドバー４０４ａ，４０４ｂを所望の位置にそれぞれ位置させると、双方のスライドバー４０４ａ，４０４ｂの位置を結ぶように直線４０４ｃが表示される。このスライドバー４０４ａ，４０４ｂ及び直線４０４ｃの位置が、船体の右舷方向におけるルート１０１からの偏差（ｙ方向の偏差）に対し重みが増加し始める位置を表している。従って、これにより、船体の右舷側におけるｙ方向位置に対する拘束度合いが設定される。
【００７８】
また、始点側及び終点側の回転バー４０５及び４０６の回転角は、それぞれ、始点側及び終点側のウエイポイントにおける方位角の目標値を表している。従って、これらの回転バー４０５及び４０６を所望の回転角となるよう位置させることにより、始点側及び終点側のウエイポイントにおける方位角の目標値が設定される。そして、始点側及び終点側の回転バー４０７ａ及び４０７ｂの回転角は、それぞれ、始点側及び終点側のウエイポイントにおいて船体の左舷方向における方位角の目標値からの偏差に対し重みが増加し始める方位角を表している。従って、これらの回転バー４０７ａ及び４０７ｂを所望の回転角となるよう位置させることにより、始点側及び終点側のウエイポイントにおける船体の左舷側への方位角の変化に対する拘束度合いが設定される。また、始点側及び終点側の回転バー４０８ａ及び４０８ｂの回転角は、それぞれ、始点側及び終点側のウエイポイントにおいて船体の右舷方向における方位角の目標値からの偏差に対し重みが増加し始める方位角を表している。従って、これらの回転バー４０８ａ及び４０８ｂを所望の回転角となるよう位置させることにより、始点側及び終点側のウエイポイントにおける船体の右舷側への方位角変化に対する拘束度合いが設定される。なお、本実施の形態では、図示しないが、所定の操作により、後述するように任意の区間において始点側から終点側に向けて船体の方位角に対する拘束度合いが変化するように設定することができる。
【００７９】
また、スライドバー４１１の位置は、船体の速度に対し重みが増加し始める速度を表している。従って、スライドバー４１１を所望の位置に位置させることにより、船体の速度の最大値が設定される。
【００８０】
そして、このように設定された各種拘束度合いの情報が制御装置３（図１参照）に転送され記憶される。なお、上記各バー４０３〜４０９，４１１は、例えばマウスで操作される。
【００８１】
図１９は位置及び方位角に対する重みの設定例を示す模式図であって、（ａ）〜（ｃ）は位置に対する重みの設定例を示す図、（ｄ）は重み一定の領域の長さを目標点とするウエイポイントまでの距離の関数として設定する例を示す図、（ｅ）〜（ｇ）は方位角に対する重みの設定例を示す図である。
【００８２】
本実施の形態では、前述の始点側及び終点側におけるｙ方向位置に対する重みが、それぞれ、図１９（ｂ），図１９（ｃ）に示すように設定される。この始点側及び終点側における重み一定領域の長さＷｒｓ，Ｗｌｓ，Ｗｒｅ，Ｗｌｅが、それぞれ、図１８のスライドバー４０４ａ，４０３ａ，４０４ｂ，４０３ｂの位置に対応している。また、右舷側及び左舷側の重み一定領域の長さＷｒ，Ｗｌが、図１９（ｄ）に示すように、目標点とするウエイポイントＷＰｉ＋１までの距離（換言すればｘ方向位置）に対して変化するように設定される。右舷側及び左舷側の重み一定領域の長さＷｒ，Ｗｌのｘ方向位置に対する変化を示す直線が、それぞれ、図１８の直線４０４ｃ，４０３ｃに対応している。また、本実施の形態では、図１９（ａ）に示すように、ｘ方向位置に対する重みを設定することもできる。このｘ方向位置に対する重みは一定値とすることもできる。また、変化させる場合は、例えば、右舷側又は左舷側のｙ方向位置に対する拘束度合いの関数になるように設定される。
【００８３】
また、本実施の形態では、前述の始点側及び終点側における方位角に対する重みが、それぞれ、図１９（ｅ），図１９（ｆ）に示すように設定される。この始点側及び終点側における重み一定領域の長さＲｒｓ，Ｒｌｓ，Ｒｒｅ，Ｒｌｅが、それぞれ、図１８の回転バー４０８ａ，４０７ａ，４０８ｂ，４０７ｂの方位角の目標値からの偏差に対応している。また、右舷側及び左舷側への旋回における重み一定領域の長さＲｒ，Ｒｌが、図１９（ｇ）に示すように、目標点とするウエイポイントＷＰｉ＋１までの距離（換言すればｘ方向位置）に対して変化するように設定される。
【００８４】
図２０は任意の状態量に対する重みの変化を示す曲線の作成方法を示す図である。この曲線は、図２０に示すように、まず、重みを示す複数の点をプロットし、次いでそれらの点を近似する曲線で結ぶことにより作成される。
【００８５】
図２１（ａ）〜図２１（ｏ）にこのようにして作成した状態量に対する重みの変化曲線の例を示す。
実施の形態３
本発明の実施の形態３では、実施の形態１の図４において、ルート１０１が始点及び終点の２つのウエイポイントのみを有しかつこの２つのウエイポイントが同一点であるように設定される。これにより、船体５が定点に位置するように制御される。しかも、その制御をルート１０１に沿って航行する場合と同じアルゴリズム（手順）で設定することができるので、制御装置を簡素化することができる。
実施の形態４
本発明の実施の形態４では、実施の形態１の構成において、図４に示すガイドポイントＧＰを目標点として設定する。この場合、ローカル座標系及びルートへの拘束度合いがリアルタイムで更新される。このような構成とすると、ウエイポイントの設定を省略することができる。
実施の形態５
図２２は本発明の実施の形態５に係る操船装置の動作及び構成を示す図であって、（ａ）は障害物の回避動作を示す図、（ｂ）はｙ方向位置に対する重みの変化を示す図である。
【００８６】
本実施の形態では、操船装置１（図１参照）が、障害物検出装置を備えている。障害物検出装置はレーダ、超音波音波探知機等で構成されている。図２２（ａ）において、符号５０２はこの障害物検出装置の障害物検知範囲を示している。障害物検出装置が船体５の近辺に障害物５０１を検出すると、図２２（ｂ）に示すように、制御装置３（図１参照）がｙ方向位置に対する重みを、障害物５０１が存在する領域の手前で増加するように設定する。すなわち、ｙ方向位置に対する重みが障害物に関する情報に基づいてリアルタイムで更新される。これにより、図２２（ａ）に示すように、船体５が障害物５０１を回避するように移動する。その結果、船５が、ルート１０１の近辺に存在する障害物５０１を自動的に回避して安全に航行することができる。
実施の形態６
本発明の実施の形態６では、評価関数の重みが時間の関数として設定される。例えば、船体の速度に対する重みを一定時間経過後に変化するよう設定すると、その時間経過後に船体の速度が変化する。従って、重みを適宜設定することにより、船を自動的に停止又は発進させることができる。
実施の形態７
本発明の実施の形態７ではイベントを検知すると船体の速度が変化するように構成される。
【００８７】
第１の構成例として、評価関数の重みがイベント信号の関数として設定される。例えば、実施の形態１の図２において、操作ボタンを押すとセンサがこれを検知して所定の信号（イベント信号）が出力されるよう制御装置３が構成され、この所定の信号がルート最適化演算部８に入力されると、船体の速度に対する重みが変化するよう設定される。これにより、操作ボタンを押すと船体の速度が変化し、停止又は発進も可能である。
【００８８】
第２の構成例として、前記イベント信号がルート最適化演算部８に入力されると、目標点が現在位置以外の点（例えば現在の目標点（次のウエイポイントＷＰ））と現在位置との間で切り換えて設定される。これにより、航行中に操作ボタンを押すと船体が停止し、停止中に操作ボタンを押すと船体が発進する。
【００８９】
この場合、厳密に言えば、目標点を現在位置に切り換えると、船はその動特性によってその切り替え後の目標点を行き過ぎてしまい、戻るように制御される。従って、その分、停止のタイミングが遅れることになる。そこで、船の動特性を考慮して、現時点で船体に進行方向と逆方向の所定推力を作用させた場合に船が停止する位置を予測し、その予測位置に目標点を切り換えることがより好ましい。このように構成すると、より短時間で船を停止させることができる。この場合、船を発進させるには、前記予測位置以外の点に目標点を切り換えればよい。
実施の形態８
本発明の実施の形態８は、本発明を船以外の移動体の操縦に適用するものである。実施の形態１の（１）〜（１２）式から明らかなように、（３）式の評価関数を用いた最適化制御は、船に限らず一般の移動体に適用することができる。特に、移動体が抵抗や摩擦等の力及びモーメントを受けながら二次元空間を移動する場合には、（３）〜（１２）式をそのまま適用することができる。また、移動体が流体抵抗を受けながら三次元空間を移動する場合には、（３）式の評価関数において、ｚ方向位置、ｘ−ｚ平面内の方位角、及びｙ−ｚ平面内の方位角に関する状態量及び推力（制御入力）を追加し、（４）〜（９）式の運動方程式において、ｚ軸に関する項を追加することにより、これらの式を三次元空間を移動する移動体の制御に適用することができる。
【００９０】
このように構成することにより、外乱が作用しても所与の条件内でパワーを最小にしつつルートに沿うよう移動体を自動操縦することができる。
［実施例８］
本実施例は、本発明を航空機の操縦へ適用する例を示す。
【００９１】
図２３は本実施例に係る航空機の操縦装置の制御系統の構成を示すブロック図、図２４は本実施例における航空機のモデル及び状態量の定義を示す図である。図２３において図１と同一符号は同一又は相当する部分を示す。
【００９２】
図２３に示すように、本実施例の航空機の操縦装置６０１は、制御対象は機体６０２であり、かつ図１の操船装置１の方位角計測装置７に代えてオイラー角計測装置６０３を備えている。その他の点は操船装置１と同様である。オイラー角計測装置６０３は例えばジャイロコンパスで構成され、機体のオイラー角を計測してこれをオイラー角情報として出力する。
【００９３】
図２４に示すように、本実施例の機体の制御において採用されている航空機モデルでは、以下の１８の状態量が定義されている。
【００９４】
すなわち、（ｘ，ｙ，ｚ）：地球固定座標系における機体重心の位置、（ｕ，ｖ，ｗ）：機体の速度ベクトルの機体固定座標軸成分、（ｐ，ｑ，ｒ）：機体の角速度ベクトルの機体固定軸成分、（Θ，Φ，Ψ）：オイラー角（地面固定座標系に対する機体軸の向きを定義する角）、（Ｘ，Ｙ，Ｚ）：並進運動の推進力、（Ｌ，Ｍ，Ｎ）：回転運動の推進力モーメント。
【００９５】
このように、本実施例の状態量は、実施の形態１（図５）に示す状態量と比べると、ｚ軸に関する状態量とオイラー角（Θ，Φ，Ψ）とが追加されている。
【００９６】
この航空機モデルの運動方程式は下記のようになる。
【００９７】
【数５】
2004042884.tif 000007
【００９８】
ここで、Ｘａ，Ｙａ，Ｚａ，Ｌａ，Ｍａ，Ｎａは、状態量ｕ，ｖ，ｗ，ｐ，ｑ，ｒ，Θ，Φにより決まる空気力及び空気力モーメントである。また、Ｘｒ，Ｙｒ，Ｚｒ，Ｌｒ，Ｍｒ，Ｎｒは、入力である。
【００９９】
また、本実施例の非線形最適制御における評価関数は、以下の式（３１）のように設定されている。
【０１００】
【数６】
2004042884.tif 000008
【０１０１】
ここで、ｘｒｅｆは目標状態、すなわち目標点とするＷＰにおける位置及び状態である。
【０１０２】
図２５は本実施例の機体制御における航空機の飛行ルート設定例を示す模式図である。
【０１０３】
図２５に示すように、ここでは、互い直交する東西軸、南北軸、及び高度軸からなる３次元座標軸で表される３次元空間（地上空間）中に１以上のウエイポイントＷＰが設定され、この設定されたウエイポイントを直線で結ぶようにして飛行ルート６０４が設定される。ここでは、航空機の着陸における誘導ルートが示されている。
【０１０４】
図２６は本実施例による機体制御のシミュレーション結果を示す図である。ここでは、図２５の誘導ルート６０４を飛行する場合におけるシミュレーション結果が示されている。図２６に示すように、本実施例の機体制御によれば、航空機６０２を最適な移動経路６０５で自動操縦することができる。
［実施例９］
本実施例は、本発明を水中航行船の操縦へ適用する例を示す。
【０１０５】
図２７は本実施例における水中航行船のモデルにおける状態量の定義を示す表である。なお、図２７中の状態量［Ｃ］についての、船体姿勢角のオイラー角表示と方向余弦マトリクス表示との関係は下式で示される。
【０１０６】
【数７】
2004042884.tif 000009
【０１０７】
また、本実施例では、制御対象としての水中航行船は潜水艇である。
【０１０８】
この水中航行船モデルの運動方程式は下記のようになる。
【０１０９】
【数８】
2004042884.tif 000010
【０１１０】
本実施例における状態量は、記号が実施例８と異なっているが、実質的には実施例８と同様である。［ＦＷＡ］，［ＦＴＨ］，［ＦＰ］が実施例８のＸ，Ｙ，Ｚ，Ｌ，Ｍ，Ｎに相当する。
【０１１１】
また、本実施例の非線形最適制御における評価関数は実施例８と同様に設定されている。
【０１１２】
次に、本実施例による船体制御のシミュレーション結果を従来のＰＩＤによる船体制御と比較して説明する。
【０１１３】
図２８及び図２９は従来のＰＩＤによる船体制御のシミュレーション結果を示す図であって、図２８（ａ）は航路軌跡を示す図、図２８（ｂ）は横舵角、縦舵角、及び潜舵角（以下、舵角と総称する）の変化を示す図、図２９（ａ）はロール角、ピッチ角、及びヨー角（以下、姿勢角と総称する）の変化を示す図、図２９（ｂ）は船速及びヨーレートの変化を示す図である。
【０１１４】
また、図３０及び図３１は本実施例による船体制御のシミュレーション結果を示す図であって、図３０（ａ）は航路軌跡を示す図、図３０（ｂ）は舵角の変化を示す図、図３１（ａ）は姿勢角の変化を示す図、図３１（ｂ）は船速及びヨーレートの変化を示す図である。
【０１１５】
このシミュレーション結果によれば、図２８（ａ）から明らかなように、従来のＰＩＤによる船体制御では、船体の急激な旋回が不可能であるのに対し、図３０（ａ）から明らかなように、本実施例による船体制御では、船体の急激な旋回が可能となっている。
【０１１６】
【発明の効果】
本発明は、以上に説明したような形態で実施され、容易な経路および条件設定で、所与の制約条件内でパワーを最小にしつつ、移動体を最適な移動経路で自動操縦することができるという効果を奏する。また、例えば外乱が作用しても、必要以上に予定移動経路に固着することなく、所与の条件内でパワーを最小にしつつ移動可能である。
【図面の簡単な説明】
【図１】図１は本発明の実施の形態１に係る移動体の操縦装置の制御系統の構成を示すブロック図である。
【図２】図１の制御装置の構成を示すブロック図である。
【図３】個別アクチュエータの配置を示す模式図である。
【図４】ルートの設定方法を示す模式図である。
【図５】状態量の定義を示すグラフである。
【図６】評価関数の重みの設定例を示す模式図であって、（ａ）はルートの状態を示す図、（ｂ）は重みの変化を示す図である。
【図７】拘束レベルの定義を例示する表である。
【図８】船体の制御状態を示す図であって、（ａ）は本実施の形態を示す図、（ｂ）は従来例を示す図である。
【図９】実施例１に係るシミュレーション結果を示す図である。
【図１０】実施例２に係るシミュレーション結果を示す図である。
【図１１】実施例３に係るシミュレーション結果を示す図である。
【図１２】実施例４に係るシミュレーション結果を示す図である。
【図１３】実施例５に係るシミュレーションにおける船のルートを示す模式図である。
【図１４】実施例５に係るシミュレーション結果を示す図である。
【図１５】実施例６に係るシミュレーション結果を示す図である。
【図１６】実施例７に係るシミュレーション結果を示す図である。
【図１７】本発明の実施の形態２に係るＨＭＩによるルート設定画面を示す模式図であって、（ａ）は進入禁止区域の設定を示す図、（ｂ）はルートを途中まで設定した状態を示す図、（ｃ）はルートの設定が完了した状態を示す図である。
【図１８】ルートへの拘束度合いの設定を示す模式図である。
【図１９】位置及び方位角に対する重みの設定例を示す模式図であって、（ａ）〜（ｃ）は位置に対する重みの設定例を示す図、（ｄ）は重み一定の領域の長さを目標点とするウエイポイントまでの距離の関数として設定する例を示す図、（ｅ）〜（ｇ）は方位角に対する重みの設定例を示す図である。
【図２０】任意の状態量に対する重みの変化を示す曲線の作成方法を示す図である。
【図２１】状態量に対する重みの変化を示す曲線の例を示す図である。
【図２２】本発明の実施の形態５に係る操船装置の動作及び構成を示す図であって、（ａ）は障害物の回避動作を示す図、（ｂ）はｙ方向位置に対する重みの変化を示す図である。
【図２３】実施例８に係る航空機の操縦装置の制御系統の構成を示すブロック図である。
【図２４】実施例８における航空機のモデル及び状態量の定義を示す図である。
【図２５】実施例８の機体制御における航空機の飛行ルート設定例を示す模式図である。
【図２６】実施例８による機体制御のシミュレーション結果を示す図である。
【図２７】実施例９における水中航行船のモデルにおける状態量の定義を示す表である。
【図２８】従来のＰＩＤによる船体制御のシミュレーション結果を示す図であって、（ａ）は航路軌跡を示す図、（ｂ）は舵角の変化を示す図である。
【図２９】従来のＰＩＤによる船体制御のシミュレーション結果を示す図であって、（ａ）は姿勢角の変化を示す図、（ｂ）は船速及びヨーレートの変化を示す図である。
【図３０】実施例９による船体制御のシミュレーション結果を示す図であって、（ａ）は航路軌跡を示す図、（ｂ）は舵角の変化を示す図である。
【図３１】実施例９による船体制御のシミュレーション結果を示す図であって、（ａ）は姿勢角の変化を示す図、（ｂ）は船速及びヨーレートの変化を示す図である。
【符号の説明】
１　操船装置
２　ルート情報入力装置
３　制御装置
４　アクチュエータ
５　船体
６　位置計測装置
７　方位角計測装置
８　ルート最適化演算部
９　推力配分部
１０　オブザーバ
２１　スラスタ
２２，２３　　プロペラ
２４，２５　　舵
１０１　　ルート
１０２　　区間
２０１　　進入禁止区域
３０１　　桟橋
３０２　　防波堤
３０２ａ　開口部
４０１　　ルート設定画面
４０２　　ルートへの拘束度合い設定画面
４０３ａ，４０３ｂ，４０４ａ，４０４ｂ　　スライドバー
４０３ｃ，４０４ｃ　　直線
４０５，４０６，４０７ａ，４０７ｂ，４０８ａ，４０８ｂ　　回転バー
４１１　　スライドバー
５０１　　障害物
５０２　　障害物検知範囲
６０１　　操縦装置
６０２　　機体
６０３　　オイラー角計測装置
６０４　　飛行ルート
６０５　　移動経路
Ａ１　　重み一定の領域
Ａ２　　重み増加の領域
Ｄ　先行距離
ＧＰ　ガイドポイント
Ｖ０　　重み下限値
Ｗｒｓ，Ｗｌｓ，Ｗｒｅ，Ｗｌｅ，Ｒｒｓ，Ｒｌｓ，Ｒｒｅ，Ｒｌｅ，Ｗｒ，Ｗｌ，Ｒｒ，Ｒｌ　　重み一定領域の　　　　　　　　　　　　　　　　　　　　　　　　　　　　　　長さ
ＷＰ　ウエイポイント
【図１】
2004042884.tif 000011
【図２】
2004042884.tif 000012
【図３】
2004042884.tif 000013
【図４】
2004042884.tif 000014
【図５】
2004042884.tif 000015
【図６】
2004042884.tif 000016
【図７】
2004042884.tif 000017
【図８】
2004042884.tif 000018
【図９】
2004042884.tif 000019
【図１０】
2004042884.tif 000020
【図１１】
2004042884.tif 000021
【図１２】
2004042884.tif 000022
【図１３】
2004042884.tif 000023
【図１４】
2004042884.tif 000024
【図１５】
2004042884.tif 000025
【図１６】
2004042884.tif 000026
【図１７】
2004042884.tif 000027
【図１８】
2004042884.tif 000028
【図１９】
2004042884.tif 000029
【図２０】
2004042884.tif 000030
【図２１】
2004042884.tif 000031
【図２２】
2004042884.tif 000032
【図２３】
2004042884.tif 000033
【図２４】
2004042884.tif 000034
【図２５】
2004042884.tif 000035
【図２６】
2004042884.tif 000036
【図２７】
2004042884.tif 000037
【図２８】
2004042884.tif 000038
【図２９】
2004042884.tif 000039
【図３０】
2004042884.tif 000040
【図３１】
2004042884.tif 000041