JP6953773B2 - 移動体の制御方法および移動体制御システム - Google Patents

Description

本発明は、自律制御型の移動体に障害物回避を行わせるために用いる移動体の制御方法と該方法の実施に用いる移動体制御システムに関するものである。

自律移動を行う移動体の一つとしては、ＡＵＶ（Autonomous Underwater Vehicle）と呼ばれる無人の水中移動体が知られている。

この種の水中移動体は、海底資源探査、機雷の探査とその除去、海洋遭難者の探索（探査）などに利用することが考えられている。

このような水中移動体の運用を行う上で重要になる技術の一つが、他の移動体や岩礁などの障害物との衝突回避である。

なお、移動体を自律制御して障害物回避を行わせる技術としては、たとえば、次のような車両の衝突回避制御に関する技術が従来提案されている（たとえば、特許文献１参照）。

これは、自車の走行状態に関するデータと、ターゲットと自車間の距離およびターゲットの相対速度に関するデータとから自車モデルおよびターゲットモデルを作成し、モデル予測制御によって、自車が衝突回避のための最適ルートを走行するためのアクチュエータに関する最適操作量を検索し、得られた最適操作量を自車のアクチュエータ、たとえば、ブレーキ、ステアリング、更にはアクセルに入力するという技術である。この衝突回避制御の技術によれば、車両が最適ルートを自動的に走行して衝突を回避させることができるとされている。

特開２００７−２７６５０８号公報

ところで、車両は、車輪と、固定された地面との間の摩擦力を利用して推進力や旋回力を得るものである。そのため、堅固な地面を走行する車両では、ステアリングにより操作される操舵輪の切れ角（舵角）と、ブレーキやアクセルにより操作される駆動輪の回転数が決まれば、車両の向きの変化速度や変化量、移動速度や移動量は、ほぼ一意に定まる。

ところが、水中移動体は、自機の状態量を変化させる移動装置として、たとえば、プロペラのような推進手段と、舵のような操舵手段を備えた構成である場合、推進手段でプロペラの回転数を決めても、水中移動体の慣性の影響や、水中移動体の胴体、舵やプロペラに押された水が移動してしまうことの影響などに起因して、水中移動体の移動速度や移動量は一意には定まらない。

また、操舵手段で舵の舵角を決めても、舵の利きは水中移動体の移動速度の影響を受けやすいため、水中移動体の方向の変化の速度や変化量は一意には定まらない。

更に、水中移動体は、使用する環境に潮流などの外乱が存在している場合があり、その場合は、水中移動体の位置や姿勢、移動速度、姿勢変化に関する角速度といった状態量が、外乱の影響を受けて変化する。

しかも、水中移動体では、水面下を移動するときの自機の状態量の検出手段として慣性航法装置が用いられることが多いが、慣性航法装置は、水中移動体の使用時間が長くなるにしたがって誤差が蓄積するという特性を有している。そのため、水中移動体では、慣性航法装置で検出している自機の状態量の検出結果に誤差が含まれる可能性もある。

したがって、水中移動体は、制御される状態量や検出される状態量が不確かさを含んでいるため、特許文献１に示された衝突回避制御の手法は、水中移動体の衝突回避に適用することはできない。

なお、制御される状態量が不確かさを含み、しかも外乱を受ける移動体としては、水中移動体のほかに、潮流や風を外乱として受ける自律制御型の船舶や、ＡＳＶなどといわれる水上移動体、風を外乱として受ける自律制御型の航空機や、ＵＡＶなどといわれる無人航空機、太陽からの電磁波の放射圧を外乱として受ける宇宙機、更には、傾斜のある軟弱な地面を走行する自律制御型のクローラ型や車輪型の自律走行車もある。ここで、状態量に関する不確かさとは、移動体の位置や姿勢、移動速度、姿勢変化に関する角速度といった状態量を制御するときに、状態量検出手段の検出精度には限界があるため、真の状態量を検出できず、検出した値が、真の状態量に対してたとえば正規分布のような確率分布に従い、一意には定まらないことを意味している。

そこで、本発明は、自機の状態量の制御に不確かさを有する移動体について、外乱が存在する環境下で障害物回避を行わせることができる移動体の制御方法および移動体制御システムを提供しようとするものである。

