JP7450206B2 - 複数のビークルの移動制御方法、移動制御装置、移動制御システム、プログラム及び記録媒体 - Google Patents

Description

本開示は、複数のビークルの移動制御方法、移動制御装置、移動制御システム、プログラム及び記録媒体に関する。

複数のビークルを移動させる技術（Ｓｗａｒｍ技術）の一つとして、相互に干渉しない担当領域を設定し、移動体の自立制御を行い、それぞれのビークルで観測を行う方法が知られている（例えば、特許文献１）。

特開２０１９－７４９１８号公報

ここで、ビークルの移動制御としては、ビークルによって捜索対象を観測しつつ、捜索対象に対してビークルを移動させる制御がある。このような制御を行う場合、担当領域で分割すると、ビークルにより捜索対象が計測できない場合が生じる。特許文献１のシステムでで、ビークルを適切に移動させるためには、例えば、担当領域の重み付け処理等が必要になる。重み付け処理を状況に応じて実行するのは作業者の負担になる。また、捜索対象をビークルで観測する条件を満たすために、ビークルが実行できない動きを含む移動の制御が生じる場合があり、制御に矛盾が生じ、デットロックにより、ビークルの移動が制御できなくなることがある。

本開示は、上記の事情に鑑みてなされたものであり、ビークルによる捜索対象の観測を好適に維持しつつ、ビークルの移動を協調させることができる複数のビークルの移動制御方法、移動制御装置、移動制御システム、プログラム及び記録媒体を提供することを目的とする。

上述した課題を解決し、目的を達成するために、複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させる複数のビークルの移動制御方法であって、前記複数のビークルの各々の位置及び姿勢を取得するステップと、前記捜索対象の位置を特定するステップと、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出するステップと、算出した移動軌跡で前記複数のビークルが移動した場合、前記捜索対象の観測条件を満たしているか判定するステップと、前記観測条件を満たしていない場合、前記移動軌跡を算出するステップに戻るステップと、前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含む移動制御方法を提供する。

また、複数のビークルの移動を制御する移動制御装置であって、前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報と姿勢の情報を取得し、かつ、少なくとも１つの捜索対象の位置の情報を取得する通信部と、取得した前記ビークルの現在の位置の情報と姿勢の情報と少なくとも１つの捜索対象の位置の情報に基づいて、前記捜索対象の観測条件を満たす前記ビークルの移動軌跡を算出する制御部と、を含み、前記制御部は、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出し、算出した移動軌跡で前記複数のビークルが移動した場合、前記捜索対象の観測条件を満たしているか判定し、前記観測条件を満たしていない場合、再度前記移動軌跡を算出し、前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する移動制御装置を提供する。

また、複数のビークルと、複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させる移動制御装置と、を備える移動制御システムであって、前記移動制御装置は、前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報と姿勢の情報を取得する位置検出部と、少なくとも１つの捜索対象の位置の情報を取得する対象物検出部と、取得した前記ビークルの現在の位置の情報と姿勢の情報と少なくとも１つの捜索対象の位置の情報に基づいて、前記捜索対象の観測条件を満たす前記ビークルの移動軌跡を算出する制御部と、を含み、前記制御部は、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出し、算出した移動軌跡で前記複数のビークルが移動した場合、前記捜索対象の観測条件を満たしているか判定し、前記観測条件を満たしていない場合、再度前記移動軌跡を算出し、前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する移動制御システムを提供する。

また、複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させるプログラムであって、コンピュータに、前記複数のビークルの各々の位置及び姿勢を取得するステップと、前記捜索対象の位置を特定するステップと、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出するステップと、算出した移動軌跡で前記複数のビークルが移動した場合、前記捜索対象の観測条件を満たしているか判定するステップと、前記観測条件を満たしていない場合、前記移動軌跡を算出するステップに戻るステップと、前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を実行させるためのプログラムを提供する。

また、複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させるプログラムを記録したコンピュータ読み取り可能な記録媒体であって、コンピュータに、前記複数のビークルの各々の位置及び姿勢を取得するステップと、前記捜索対象の位置を特定するステップと、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出するステップと、算出した移動軌跡で前記複数のビークルが移動した場合、前記捜索対象の観測条件を満たしているか判定するステップと、前記観測条件を満たしていない場合、前記移動軌跡を算出するステップに戻るステップと、前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体を提供する。

本開示の少なくとも一実施形態によれば、ビークルによる捜索対象の観測を好適に維持しつつ、ビークルの移動を協調させることができる。

図１は、第１実施形態の移動制御装置を含む移動制御システムの主要構成を示すブロック図である。 図２は、ビークルと捜索対象との関係の一例を示す表である。 図３は、１つのビークルの観測範囲の一例を示す模式図である。 図４は、複数のビークルが各々の目標位置まで個別に移動する例を示す模式図である。 図５は、複数のビークルが各々の目標位置まで個別に移動する例を示す模式図である。 図６は、第１実施形態で移動制御システムが行う処理の流れを示すフローチャートである。 図７は、第２実施形態で移動制御システムが行う処理の一例を示す模式図である。 図８は、第４実施形態で移動制御システムが行う処理の一例を示す模式図である。 図９は、第４実施形態で移動制御システムが行う処理の一例を示す模式図である。 図１０は、第５実施形態で移動制御システムが行う処理の一例を示す模式図である。

以下に、図面に基づいて、本開示の幾つかの実施形態について詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能である。

［第１実施形態］
図１は、第１実施形態の移動制御装置１０を含む移動制御システム１の主要構成を示すブロック図である。移動制御システム１は、複数のビークルＢの移動経路を制御するシステムである。移動制御装置１０は、複数のビークルＢの移動を制御する。図１等では、複数のビークルＢの各々を区別する目的で、符号Ｂ１，Ｂ２，Ｂ３…を付している。ビークルＢは、地面を走行する移動体として、空中を飛行する移動体として、水中を移動する移動体としてもよい。したがって、ビークルＢは、３次元で移動できる移動体も含むが、以下では説明のため、２次元平面を移動する場合として説明する。

