JP2004355105A - 移動体の制御方法及び制御装置 - Google Patents

Abstract

【課題】最適な計画航路を事前に設定する必要がなく、また、所定の制約条件内でエネルギーの消費を抑制した上で、移動体を予定移動経路（ルート）に沿って移動させるように制御可能な移動体の制御方法及び制御装置の提供。
【解決手段】船体１が航行するフィールド上に複数のウェイポイントＷＰを設定し、各ウェイポイントＷＰを線分で結ぶことにより定義されたルート１００に沿って、所定の速度及び方位角でターゲットポイントＰ_Ｔを移動させる。一方で、船体１の状態量の目標値を現時刻から設定された時間（設定時間）Ｔ後の終端時間のみで設定する終端状態量拘束条件と、該船体１の推力指令値を含む評価関数と、該船体１について取得した現時刻での状態量とに基づき、最適化演算を実行して最適な推力指令値を算出し、時間Ｔの遅れで前記ターゲットポイントＰ_Ｔを追従するように船体１を移動させる。
【選択図】 図５

Description

【０００１】
【発明の属する技術分野】
本発明は、移動体の制御方法及び制御装置に関する。特に、移動体を予定移動経路に沿って自動的に移動させる制御方法及び制御装置に関する。
【０００２】
【従来の技術】
移動体を自動操縦させるべく制御する方法として、ＧＰＳ等の高精度の測位システムを用いて船体位置を制御するＤＰＳ（Ｄｙｎａｍｉｃ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）が知られている。該ＤＰＳを用いた操船方法は、海洋観測船による海洋調査、作業船を用いたケーブル敷設作業等、船体位置を高い精度で制御する必要がある海洋作業に広く使用されている。
【０００３】
このような操船方法を実現するものとして、既に様々の提案がなされている（特許文献１参照）。該特許文献１に示された自動操船方法の場合、船舶が通過すべき所定の通過点を考慮し、事前に連続的な計画航路が構成される。そして、現在の船体位置から所定距離だけ離隔した前記計画経路上を移動する目標点を設定し、この目標点と現在位置とから航路偏差及び方位偏差を算出する。算出した偏差に基づき、船体に備えられた複数個のアクチュエータの操作量を算出し、この操作量に基づいて各アクチュエータを駆動させることにより操船する。
【０００４】
また、例えば予め定められた複数の通過点を線分のみを用いて結ぶことにより前記計画航路を構成すると、ある通過点（変針点）を通過した直後に算出される前記方位偏差が急激に変化する場合がある。この場合、各アクチュエータを駆動させる操作量も急激に変化するが、アクチュエータにかかる負荷、消費エネルギー、搭乗者が存在する場合の乗り心地等の観点からは操作量の急激な変化は好ましくない。
【０００５】
そこで、前記特許文献１では前記計画航路を、所定半径の円弧を介して線分を繋げていくことによって構成している。従って、移動体が変針点を通過する場合であっても、該変針点近傍での方位偏差の急激な変化が緩和され、舵、スラスタ、及び推進用プロペラ等のアクチュエータの操作量も急激に変化することがなく、比較的円滑に航行できるようになっている。
【０００６】
【特許文献１】
特開平２００１−２８７６９７号公報
【０００７】
【発明が解決しようとする課題】
ところで、前記特許文献１に開示された方法によれば、通過点を通る連続的な航路を事前に設定しておく必要があるが、この作業は容易ではない。例えば、航路上の変針点を含む区間は、船舶の旋回性能及び消費エネルギー等を考慮して適切な半径の円弧を用いて構成する必要がある。しかし、船舶の適切な旋回半径は航行速度により異なり、航行速度が大きい場合は旋回半径が大きく、航行速度が小さい場合は旋回半径が小さい。そして、事前に設定された航路の旋回半径が、船舶の航行速度に対する適切な旋回半径に比べて小さい場合、各アクチュエータの操作量が比較的急激に変化することとなり、消費エネルギーも過大になる。このように、事前に最適な計画航路を設定することは困難である。また、計画航路が線分から円弧、又は円弧から線分へと切り替わる箇所では、目標点の位置及び方位が急変し、これに伴ってアクチュエータの操作量も少なからず急変するため、特許文献１に係る自動操船方法では必ずしも円滑な航行を十分には実現することができない。
【０００８】
そこで、本発明は、最適な計画航路を事前に設定する必要がなく、また、移動体の運動特性を考慮し、所定の制約条件内でエネルギーの消費を抑制した上で、移動体を予定移動経路に沿って移動させるように制御可能な制御方法及び制御装置を提供することを目的とする。
【０００９】
【課題を解決するための手段】
本発明は上記のような事情に鑑みてなされたものであり、本発明に係る移動体の制御方法は、移動体の予定移動経路と、該予定移動経路上を移動し得る仮想点から成るターゲットポイントの動作条件とを設定し、前記ターゲットポイントを前記動作条件に基づいて動作させ、前記移動体を、現時刻から設定時間だけ未来に、前記動作条件に基づいて動作する前記ターゲットポイントの前記現時刻での位置に到達させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出し、該推力に基づいて前記ターゲットポイントを追従するように前記移動体を移動させる（請求項１）。
【００１０】
前記制御方法を実現するために、本発明に係る移動体の制御装置は、移動体の予定移動経路を設定する手段と、該予定移動経路上を移動し得る仮想点から成るターゲットポイントの動作条件を設定する手段と、設定時間を設定する手段と、前記移動体を、現時刻から前記設定時間だけ未来に、前記動作条件に基づいて動作する前記ターゲットポイントの前記現時刻での位置に到達させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出する手段とを備える（請求項７）。
【００１１】
このような制御方法又は制御装置によれば、移動体の運動性能及び消費エネルギー等を考慮した厳密な計画航路を事前に設定せずとも、曲線状の滑らかな経路で移動体を移動させることができる。
【００１２】
概説すると、前記制御方法又は制御装置は、現時刻でのターゲットポイントの位置に、設定時間経過後の移動体が到達し得るように、前記現時刻において移動体のトラッキング動作を制御するものである。例えば、ターゲットポイントが方向を変えて移動し、その移動方向が船体の移動方向と異なった場合、前記設定時間をかけた後に前記ターゲットポイントの移動方向に一致するように船体の動作を制御する。
【００１３】
これにより、ターゲットポイントの動作条件（例えば、進行方向、進行速度等）が急激に変化した場合であっても、現時刻において移動体が備えるアクチュエータの操作量は、前記ターゲットポイントに生じる変化の割合に比べて小さい割合で変化する。従って、移動体の移動速度に応じて適切な旋回半径を算出し、該半径を有する円弧を用いて厳密に計画航路を設定する必要がなく、一の予定移動経路に基づいて様々の移動速度に対しても滑らかな経路で移動体を移動させることができる。
【００１４】
また、前記制御方法において、前記移動体に、前記現時刻から前記設定時間だけ未来に、該現時刻での前記ターゲットポイントの動作条件を獲得させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出してもよい（請求項２）。該制御方法を実現するために、前記制御装置において、前記推力を算出する手段は、前記移動体に、前記現時刻から前記設定時間だけ未来に、該現時刻での前記ターゲットポイントの動作条件を獲得させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出すべく成してあってもよい（請求項８）。
【００１５】
このような制御方法又は制御装置によれば、現時刻でのターゲットポイントの位置に移動体を到達させるだけでなく、前記ターゲットポイントの前記現時刻での動作条件（例えば、進行方向，移動速度）をも移動体に獲得させるべく、現時刻において移動体を移動させることができる。この場合、例えばターゲットポイントに設定された現時刻における進行方向又は移動速度が急激に変化した場合であっても、前記設定時間をかけてその変化に追従するように移動体は移動する。従って、移動体の操作量が急激に変化することがない。
【００１６】
また、前記制御方法において、複数の目標点を設定して該目標点を線分で結ぶことにより、前記予定移動経路を設定してもよい（請求項３）。該制御方法を実現するために、前記制御装置において、前記予定移動経路を設定する手段は、複数の目標点を設定し、該目標点を線分で結んで前記予定移動経路を設定すべく成してあってもよい（請求項９）。
【００１７】
このような制御方法又は制御装置によれば、予定移動経路の設定が比較的容易に行える。また、線分のみで構成した予定移動経路上の変針点においても、上述したように移動体のトラッキング経路を滑らかにすることができる。
【００１８】
また、前記制御方法において、前記推力値の算出においては、未定数としての推力値を含む前記移動体の操作量に関する評価関数を用い、現時刻から前記設定時間だけ未来に至る期間についての前記評価関数が、最小値又は最大値になるという条件を満たす推力を算出してもよい（請求項４）。該制御方法を実現するために、前記制御装置において、前記推力値を算出する手段は、未定数としての推力値を含む前記移動体の操作量に関する評価関数を用い、現時刻から前記設定時間だけ未来に至る期間についての前記評価関数が、最小値又は最大値になるという条件を満たす推力値を算出すべく成してあってもよい（請求項１０）。
【００１９】
このような制御方法又は制御装置によれば、現時刻から設定時間だけ未来に至る期間、評価関数の最小値又は最大値を定義する推力値を算出することができる。従って、例えば未知数として推力値のみを含む評価関数を用いた場合には、前記所定時間中の推力値を最小とする推力値を算出することができ、消費エネルギーを抑制した移動が可能である。なお、前記評価関数の最小値／最大値は、該評価関数に付される符号（正／負）に対応するものである。
【００２０】
また、前記制御方法において、前記設定時間を変更してもよく（請求項５）、そのため、前記制御装置において、前記設定時間を変更する手段を備えていてもよい（請求項１１）。前記設定時間を比較的長時間とした場合、ターゲットポイントの動作条件の変化が移動体に備えられたアクチュエータの操作量の変化に及ぼす影響の度合いは低くなり、例えば、移動時におけるスラスタ変動を抑制することができる。他方、前記設定時間を比較的短時間とした場合、ターゲットポイントの動作条件の変化が前記アクチュエータの操作量の変化に及ぼす影響の度合いは高くなり、例えば、予定移動経路に対して厳密に移動体をトラッキングさせることができる。このように、設定する設定時間を変更するだけで、移動体の移動特性等を容易に変更することができる。
【００２１】
また、前記制御方法において、前記ターゲットポイントの位置を固定して前記移動体の定点保持を行ってもよく（請求項６）、そのため、前記制御装置において、前記ターゲットポイントの位置を固定する手段を備えていてもよい（請求項１２）。ターゲットポイントの位置を固定することにより、移動体は該ターゲットポイントへ向かって移動体の運動特性を考慮しながら徐々に速度を落として近づいていき、最終的には該ターゲットポイントにて停止させることができる。
【００２２】
また、ターゲットポイントの位置を固定すると共に、設定する設定時間を適宜変更することにより、定点保持する際の移動体の減速時の加速度を変えることができる。例えば、比較的長時間に設定した場合には、減速時の加速度の絶対値が小さくなり、長時間かけて保持位置へ到達して停止し、比較的短時間に設定した場合には、短時間で保持位置へ到達して停止する。
【００２３】
【発明の実施の形態】
以下、本発明の実施の形態について、図面を参照しながら説明する。図１は、本発明の実施の形態に係る移動体の制御装置を含む制御系の構成を示すブロック図である。本実施の形態では、図１に示すように移動体として船体１を例示しており、該船体１は操船装置２によってその動作が制御される。該操船装置２は、位置計測装置３、方位角計測装置４、ルート情報入力装置５、本発明に係る制御装置６、及び推進機構７を備えている。なお、移動体としては前記船体１に限られず、航空機、ホバークラフト、磁気浮上体その他の移動体にも本発明を適用することが可能である。
【００２４】
前記位置計測装置３は、例えばＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を用いて構成され、船体１の位置を計測して得られる位置情報を出力する。方位角計測装置４は、例えばジャイロコンパスを用いて構成され、船体１の船首が向く方位を計測して得られる方位角情報を出力する。
【００２５】
ルート情報入力装置５は、コンピュータに接続されたディスプレイ、マウス、キーボード等から成るＨＭＩ（Ｈｕｍａｎ Ｍａｃｈｉｎｅ Ｉｎｔｅｒｆａｃｅ）で構成されている。該ルート情報入力装置５は、船体１の予定移動経路（以下、「ルート」という）に関する情報その他の情報のオペレータによる外部入力を受け付け、また、入力された情報又は内部に記憶された情報を制御装置６へ出力する。
【００２６】
制御装置６は、コンピュータで構成され、ＣＰＵ６１，入出力インタフェース６２，並びに、コンピュータプログラムその他各種情報を格納すべくＲＡＭ，ＲＯＭ，ＨＤＤ（ハードディスクドライブ）等から成る記憶装置６３等を備える。該制御装置６では、前記ルート情報入力装置５から入力される情報（以下、「ルート情報」という）と、前記位置計測装置３及び方位角計測装置４から夫々入力される位置情報及び方位角情報と、その他前記記憶装置６３に格納された所定のコンピュータプログラム等とに基づいて前記ＣＰＵ６１が演算処理を行う。その結果、前記制御装置６は、推進機構７へ出力する動作指令を算出し、船体１の非線形の最適化フィードバック制御を実行する。推進機構７は複数のアクチュエータ（図３参照）を備えており、該アクチュエータは前記制御装置６から入力された動作指令に従って駆動する。
【００２７】
図２は、制御装置６の機能ブロック図であり、図１に示す入出力インタフェース６２から入力された情報及び記憶装置６３に格納された情報に基づき、ＣＰＵ６１が演算処理を行うことによって該制御装置６が実現可能な機能について示している。図示するように該制御装置６は、実現可能な機能としてルート最適化演算部８、推力配分部９、及びオブザーバ１０を有している。
【００２８】
ルート最適化演算部８は、ルート情報入力装置５から入力されたルート情報と、後述する船体１の状態量とに基づき、後述する評価関数を最小化するようにルート最適化の演算を行い、船体１に与えるべき推力を示す推力指令を出力する。推力配分部９は、前記ルート最適化演算部８から入力された推力指令に基づき、推進機構７が備える複数のアクチュエータへの推力配分を決定し、該アクチュエータへ個別に動作指令を出力する。オブザーバ１０は、位置情報及び方位角情報等に基づいて、後述するようにルート最適化演算に用いる船体１の状態量をルート最適化演算部８へ出力する。
【００２９】
該オブザーバ１０は、その基本構成及び機能として周知のものを用いることができ、ここではその動作について簡単に説明しておく。該オブザーバ１０は、一方で、船体１の運動及び推進機構７の動作に関して予め設定された数式モデルに、推力配分部９から出力される動作指令を制御入力として入力し、その演算結果を取得する。他方で、位置計測装置３から出力された位置情報と、方位角計測装置４から出力された方位角情報との入力を受け付ける。そして、前記演算結果と、位置情報及び方位角情報とを比較し、両者の差分を再び前記数式モデルへ入力することにより、後述するように船体１の推定値としての状態量（Ｘ，Ｙ，Ψ，ｕ，ｖ，ｒ）を取得する。取得した状態量は、前述したようにルート最適化演算部８へ出力する。
【００３０】
図３は、推進機構７として船体１に設けられたアクチュエータの配置例を示す模式図である。図３に示すように該推進機構７はアクチュエータとして、１つのスラスタ２１と、２つのプロペラ２２，２３と、２つの舵２４，２５とを備えている。船体１は、これらのアクチュエータ２１〜２５が駆動することにより、任意の二次元方向への移動、旋回等が可能になっている。なお、推進機構７は、異なる種類及び数のアクチュエータを用いて異なる配置構成によって実現されていてもよい。
【００３１】
次に、上述したような構成を成す操船装置２を用いて船体１をルートに従って移動させるにあたっての設定事項について説明する。該設定事項は、図１に示すルート情報入力装置５を用いて行うことができ、設定された事項は制御装置６にて記憶される。
【００３２】
［設定事項について］
図４は、本実施の形態において定義する船体１の状態量を説明するための模式図であり、図示されている各状態量は下記のように定義されている。
Ｘ：船体１に設けられた制御位置のｘ座標
Ｙ：船体１に設けられた制御位置のｙ座標
Ψ：ｘ軸と船体１の前後方向との成す方位角（右舷回頭方向を正）
ｕ：ｘ_ｓ軸方向の速度
ｖ：ｙ_ｓ軸方向の速度
ｒ：回頭角速度（右舷回頭方向を正）
なお、図４に示すｘ−ｙ座標系は地球固定絶対座標系である。また、ｘ_ｓ−ｙ_ｓ座標系は船体固定座標系であり、船首方向をｘ_ｓ軸の正、該ｘ_ｓ軸に直交する右舷方向をｙ_ｓ軸の正としている。
【００３３】
図５は、前記船体１のルートとターゲットポイントを説明するための模式図である。図５に示すように、船体１を航行させるフィールド上（例えば、海上）に複数の目標点（以下、「ウェイポイント」という）ＷＰ（ＷＰ_ｋ−１，ＷＰ_ｋ，ＷＰ_ｋ＋１，ＷＰ_ｋ＋２）を設定する。本実施の形態では、地球固定絶対座標系であるｘ−ｙ座標系をフィールド上に設け、各ウェイポイントＷＰの位置を該ｘ−ｙ座標系で設定する。そして、これらのウェイポイントＷＰを線分で結ぶことにより経路を形成し、該経路をルート１００として定義している。従って、ウェイポイントＷＰを複数設定することにより、ルート１００は必然的に設定される。
【００３４】
該ルート１００上には、船体１に先行して前記ルート１００上を移動する仮想点であるターゲットポイントＰ_Ｔを設定する。例えば図５に示すように、ｘ−ｙ座標系を用いて表したウェイポイントＷＰ_ｋ−１，ＷＰ_ｋの位置を（Ｘ_ｋ−１，Ｙ_ｋ−１），（Ｘ_ｋ，Ｙ_ｋ）とした場合、該ウェイポイントＷＰ_ｋ−１，ＷＰ_ｋ間のルート１００上を移動する前記ターゲットポイントＰ_Ｔの位置（Ｘ_ｆ，Ｙ_ｆ）は、下記（１）式及び（２）式によって表される。
【００３５】
【数１】