本発明は、前記課題を解決するために、状態量検出部と、障害物検知部と、移動装置とを備えた移動体の制御を行う移動体の制御方法であって、前記状態量検出部が前記移動体の現在の状態量を検出する処理と、前記障害物検知部が障害物を検知する処理と、を行い、更に、計算部は、前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する処理と、前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する処理と、前記マージン関数を前記障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、前記移動装置の最適制御量を求める処理と、前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える処理と、を行うようにする移動体の制御方法とする。

前記移動体は、水中移動体とし、前記水中移動体に働く外乱は潮流とする方法としてある。

また、移動体に設けられた状態量検出部、障害物検知部および移動装置と、計算部とを有し、前記計算部は、前記状態量検出部が検出した前記移動体の現在の状態量と、前記障害物検知部が検知した障害物の情報を受け取る機能と、前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する機能と、前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する機能と、前記マージン関数を前記障害物回避制約関数に加えた関数を制約として、モデル予測制御を行って、前記移動装置の最適制御量を求める機能と、前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える機能と、を備える構成を有する移動体制御システムとする。

本発明の移動体の制御方法および移動体制御システムによれば、自機の状態量の制御に不確かさを有する移動体について、外乱が存在する環境下で障害物回避を行わせることができる。

移動体制御システムの第１実施形態を示す概要図である。 移動体が障害物回避を行う状態を示す概要図である。 外乱の方位角度の設定方法を説明するための図である。 第１実施形態の変形例を示す概要図である。

以下、本発明を実施するための形態を図面を参照して説明する。

［第１実施形態］
図１乃至図３は、移動体の制御方法および移動体制御システムの第１実施形態として、本発明を、移動体としての水中移動体に適用した例を示すものである。また本実施形態では、移動体に働く外乱は、水中移動体に働く潮流とした例を示すものである。本実施形態では、水平方向の２次元座標の空間内で、水中移動体に障害物回避を行わせる場合について説明する。

図１は、移動体制御システムの第１実施形態を示す概要図である。図２は、移動体の制御方法により設定される障害物回避のための移動経路を示すもので、図２（ａ）は、本実施形態により設定される移動経路を示す概要図、図２（ｂ）は、比較例として、外乱の方向を考慮しない場合に設定される移動経路を示す概要図である。図３は、潮流の方位角度の設定方法を説明するための概要図である。

本実施形態の移動体制御システムは、図１に示すもので、水中移動体１に搭載された状態量検出部２と、障害物検知部３と、移動装置４と、計算部５と、記憶部６とを備えた構成とされている。

状態量検出部２は、水中移動体１の自機の位置と、移動する速度と、自機が向いている方位を含む姿勢と、自機の向く方位の変化に関する角速度とを、状態量として検出する機能を備えている。この機能を実現する状態量検出部２の構成例としては、たとえば、慣性航法装置を備えた構成や、慣性航法装置とドップラー式超音波速度計とを備えた構成とすればよい。

状態量検出部２は、水中移動体１の自機の位置や姿勢は、地球表面に固定された二次元の座標系における座標として検出するようにすればよい。なお、この二次元の直交座標系としては、緯度と経度とによる座標系を用いることが好適であるが、それに限定されるものではない。

なお、状態量検出部２は、前記状態量の検出機能を実現することができれば、前記した以外の任意の機器構成を採用してもよいことは勿論である。

状態量検出部２は、状態量の検出を行うと、その検出結果を計算部５へ送るようにしてある。

障害物検知部３は、水中移動体１が移動するときに、図２（ａ）に示すように自機の進行方向の前方に位置していて、自機の移動の障害となる障害物７を検知する機能を備えている。また、水中移動体１がウェイポイント制御のように設定された移動経路に沿い移動する場合は、障害物検知部３は、移動経路における水中移動体１が現在位置している地点よりも下流側の移動経路上に存在する障害物７を検知する機能を備えている。これらの機能を実現する障害物検知部３の具体的な構成例としては、たとえば、音響ソナーを備えた構成とすればよい。なお、障害物検知部３は、前記のような障害物７の検知機能を備えていれば、音響ソナーと他の検知手段（センサ）とを組み合わせて備えた構成としてもよいし、音響ソナー以外の検知手段を備えた構成としてもよい。