複数のビークルＢの各々は、位置検出部５１と、対象検出部５２と、通信部５３と、動力部５４とを備える。位置検出部５１は、当該位置検出部５１が設けられたビークルＢの位置を検出する。位置検出部５１の具体的構成例として、グローバル・ポジショニング・システム（ＧＰＳ：Global Positioning System）等の測位システムを利用して位置を検出するための測位装置が挙げられる。位置検出部５１は、所定の起点に対する位置を検出する慣性航法装置であってもよい。

対象検出部５２は、観測する対象である捜索対象を検出する。対象検出部５２は、画像、温度、音波、電波塔で対象物を検出する。対象検出部５２は、対象物から出力される情報を取得するセンサ、いわゆるパッシブセンサを用いることが好ましい。これにより、対象検出部５２は、捜索対象を検出するために出力波を出力必要が無くなり、装置構成を簡単にすることができる。

通信部５３は、移動制御装置１０と通信を行う。通信部５３の具体的構成例として、無線通信装置が挙げられる。通信部５３は、移動制御装置１０と有線通信を行う構成であってもよい。

動力部５４は、ビークルＢを移動させる動力として機能する。動力部５４の具体的構成は、ビークルＢの運用形態に応じる。一例として、ビークルＢが地上を走行するビークルである場合、動力部５４は、複数の車輪と、当該複数の車輪の一部又は全部を駆動する原動機を含む。ここに例示した動力部５４の具体的構成はあくまで一例であってこれに限られるものでない。動力部５４は、ビークルＢを移動可能にする動力として機能すればよい。

移動制御装置１０は、通信部２０と、制御部３０とを備える。通信部２０は、複数のビークルＢと通信を行う。移動制御装置１０とビークルＢとの通信は、位置検出部５１と通信部２０との通信によって行われる。通信部２０の具体的構成は、位置検出部５１と共通する。

制御部３０は、演算部３１と、記憶部３２とを備える。演算部３１は、ＣＰＵ（Central Processing Unit）等の演算回路を含み、複数のビークルＢの移動制御に関する各種の処理を行う。記憶部３２は、演算部３１の処理に用いられるソフトウェア・プログラム（以下、単にプログラムと記載）及びデータを記憶する。このプログラムは、記憶部３２に記憶されていてもよいし、コンピュータである移動制御装置１０が読み取り可能な記録媒体に記録されていてもよい。この場合、移動制御装置１０は、当該記録媒体からプログラムを読み出すための読出装置を備える。また、記憶部３２は、通信部２０を介して取得されたビークルＢに関する情報を記憶する。例えば、後述する複数のビークルＢの移動を制御する際に用いる制約条件、観測条件等を記憶する。また、後述する複数のビークルＢの捜索対象（例えば、捜索対象Ｔ１，Ｔ２，…，Ｔｍ）を示す情報は、記憶部３２に記憶されている。捜索対象は、ビークルＢが観察する対象や、目標位置とする場所である。捜索対象は、固定されていても、移動してもよい。

次に、図２から図６を用いて、移動制御システム１による複数のビークルの移動を説明する。図２は、ビークルと捜索対象との関係の一例を示す表である。図３は、１つのビークルの観測範囲の一例を示す模式図である。図４及び図５は、それぞれ複数のビークルが各々の目標位置まで個別に移動する例を示す模式図である。図６は、第１実施形態で移動制御システムが行う処理の流れを示すフローチャートである。

本実施形態の移動制御システム１は、複数のビークルＭで、少なくとも１つ以上の捜索対象（指定場所、指定位置、指定対象物）を捕捉しつつ、複数のビークルＭの移動を制御する。複数のビークルＢの各々は、位置検出部５１によって取得された位置を示す情報を、通信部５３を介して移動制御装置１０に送信する。さらに、複数のビークルＢの各々は、対象検出部５２によって取得された捜索対象を示す情報を、通信部５３を介して移動制御装置１０に送信する。ビークルＢは、捜索対象を示す情報として、捜索対象を捕捉したかと捜索対象との距離の情報を含んでも、捜索対象Ｔの位置情報を含んでもよい。記憶部３２は、複数のビークルＢの各々の位置を示す情報と、捜索対象Ｔの補足情報を累積的に記憶する。演算部３１は、複数のビークルＢの各々の位置を示す情報と、捜索対象を示す情報と、制約条件、観測条件に基づいて、複数のビークルＢの各々の移動経路を算出して評価し、評価した結果に基づいて移動経路を決定し、決定した移動経路に基づいて、複数のビークルＢの各々の制御入力を算出し、制御入力を通信部２０を介して複数のビークルＢに個別に送信する。ここで、制御入力は、単位時間で所定条件を満たしながら複数のビークルＢを各々の移動させるための移動方向及び移動速度を示す情報として機能する。ビークルＢは、制御入力に従って移動するように動力部５４を動作させる。また、記憶部３２は、複数のビークルＢの各々に送信された制御入力を累積的に記憶する。

本実施形態の移動制御システム１は、捜索対象Ｔを観測しているビークルＢが、設定された台数以上である状態を維持して、ビークルＢの移動を制御する。捜索対象Ｔが複数ある場合は、それぞれの捜索対象が設定されている。捜索対象Ｔを観測しているビークルＢの数は、ユーザが設定することができる。また捜索対象Ｔが識別可能な場合、捜索対象Ｔ毎に、捜索対象Ｔを観測しているビークルＢの数を設定してもよい。例えば、制約条件として、捜索対象Ｔを観測しているビークルＢの数を２台と設定した場合、捜索対象Ｔを観測しているビークルＢが２台以上であればよく、３代でも４台でもよい。