なお、Ｖ_ＳはターゲットポイントＰ_Ｔの移動速度、Ｔ_Ｓは制御装置６のサンプリング周期、αはウェイポイントＷＰ_ｋ−１，ＷＰ_ｋを結ぶ線分とｘ軸との成す角であり、該角αは、該ウェイポイントＷＰ_ｋ−１，ＷＰ_ｋの座標を用いて、
α＝ａｔａｎ２（Ｙ_ｋ−Ｙ_ｋ−１，Ｘ_ｋ−Ｘ_ｋ−１）
と表される。
【００３６】
線分で結ばれたウェイポイントＷＰ間の各区間（図５参照）には、前記ターゲットポイントＰ_Ｔの動作条件に関する種々の条件を関連付けることが可能であり、これらの条件は、夫々の区間の属性として制御装置６が備える記憶装置６３に格納される。
【００３７】
図６は、各区間の属性の一例を示す図表である。図６に示すように、ウェイポイントＷＰ_ｋ−１，ＷＰ_ｋの間の区間ＬＥＧ_ｋ、ウェイポイントＷＰ_ｋ，ＷＰ_ｋ＋１の間の区間ＬＥＧ_ｋ＋１、ウェイポイントＷＰ_ｋ＋１，ＷＰ_ｋ＋２の間の区間ＬＥＧ_ｋ＋２等の各区間には、該各区間の属性として、ターゲットポイントＰ_Ｔの移動速度Ｖ_Ｓｉ（ｉ＝…，ｋ，ｋ＋１，ｋ＋２，…）、該ターゲットポイントＰ_Ｔの移動方向を示す方位角Ψ_ｆｉ（ｉ＝…，ｋ，ｋ＋１，ｋ＋２…）等が関連付けられている。
【００３８】
本実施の形態では、仮想点である前記ターゲットポイントＰ_Ｔを、現時刻に位置する区間に対して図６に示すように関連付けられた該区間の属性に従ってルート１００上を移動させる。そして、後述するように船体１を制御し、該船体１を前記ターゲットポイントＰ_Ｔに追従させるようにして、ルート１００に沿って移動（ルートトラッキング）させる。以下、ターゲットポイントＰ_Ｔの動作制御と、該ターゲットポイントＰ_Ｔに追従するように船体１をルートトラッキングさせるべく実行する最適化演算について説明する。
【００３９】
［ターゲットポイントの動作について］
図７は、船体１のルートトラッキング時におけるターゲットポイントＰ_Ｔの動作制御の流れを示すフローチャートであり、図８は、該フローチャートに示されるターゲットポイントＰ_Ｔの動作を例示する模式図である。なお、図７のフローチャートは、複数のウェイポイントを経由するルートに沿って船体１をトラッキングさせ、最終のウェイポイントにて定点保持を行う場合のターゲットポイントＰ_Ｔの動作制御の流れを示しており、制御装置６のサンプリング周期Ｔ_Ｓ毎に実行される。
【００４０】
初めに、オペレータがルート情報入力装置５を操作することにより、ウェイポイントＷＰ_ｋに関する情報、各区間ＬＥＧ_ｉ（図５参照）に関連付けされる属性、及び後述する時間Ｔの初期値が予め記憶装置６３に記憶される（Ｓ１）。
【００４１】
前記ウェイポイントＷＰ_ｋに関する情報としては、フィールド上に設けるウェイポイントＷＰ_ｋの数（ｋＭＡＸ）、複数設けられた各ウェイポイントＷＰ_ｋの座標、及び各ウェイポイントＷＰ_ｋに対して経由順に割り当てる番号ｋ（ｋ＝０，１，…，ｋＭＡＸ、但し、ＷＰ_０は制御開始時における船体位置に対応する）を含む。各区間ＬＥＧ_ｉの属性としては、移動速度Ｖ_Ｓｉ（ｉ＝１，…，ｋＭＡＸ）、及び方位角Ψ_ｆｉ（ｉ＝１，…，ｋＭＡＸ）を含み、該属性は各区間ＬＥＧ_ｉにおけるターゲットポイントＰ_Ｔの動作条件に相当する。
【００４２】
また、前記時間（設定時間）Ｔは、概説すればターゲットポイントＰ_Ｔに追従するように移動する船体１の該ターゲットポイントＰ_Ｔに対する遅れ時間に相当するものである。即ち、船体１は、現時刻でのターゲットポイントＰ_Ｔの位置に、前記時間Ｔだけ経過した未来に到達し得るように動作が制御され、且つ、現時刻でのターゲットポイントＰ_Ｔの動作条件（存在する区間の属性）を、前記時間Ｔだけ経過した未来に獲得し得るように動作が制御される。
【００４３】
次に、船体１のトラッキング制御にあたり、制御装置６のルート最適化演算部８にてｋ＝１が設定され（Ｓ２）、ターゲットポイントＰ_Ｔの位置，移動速度，及び方位角が、夫々ウェイポイントＷＰ_０の座標（Ｘ_０，Ｙ_０），Ｖ_Ｓ１，Ψ_ｆ１に設定され、該ターゲットポイントＰ_Ｔは、座標（Ｘ_０，Ｙ_０）から速度Ｖ_Ｓ１で移動を開始する（Ｓ３，図８に示すＰ_Ｔ１参照）。
【００４４】
ルート最適化演算部８は、オペレータの操作によりルート情報入力装置５から時間Ｔを変更する旨の入力があるか否かを判別し（Ｓ４）、入力があると判別した場合（Ｓ４：ＹＥＳ）には、これに従って時間Ｔの変更を設定する（Ｓ５）。なお、時間Ｔの変更の有無はターゲットポイントＰ_Ｔの動作に直接影響を与えるものではなく、後述する最適化演算（Ｓ１４参照）を実行することにより制御される船体１のトラッキング動作に作用するものである。
【００４５】
時間Ｔの変更を設定した場合（Ｓ５）、又は時間Ｔを変更する旨の入力がないと判別した場合（Ｓ４：ＮＯ）は、現時点でのｋの値がフィールド上に設けられたウェイポイントＷＰ_ｋの数ｋＭＡＸより大きいか否かを判別する（Ｓ６）。即ち、ターゲットポイントＰ_ＴがウェイポイントＷＰ_ｋＭＡＸを通過したか否かを判別する。
【００４６】
そして、ｋの値がｋＭＡＸの値以下であると判別した場合（Ｓ６：ＹＥＳ，図８に示すＰ_Ｔ１，Ｐ_Ｔ２，Ｐ_Ｔ３参照）、ターゲットポイントＰ_ＴがウェイポイントＷＰ_ｋを通過（又は、ウェイポイントＷＰ_ｋに到達）したか否かを判別する（Ｓ７）。通過していないと判別した場合（Ｓ７：ＮＯ，図８に示すＰ_Ｔ１，Ｐ_Ｔ２参照）、ターゲットポイントＰ_Ｔの座標を更新し（Ｓ８）、後述する最適化演算を実行し（Ｓ１４）、再びステップ４からの動作を実行する。
【００４７】
ここで、ステップ８にて行うターゲットポイントＰ_Ｔの座標の更新は、更新直前の座標（Ｘ_ｆ，Ｙ_ｆ）と、更新時に位置する区間ＬＥＧ_ｉの属性として設定されている移動速度Ｖ_Ｓｉ及び方位角Ψ_ｆｉと、制御装置６のサンプリング周期Ｔ_Ｓとを用い、（１）式及び（２）式に基づいて新たな座標を算出することによって行う。
【００４８】
一方、ステップ６にてｋの値がｋＭＡＸの値以下（Ｓ６：ＮＯ）であると判別した後に、ターゲットポイントＰ_ＴがウェイポイントＷＰ_ｋを通過したと判別した場合（Ｓ７：ＹＥＳ，図８に示すＰ_Ｔ３，Ｐ_Ｔ４参照）は、ターゲットポイントＰ_Ｔの座標をウェイポイントＷＰ_ｋの座標（Ｘ_ｋ，Ｙ_ｋ）に更新し（Ｓ９）、ｋの値を１だけ増加させた後（Ｓ１０）、そのｋの値がｋＭＡＸの値より大きいか否かを判別する（Ｓ１１）。
【００４９】
なお、ステップ９では計算の便宜上、ウェイポイントＷＰ_ｋを通過した直後のターゲットポイントＰ_Ｔの座標を前記ウェイポイントＷＰ_ｋの座標と近似し、ステップ８と同様の手法によりターゲットポイントＰ_Ｔの座標を更新している。しかし、実際にターゲットポイントＰ_Ｔの位置を算出してその座標を更新してもよい。
【００５０】
ｋの値がｋＭＡＸの値以下であると判別した場合（Ｓ１１：ＮＯ，図８に示すＰ_Ｔ３参照）、ターゲットポイントＰ_Ｔの移動速度及び方位角を、ステップ１０にて設定されたｋの値に対応する区間ＬＥＧ_ｋの属性に基づいてＶ_Ｓｋ，Ψ_ｆｋに更新し（Ｓ１２）、後述する最適化演算を実行し（Ｓ１４）、再びステップ４からの動作を実行する。
【００５１】
他方、ステップ６にてｋの値がｋＭＡＸの値より大きいと判別した場合（Ｓ６：ＹＥＳ，図８に示すＰ_Ｔ４参照）、又は、ステップ１１にてｋの値がｋＭＡＸの値より大きいと判別した場合（Ｓ１１：ＹＥＳ，図８に示すＰ_Ｔ４参照）は、ターゲットポイントＰ_Ｔの移動速度をＶ_Ｓｉ＝０とし（Ｓ１３）、後述する最適化演算を実行し（Ｓ１４）、再びステップ４からの動作を実行する。
【００５２】
なお、ｋ＝１と設定した後（Ｓ２）、最初に行うステップ６の動作にてｋ＞ｋＭＡＸ（Ｓ６：ＹＥＳ）と判別されるのは、ｋＭＡＸ＝０の場合であり、制御開始時における船体位置（図８に示すＷＰ_０参照）にターゲットポイントＰ_Ｔを固定して、この位置に船体１を保持しようとしていることを意味する。
【００５３】
また、ｋＭＡＸ＞０の場合であってステップ１１にてｋ＞ｋＭＡＸ（Ｓ１１：ＹＥＳ）と判別されるのは、１以上のウェイポイントＷＰを経由した後に最後のウェイポイントＷＰ_ｋＭＡＸにてターゲットポイントＰ_Ｔを固定し、該ウェイポイントＷＰ_ｋＭＡＸにて船体１を保持しようとしていることを意味する。
【００５４】
更に、ｋＭＡＸ＞０の場合であってステップ６にてｋ＞ｋＭＡＸ（Ｓ６：ＹＥＳ）と判別されるのは、現時点で既に船体１がウェイポイントＷＰ_ｋＭＡＸに保持されており、その後引き続いて該ウェイポイントＷＰ_ｋＭＡＸに船体１を保持しようとしていることを意味する。
【００５５】
［最適化演算について］
次に、ステップ１４にて実行される最適化演算について説明する。制御系の状態ベクトル（状態量）をｘ（ｔ）、該制御系へ入力する制御信号の入力ベクトルをｕ（ｔ）、既知の時変パラメータをｐ（ｔ）とすると、一般に該制御系の状態方程式は下記（３）式のように表される。
【００５６】
【数２】