障害物検知部３は、障害物７の検知を行うと、検知された障害物７の情報を、計算部５へ送る機能を備えている。なお、この際、障害物検知部３は、障害物７について、自機の現在位置を基準とする相対位置の情報を計算部５へ送るようにすればよい。この場合、計算部５では、障害物検知部３より送られる障害物７の情報と、状態量検出部２より送られる自機の位置と姿勢の情報とを基に、地球表面に固定された二次元の座標系における障害物７の位置座標を計算により求めることができる。なお、この計算の機能を障害物検知部３が備えて、計算により求めた障害物７の位置座標の情報を、計算部５に送るようにしてもよい。

移動装置４は、水中移動体１に備えたスラスタや舵などのアクチュエータであり、この移動装置４の制御出力により、前記した水中移動体１の位置、速度、姿勢、角速度の状態量が変化する。

移動装置４は、計算部５より指令（制御入力）を受け取ると、その指令に従って運転、動作する機能を備えている。

なお、移動装置４は、水中移動体１の前記状態量を変化させることができれば、移動装置の形式や、水中移動体１における配置、数などは任意の構成としてもよいことは勿論である。

記憶部６は、水中移動体１の自機の移動経路を記憶する機能と、移動経路が設定された環境に存在する潮流Ｔ（図２（ａ）参照）の方向に関するデータ（以下、潮流方向データという）を記憶する機能とを備えている。

水中移動体１の移動の制御を、たとえば、ウェイポイント方式で行う場合は、記憶部６は、移動経路をウェイポイントファイルで記憶するようにすればよい。なお、記憶部６で水中移動体１の移動経路を記憶する形式は、ウェイポイントファイルに限られるものではなく、水中移動体１の移動を制御する方式に合わせて適宜変更してよいことは勿論である。

記憶部６が記憶する潮流方向データは、たとえば、海上保安庁のデータベースなどで海域ごとに公開されている潮流Ｔに関する情報から取得することが好適であるが、水中移動体１の移動経路が設定された環境についての潮流方向データであれば、他の情報源によるものであってもよい。

計算部５は、状態量検出部２から水中移動体１の状態量の検出結果を受け取ると共に、障害物検知部３で検知された障害物７の情報を障害物検知部３から受け取ると、以下の処理により移動装置４へ指令を与える機能を備えている。

計算部５には、事前に求められた水中移動体１の状態遷移方程式が、（１）式として与えられる。

ｘ_ｉ＋１＝ｆ（ｘ_ｉ，ｕ_ｉ） ・・・（１）
ただし、ｘ_ｉは水中移動体の状態量、ｕ_ｉは制御量（移動装置４の制御出力）、ｉは離散時間

この状態で、計算部５は、状態量検出部２から水中移動体１の自機の現在の状態量の検出結果を受け取ると、現在の状態量をｘ_１とする。

また、計算部５は、障害物検知部３より障害物７の情報を受け取ると、検知された障害物７の位置とサイズを基に、障害物７の中心位置をｏ_１、サイズを半径Ｄ_Ｍａｘの球または円で近似する。

更に、計算部５は、記憶部６より、図２（ａ）に示すように、水中移動体１の現在位置からの移動目標となる目標位置８に関する情報を取得して、水中移動体１が目標位置８に存在するときの状態量である目標状態量ｘ^ｄを得る。

更にまた、計算部５は、記憶部６より、水中移動体１の現在位置に関する潮流方向データを取得して、図３に示すように、水中移動体１の現在位置での潮流方向角度φを得る。図３では、潮流方向角度φは、たとえば、状態量検出部２で用いるのと同じ地球表面に固定された二次元の座標系にて、潮流Ｔの方向が、基準（０度）とするＸ軸の正方向から、反時計回り方向に傾斜する角度で設定してある。なお、潮流方向角度φは、状態量検出部２で用いるのと同じ地球表面に固定された二次元の座標系で、水中移動体１に作用する方向を明確に定義することができれば、任意の方向を基準に設定してもよいことは勿論である。

次に、計算部５は、前記の各情報を基に制御量の計算を行う。

計算部５における制御量の算出には、モデル予測制御を利用する。モデル予測制御では、以下の（２）式に示す最適化問題を解くことで、次の移動装置４の最適制御量行列ｕを得る。

