(57)【要約】【課題】  潮流が存在する環境下で水中移動体の障害物との衝突回避を図る。【解決手段】  水中移動体１は、状態量検出部２、障害物検知部３、移動装置４、計算部５、記憶部６を備えた構成とされている。計算部５は、状態量検出部２から自機の現在の状態量を受け取ると、障害物検知部３が検知した障害物７の位置の情報に基づいて、自機が障害物７に衝突しない条件としての障害物回避制約関数を算出し、また、自機の状態遷移方程式に対する自機の状態量の不確かさと、自機に働く外乱を基にマージン関数を算出し、次いで、算出されたマージン関数を障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、移動装置４の最適制御量を求める。その後、計算部５は、求めた最適制御量を、移動装置４へ、移動装置４の制御量の目標値として与える。【選択図】図１
2018181166.tif 000002
【特許請求の範囲】
【請求項１】
状態量検出部と、障害物検知部と、移動装置とを備えた移動体の制御を行う移動体の制御方法であって、
前記状態量検出部が前記移動体の現在の状態量を検出する処理と、
前記障害物検知部が障害物を検知する処理と、を行い、
更に、計算部は、
前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する処理と、
前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する処理と、
前記マージン関数を前記障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、前記移動装置の最適制御量を求める処理と、
前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える処理と、を行うこと
を特徴とする移動体の制御方法。
【請求項２】
前記移動体は、水中移動体とし、前記水中移動体に働く外乱は潮流とする
請求項１記載の移動体の制御方法。
【請求項３】
移動体に設けられた状態量検出部、障害物検知部および移動装置と、
計算部とを有し、
前記計算部は、
前記状態量検出部が検出した前記移動体の現在の状態量と、前記障害物検知部が検知した障害物の情報を受け取る機能と、
前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する機能と、
前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する機能と、
前記マージン関数を前記障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、前記移動装置の最適制御量を求める機能と、
前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える機能と、を備えること
を特徴とする移動体制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律制御型の移動体に障害物回避を行わせるために用いる移動体の制御方法と該方法の実施に用いる移動体制御システムに関するものである。
【背景技術】
【０００２】
自律移動を行う移動体の一つとしては、ＡＵＶ（Autonomous Underwater Vehicle）と呼ばれる無人の水中移動体が知られている。
【０００３】
この種の水中移動体は、海底資源探査、機雷の探査とその除去、海洋遭難者の探索（探査）などに利用することが考えられている。
【０００４】
このような水中移動体の運用を行う上で重要になる技術の一つが、他の移動体や岩礁などの障害物との衝突回避である。
【０００５】
なお、移動体を自律制御して障害物回避を行わせる技術としては、たとえば、次のような車両の衝突回避制御に関する技術が従来提案されている（たとえば、特許文献１参照）。
【０００６】
これは、自車の走行状態に関するデータと、ターゲットと自車間の距離およびターゲットの相対速度に関するデータとから自車モデルおよびターゲットモデルを作成し、モデル予測制御によって、自車が衝突回避のための最適ルートを走行するためのアクチュエータに関する最適操作量を検索し、得られた最適操作量を自車のアクチュエータ、たとえば、ブレーキ、ステアリング、更にはアクセルに入力するという技術である。この衝突回避制御の技術によれば、車両が最適ルートを自動的に走行して衝突を回避させることができるとされている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００７−２７６５０８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ところで、車両は、車輪と、固定された地面との間の摩擦力を利用して推進力や旋回力を得るものである。そのため、堅固な地面を走行する車両では、ステアリングにより操作される操舵輪の切れ角（舵角）と、ブレーキやアクセルにより操作される駆動輪の回転数が決まれば、車両の向きの変化速度や変化量、移動速度や移動量は、ほぼ一意に定まる。
【０００９】
ところが、水中移動体は、自機の状態量を変化させる移動装置として、たとえば、プロペラのような推進手段と、舵のような操舵手段を備えた構成である場合、推進手段でプロペラの回転数を決めても、水中移動体の慣性の影響や、水中移動体の胴体、舵やプロペラに押された水が移動してしまうことの影響などに起因して、水中移動体の移動速度や移動量は一意には定まらない。
【００１０】
また、操舵手段で舵の舵角を決めても、舵の利きは水中移動体の移動速度の影響を受けやすいため、水中移動体の方向の変化の速度や変化量は一意には定まらない。
【００１１】
更に、水中移動体は、使用する環境に潮流などの外乱が存在している場合があり、その場合は、水中移動体の位置や姿勢、移動速度、姿勢変化に関する角速度といった状態量が、外乱の影響を受けて変化する。
【００１２】
しかも、水中移動体では、水面下を移動するときの自機の状態量の検出手段として慣性航法装置が用いられることが多いが、慣性航法装置は、水中移動体の使用時間が長くなるにしたがって誤差が蓄積するという特性を有している。そのため、水中移動体では、慣性航法装置で検出している自機の状態量の検出結果に誤差が含まれる可能性もある。
【００１３】
したがって、水中移動体は、制御される状態量や検出される状態量が不確かさを含んでいるため、特許文献１に示された衝突回避制御の手法は、水中移動体の衝突回避に適用することはできない。
【００１４】
なお、制御される状態量が不確かさを含み、しかも外乱を受ける移動体としては、水中移動体のほかに、潮流や風を外乱として受ける自律制御型の船舶や、ＡＳＶなどといわれる水上移動体、風を外乱として受ける自律制御型の航空機や、ＵＡＶなどといわれる無人航空機、太陽からの電磁波の放射圧を外乱として受ける宇宙機、更には、傾斜のある軟弱な地面を走行する自律制御型のクローラ型や車輪型の自律走行車もある。ここで、状態量に関する不確かさとは、移動体の位置や姿勢、移動速度、姿勢変化に関する角速度といった状態量を制御するときに、状態量検出手段の検出精度には限界があるため、真の状態量を検出できず、検出した値が、真の状態量に対してたとえば正規分布のような確率分布に従い、一意には定まらないことを意味している。
【００１５】
そこで、本発明は、自機の状態量の制御に不確かさを有する移動体について、外乱が存在する環境下で障害物回避を行わせることができる移動体の制御方法および移動体制御システムを提供しようとするものである。
【課題を解決するための手段】
【００１６】
本発明は、前記課題を解決するために、状態量検出部と、障害物検知部と、移動装置とを備えた移動体の制御を行う移動体の制御方法であって、前記状態量検出部が前記移動体の現在の状態量を検出する処理と、前記障害物検知部が障害物を検知する処理と、を行い、更に、計算部は、前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する処理と、前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する処理と、前記マージン関数を前記障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、前記移動装置の最適制御量を求める処理と、前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える処理と、を行うようにする移動体の制御方法とする。
【００１７】
前記移動体は、水中移動体とし、前記水中移動体に働く外乱は潮流とする方法としてある。
【００１８】
また、移動体に設けられた状態量検出部、障害物検知部および移動装置と、計算部とを有し、前記計算部は、前記状態量検出部が検出した前記移動体の現在の状態量と、前記障害物検知部が検知した障害物の情報を受け取る機能と、前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する機能と、前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する機能と、前記マージン関数を前記障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、前記移動装置の最適制御量を求める機能と、前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える機能と、を備える構成を有する移動体制御システムとする。
【発明の効果】
【００１９】
本発明の移動体の制御方法および移動体制御システムによれば、自機の状態量の制御に不確かさを有する移動体について、外乱が存在する環境下で障害物回避を行わせることができる。
【図面の簡単な説明】
【００２０】
【図１】移動体制御システムの第１実施形態を示す概要図である。
【図２】移動体が障害物回避を行う状態を示す概要図である。
【図３】外乱の方位角度の設定方法を説明するための図である。
【図４】第１実施形態の変形例を示す概要図である。
【発明を実施するための形態】
【００２１】
以下、本発明を実施するための形態を図面を参照して説明する。
【００２２】
［第１実施形態］
図１乃至図３は、移動体の制御方法および移動体制御システムの第１実施形態として、本発明を、移動体としての水中移動体に適用した例を示すものである。また本実施形態では、移動体に働く外乱は、水中移動体に働く潮流とした例を示すものである。本実施形態では、水平方向の２次元座標の空間内で、水中移動体に障害物回避を行わせる場合について説明する。
【００２３】
図１は、移動体制御システムの第１実施形態を示す概要図である。図２は、移動体の制御方法により設定される障害物回避のための移動経路を示すもので、図２（ａ）は、本実施形態により設定される移動経路を示す概要図、図２（ｂ）は、比較例として、外乱の方向を考慮しない場合に設定される移動経路を示す概要図である。図３は、潮流の方位角度の設定方法を説明するための概要図である。
【００２４】
本実施形態の移動体制御システムは、図１に示すもので、水中移動体１に搭載された状態量検出部２と、障害物検知部３と、移動装置４と、計算部５と、記憶部６とを備えた構成とされている。
【００２５】
状態量検出部２は、水中移動体１の自機の位置と、移動する速度と、自機が向いている方位を含む姿勢と、自機の向く方位の変化に関する角速度とを、状態量として検出する機能を備えている。この機能を実現する状態量検出部２の構成例としては、たとえば、慣性航法装置を備えた構成や、慣性航法装置とドップラー式超音波速度計とを備えた構成とすればよい。
【００２６】
状態量検出部２は、水中移動体１の自機の位置や姿勢は、地球表面に固定された二次元の座標系における座標として検出するようにすればよい。なお、この二次元の直交座標系としては、緯度と経度とによる座標系を用いることが好適であるが、それに限定されるものではない。
【００２７】
なお、状態量検出部２は、前記状態量の検出機能を実現することができれば、前記した以外の任意の機器構成を採用してもよいことは勿論である。
【００２８】
状態量検出部２は、状態量の検出を行うと、その検出結果を計算部５へ送るようにしてある。
【００２９】
障害物検知部３は、水中移動体１が移動するときに、図２（ａ）に示すように自機の進行方向の前方に位置していて、自機の移動の障害となる障害物７を検知する機能を備えている。また、水中移動体１がウェイポイント制御のように設定された移動経路に沿い移動する場合は、障害物検知部３は、移動経路における水中移動体１が現在位置している地点よりも下流側の移動経路上に存在する障害物７を検知する機能を備えている。これらの機能を実現する障害物検知部３の具体的な構成例としては、たとえば、音響ソナーを備えた構成とすればよい。なお、障害物検知部３は、前記のような障害物７の検知機能を備えていれば、音響ソナーと他の検知手段（センサ）とを組み合わせて備えた構成としてもよいし、音響ソナー以外の検知手段を備えた構成としてもよい。
【００３０】
障害物検知部３は、障害物７の検知を行うと、検知された障害物７の情報を、計算部５へ送る機能を備えている。なお、この際、障害物検知部３は、障害物７について、自機の現在位置を基準とする相対位置の情報を計算部５へ送るようにすればよい。この場合、計算部５では、障害物検知部３より送られる障害物７の情報と、状態量検出部２より送られる自機の位置と姿勢の情報とを基に、地球表面に固定された二次元の座標系における障害物７の位置座標を計算により求めることができる。なお、この計算の機能を障害物検知部３が備えて、計算により求めた障害物７の位置座標の情報を、計算部５に送るようにしてもよい。
【００３１】
移動装置４は、水中移動体１に備えたスラスタや舵などのアクチュエータであり、この移動装置４の制御出力により、前記した水中移動体１の位置、速度、姿勢、角速度の状態量が変化する。
【００３２】
移動装置４は、計算部５より指令（制御入力）を受け取ると、その指令に従って運転、動作する機能を備えている。
【００３３】
なお、移動装置４は、水中移動体１の前記状態量を変化させることができれば、移動装置の形式や、水中移動体１における配置、数などは任意の構成としてもよいことは勿論である。
【００３４】
記憶部６は、水中移動体１の自機の移動経路を記憶する機能と、移動経路が設定された環境に存在する潮流Ｔ（図２（ａ）参照）の方向に関するデータ（以下、潮流方向データという）を記憶する機能とを備えている。
【００３５】
水中移動体１の移動の制御を、たとえば、ウェイポイント方式で行う場合は、記憶部６は、移動経路をウェイポイントファイルで記憶するようにすればよい。なお、記憶部６で水中移動体１の移動経路を記憶する形式は、ウェイポイントファイルに限られるものではなく、水中移動体１の移動を制御する方式に合わせて適宜変更してよいことは勿論である。
【００３６】
記憶部６が記憶する潮流方向データは、たとえば、海上保安庁のデータベースなどで海域ごとに公開されている潮流Ｔに関する情報から取得することが好適であるが、水中移動体１の移動経路が設定された環境についての潮流方向データであれば、他の情報源によるものであってもよい。
【００３７】
計算部５は、状態量検出部２から水中移動体１の状態量の検出結果を受け取ると共に、障害物検知部３で検知された障害物７の情報を障害物検知部３から受け取ると、以下の処理により移動装置４へ指令を与える機能を備えている。
【００３８】
計算部５には、事前に求められた水中移動体１の状態遷移方程式が、（１）式として与えられる。
【００３９】
ｘｉ＋１＝ｆ（ｘｉ，ｕｉ）                ・・・（１）
ただし、ｘｉは水中移動体の状態量、ｕｉは制御量（移動装置４の制御出力）、ｉは離散時間
【００４０】
この状態で、計算部５は、状態量検出部２から水中移動体１の自機の現在の状態量の検出結果を受け取ると、現在の状態量をｘ１とする。
【００４１】
また、計算部５は、障害物検知部３より障害物７の情報を受け取ると、検知された障害物７の位置とサイズを基に、障害物７の中心位置をｏ１、サイズを半径ＤＭａｘの球または円で近似する。
【００４２】
更に、計算部５は、記憶部６より、図２（ａ）に示すように、水中移動体１の現在位置からの移動目標となる目標位置８に関する情報を取得して、水中移動体１が目標位置８に存在するときの状態量である目標状態量ｘｄを得る。
【００４３】
更にまた、計算部５は、記憶部６より、水中移動体１の現在位置に関する潮流方向データを取得して、図３に示すように、水中移動体１の現在位置での潮流方向角度φを得る。図３では、潮流方向角度φは、たとえば、状態量検出部２で用いるのと同じ地球表面に固定された二次元の座標系にて、潮流Ｔの方向が、基準（０度）とするＸ軸の正方向から、反時計回り方向に傾斜する角度で設定してある。なお、潮流方向角度φは、状態量検出部２で用いるのと同じ地球表面に固定された二次元の座標系で、水中移動体１に作用する方向を明確に定義することができれば、任意の方向を基準に設定してもよいことは勿論である。
【００４４】
次に、計算部５は、前記の各情報を基に制御量の計算を行う。
【００４５】
計算部５における制御量の算出には、モデル予測制御を利用する。モデル予測制御では、以下の（２）式に示す最適化問題を解くことで、次の移動装置４の最適制御量行列ｕを得る。
【００４６】
【数１】
2018181166.tif 000003
【００４７】
【数２】
2018181166.tif 000004
【００４８】
なお、最適化問題の解法は、ニュートン法など、従来一般的に用いられている解法を用いればよい。
【００４９】
前記（２）式におけるＪ（ｕ）は、評価関数、ｈｉ（ｕ）は衝突回避制約関数であり、それぞれ以下の（３）式、（４）式で記述される。
【００５０】
【数３】
2018181166.tif 000005
【００５１】
【数４】
2018181166.tif 000006
ただし、ｇｉ（ｕ）は、障害物回避制約関数であり、ｇｉ（ｕ）＝ＤＭａｘ−Ｄｉ＞０で記述される。
Ｄｉは、ｉステップ目の水中移動体１の位置と障害物７の中心位置ｏ１との間の距離である。ｉステップ目の水中移動体１の位置の座標を（Ｘｉ，Ｙｉ）とし、障害物７の中心位置ｏ１の座標を（Ｘｏ，Ｙｏ）とすると、Ｄｉは、以下の式で記述される。
【００５２】
【数５】
2018181166.tif 000007
【００５３】
なお、前記（３）式で示した評価関数は、一例であり、この形式以外にも、モデル予測制御で一般的に使用される他の評価関数を採用してもよい。また、前記した障害物回避制約関数は、水中移動体１が障害物７に衝突しない条件の一例であり、障害物７のサイズを近似する形状が球や円以外の場合は、その形状に応じて、ｉステップ目の水中移動体１の位置が障害物７と干渉しない条件を表す関数を適宜採用すればよい。
【００５４】
水中移動体１の状態量ｘｉは、前記（１）式で示した状態遷移方程式と、状態量検出部２で検出した現在の状態値ｘ１と、最適制御量行列ｕとから求める。
【００５５】
更に、前記（４）式におけるσｉは、水中移動体１の状態量の不確かさに応じて算出されるマージン関数（マージン項）である。
【００５６】
次に、σｉの導出方法について説明する。
【００５７】
σｉは、障害物回避制約関数の偏微分行列Ｂｉ（ｉ＝０…Ｎｐ）と、水中移動体１の誤差分散行列ＣｏｖＸｉ（ｉ＝０…Ｎｐ）とから求めることができる。
【００５８】
障害物回避制約関数の偏微分行列Ｂｉ（ｉ＝０…Ｎｐ）は、次の（５）式で示される。
【００５９】
【数６】
2018181166.tif 000008
【００６０】
また、水中移動体１の誤差分散行列ＣｏｖＸｉ（ｉ＝０…Ｎｐ）は、次の（６）式で示される。
【００６１】
【数７】
2018181166.tif 000009
【００６２】
ここで、前記（６）式のｄτは、１ホライズン当たりの時間である。また、Ａｉは、前記（１）式の偏微分行列であり、以下の（７）式で与えられる。
【００６３】
【数８】
2018181166.tif 000010
【００６４】
前記（６）式のＮｐｒｏｃは、水中移動体１のプロセスノイズであり、カルマンフィルタなどの一般的な状態推定手法で設定されるプロセスノイズと同じ値でよい。
【００６５】
前記（６）式のＲ（φ）は、回転行列であり、以下の式（８）で表される。
【００６６】
【数９】
2018181166.tif 000011
【００６７】
前記（６）式のＮｆは、潮流に関するプロセスノイズである。Ｎｆは、たとえば、想定される潮流速度をｖｆとして、以下の（９）式で与えられる。
【００６８】
【数１０】
2018181166.tif 000012
【００６９】
なお、ｖｆは、水中移動体１を移動させる領域についての潮流速度が得られれば、それに応じて値を適宜変化させることができるため、制御性能の向上化につながる。しかし、水中移動体１を移動させる領域についての潮流速度は、未知な状態であっても構わない。このように潮流速度が未知の場合は、設定値の指針として、ｖｆは実際の潮流速度よりも大きな値に設定しておけば、水中移動体１が障害物７と衝突する可能性の低減化を図ることができる。したがって、外乱としての潮流Ｔは、潮流方向角度φが一定方向に定まっていれば、潮流速度は一定であってもよいし、たとえば波の影響などにより、周期的な変化を生じていてもよい。更には、潮流速度の周期的な変化としては、潮流方向角度φが一定で、強度がプラス方向からマイナス方向に変化するようなものを含んでいてもよい。
【００７０】
また、前記式（９）におけるαは、図２（ａ）に破線で示す楕円の長軸と短軸との比率を意味するパラメータであり、範囲は、０＜α＜１である。したがって、αが小さいほど短軸に対して長軸が長くなる。
【００７１】
前記（５）式から（９）式を基に、σｉは、以下の（１０）式で定義することができる。
【００７２】
【数１１】
2018181166.tif 000013
【００７３】
このように、本実施形態では、σｉを前記（１０）式で定義することにより、水中移動体１の状態量の不確かさに応じた適切なマージンを設定することができる。
【００７４】
計算部５は、以上の計算に基づいて、前記（２）式に示した最適化問題を解いて移動装置４の最適制御量行列ｕが求まると、その第１成分ｕ１を、移動装置４の制御量の目標値として移動装置４へ与える処理を行う機能を備えている。
【００７５】
なお、前記においては、ｘｉは位置・姿勢、速度、角速度の６変数を状態量としたが、これは一例であり、本実施形態では、状態量は、変数の数には依存しない。
【００７６】
以上の構成としてある本実施形態の移動体制御システムを用いて水中移動体１の制御方法を実施すると、水中移動体１は、図２（ａ）に示すように、目標位置８に向かう移動経路の移動中に、障害物検知部３（図１参照）によって進行方向の前方に存在している障害物７を検知すると、計算部５が前記した処理を行い、その結果得られた移動装置４の最適制御量行列ｕの第１成分ｕ１を、移動装置４の制御量の目標値として移動装置４へ与える。
【００７７】
したがって、移動装置４は、計算部５より受け取った制御量の目標値に応じて運転や動作が制御される。
【００７８】
これにより、図２（ａ）に示すように、ｉステップ目の水中移動体１の位置ｐｉ（ｉ＝０…Ｎｐ）については、それぞれ図２（ａ）に破線の楕円で示すような状態量の不確かさを考慮し、更に潮流に関するプロセスノイズを考慮した移動範囲９が設定され、この移動範囲９が、障害物７に重ならない移動経路を予測して設定することができる。
【００７９】
したがって、本実施形態の移動体の制御方法および移動体制御システムによれば、自機の状態量の制御に不確かさを有する水中移動体１について、外乱が存在する環境下で障害物７の回避を行わせることができる。
【００８０】
なお、図２（ｂ）は、比較として、潮流に関するプロセスノイズＮｆについて、潮流方向角度φに基づく調整を行わない場合、すなわち、潮流に関するプロセスノイズＮｆを全方位に考慮した場合のもので、この場合、ｉステップ目の水中移動体１の位置ｐｉ（ｉ＝０…Ｎｐ）について状態量の不確かさを考慮した移動範囲９ａは、図２（ｂ）に破線で示すように円になる。これは、潮流方向と直交する方向にも移動範囲９ａを拡大していて、不要な領域にまでプロセスノイズを見積もっていることを示しており、状態量の不確かさを過剰に見積もっていることになる。その結果、移動範囲９ａが、障害物７に重ならないように設定される移動経路は、障害物７をより大きく迂回するものとなり、図２（ｂ）に示すように、障害物７と目標位置８との距離によっては、水中移動体１が目標位置８に到達する移動経路を設定することが困難になる可能性もある。
【００８１】
これに対し、本実施形態では、図２（ａ）に示すように、潮流に関するプロセスノイズを考慮した移動範囲９が、潮流方向に沿う方向にのみ範囲を広げた楕円形になる。したがって、本実施形態では、水中移動体１の移動経路は、潮流方向と直交する方向に関しては、より障害物７に接近した配置で設定することができる。このため、本実施形態では、障害物７との衝突回避のために迂回する距離の短縮化を図ることができる。
【００８２】
また、本実施形態によれば、得られる情報からの状態量の不確かさの見積もりを、より小さく抑えることができるため、ロバスト性能と制御性能の両立を図ることができる。
【００８３】
なお、本実施形態では、水平方向の２次元座標の空間内で、水中移動体１に障害物７の回避を行わせる場合について説明したが、本実施形態の移動体の制御方法および移動体制御システムは、３次元座標の空間内で、水中移動体１に障害物７の回避を行わせる場合に適用してもよい。
【００８４】
この場合は、水中移動体１の状態遷移方程式を、３次元座標空間についての状態遷移方程式として設定すると共に、障害物７を、中心位置をｏ１、サイズを半径ＤＭａｘの球で近似するようにすればよい。更に、潮流方向角度φは、３次元座標空間で設定するようにすればよい。
【００８５】
また、本実施形態では、潮流方向角度φを、記憶部６に記憶した潮流方向データから求めるものとして説明したが、水中移動体１の対地速度と対水速度との差や、その他、従来採用あるいは提案されている潮流の推定手法を用いて、潮流方向角度φを推定するようにしてもよい。
【００８６】
［第１実施形態の変形例］
図４は、第１実施形態の変形例として、移動体制御システムの別の構成を示す概要図である。
【００８７】
なお、図４において、第１実施形態と同一のものには同一符号を付して、その説明を省略する。
【００８８】
本変形例の移動体制御システムは、図４に示すように、第１実施形態の移動体制御システムと同様の構成において、水中移動体１に計算部５と記憶部６を備える構成に代えて、計算部５および記憶部６を、水中移動体１の外部の管制局１０に備える構成としたものである。
【００８９】
管制局１０は、たとえば、船舶や水上移動体に備える水上局や、地上局である。
【００９０】
更に、水中移動体１と管制局１０は、相互通信を行うための通信機１１、通信機１２を備えた構成とされている。
【００９１】
通信機１１と通信機１２との通信方式は、超音波などの音響信号による音響通信であってもよいし、光ファイバを介した光信号や電線を介した電気信号による有線通信であってもよいし、その他任意の通信方式を採用してもよいことは勿論である。
【００９２】
更に、管制局１０が地上局の場合は、音響通信や有線通信で水中移動体１と直接通信を行うことは困難なため、この場合は、管制局１０と水中移動体１との間の通信を、船舶や水上移動体に備えた中継器で中継させるようにすればよい。
【００９３】
本変形例の移動体制御システムでは、水中移動体１は、状態量検出部２による状態量の検出結果、および、障害物検知部３により検知された障害物７の情報を、通信機１１と通信機１２を介して、管制局１０の計算部５へ送る。
【００９４】
管制局１０の計算部５は、水中移動体１から状態量の検出結果、および、障害物７の情報を受け取ると、記憶部６より水中移動体１の現在位置に関する潮流方向データを取得して、第１実施形態における計算部５の処理と同様の処理を実行する。
【００９５】
その後、管制局１０は、計算部５の処理の結果得られた移動装置４の最適制御量行列ｕの第１成分ｕ１を、移動装置４の制御量の目標値として、通信機１２と通信機１１を介して水中移動体１へ送る。
【００９６】
水中移動体１では、管制局１０より受け取った移動装置４の制御量の目標値を、移動装置４へ与える。
【００９７】
これにより、本変形例によっても、第１実施形態の移動体制御システムと同様に使用して同様の効果を得ることができる。
【００９８】
［第１実施形態の第１応用例］
前記第１実施形態では、障害物７は位置が固定されているものとして説明した。
【００９９】
これに対し、障害物７は、たとえば、同時に運用されている別の水中移動体などの移動する障害物７であってもよい。
【０１００】
この場合は、計算部５にて、ｉステップ目の水中移動体１の位置と障害物の中心位置ｏ１との間の距離Ｄｉを求める際に、ｉステップ目の障害物７の中心位置ｏ１のＸ座標の値と、Ｙ座標の値を、障害物７の運動方程式に基づいて設定するようにすればよい。
【０１０１】
たとえば、障害物７が別の水中移動体である場合は、その別の水中移動体の現在位置の情報を得ると共に、その現在位置と１ステップ前の時点での位置との差から、障害物７となる別の水中移動体の速度の情報を得る。次いで、得られた位置情報と速度情報とを、前記（１）式と同様の状態遷移方程式に代入することで、障害物７となる別の水中移動体のｉステップ目の位置の情報を得るようにすればよい。
【０１０２】
別の手法としては、障害物７となる別の水中移動体と通信を行って、その別の水中移動体の位置、速度、姿勢、角速度の情報を取得し、それらの情報から、障害物７となる別の水中移動体のｉステップ目の位置の情報を得るようにしてもよい。
【０１０３】
また、障害物７の運動方程式が明らかでない場合は、障害物検知部３による過去の複数のステップで検知された障害物７の位置の情報を基に、障害物７が等速直線運動を行うものと仮定して運動方程式を立て、その運動方程式に基づいて、障害物７のｉステップ目の時点での中心位置ｏ１の座標を求めるようにすればよい。
【０１０４】
本応用例によれば、第１実施形態と同様の効果を得ることができることに加えて、自機の状態量の制御に不確かさを有する水中移動体１について、外乱が存在する環境下で、移動する障害物７の回避を行わせることができる。
【０１０５】
［第１実施形態の第２応用例］
前記第１実施形態では、水中移動体１の状態遷移方程式として、水中移動体の状態量であるｘｉと、制御量であるｕｉとを変数とする前記（１）式を示した。また、潮流の影響は、前記（６）式に、潮流に関するプロセスノイズＮｆを導入することで考慮するものとした。
【０１０６】
これに対し、水中移動体１の状態遷移方程式は、以下の（１Ａ）式のように、水中移動体１に加えて、水中移動体１の現在位置に存在する潮流の潮流方向角度φと潮流速度ｖｆの影響を受けることを考慮して拡張させた状態遷移方程式として設定するようにしてもよい。
【０１０７】
ｘｉ＋１＝ｆ（ｘｉ，ｕｉ，ｖｆ，φ）                ・・・（１Ａ）
【０１０８】
この場合は、計算部５では、第１実施形態と同様の処理を行う際に、水中移動体１の誤差分散行列ＣｏｖＸｉ（ｉ＝０…Ｎｐ）を示す（６）式を、Ｎｆを省いた式として処理を行うようにすればよい。
【０１０９】
本応用例によれば、第１実施形態と同様の効果を得ることができる。
【０１１０】
［第１実施形態の第３応用例］
前記第１実施形態では、図２（ａ）に示したように、ｉステップ目の水中移動体１の位置ｐｉ（ｉ＝０…Ｎｐ）は、水中移動体１の状態量の不確かさと潮流に関するプロセスノイズを考慮した移動範囲９の楕円の中心を通るように設定されるものとした。
【０１１１】
これに対し、ｉステップ目の水中移動体１の位置ｐｉ（ｉ＝０…Ｎｐ）は、前記移動範囲９の中心からずれた位置を通るように設定してもよい。
【０１１２】
この場合は、潮流方向角度φと潮流速度ｖｆによって水中移動体が潮流Ｔの下流側へ移動させられると想定される量を、シフト量（Ｘｓ，Ｙｓ）として設定し、ｉステップ目の水中移動体１の位置と障害物の中心位置ｏ１との間の距離Ｄｉを求める式に対し、以下のような補正を加えるようにすればよい。
【０１１３】
【数１２】
2018181166.tif 000014
【０１１４】
本応用例によれば、第１実施形態と同様の効果に加えて、水中移動体１は、潮流Ｔが存在する環境下では、潮流Ｔの下流側となる方向へ移動する可能性が、潮流Ｔの上流側となる方向へ移動する可能性よりも高いという点を考慮して、水中移動体１の障害物７を回避する移動経路を設定することができる。
【０１１５】
［他の移動体へ応用］
前記第１実施形態とその変形例および応用例では、本発明の移動体の制御方法および移動体制御システムを、潮流が存在する環境下で移動する水中移動体に適用した例を示した。
【０１１６】
これに対し、本発明の移動体の制御方法および移動体制御システムは、制御される状態量が不確かさを含み、しかも作用する力の方向が一定となる外乱を受ける移動体であれば、以下のように、水中移動体１以外の移動体に適用してもよい。
【０１１７】
本発明の移動体の制御方法および移動体制御システムは、潮流や風を外乱として受ける自律制御型の水上移動体に適用してもよい。なお、水上移動体は、自動操縦を行う船舶を含むものとする。
【０１１８】
この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、潮流と風による外乱が水上移動体に働く方向に置き換えるようにすればよい。
【０１１９】
なお、水上移動体に備える状態量検出部は、全地球航法衛星システム（ＧＮＳＳ）の受信部を含んだ構成としてよい。また、水上移動体に備える障害物検知部および移動装置は、従来水上移動体に採用されているか、あるいは、採用することが提案されている障害物検知部および移動装置を適宜選定すればよいことは勿論である。
【０１２０】
これにより、自機の状態量の制御に不確かさを有する水上移動体について、作用する力の方向が一定となる外乱が存在する環境下で障害物回避を行わせることができる。
【０１２１】
本発明の移動体の制御方法および移動体制御システムは、風を外乱として受ける自律制御型の航空機に適用してもよい。なお、航空機は、自動操縦を行う有人の航空機、および無人航空機のいずれであってもよい。
【０１２２】
この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、風による外乱が航空機に働く方向に置き換えるようにすればよい。
【０１２３】
なお、航空機に備える状態量検出部は、全地球航法衛星システム（ＧＮＳＳ）の受信部を含んだ構成としてよい。また、航空機に備える障害物検知部および移動装置は、従来航空機に採用されているか、あるいは、採用することが提案されている障害物検知部および移動装置を適宜選定すればよいことは勿論である。
【０１２４】
これにより、自機の状態量の制御に不確かさを有する航空機について、作用する力の方向が一定となる外乱が存在する環境下で障害物回避を行わせることができる。
【０１２５】
本発明の移動体の制御方法および移動体制御システムは、太陽などからの電磁波の放射圧を外乱として受ける宇宙機に適用してもよい。
【０１２６】
この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、電磁波の放射圧よる外乱が宇宙機に働く方向に置き換えるようにすればよい。
【０１２７】
なお、宇宙機に備える状態量検出部、障害物検知部および移動装置は、従来宇宙機に採用されているか、あるいは、採用することが提案されている状態量検出部、障害物検知部および移動装置を適宜選定すればよいことは勿論である。
【０１２８】
これにより、自機の状態量の制御に不確かさを有する宇宙機について、作用する力の方向が一定となる外乱が存在する環境下で障害物回避を行わせることができる。
【０１２９】
本発明の移動体の制御方法および移動体制御システムは、傾斜のある軟弱な地面を走行する自律制御型のクローラ型や車輪型の自律走行車に適用してもよい。なお、自律走行車は、自動操縦を行う有人の自律走行車、および無人の自律走行車のいずれであってもよい。
【０１３０】
この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、自律走行車が位置している地面の下向きの傾斜角度が最大となる方位に置き換えるようにすればよい。
【０１３１】
なお、自律走行車に備える状態量検出部、障害物検知部および移動装置は、従来自律走行車に採用されているか、あるいは、採用することが提案されている状態量検出部、障害物検知部および移動装置を適宜選定すればよいことは勿論である。
【０１３２】
これにより、走行時に軟弱な地面とクローラや車輪との間で滑りを生じると共に、地面の傾斜に従って滑り落ちる現象が生じるために、自機の状態量の制御に不確かさを有する自律走行車について、障害物回避を行わせることができる。
【０１３３】
なお、本発明は前記実施形態とその変形例と応用例にのみ限定されるものではない。
【０１３４】
本発明の移動体の制御方法および移動体制御システムは、制御される状態量が不確かさを含み、しかも作用する力の方向が一定となる外乱を受ける移動体であれば、前記した以外のいかなる移動体に適用してもよい。
【０１３５】
たとえば、移動体に備えた状態量検出部における検出精度があまり高くないことに起因して、制御される状態量が不確かさを含むような移動体に適用してもよい。
【０１３６】
その他、本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。
【符号の説明】
【０１３７】
１  水中移動体（移動体）、２  状態量検出部、３  障害物検知部、４  移動装置、５  計算部、７  障害物
【図１】
2018181166.tif 000015
【図２】
2018181166.tif 000016
【図３】
2018181166.tif 000017
【図４】
2018181166.tif 000018