また、船体１に設けられた制御位置を、例えば図４に示すｘ_ｓ−ｙ_ｓ座標系を用いて（−ｘ_ｃ，０）とした場合、図４に示す状態量を有する該船体１についての上記（３）式の状態方程式は、下記（４）式のように具体的に与えることができる。
【００５７】
【数３】

なお、Ｍ_１は船体１の質量及び前後付加質量の合計、Ｍ_２は船体１の質量及び左右付加質量の合計、Ｉは船体１の慣性モーメント及び付加慣性モーメントの合計である。ｕ_１は船体１の前後方向の推力指令値、ｕ_２は船体１の左右方向の推力指令値、ｕ_３は船体１の旋回モーメント推力指令値である。Ｘ_Ｈは船体１に対して図４に示すｘ_ｓ軸向きに作用する流体力、Ｙ_Ｈは船体１に対してｙ_ｓ軸向きに作用する流体力、Ｎ_Ｈは船体１に対して右舷回頭向きに作用する流体力を示す。該流体力Ｘ_Ｈ，Ｙ_Ｈ，Ｎ_Ｈは、各方向の速度及び角速度の関数として近似したモデルを使用することができる。
【００５８】
また、上記最適化演算においては、下記（５）式に示す評価関数Ｊと（６）式に示す終端状態量拘束条件Ψとを用いる。
【００５９】
【数４】