また、ビークルＢが、対象検出部５２で検出できる範囲に複数の捜索対象Ｔが含まれる場合、観測できる複数の捜索対象Ｔを当該ビークルＢで観測できる捜索対象とすることができる。２台以上のビークルが全ての捜索対象を同時に観測するとは、例えば、ビークルＢ１とビークルＢ２の両方の視野内（検出できる範囲）に捜索対象Ｔ１あり、ビークルＢ１とビークルＢ３の両方の視野内（検出できる範囲）に捜索対象Ｔ２がある。ここで、捜索対象が２つで、他にもビークルがあり、上記関係を満足している場合、例えば、ビークルＢ４は、視野内（検出できる範囲）に捜索対象がなくてもよい。

次に、制御対象のビークルＢとビークルＢの位置、姿勢と、捜索対象Ｔの位置と、ビークルＢに対して入力する移動の指示である制御入力との関係を説明する。まず、ビークルＢ_ｍの２次元平面における位置姿勢は、

と表すことができる。ここで，Ｍは、制御対象であるビークル群のビークルＢの台数を表し、ｋは離散時間のインデクスを表す。

次に、２次元平面におけるビークルＢｍの位置のみのベクトルは、

と表すことができる。

次に、捜索対象Ｔ_ｍの２次元平面の位置は、

と表すことができる。なお、本説明では、簡単のためビークルＢと、捜索対象Ｔの数を同じＭとし、識別文字をｍとしているが、数は、特に限定されない。ビークルＢは、２台以上あればよく、捜索対象Ｔは１つ以上あればよい。

次に、ビークルＢ_ｍへの制御入力は、ｕ_ｍ（ｋ）とする。制御入力は、一例として、速度指令値ｖ_ｍ（ｋ）［ｍ／ｓｅｃ］とヨーレート指令値φ_ｍ（ｋ）［ｒａｄ／ｓｅｃ］の組み合わせなどがある。なお、制御入力は、ビークルＢの移動を制御できる指令値であればよく、パラメータは必ずしもこれらに限定されるものではない。

以下では，制御入力ｕ_ｍ（ｋ）を、

とする。

ビークルＢ_ｍの現在位置姿勢がｐ´_ｍ（ｋ）とした場合、制御入力ｕ_ｍ（ｋ）が印加されると、ｋ＋１時点でのビークルＢ_ｍの位置は、ダイナミクスを表す関数ｈにより、

となる。

ここで、このダイナミクスを示す関数ｈは、本実施形態では２次元平面であるため、例えば、陸上を走行するビークルの場合

となる。ここで、Ｔ_ｃは、速度などを制御する際の制御周期を表す。上記は一例であり、空中ドローンや水中ビークルなど、ビークルの種類によって異なる。なお、技術は、ダイナミクスを示す関数ｈは、対象のビークルに基づいて、取得して適用すればよい。

移動制御システム１は、ビークルＢの位置姿勢情報ｐ´_ｍ（ｋ），ｍ＝１，２，３，・・・，Ｍと、捜索対象Ｔの位置情報ｑ_ｍ（ｋ），ｍ＝１，２，３，・・・，Ｍを入力とし、非ホロノミックなビークル群が対象検出部５２により捜索対象群を被覆するための、適切な制御入力ｕ_ｍ（ｋ），ｍ＝１，２，３，・・・，Ｍを算出する。

移動制御装置１０は、ビークルＢは、対象検出部５２のセンサが観測できる範囲の情報を有する。本実施形態では、ビークルＢは、図３に示すように、対象検出部５２の視野Ａがコーンの形状であり、その視野角が±βである。例えば、ビークルＢ_１は、視野Ａに捜索対象Ｔ_１が含まれる場合、捜索対象Ｔ_１が検出できる範囲にある捜索対象となる。

このとき、常に全ての捜索対象Ｔが同時に２台以上のビークルＢの視野内に入れる制約条件（観測条件）は、

（式１）
となる。なお、本実施形態では、下限値のみ不等号で示すが、等号で示したり、上限値を設定したりしてもよい。

ここで、各ビークルＢ_ｍと、各捜索対象Ｔ_ｍと、の関係は、図２に示す関係となり、まとめると下記となる。

である。また、Ｐ´（ｋ＋ｎ）＝［ｐ_１´^Ｔ（ｋ＋ｎ）・・・ｐｍ´^Ｔ（ｋ＋ｎ）］^Ｔであり、ｃはパラメータである。角度ψ_ｍｌは、ビークルＢ_ｍから見た捜索対象Ｔ_ｌの方位を表す。

本実施形態の移動制御装置１０は、先読を含んでおり、先読ステップ数(予測ホライズン)をＮ_ＰＨとする。移動制御装置１０は、上述のダイナミクスのモデルを用いて

として、ビークルＢ_ｍの将来の状態を予測する。関数ｆは、捜索対象Ｔ_ｌに対するビークル群全体の観測精度となる。個々の評価値Ｓ_ｍｌは、ビークルＢ_ｍが捜索対象Ｔ_ｌを観測できているかどうかを表している。観測できている（視野内に捜索対象が入っている）場合はＳ_ｍｌ＝１となり、観測できていない場合Ｓ_ｍｌ＝０となる。本実施形態体は、２台以上のビークルＢ_ｍが捜索対象Ｔ_ｌ，ｌ＝１，２，３，・・・，Ｍを同時に観測できている必要がある。図２において、各列に２台以上の“１”がある場合、条件を満足する。その条件を不等式の形で表現すると、上記制約条件の（式１）となる。