なお、最適化問題の解法は、ニュートン法など、従来一般的に用いられている解法を用いればよい。

前記（２）式におけるＪ（ｕ）は、評価関数、ｈ_ｉ（ｕ）は衝突回避制約関数であり、それぞれ以下の（３）式、（４）式で記述される。

ただし、ｇ_ｉ（ｕ）は、障害物回避制約関数であり、ｇ_ｉ（ｕ）＝Ｄ_Ｍａｘ−Ｄ_ｉ＞０で記述される。
Ｄ_ｉは、ｉステップ目の水中移動体１の位置と障害物７の中心位置ｏ_１との間の距離である。ｉステップ目の水中移動体１の位置の座標を（Ｘ_ｉ，Ｙ_ｉ）とし、障害物７の中心位置ｏ_１の座標を（Ｘ_ｏ，Ｙ_ｏ）とすると、Ｄ_ｉは、以下の式で記述される。

なお、前記（３）式で示した評価関数は、一例であり、この形式以外にも、モデル予測制御で一般的に使用される他の評価関数を採用してもよい。また、前記した障害物回避制約関数は、水中移動体１が障害物７に衝突しない条件の一例であり、障害物７のサイズを近似する形状が球や円以外の場合は、その形状に応じて、ｉステップ目の水中移動体１の位置が障害物７と干渉しない条件を表す関数を適宜採用すればよい。

水中移動体１の状態量ｘ_ｉは、前記（１）式で示した状態遷移方程式と、状態量検出部２で検出した現在の状態値ｘ_１と、最適制御量行列ｕとから求める。

更に、前記（４）式におけるσ_ｉは、水中移動体１の状態量の不確かさに応じて算出されるマージン関数（マージン項）である。

次に、σ_ｉの導出方法について説明する。

σ_ｉは、障害物回避制約関数の偏微分行列Ｂ_ｉ（ｉ＝０…Ｎ_ｐ）と、水中移動体１の誤差分散行列ＣｏｖＸ_ｉ（ｉ＝０…Ｎ_ｐ）とから求めることができる。

障害物回避制約関数の偏微分行列Ｂ_ｉ（ｉ＝０…Ｎ_ｐ）は、次の（５）式で示される。

また、水中移動体１の誤差分散行列ＣｏｖＸ_ｉ（ｉ＝０…Ｎ_ｐ）は、次の（６）式で示される。

ここで、前記（６）式のｄτは、１ホライズン当たりの時間である。また、Ａ_ｉは、前記（１）式の偏微分行列であり、以下の（７）式で与えられる。

前記（６）式のＮ_ｐｒｏｃは、水中移動体１のプロセスノイズであり、カルマンフィルタなどの一般的な状態推定手法で設定されるプロセスノイズと同じ値でよい。

前記（６）式のＲ（φ）は、回転行列であり、以下の式（８）で表される。

前記（６）式のＮ_ｆは、潮流に関するプロセスノイズである。Ｎ_ｆは、たとえば、想定される潮流速度をｖ_ｆとして、以下の（９）式で与えられる。

なお、ｖ_ｆは、水中移動体１を移動させる領域についての潮流速度が得られれば、それに応じて値を適宜変化させることができるため、制御性能の向上化につながる。しかし、水中移動体１を移動させる領域についての潮流速度は、未知な状態であっても構わない。このように潮流速度が未知の場合は、設定値の指針として、ｖ_ｆは実際の潮流速度よりも大きな値に設定しておけば、水中移動体１が障害物７と衝突する可能性の低減化を図ることができる。したがって、外乱としての潮流Ｔは、潮流方向角度φが一定方向に定まっていれば、潮流速度は一定であってもよいし、たとえば波の影響などにより、周期的な変化を生じていてもよい。更には、潮流速度の周期的な変化としては、潮流方向角度φが一定で、強度がプラス方向からマイナス方向に変化するようなものを含んでいてもよい。

また、前記式（９）におけるαは、図２（ａ）に破線で示す楕円の長軸と短軸との比率を意味するパラメータであり、範囲は、０＜α＜１である。したがって、αが小さいほど短軸に対して長軸が長くなる。

前記（５）式から（９）式を基に、σ_ｉは、以下の（１０）式で定義することができる。

このように、本実施形態では、σ_ｉを前記（１０）式で定義することにより、水中移動体１の状態量の不確かさに応じた適切なマージンを設定することができる。