なお、ｒ_１，ｒ_２，ｒ_３は前記推力指令値ｕ_１，ｕ_２，ｕ_３の夫々に掛かる重み係数を表しており、ここでは全て正の値とする。また、ｘ_ｆ（ｔ＋Ｔ）は時刻ｔ＋Ｔにおける船体１の状態量の目標値を表している。
【００６０】
上記（５）式に示される評価関数Ｊは、時刻ｔ〜ｔ＋Ｔ間における船体１の移動に係る消費エネルギーに関するものであり、船体１の運動特性を考慮して該評価関数Ｊを最小（ｒ_１，ｒ_２，ｒ_３＜０の場合は、最大）にすることは、前記時刻ｔ〜ｔ＋Ｔ間における消費エネルギーを最小に抑えることを意味する。なお、前記評価関数Ｊには推力指令値ｕ_１，ｕ_２，ｕ_３が含まれているが、必要に応じて船体１の状態量を含めてもよい。
【００６１】
また、（６）式に示される終端状態量拘束条件Ψは、現時刻ｔから時間Ｔだけ未来の時刻ｔ＋Ｔに、該現時刻ｔでのターゲットポイントＰ_Ｔの動作条件を、船体１の状態量として獲得させるためのものである。このように、本実施の形態では船体１の状態量の目標値を現時刻ｔから時間Ｔ後の終端時間のみで設定し、船体位置制御問題を終端位置固定問題として定式化している。従って、後述するシミュレーション結果にも示されるように、時間Ｔを適宜設定することにより、ルート１００上の変針点にてターゲットポイントＰ_Ｔの移動速度及び方位角が大きく変化しても、船体１の各推力指令値の急変を抑制することができる。
【００６２】
ステップ１４に示す最適化演算では、初めに、現時点における船体１の状態量ｘ（ｔ）を取得する。即ち、図３に示すように、位置計測装置３が取得した船体１の位置情報と、方位角計測装置４が取得した船体１の方位角情報とを用い、制御装置６のオブザーバ１０によって各状態量ｘ（ｔ）を算出する。一方で、上記（６）式に示す終端状態量拘束条件と、時刻ｔにおけるターゲットポイントＰ_Ｔの動作条件（Ｖ_Ｓｉ，Ψ_ｆｉ）及び座標（Ｘ_ｆｋ，Ｙ_ｆｋ）とに基づき、時刻ｔ＋Ｔにおける船体１の目標状態量であるｘ（ｔ＋Ｔ）を取得する。そして最終的に、現時刻ｔでの船体１の状態量ｘ（ｔ），前記目標状態量ｘ（ｔ＋Ｔ），及び（４）式に示す状態方程式に基づき、移動体の運動特性を考慮して（５）式に示す評価関数を最小にする推力指令値ｕ_１，ｕ_２，ｕ_３を算出する。
【００６３】
なお、上記終端状態量固定問題における最適性の条件は、下記（７）〜（１０）式によって与えられる。
【００６４】
【数５】