本実施形態は全ての捜索対象を同時に２台以上のビークルで観測する場合である。制約条件を一般化してＤ台以上のビークルに全ての捜索対象を同時観測させる制約である観測条件（制約条件）は、

となる。なお、捜索対象が検出できる範囲にあるビークルの台数であるＤは、全てのビークルの数Ｍ以下、つまりＭ≧Ｄである。

ここで、移動制御装置１０は、（式１）を制約条件としてビークル群で、全ての捜索対象が設定した台数のビークルで検出できる状態できるように、制約付き最適化問題を解いて制御入力の最適解ｕ_ｍ（ｋ）を求める。ここで、図４に示す移動制御システム１は、全ての捜索対象が２台以上のビークルで観測できており、制約条件を満足している状態である。これに対して、図５に示す移動制御システム１は、捜索対象Ｔ_３が観測可能なビークルが１台のみであるので、制約条件を満たしていない。移動制御装置１０は、ビークルＢの移動に制限がある場合、例えば、旋回角に制約がある場合、図４に示す位置にいると、移動の制約により、所定時間後に図５に示す状態になり、制約条件を満たす制御入力が算出できないデットロック状態となる。

本実施形態の移動制御装置１０は、先読量Ｎ_ＰＨを用いて、モデル予測制御の考え方に基づいて最適化問題

（式２）
を解き，制御入力の最適解の系列ｕ_ｍ（ｋ），ｕ_ｍ（ｋ＋１），・・・，ｕ_ｍ（ｋ＋Ｎ_ＰＨ－１），ｍ＝１，２，３，・・・，Ｍを求める。ここで、将来の予測位置姿勢Ｐ´（ｋ＋ｎ），ｎ＝１，２，３，・・・，Ｎ_ＰＨ－１は、制御入力の関数である。なお、この最適化問題の解法としては，逐次二次計画法や内点法などの公知技術を用いることができる。

このように、移動制御装置１０は、先読量（先読ステップ分）Ｎ_ＰＨを用いて、上記の式（１）の制約条件を満たすように、捜索対象Ｔの位置に基づいて、先読量分のビークルＢの移動経路を算出し、算出した移動経路に基づいて、単位時間分のビークルＢの移動を算出し、算出結果に基づいて、ビークルＢに対する制御入力を算出する。

以下、図６を用いて、移動制御装置１０で実行する処理の一例を説明する。移動制御装置１０は、複数のビークルＢの位置、姿勢を検出する（ステップＳ１２）。位置制御装置１０は、通信部７０を介して、ビークルＢから位置検出部５１で検出した位置、姿勢の情報を取得する。次に、移動制御装置１０は、複数の捜索対象の位置を計測する（ステップＳ１４）。移動制御装置１０は、複数のビークルＢの対象検出部５２で検出した捜索対象の情報を処理して、捜索対象の位置を計測する。なお、移動制御装置１０は、捜索対象の位置が既知の場合、既知の情報を用いて捜索対象の位置を計測結果としてもよい。また、移動制御装置１０は、ビークルＢ以外の機器から捜索対象Ｔの位置の情報を取得し、計測結果としてもよい。

次に、移動制御装置１０は、ｌ＜Ｍかを判定する（ステップＳ１６）。ここで、ｌは、捜索対象の識別子である。Ｍは、本実施形態では捜索対象の全数である。ｌは、処理開始時は、１となる。移動制御装置１０は、ｌ＜Ｍである（ステップＳ１６でＹｅｓ）、つまり全ての捜索対象の処理が完了していないと判定した場合、ビークルの番号を初期化、つまりｍ＝１とする（ステップＳ１８）。次に、移動制御装置１０は、ｍ＜Ｍかを判定する（ステップＳ２０）。移動制御装置１０は、ｍ＜Ｍである（ステップＳ２０でＹｅｓ）、つまり全てのビークルの処理が完了していないと判定した場合、ビークルｍから見た捜索対象ｌの方位ψ_ｍｌを計算または計測し（ステップＳ２２）、ビークルｍによる捜索対象ｌの観測成否を計測し（ステップＳ２４）、ビークル番号を更新、つまりｍ＝ｍ＋１する（ステップＳ２６）。ここで、ビークルが、捜索対象が観測できるかは、ｓ_ｍｌ＝ｔａｎｈ｛ｃ（β－ψ_ｍｌ）｝で算出できる。

次に、移動制御装置１０は、ｍ＜Ｍではない（ステップＳ２０でＮｏ）、つまり、対象の捜索対象について全てのビークルの処理が完了していると判定した場合、群全体による捜索対象の観測成否計算の計算を行う（ステップＳ３０）。つまり、捜索対象を検出できる範囲にあるビークルが何台あるかを計算する。移動制御装置１０は、計算が完了したら、捜索対象の番号を更新し、つまりｌ＝ｌ＋１とし、ステップＳ１６に戻る。移動制御装置１０は、ステップＳ１２からステップＳ３２の処理を繰り返し計算で行うことで、捜索対象とビークルの関係を算出する。

移動制御装置１０は、ｌ＜Ｍではない（ステップＳ１６でＮｏ）、つまり全ての捜索対象の処理が完了していると判定した場合、（式２）の制限付き最適化問題を解き、最適な制御入力Ｕ（ｋ）を算出する（ステップＳ４０）。移動制御装置１０は、先読ステップ数Ｎ_ＰＨを用いて、制約条件を満たすビークルの移動を算出する。つまり、それぞれの捜索対象を設定した数、本実施形態では２台のビークルで計測し続けることができるビークルの移動軌跡を算出する。移動制御装置１０は、算出結果に基づいて、単位時間分のビークルの制御入力を算出する。単位時間は、先読ステップ数分に対応する時間よりも短い時間である。移動制御装置１０は、実際に制御入力で移動させる時間よりも、（先読ステップ数－単位時間）長い時間の移動経路を算出し、算出結果から単位時間分の移動の制御入力を算出する。