計算部５は、以上の計算に基づいて、前記（２）式に示した最適化問題を解いて移動装置４の最適制御量行列ｕが求まると、その第１成分ｕ_１を、移動装置４の制御量の目標値として移動装置４へ与える処理を行う機能を備えている。

なお、前記においては、ｘ_ｉは位置・姿勢、速度、角速度の６変数を状態量としたが、これは一例であり、本実施形態では、状態量は、変数の数には依存しない。

以上の構成としてある本実施形態の移動体制御システムを用いて水中移動体１の制御方法を実施すると、水中移動体１は、図２（ａ）に示すように、目標位置８に向かう移動経路の移動中に、障害物検知部３（図１参照）によって進行方向の前方に存在している障害物７を検知すると、計算部５が前記した処理を行い、その結果得られた移動装置４の最適制御量行列ｕの第１成分ｕ_１を、移動装置４の制御量の目標値として移動装置４へ与える。

したがって、移動装置４は、計算部５より受け取った制御量の目標値に応じて運転や動作が制御される。

これにより、図２（ａ）に示すように、ｉステップ目の水中移動体１の位置ｐ_ｉ（ｉ＝０…Ｎ_ｐ）については、それぞれ図２（ａ）に破線の楕円で示すような状態量の不確かさを考慮し、更に潮流に関するプロセスノイズを考慮した移動範囲９が設定され、この移動範囲９が、障害物７に重ならない移動経路を予測して設定することができる。

したがって、本実施形態の移動体の制御方法および移動体制御システムによれば、自機の状態量の制御に不確かさを有する水中移動体１について、外乱が存在する環境下で障害物７の回避を行わせることができる。

なお、図２（ｂ）は、比較として、潮流に関するプロセスノイズＮ_ｆについて、潮流方向角度φに基づく調整を行わない場合、すなわち、潮流に関するプロセスノイズＮ_ｆを全方位に考慮した場合のもので、この場合、ｉステップ目の水中移動体１の位置ｐ_ｉ（ｉ＝０…Ｎｐ）について状態量の不確かさを考慮した移動範囲９ａは、図２（ｂ）に破線で示すように円になる。これは、潮流方向と直交する方向にも移動範囲９ａを拡大していて、不要な領域にまでプロセスノイズを見積もっていることを示しており、状態量の不確かさを過剰に見積もっていることになる。その結果、移動範囲９ａが、障害物７に重ならないように設定される移動経路は、障害物７をより大きく迂回するものとなり、図２（ｂ）に示すように、障害物７と目標位置８との距離によっては、水中移動体１が目標位置８に到達する移動経路を設定することが困難になる可能性もある。

これに対し、本実施形態では、図２（ａ）に示すように、潮流に関するプロセスノイズを考慮した移動範囲９が、潮流方向に沿う方向にのみ範囲を広げた楕円形になる。したがって、本実施形態では、水中移動体１の移動経路は、潮流方向と直交する方向に関しては、より障害物７に接近した配置で設定することができる。このため、本実施形態では、障害物７との衝突回避のために迂回する距離の短縮化を図ることができる。

また、本実施形態によれば、得られる情報からの状態量の不確かさの見積もりを、より小さく抑えることができるため、ロバスト性能と制御性能の両立を図ることができる。

なお、本実施形態では、水平方向の２次元座標の空間内で、水中移動体１に障害物７の回避を行わせる場合について説明したが、本実施形態の移動体の制御方法および移動体制御システムは、３次元座標の空間内で、水中移動体１に障害物７の回避を行わせる場合に適用してもよい。

この場合は、水中移動体１の状態遷移方程式を、３次元座標空間についての状態遷移方程式として設定すると共に、障害物７を、中心位置をｏ_１、サイズを半径Ｄ_Ｍａｘの球で近似するようにすればよい。更に、潮流方向角度φは、３次元座標空間で設定するようにすればよい。

また、本実施形態では、潮流方向角度φを、記憶部６に記憶した潮流方向データから求めるものとして説明したが、水中移動体１の対地速度と対水速度との差や、その他、従来採用あるいは提案されている潮流の推定手法を用いて、潮流方向角度φを推定するようにしてもよい。