ここで、ＨはハミルトニアンＬ＋λｆ、λはｘの共状態行ベクトル、νは（６）式に示す終端状態量拘束条件に対するラグランジェ乗数行ベクトルである。また、μは時間ｔ〜ｔ＋Ｔで定義される変数ベクトルμ＝Ψ_ｘ（τ）である。ｘ_０はサンプリング周期毎にフィードバックされる状態変数の初期値である。
【００６５】
このようにして算出された推力指令値ｕ_１〜ｕ_３は、時間の関数として表され、時刻ｔ〜ｔ＋Ｔ間の消費エネルギーを最小限に抑制した上で、時刻ｔ＋Ｔにおいて船体１に目標状態量ｘ（ｔ＋Ｔ）を達成させるものである。そして、ルート最適化演算部８では、前記推力指令値ｕ_１〜ｕ_３の現時刻ｔにおける値（即ち、現時刻直後に船体１に付与すべき推力に対応する推力指令値）を取得し、図３に示すように推力配分部９へ推力指令として出力する。推力配分部９は前記推力指令に基づき、推進機構７、即ち図２に示した各アクチュエータ２１〜２５への推力配分を決定し、該アクチュエータ２１〜２５へ個別に動作指令を出力する。
【００６６】
［シミュレーション結果について］
次に、本実施の形態に係る船体１の制御方法及び制御装置を用いて行ったシミュレーションの結果について説明する。図９は予め設定されたシミュレーション条件を示すグラフである。
【００６７】
図９に示すように本シミュレーションでは制御開始時の船体位置ＷＰ_０を原点とする座標系に２つのウェイポイントＷＰ_１，ＷＰ_２が設定されている。該ウェイポイントＷＰ_１の座標は（Ｘ_１，Ｙ_１）＝（２００，０）、ウェイポイントＷＰ_２の座標は（Ｘ_２，Ｙ_２）＝（４００，２００）である。ウェイポイントＷＰ_０，ＷＰ_１間の区間ＬＥＧ_１の属性は、速度Ｖ_Ｓ１＝０．５ｍ／ｓ，方位角Ψ_ｆ１＝０度であり、ウェイポイントＷＰ_１，ＷＰ_２間の区間ＬＥＧ_２の属性は、速度Ｖ_Ｓ２＝０．５ｍ／ｓ，方位角Ψ_ｆ２＝４５度である。また、時間Ｔ＝２０ｓｅｃ（ケース１），Ｔ＝８０ｓｅｃ（ケース２）の２ケースについてシミュレーションを行った。
【００６８】
本シミュレーションの結果を図１０〜１４に示す。図１０は、ｘ−ｙ座標系にプロットしたケース１，２における船体１の航跡図であり、破線はケース１を、実線はケース２を示している。該航跡図では、特に変針点であるウェイポイントＷＰ_１を含む周辺における船体１の航跡を示している。
【００６９】
図１１は、ケース１について、制御開始時からの経過時間（ｓｅｃ）に対する船体１の推力指令値ｕ_１，ｕ_２，ｕ_３を別個に示すグラフであり、図１１（ａ）は船体１の前後方向の推力指令値（ｕ_１）を、図１１（ｂ）は船体１の左右方向の推力指令値（ｕ_２）を、図１１（ｃ）は船体１の旋回モーメント推力指令値（ｕ_３）を夫々示している。図１２は、ケース１について、制御開始時からの経過時間（ｓｅｃ）に対する船体１の状態量Ｘ，Ｙ，Ψを別個に示すグラフであり、図１２（ａ）は船体位置のｘ座標（Ｘ）を、図１２（ｂ）は船体位置のｙ座標（Ｙ）を、図１２（ｃ）は船体の方位角（Ψ）を夫々示している。
【００７０】
また、図１３は、ケース１について、制御開始時からの経過時間（ｓｅｃ）に対する船体１の推力指令値ｕ_１，ｕ_２，ｕ_３を別個に示すグラフであり、図１３（ａ）は船体１の前後方向の推力指令値（ｕ_１）を、図１３（ｂ）は船体１の左右方向の推力指令値（ｕ_２）を、図１３（ｃ）は船体１の旋回モーメント推力指令値（ｕ_３）を夫々示している。図１４は、ケース２について、制御開始時からの経過時間（ｓｅｃ）に対する船体１の状態量Ｘ，Ｙ，Ψを別個に示すグラフであり、図１４（ａ）は船体位置のｘ座標（Ｘ）を、図１４（ｂ）は船体位置のｙ座標（Ｙ）を、図１４（ｃ）は船体の方位角（Ψ）を夫々示している。
【００７１】
図１０から分かるように、ケース２に比べてケース１では、変針点であるウェイポイントＷＰ_１に対してより近接した航跡が形成されている。従って、時間Ｔを比較的短時間に設定することにより、ルートに沿ってより厳密に船体１をトラッキングさせ得ることが分かる。
【００７２】
また、図１１及び図１３を対比して分かるように、各推力指令値ｕ_１〜ｕ_３の変動（スラスタ変動）は、ケース１に比べてケース２の場合の方が、相対的に小さくなっている。また、図１２及び図１４を対比して分かるように、船体１の位置及び方位角の変動についても、ケース１に比べてケース２の場合の方が、相対的に小さくなっている。従って、時間Ｔを比較的長時間に設定することにより、スラスタ変動を抑制でき、変針点において曲率の大きい滑らかな航跡を得ることができることが分かる。
【００７３】
上述したような制御方法及び制御装置によれば、事前に厳密な計画航路を設定する必要がなく、図５又は図８に示すように複数のウェイポイントＷＰを線分で結んだだけのルートを設定した場合であっても、変針点において船体１の操作量（即ち、アクチュエータ７への動作指令の値）の急激な変化を抑制することができる。また、船体１を、ターゲットポイントＰ_Ｔに追従するようにして、消費エネルギーを抑制しつつ移動させることができる。
【００７４】
【発明の効果】
本発明によれば、最適な計画航路を事前に設定する必要がなく、また、所定の制約条件内でエネルギーの消費を抑制した上で、移動体を予定移動経路（ルート）に沿って移動させるように制御可能な移動体の制御方法及び制御装置を提供することができる。
【図面の簡単な説明】
【図１】本発明の実施の形態に係る移動体の制御装置を含む制御系の構成を示すブロック図である。
【図２】図１に示す制御装置の機能ブロック図である。
【図３】図１に示す推進機構として船体に設けられたアクチュエータの配置例を示す模式図である。
【図４】本実施の形態に係る船体の状態量の定義を説明するための模式図である。
【図５】船体の予定移動経路（ルート）とターゲットポイントとを説明するための模式図である。
【図６】ルート上の各区間の属性の一例を示す図表である。
【図７】船体のルートトラッキング時におけるターゲットポイントの動作制御の流れを示すフローチャートである。
【図８】図７のフローチャートに示されるターゲットポイントの動作を例示する模式図である。
【図９】本実施の形態に係る船体の制御方法及び制御装置を用いて行ったシミュレーションの条件を示すグラフである。
【図１０】本実施の形態に係る船体の制御方法及び制御装置を用いて行ったシミュレーションの結果を示すグラフである。
【図１１】本実施の形態に係る船体の制御方法及び制御装置を用いて行ったシミュレーションの結果を示すグラフである。
【図１２】本実施の形態に係る船体の制御方法及び制御装置を用いて行ったシミュレーションの結果を示すグラフである。
【図１３】本実施の形態に係る船体の制御方法及び制御装置を用いて行ったシミュレーションの結果を示すグラフである。
【図１４】本実施の形態に係る船体の制御方法及び制御装置を用いて行ったシミュレーションの結果を示すグラフである。
【符号の説明】
１ 船体
２ 操船装置
３ 位置計測装置
４ 方位角計測装置
５ ルート情報入力装置
６ 制御装置
７ 推進機構
８ ルート最適化演算部
９ 推力配分部
１０ オブザーバ
２１ スラスタ
２２，２３ プロペラ
２４，２５ 舵
Ｐ_Ｔ ターゲットポイント
Ｔ 時間（設定時間）
ｕ_１，ｕ_２，ｕ_３ 推力指令値（推力値）
ＷＰ ウェイポイント（目標点）