移動制御装置１０は、算出した制御入力Ｕ（ｋ）を速度、ヨーレートの制御指令値として、各ビークルに送信する（ステップＳ４２）。各ビークルは、入力された制御指令値に基づいて、ビークルの速度・姿勢を制御し（ステップＳ４４）、単位時間経過後、時間を更新する（ステップＳ４６）。移動制御装置１０は、処理終了かを判定し（ステップＳ４８）、処理終了ではない（ステップＳ４８でＮｏ）と判定した場合、ステップＳ１２に戻り、処理終了である（ステップＳ４８でＹｅｓ）と判定した場合、本処理を終了する。

移動制御システム１は、モデル予測制御の考え方を用い、先読ステップ数(予測ホライズン)をＮ_ＰＨとし、捜索対象を観測できるビークルの数の制約条件をビークル群の将来位置で満たすかを予測（先読）して、移動経路を算出し、算出した移動経路に基づいて、単位時間分の移動を決定する。これにより、ビークルが移動した後、次に移動する場合に、ビークルの移動可能な領域の移動で、制約条件を満たす移動ができない、デットロックが発生することを抑制できる。また、制約条件として、捜索対象を観測できる範囲にあるビークルの数を用いることで、制約条件を調整するのみで、１つの捜索対象を観測するのに必要なビークルの数を，ユーザが容易に指定できる。

また、本実施形態では、最適化計算時に捜索対象の位置を検出した位置としたが、捜索対象の移動を予測し、捜索対象の位置を経過時間に応じて移動させるようにしてもよい。この場合、過去の捜索対象の移動軌跡に基づいて予測してもよいし、所定の条件で移動させるようにしてもよい。また、捜索対象は固定してもよい。

［第２実施形態］
次に、第２実施形態について説明する。第１実施形態と同様の構成については、同じ符号を付して説明を省略する。特筆する事項を除いて、第２実施形態は、第１実施形態と同様である。図７は、第２実施形態で移動制御システムが行う処理の一例を示す模式図である。

第２実施形態の移動制御装置１０は、評価関数Ｊ（Ｕ（ｋ）、Ｕ（ｋ）、・・・、Ｕ（ｋ＋Ｎ_ＰＨ＋１））として、ビークル群がそれぞれの捜索対象に同時到達することを条件とする。第２実施形態の移動制御装置１０は、制約条件の台数のビークルが捜索対象を観測しながら、それぞれのビークルの捜索対象に、当該ビークルが同時に到達することができる。ビークルが捜索対象に到達させるために、ビークルと捜索対象の相対距離が、目標時間時点で０にする必要がある。移動制御装置１０は、評価関数として

（式３）
を用いる。

本実施形態のモデル予測制御の評価関数は、第１実施形態と同様に、先読処理を行うため、先読ステップＮ_ＰＨを含む。具体的には、Ｔｍ（ｋ＋Ｎ_ＰＨ）は、Ｕ（ｋ）、Ｕ（ｋ）、・・・、Ｕ（ｋ＋Ｎ_ＰＨ＋１）の関数である。

また、本実施形態の移動制御装置１０は、各ビークルが捜索対象に到達時間も調停するために、制約条件に

（式４）
を含む。ここで、Ｔ_ｍ，Ａ（ｋ＋ｍ）は、ビークルＢ_ｍの捜索対象Ｔ_ｍへの到達予想時刻である。Ｔ_ｍ，Ａ（ｋ＋ｍ）は、現在時刻ｔ_ｎｏｗを用いて、

となる。Ｔ_ｓｅｔは、ユーザが指定したビークル群の捜索対象への同時到達の目標時刻である。

本実施形態の移動制御システム１は、評価関数に（式３）を用い、第１実施形態の（式１）の制約条件と到達タイミングに関する（式４）の制約条件を設定することで、ビークル群全体が捜索対象を常に観測し続けながら、デッドロックになることを抑制しつつ、捜索対象位置へ指定した時刻に到達できるようになる。

本実施形態の評価関数は、途中の経路の評価を行わず、先読の最終到達地点（目標時間時点）と捜索対象位置の偏差のみを評価する関数となる。これにより、途中の経路の自由度が高くなり、デットロックの発生をより抑制することができる。一例として、途中経路では、捜索対象を観測し易い位置へビークルが移動するために、敢えて遠回りをするなどの動作も生成される。具体的には、図７に示すように、途中の経路も評価すると、より早く捜索対象に近づくために、経路１０２の評価が高くなるが、本実施形態では、途中の経路を評価しないことで、移動の自由度が高くなり、先読処理時に、捜索対象がより確実観測でき、また、ビークルの移動の制約の範囲で移動できる経路１０４を算出することができる。

［第３実施形態］
次に、第３実施形態について説明する。第１実施形態と同様の構成については、同じ符号を付して説明を省略する。特筆する事項を除いて、第３実施形態は、第１実施形態と同様である。

本実施形態の移動制御システム１は、第１実施形態、第２実施形態の評価関数・制約条件に加えて、ビークル同士の衝突回避の条件、制御入力の上下限の条件を制約条件として含む。

本実施形態の移動制御システム１は、ビークルの衝突を回避するために、将来予測における各時刻でそれぞれの相対距離が閾値ｄ以上となるに制約条件を含む。具体的には、

が制約条件として、最適化問題に含まれる。