［第１実施形態の変形例］
図４は、第１実施形態の変形例として、移動体制御システムの別の構成を示す概要図である。

なお、図４において、第１実施形態と同一のものには同一符号を付して、その説明を省略する。

本変形例の移動体制御システムは、図４に示すように、第１実施形態の移動体制御システムと同様の構成において、水中移動体１に計算部５と記憶部６を備える構成に代えて、計算部５および記憶部６を、水中移動体１の外部の管制局１０に備える構成としたものである。

管制局１０は、たとえば、船舶や水上移動体に備える水上局や、地上局である。

更に、水中移動体１と管制局１０は、相互通信を行うための通信機１１、通信機１２を備えた構成とされている。

通信機１１と通信機１２との通信方式は、超音波などの音響信号による音響通信であってもよいし、光ファイバを介した光信号や電線を介した電気信号による有線通信であってもよいし、その他任意の通信方式を採用してもよいことは勿論である。

更に、管制局１０が地上局の場合は、音響通信や有線通信で水中移動体１と直接通信を行うことは困難なため、この場合は、管制局１０と水中移動体１との間の通信を、船舶や水上移動体に備えた中継器で中継させるようにすればよい。

本変形例の移動体制御システムでは、水中移動体１は、状態量検出部２による状態量の検出結果、および、障害物検知部３により検知された障害物７の情報を、通信機１１と通信機１２を介して、管制局１０の計算部５へ送る。

管制局１０の計算部５は、水中移動体１から状態量の検出結果、および、障害物７の情報を受け取ると、記憶部６より水中移動体１の現在位置に関する潮流方向データを取得して、第１実施形態における計算部５の処理と同様の処理を実行する。

その後、管制局１０は、計算部５の処理の結果得られた移動装置４の最適制御量行列ｕの第１成分ｕ_１を、移動装置４の制御量の目標値として、通信機１２と通信機１１を介して水中移動体１へ送る。

水中移動体１では、管制局１０より受け取った移動装置４の制御量の目標値を、移動装置４へ与える。

これにより、本変形例によっても、第１実施形態の移動体制御システムと同様に使用して同様の効果を得ることができる。

［第１実施形態の第１応用例］
前記第１実施形態では、障害物７は位置が固定されているものとして説明した。

これに対し、障害物７は、たとえば、同時に運用されている別の水中移動体などの移動する障害物７であってもよい。

この場合は、計算部５にて、ｉステップ目の水中移動体１の位置と障害物の中心位置ｏ_１との間の距離Ｄ_ｉを求める際に、ｉステップ目の障害物７の中心位置ｏ_１のＸ座標の値と、Ｙ座標の値を、障害物７の運動方程式に基づいて設定するようにすればよい。

たとえば、障害物７が別の水中移動体である場合は、その別の水中移動体の現在位置の情報を得ると共に、その現在位置と１ステップ前の時点での位置との差から、障害物７となる別の水中移動体の速度の情報を得る。次いで、得られた位置情報と速度情報とを、前記（１）式と同様の状態遷移方程式に代入することで、障害物７となる別の水中移動体のｉステップ目の位置の情報を得るようにすればよい。

別の手法としては、障害物７となる別の水中移動体と通信を行って、その別の水中移動体の位置、速度、姿勢、角速度の情報を取得し、それらの情報から、障害物７となる別の水中移動体のｉステップ目の位置の情報を得るようにしてもよい。

また、障害物７の運動方程式が明らかでない場合は、障害物検知部３による過去の複数のステップで検知された障害物７の位置の情報を基に、障害物７が等速直線運動を行うものと仮定して運動方程式を立て、その運動方程式に基づいて、障害物７のｉステップ目の時点での中心位置ｏ_１の座標を求めるようにすればよい。

本応用例によれば、第１実施形態と同様の効果を得ることができることに加えて、自機の状態量の制御に不確かさを有する水中移動体１について、外乱が存在する環境下で、移動する障害物７の回避を行わせることができる。

［第１実施形態の第２応用例］
前記第１実施形態では、水中移動体１の状態遷移方程式として、水中移動体の状態量であるｘ_ｉと、制御量であるｕ_ｉとを変数とする前記（１）式を示した。また、潮流の影響は、前記（６）式に、潮流に関するプロセスノイズＮ_ｆを導入することで考慮するものとした。