Claims (12)

1. 移動体の予定移動経路と、該予定移動経路上を移動し得る仮想点から成るターゲットポイントの動作条件とを設定し、
   前記ターゲットポイントを前記動作条件に基づいて動作させ、
   前記移動体を、現時刻から設定時間だけ未来に、前記動作条件に基づいて動作する前記ターゲットポイントの前記現時刻での位置に到達させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出し、
   該推力に基づいて前記ターゲットポイントを追従するように前記移動体を移動させることを特徴とする移動体の制御方法。
2. 前記移動体に、前記現時刻から前記設定時間だけ未来に、該現時刻での前記ターゲットポイントの動作条件を獲得させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出することを特徴とする請求項１に記載の制御方法。
3. 複数の目標点を設定して該目標点を線分で結ぶことにより、前記予定移動経路を設定することを特徴とする請求項１又は２に記載の移動体の制御方法。
4. 前記推力の算出においては、未定数としての推力値を含む前記移動体の操作量に関する評価関数を用い、現時刻から前記設定時間だけ未来に至る期間についての前記評価関数が、最小値又は最大値になるという条件を満たす推力を算出することを特徴とする請求項１乃至３の何れかに記載の移動体の制御方法。
5. 前記設定時間を変更することを特徴とする請求項１乃至４の何れかに記載の移動体の制御方法。
6. 前記ターゲットポイントの位置を固定して前記移動体の定点保持を行うことを特徴とする請求項１乃至５の何れかに記載の移動体の制御方法。
7. 移動体の予定移動経路を設定する手段と、
   該予定移動経路上を移動し得る仮想点から成るターゲットポイントの動作条件を設定する手段と、
   設定時間を設定する手段と、
   前記移動体を、現時刻から前記設定時間だけ未来に、前記動作条件に基づいて動作する前記ターゲットポイントの前記現時刻での位置に到達させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出する手段と
   を備えることを特徴とする移動体の制御装置。
8. 前記推力を算出する手段は、前記移動体に、前記現時刻から前記設定時間だけ未来に、該現時刻での前記ターゲットポイントの動作条件を獲得させるべく、前記現時刻直後に前記移動体に付与すべき推力を算出すべく成してあることを特徴とする請求項７に記載の移動体の制御装置。
9. 前記予定移動経路を設定する手段は、複数の目標点を設定し、該目標点を線分で結んで前記予定移動経路を設定すべく成してあることを特徴とする請求項７又は８に記載の移動体の制御装置。
10. 前記推力値を算出する手段は、未定数としての推力値を含む前記移動体の操作量に関する評価関数を用い、現時刻から前記所定時間だけ未来に至る期間についての前記評価関数が、最小値又は最大値になるという条件を満たす推力を算出すべく成してあることを特徴とする請求項７乃至９の何れかに記載の移動体の制御装置。
11. 前記設定時間を変更する手段を備えることを特徴とする請求項７乃至１０の何れかに記載の移動体の制御装置。
12. 前記ターゲットポイントの位置を固定する手段を備えることを特徴とする請求項７乃至１１の何れかに記載の移動体の制御装置。

Images