また、本実施形態の移動制御システム１は、ビークルが制御入力に対応して、算出した軌跡で移動できるように、制御入力に対する制約条件を含む。移動制御システム１は、制御入力の制限値として、ビークルの速度やヨーレートなど上下限が設定される。これにより、過大な制御入力が出力されビークルを故障すること、飛行するビークルの場合に降下してしまうことをすることができる。制御入力に対する制約条件としては、

がある。ここで、ｖ_ＭＩＮとｖ_ＭＡＸは、それぞれ速度指令の下限と上限を表す。下限がｖ_ＭＩＮ＞０であれば、ビークルを停止させないように最適な制御入力を生成することができる。また、φ_ＭＩＮとφ_ＭＡＸは、それぞれヨーレート指令の下限と上限を表す。左旋回と右旋回で同等の制約を課す場合は、φ_ＭＩＮ＝－φ_ＭＡＸとすれば良い。

これらの制約条件も最適化問題に含み、これらの条件に基づいて最適化問題を解くことで、制約条件を満足する制御を実行することができる。移動制御システム１は、ビークルの相対的な位置や、ビークルの移動性能に基づいて、各種制約条件を設定することができる。移動制御システム１は、最適化問題の制約条件に追加することで、ビークルの移動軌跡を先読で算出する際に、制約条件を満たしたビークルの移動軌跡を算出するｔ子尾ができる。

［第４実施形態］
次に、第４実施形態について説明する。第１実施形態と同様の構成については、同じ符号を付して説明を省略する。特筆する事項を除いて、第４実施形態は、第１実施形態と同様である。図８は、第４実施形態で移動制御システムが行う処理の一例を示す模式図である。図９は、第４実施形態で移動制御システムが行う処理の一例を示す模式図である。

本実施形態の移動制御システム１は、予測ホライズンＮ_ＰＨを用いて算出した移動経路の地点間の情報に基づいて、経路の隣接する地点、一例として予測ホライズンＮ_ＰＨを用いて算出した移動経路の現時点から次の地点までの経路を並行で計算する。なお、本実施形態は、第２実施形態と組み合わせ実施することが好ましい。第２実施形態の移動制御システム１は、同時到達の制約条件を含むため、捜索対象位置までの完全な経路を生成する必要がある。そのため、予測ホライズンＮ_ＰＨが大きくなり、制約付き最適化問題を解く際の処理時間が長くなってしまう場合がある。処理時間を短くするためには、単位時間（先読ステップの１ステップの間隔）を長く設定することになり、移動制御の精度が低下することを抑制できる。

本実施形態の移動制御システム１は、各ビークルの将来位置予測の

のステップｋ，ｋ＋１，ｋ＋２，・・・の間隔をＴ_ＭＰＣ［ｓｅｃ］とする。本実施形態で用いる時間間隔Ｔ_ｃは、Ｔ_ＭＰＣ＞＞Ｔ_Ｃを満たす。また、ダイナミクスｈを用いて将来の状態を予測する際、Ｔ_ＭＰＣ［ｓｅｃ］間は、同じ制御入力をビークルに印加し続けるものと仮定する。すなわち、

とダイナミクスを設定し、将来位置を予測する。

移動制御システム１は、第１実施形態から第３実施形態と同様に、上記ダイナミクスを用いて，時間間隔Ｔ_ＭＰＣで捜索対象位置までの経路を計画する。時間間隔Ｔ_ＭＰＣで算出した捜索対象位置までの経路の計画を「グローバル経路計画」とする。またＰ´（ｋ＋ｎ）＝［Ｔ１´^Ｔ（ｋ＋ｎ）・・・Ｔｍ´^Ｔ（ｋ＋ｎ）］^Ｔを、「ウェイポイント」とする。

グローバル経路計画は、ウェイポイントでの（式１）の捜索対象の観測の条件や、各種制約条件を満足するが、ウェイポイント間の移動中に条件を満たさない場合が含まれる可能性がある。

本実施形態の移動制御システム１は、上述した時間間隔Ｔ_ｃで動作するローカルシステムを用い、グローバル経路計画のウェイポイント間の移動時の制御入力を作成し、ビークルを制御する。以下、移動制御システム１の移動制御装置１０のローカルシステムの処理について説明する。なお、本実施形態では、ローカルシステムの処理を移動制御装置１０で実行するとしたが、ローカルシステムの処理は、各ビークルで実行してもよい。

移動制御装置１０は、グローバル計画で用いる先読ステップを含む処理を行わずに、移動経路を算出し、制御入力を算出する。これにより、計算の処理量を低減し、演算処理を早くすることができる。移動制御装置１０は、ローカルシステムの目標位置を、捜索対象位置ではなく、次のウェイポイントとする。ウェイポイントは、Ｐ´（ｋ＋ｎ）＝［Ｔ１´^Ｔ（ｋ＋ｎ）・・・Ｔｍ´^Ｔ（ｋ＋ｎ）］^Ｔと記載しているように、各ビークルの単位時間ごとの目標位置となる。したがって、移動制御装置１０は、単位時間経過後にウェイポイントに到着することが制約条件となる。

移動制御装置１０は、評価関数を

とする。これにより、グローバル経路計画が生成した経路に可能な限り沿ってビークル群を移動させる経路の評価が高くすることができ、ビークルがグローバル経路計画が生成した経路から大きく逸脱することを抑制できる。

ここで、ｗ_ｍ ^τは、

である。下位制御周期Ｔ_ｃ[ｓｅｃ]毎にτを０からＴ_Ｃ／Ｔ_ＭＰＣに更新していき，次のウェイポイントにビークル群を移動させる。上記式に示す通り、最適化の対象は、次の制御入力のみである。したがって、上述したようにローカルシステムは、先読は行わない。

移動制御装置１０は、ローカルシステムもウェイポイント間の移動時も常に捜索対象を観測させるため、グローバル経路計画と同じ制約条件を含む。つまり、ローカルシステムは、