これに対し、水中移動体１の状態遷移方程式は、以下の（１Ａ）式のように、水中移動体１に加えて、水中移動体１の現在位置に存在する潮流の潮流方向角度φと潮流速度ｖ_ｆの影響を受けることを考慮して拡張させた状態遷移方程式として設定するようにしてもよい。

ｘ_ｉ＋１＝ｆ（ｘ_ｉ，ｕ_ｉ，ｖ_ｆ，φ） ・・・（１Ａ）

この場合は、計算部５では、第１実施形態と同様の処理を行う際に、水中移動体１の誤差分散行列ＣｏｖＸ_ｉ（ｉ＝０…Ｎ_ｐ）を示す（６）式を、Ｎ_ｆを省いた式として処理を行うようにすればよい。

本応用例によれば、第１実施形態と同様の効果を得ることができる。

［第１実施形態の第３応用例］
前記第１実施形態では、図２（ａ）に示したように、ｉステップ目の水中移動体１の位置ｐ_ｉ（ｉ＝０…Ｎ_ｐ）は、水中移動体１の状態量の不確かさと潮流に関するプロセスノイズを考慮した移動範囲９の楕円の中心を通るように設定されるものとした。

これに対し、ｉステップ目の水中移動体１の位置ｐ_ｉ（ｉ＝０…Ｎ_ｐ）は、前記移動範囲９の中心からずれた位置を通るように設定してもよい。

この場合は、潮流方向角度φと潮流速度ｖ_ｆによって水中移動体が潮流Ｔの下流側へ移動させられると想定される量を、シフト量（Ｘｓ，Ｙｓ）として設定し、ｉステップ目の水中移動体１の位置と障害物の中心位置ｏ_１との間の距離Ｄ_ｉを求める式に対し、以下のような補正を加えるようにすればよい。

本応用例によれば、第１実施形態と同様の効果に加えて、水中移動体１は、潮流Ｔが存在する環境下では、潮流Ｔの下流側となる方向へ移動する可能性が、潮流Ｔの上流側となる方向へ移動する可能性よりも高いという点を考慮して、水中移動体１の障害物７を回避する移動経路を設定することができる。

［他の移動体へ応用］
前記第１実施形態とその変形例および応用例では、本発明の移動体の制御方法および移動体制御システムを、潮流が存在する環境下で移動する水中移動体に適用した例を示した。

これに対し、本発明の移動体の制御方法および移動体制御システムは、制御される状態量が不確かさを含み、しかも作用する力の方向が一定となる外乱を受ける移動体であれば、以下のように、水中移動体１以外の移動体に適用してもよい。

本発明の移動体の制御方法および移動体制御システムは、潮流や風を外乱として受ける自律制御型の水上移動体に適用してもよい。なお、水上移動体は、自動操縦を行う船舶を含むものとする。

この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、潮流と風による外乱が水上移動体に働く方向に置き換えるようにすればよい。

なお、水上移動体に備える状態量検出部は、全地球航法衛星システム（ＧＮＳＳ）の受信部を含んだ構成としてよい。また、水上移動体に備える障害物検知部および移動装置は、従来水上移動体に採用されているか、あるいは、採用することが提案されている障害物検知部および移動装置を適宜選定すればよいことは勿論である。

これにより、自機の状態量の制御に不確かさを有する水上移動体について、作用する力の方向が一定となる外乱が存在する環境下で障害物回避を行わせることができる。

本発明の移動体の制御方法および移動体制御システムは、風を外乱として受ける自律制御型の航空機に適用してもよい。なお、航空機は、自動操縦を行う有人の航空機、および無人航空機のいずれであってもよい。

この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、風による外乱が航空機に働く方向に置き換えるようにすればよい。

なお、航空機に備える状態量検出部は、全地球航法衛星システム（ＧＮＳＳ）の受信部を含んだ構成としてよい。また、航空機に備える障害物検知部および移動装置は、従来航空機に採用されているか、あるいは、採用することが提案されている障害物検知部および移動装置を適宜選定すればよいことは勿論である。

これにより、自機の状態量の制御に不確かさを有する航空機について、作用する力の方向が一定となる外乱が存在する環境下で障害物回避を行わせることができる。