が観測条件となり制約条件となる。

また、ロールシステムは、さらに、ウェイポイントＰ（ｋ＋１）に各ビークルを同時到達させるために、

を制約条件とする。ここで，α＝１，２，・・・，Ｎ_ＰＨは，次のウェイポイントＰ（ｋ＋１）がグローバル経路計画全体の何番目のウェイポイントかを表す自然数である。

移動制御装置１０は、図８に示すように、グローバル計画１２０と、ローカルシステム１２２を並行して処理する。グローバル計画１２０は、例えば、計画の算出に所要時間１２６が必要となり、時刻ｋに計算を開始すると計算完了時点１３０まで演算を行うことになる。グローバル計画１２０は、ローカルシステム１２２に対して、時刻ｋに少なくとも時刻ｋ＋１のウェイポイント情報１４０を送信する。また、グローバル計画１２０は、ローカルシステム１２２に対して、時刻ｋ＋１に少なくとも時刻ｋ＋２のウェイポイント情報１４２を送信する。一方、ローカルシステム１２２は、ウェイポイント情報１４０と現在の位置の情報に基づいて、周期Ｔ_ｃで目標位置を算出し、算出した位置情報に基づいて制御入力を算出する。ローカルシステム１２２は、次の制御入力の算出に係る時間が、周期Ｔ_ｃよりも短い時間１３２となる。移動制御システム１は、周期Ｔ_ｃで目標位置を算出することで、図９に示すように、ウェイポイント１５０とウェイポイント１６０との間を周期Ｔ_ｃ毎に算出された目標位置１６２で接続した移動経路１５１を算出することができる。

移動制御システムは、ローカルシステムで並列して処理することで、制御入力の待ち時間の発生を抑制でき、かつ、経路を詳細に調整することができる。また、ローカルシステムは、先読の処理を行わずにウェイポイントの情報を用いることで、計算を簡単にしつつ、グローバル計画に沿った移動を行うことができる。

また、移動制御装置１０は、図８に示すように、ビークルがローカルシステムで動作している間に、次のグローバル経路を計算することで、ウェイポイントの算出のための待ち時間の発生を抑制することができる。この場合、移動制御装置１０は、グローバル計画の算出時の起点に到達する前に、計算を開始することになる。したがって、移動制御装置１０は、ビークルの次のウェイポイントの位置姿勢を現在の位置、姿勢とし、すなわちビークルは周期Ｔ_ＮＰＣ毎に必ず次のウェイポイントに到達しているものと仮定して。次のグローバル経路を計画する。これにより、ウェイポイント毎の待ち時間を一切発生させず、ビークル群を移動させ続けることができる。

［第５実施形態］
次に、第５実施形態について説明する。第１実施形態と同様の構成については、同じ符号を付して説明を省略する。特筆する事項を除いて、第５実施形態は、第１実施形態と同様である。図１０は、第５実施形態で移動制御システムが行う処理の一例を示す模式図である。

本実施形態の移動制御システム１は、ビークルの捜索対象に対する移動に応じて、先読処理の負荷を適正にしつつ、経路の精度を高くする機能を含む。移動制御システム１は、グローバル経路計画、本実施形態の先読を含む移動軌跡の算出処理の処理周期（単位時間）Ｔ_ＭＰＣを固定とし、先読ステップ数(予測ホライズン)Ｎ_ＰＨも固定にすると、捜索対象の目標位置に到達する移動経路が算出されない可能性がある。また、先読ステップ数が一定の場合、演算の負荷が変わらない。また、処理周期（単位時間）Ｔ_ＭＰＣを変動させるとしても、ウェイポイントの数が変わらないと処理の負荷が高くなる。

本実施形態の移動制御システム１は、グローバル経路を計算する度に、例えば単位時間が経過する毎、ウェイポイント通過する毎に、予測ホライズンを一つずつ減らす。すなわち、移動制御システム１は、グローバル経路を計算し終えた後、Ｎ_ＰＨ＝ｍａｘ｛Ｎ_ＰＨ－１，１｝とする。

これにより、図１０に示すように、移動軌跡１７０のうち、通過後の経路１７２のウェイポイント１７８の間隔と、ビークルＢ１がこれから進行する経路１７４のウェイポイント１７８の間隔が変化すること抑制できる。また、移動軌跡１７０の変動を小さくすることができる。なお、予測ホライズンの値は徐々に減少させてればよく、減少させるタイミングは特に限定されない。

以上、第１実施形態、第２実施形態、第３実施形態、第４実施形態及び第５実施形態並びに変形例について説明したが、これらは任意に組み合わせ可能である。すなわち、第１実施形態、第２実施形態、第３実施形態、第４実施形態及び第５実施形態はいずれも他の実施形態と競合及び矛盾せず、併用できる。また、変形例は、第１実施形態、第２実施形態、第３実施形態、第４実施形態及び第５実施形態及びこれらの組み合わせによる実施形態に適用できる。

以上の説明では、複数のビークルＢの各々の制御入力の算出にモデル予測制御を適用しているが、制御入力の算出方法はこれに限られるものでない。車両の等価二輪モデルなど、非ホロノミックなモデルを用いてもよい。

実施形態及び変形例は、例として提示したものであり、発明の範囲を限定することは意図していない。実施形態及び変形例は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。実施形態及び変形例は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１ 移動制御システム
１０ 移動制御装置
２０，５３，７０ 通信部
３０ 制御部
３１ 演算部
３２ 記憶部
５１ 位置検出部
５２ 対象検出部
５４ 動力部
Ｂ，Ｂ_１，Ｂ_２，Ｂ_３，Ｂ_Ｍ ビークル
Ｔ，Ｔ_１，Ｔ_２，Ｔ_３，Ｔ_Ｍ 捜索対象

Claims (12)

1. 複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させる複数のビークルの移動制御方法であって、
   移動制御装置が、前記複数のビークルの各々の位置及び姿勢を取得するステップと、
   前記移動制御装置が、前記捜索対象の位置を特定するステップと、
   前記移動制御装置が、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出するステップと、
   前記移動制御装置が、算出した移動軌跡で前記複数のビークルが移動した場合、前記ビークルの位置及び姿勢に基づいて、観測できる範囲を算出し、観測できる範囲に前記捜索対象が観測できるかを算出し、前記捜索対象のそれぞれについて、前記捜索対象を観測できるビークルの台数が、前記捜索対象の観測条件を満たしているか判定するステップと、
   前記移動制御装置が、前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、
   を含む複数のビークルの移動制御方法。
2. 前記捜索対象の位置は、前記ビークルで検出した位置である請求項１に記載のビークルの移動制御方法。
3. 前記観測条件は、前記捜索対象を観測できる前記ビークルが設定した数以上であるかを含む請求項２に記載のビークルの移動制御方法。
4. 前記移動軌跡は、前記捜索対象に対して設定される前記ビークルが最終目標位置に、複数の前記ビークルが指定された時刻に到着する軌跡である請求項１から請求項３のいずれか一項に記載のビークルの移動制御方法。
5. 前記移動軌跡を算出するステップは、前記ビークルと他の前記ビークルとの距離を閾値距離以上とする請求項１から請求項４のいずれか一項に記載のビークルの移動制御方法。
6. 前記移動軌跡を算出するステップは、前記ビークルの所定時間の移動の距離及び方向の変動を制限する制限値に基づいて、前記ビークルの移動軌跡を算出する請求項１から請求項５のいずれか一項に記載のビークルの移動制御方法。
7. 前記ビークルの移動を制御するステップは、前記単位時間後の前記ビークルの位置及び姿勢と、現時点の前記ビークルの位置及び姿勢と、に基づいて、前記単位時間よりも短い単位周期で、前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する請求項１から請求項６のいずれか一項に記載のビークルの移動制御方法。
8. 前記移動軌跡を算出するステップは、前記ビークルの移動に基づいて、前記先読ステップのステップ数を減少させる請求項１から請求項７のいずれか一項に記載のビークルの移動制御方法。
9. 複数のビークルの移動を制御する移動制御装置であって、
   前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報と姿勢の情報を取得する位置検出部と、
   少なくとも１つの捜索対象の位置の情報を取得する対象物検出部と、
   取得した前記ビークルの現在の位置の情報と姿勢の情報と少なくとも１つの捜索対象の位置の情報に基づいて、前記捜索対象の観測条件を満たす前記ビークルの移動軌跡を算出する制御部と、を含み、
   前記制御部は、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出し、
   算出した移動軌跡で前記複数のビークルが移動した場合、前記ビークルの位置及び姿勢に基づいて、観測できる範囲を算出し、観測できる範囲に前記捜索対象が観測できるかを算出し、前記捜索対象のそれぞれについて、前記捜索対象を観測できるビークルの台数が、前記捜索対象の観測条件を満たしているか判定し、
   前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する移動制御装置。
10. 複数のビークルと、
    複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させる移動制御装置と、を備え、
    前記移動制御装置は、
    前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報と姿勢の情報を取得し、かつ、少なくとも１つの捜索対象の位置の情報を取得する通信部と、
    取得した前記ビークルの現在の位置の情報と姿勢の情報と少なくとも１つの捜索対象の位置の情報に基づいて、前記捜索対象の観測条件を満たす前記ビークルの移動軌跡を算出する制御部と、を含み、
    前記制御部は、前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出し、
    算出した移動軌跡で前記複数のビークルが移動した場合、前記ビークルの位置及び姿勢に基づいて、観測できる範囲を算出し、観測できる範囲に前記捜索対象が観測できるかを算出し、前記捜索対象のそれぞれについて、前記捜索対象を観測できるビークルの台数が、前記捜索対象の観測条件を満たしているか判定し、
    前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する移動制御システム。
11. 複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させるプログラムであって、
    コンピュータに、
    前記複数のビークルの各々の位置及び姿勢を取得するステップと、
    前記捜索対象の位置を特定するステップと、
    前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出するステップと、
    算出した移動軌跡で前記複数のビークルが移動した場合、前記ビークルの位置及び姿勢に基づいて、観測できる範囲を算出し、観測できる範囲に前記捜索対象が観測できるかを算出し、前記捜索対象のそれぞれについて、前記捜索対象を観測できるビークルの台数が、前記捜索対象の観測条件を満たしているか判定するステップと、
    前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を実行させるためのプログラム。
12. 複数のビークルにより少なくとも１つの捜索対象を観測しつつ、前記複数のビークルを移動させるプログラムを記録したコンピュータ読み取り可能な記録媒体であって、
    コンピュータに、
    前記複数のビークルの各々の位置及び姿勢を取得するステップと、
    前記捜索対象の位置を特定するステップと、
    前記複数のビークルの各々の位置及び姿勢と、前記捜索対象の位置とに基づいて、前記複数のビークルの先読ステップ分の移動軌跡を算出するステップと、
    算出した移動軌跡で前記複数のビークルが移動した場合、前記ビークルの位置及び姿勢に基づいて、観測できる範囲を算出し、観測できる範囲に前記捜索対象が観察できるかを算出し、前記捜索対象のそれぞれについて、前記捜索対象を観測できるビークルの台数が、前記捜索対象の観測条件を満たしているか判定するステップと、
    前記観測条件を満たしている移動軌跡を算出した場合、前記移動軌跡に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体。
    

Images