本発明の移動体の制御方法および移動体制御システムは、太陽などからの電磁波の放射圧を外乱として受ける宇宙機に適用してもよい。

この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、電磁波の放射圧よる外乱が宇宙機に働く方向に置き換えるようにすればよい。

なお、宇宙機に備える状態量検出部、障害物検知部および移動装置は、従来宇宙機に採用されているか、あるいは、採用することが提案されている状態量検出部、障害物検知部および移動装置を適宜選定すればよいことは勿論である。

これにより、自機の状態量の制御に不確かさを有する宇宙機について、作用する力の方向が一定となる外乱が存在する環境下で障害物回避を行わせることができる。

本発明の移動体の制御方法および移動体制御システムは、傾斜のある軟弱な地面を走行する自律制御型のクローラ型や車輪型の自律走行車に適用してもよい。なお、自律走行車は、自動操縦を行う有人の自律走行車、および無人の自律走行車のいずれであってもよい。

この場合は、前記第１実施形態とその変形例および応用例に示したと同様の構成において、潮流方向角度φを、自律走行車が位置している地面の下向きの傾斜角度が最大となる方位に置き換えるようにすればよい。

なお、自律走行車に備える状態量検出部、障害物検知部および移動装置は、従来自律走行車に採用されているか、あるいは、採用することが提案されている状態量検出部、障害物検知部および移動装置を適宜選定すればよいことは勿論である。

これにより、走行時に軟弱な地面とクローラや車輪との間で滑りを生じると共に、地面の傾斜に従って滑り落ちる現象が生じるために、自機の状態量の制御に不確かさを有する自律走行車について、障害物回避を行わせることができる。

なお、本発明は前記実施形態とその変形例と応用例にのみ限定されるものではない。

本発明の移動体の制御方法および移動体制御システムは、制御される状態量が不確かさを含み、しかも作用する力の方向が一定となる外乱を受ける移動体であれば、前記した以外のいかなる移動体に適用してもよい。

たとえば、移動体に備えた状態量検出部における検出精度があまり高くないことに起因して、制御される状態量が不確かさを含むような移動体に適用してもよい。

その他、本発明の要旨を逸脱しない範囲内で種々変更を加え得ることは勿論である。

１ 水中移動体（移動体）、２ 状態量検出部、３ 障害物検知部、４ 移動装置、５ 計算部、７ 障害物

Claims (3)

1. 状態量検出部と、障害物検知部と、移動装置と、計算部とを備えた移動体の制御を行う移動体の制御方法であって、
   前記状態量検出部が前記移動体の現在の状態量を検出する処理と、
   前記障害物検知部が障害物を検知する処理と、を行い、
   更に、前記計算部は、
   前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する処理と、
   前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する処理と、
   前記マージン関数を前記障害物回避制約関数に加えた関数を衝突回避制約関数として評価関数を最少化する最適化問題を解くことにより前記移動装置の最適制御量を求める処理と、
   前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える処理とを行い、
   前記マージン関数を前記障害物回避制約関数の偏微分行列と前記移動体の誤差分散行列から求めること
   を特徴とする移動体の制御方法。
2. 前記移動体は、水中移動体とし、前記水中移動体に働く外乱は潮流とする請求項１記載の移動体の制御方法。
3. 移動体に設けられた状態量検出部、障害物検知部および移動装置と、
   計算部とを有し、
   前記計算部は、
   前記状態量検出部が検出した前記移動体の現在の状態量と、前記障害物検知部が検知した障害物の情報を受け取る機能と、
   前記障害物検知部で検知された前記障害物の位置の情報に基づいて、前記移動体が前記障害物に衝突しない条件として障害物回避制約関数を算出する機能と、
   前記移動体の状態遷移方程式に対する前記移動体の状態量の不確かさと、前記移動体に働く外乱を基にマージン関数を算出する機能と、
   前記マージン関数を前記障害物回避制約関数に加えた関数を衝突回避制約関数として評価関数を最少化する最適化問題を解くことにより前記移動装置の最適制御量を求める機能と、
   前記求められた最適制御量を、前記移動装置へ、該移動装置の制御量の目標値として与える機能と、
   前記マージン関数を前記障害物回避制約関数の偏微分行列と前記移動体の誤差分散行列から求める機能と、を備えること
   を特徴とする移動体制御システム。

Images