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

Abstract

【課題】複数のビークルを効率よくそれぞれの目標位置に移動させることができる複数のビークルの移動制御方法を提供する。
【解決手段】複数のビークルＢ１、Ｂ２、Ｂ３の移動制御方法であって、複数のビークルが移動する制約条件及びビークルの移動経路を算出する計算周期を取得するステップと、複数のビークルの各々の位置を取得するステップと、ビークル毎の目標位置を特定するステップと、複数のビークルの各々の位置と、目標位置と、制約条件と、に基づいて、複数のビークルの先読ステップ分の移動経路を算出するステップと、移動経路に基づいて、現時点から単位時間後までのビークルの駆動条件を決定し、ビークルの移動を制御するステップと、を含み、移動経路を算出するステップは、先読ステップ毎のビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、制約条件とに基づいて、最適化計算を行い、ビークルの移動経路を算出する。
【選択図】図１

Description

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

複数のビークルを移動させる技術（Ｓｗａｒｍ技術）の一つとして、荷物を搬送する複数の搬送装置の移動を制御する方法がある（例えば、特許文献１）。特許文献１に記載の装置は、中央演算装置で、全搬送装置による総搬送時間又は総搬送距離を最短とするように評価指標を作成し、経路を設定し、経路に基づいて各搬送装置の移動を制御することが記載されている。

特許第４１３８５４１号公報

特許文献１のように、搬送装置であるビークルが通過できる通路を設定し、通路を移動する経路を設定するためには、通路の情報を予め設定する必要があり、手間がかかる。また、通路に基づいて経路を設定する場合、設定できる経路の自由度が低くなる。

また、Ｓｗａｒｍ技術としては、各ビークルの現在位置に対して領域を設定し、領域の情報に基づいて、ビークルの移動を制御する方法もある。周囲に障害物がある環境で移動経路を制御しつつ、複数のビークルをそれぞれの目標位置に移動させる処理の場合、上記制御では、移動を適切に制御することが困難である。

本開示は、上記の事情に鑑みてなされたものであり、複数のビークルをそれぞれの目的に移動させる場合に、複数のビークルを効率よくそれぞれの目標位置に移動させることができる複数のビークルの移動制御方法、移動制御装置、移動制御システム、プログラム及び記録媒体を提供することを目的とする。

上述した課題を解決し、目的を達成するために、複数のビークルを移動させる複数のビークルの移動制御方法であって、前記複数のビークルが移動する制約条件を取得するステップと、前記複数のビークルの各々の位置を取得するステップと、前記ビークル毎の目標位置を特定するステップと、前記複数のビークルの各々の位置及び姿勢と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出するステップと、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含み、前記移動経路を算出するステップは、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する複数のビークルの移動制御方法を提供する。

また、複数のビークルの移動を制御する移動制御装置であって、前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報を取得し、かつ、前記複数のビークルのそれぞれの目標位置の情報を取得する通信部と、前記複数のビークルが移動する制約条件を取得し、前記複数のビークルの各々の位置及び姿勢と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出し、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する制御部と、を含み、前記制御部は、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する複数のビークルの移動制御装置を提供する。

また、複数のビークルと、前記複数のビークルを移動させる移動制御装置と、を備え、前記移動制御装置は、前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報を取得し、かつ、前記複数のビークルのそれぞれの目標位置の情報を取得する通信部と、前記複数のビークルが移動する制約条件を取得し、前記複数のビークルの各々の位置及び姿勢と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出し、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する制御部と、を含み、前記制御部は、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する移動制御システムを提供する。

また、複数のビークルを移動させるプログラムであって、コンピュータに、前記複数のビークルが移動する制約条件を取得するステップと、前記複数のビークルの各々の位置を取得するステップと、前記ビークル毎の目標位置を特定するステップと、前記複数のビークルの各々の位置及び姿勢と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出するステップと、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含み、前記移動経路を算出するステップは、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する処理を、実行させるためのプログラムを提供する。

また、複数のビークルを移動させるプログラムを記録したコンピュータ読み取り可能な記録媒体であって、コンピュータに、前記複数のビークルが移動する制約条件を取得するステップと、前記複数のビークルの各々の位置を取得するステップと、前記ビークル毎の目標位置を特定するステップと、前記複数のビークルの各々の位置及び姿勢と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出するステップと、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含み、前記移動経路を算出するステップは、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する処理を、を実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体を提供する。

本開示の少なくとも一実施形態によれば、複数のビークルをそれぞれの目的に移動させる場合に、複数のビークルを効率よくそれぞれの目標位置に移動させることができる。

図１は、第１実施形態の移動制御装置を含む移動制御システムの主要構成を示すブロック図である。 図２は、ビークルと障害物との関係の一例を示す模式図である。 図３は、第１実施形態で移動制御システムが行う処理の流れを示すフローチャートである。 図４は、第１実施形態で移動制御システムが行う処理の一例を示す模式図である。 図５は、第３実施形態で移動制御システムが行う処理の一例を示す模式図である。 図６は、第４実施形態で移動制御システムが行う処理の一例を示す模式図である。

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

［第１実施形態］
図１は、第１実施形態の移動制御装置１０を含む移動制御システム１の主要構成を示すブロック図である。移動制御システム１は、障害物がある領域において、複数のビークルＢが目標位置Ｔへ移動する移動経路を制御するシステムである。移動制御装置１０は、複数のビークルＢの移動を制御する。図１等では、複数のビークルＢの各々を区別する目的で、Ｂ_１，Ｂ_２，Ｂ_３…Ｂ_Ｍを付している。ビークルＢは、地面を走行する移動体としても、空中を飛行する移動体としても、水中を移動する移動体としてもよい。したがって、ビークルＢは、３次元で移動できる移動体も含むが、以下では説明のため、ビークルＢが２次元平面を移動する場合として説明する。移動制御システム１の具体的な一例としては、倉庫等で資材、物品を移動させるビークルの移動制御に用いる場合がある。この場合、目標位置は、運搬対象物がある位置、運搬対象物を搬送する位置となる。また、対象領域の障害物としては、搬送対象ではない荷物、物資、壁、等がある。

移動制御システム１は、複数のビークルＢと、移動制御装置１０と、管理端末４０と、ビークル検出装置６０と、を含む。複数のビークルＢの各々は、位置検出部５１と、通信部５３と、動力部５４とを備える。位置検出部５１は、当該位置検出部５１が設けられたビークルＢの位置を検出する。位置検出部５１の具体的構成例として、対象領域内の複数位置に配置した基準機と信号の授受を行い、複数の基準機との相対的な位置で自機の位置を検出する測定装置、周囲の画像や形状情報を取得し、予め準備した対象領域内の３次元形状データと照合して自機の位置を検出する測定装置、グローバル・ポジショニング・システム（ＧＰＳ：Global Positioning System）等の測位システムを利用して位置を検出するための測位装置が挙げられる。位置検出部５１は、所定の起点に対する位置を検出する慣性航法装置であってもよい。

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

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

制御部５５は、通信部５３を介して取得した情報に基づいて、動力部５４の挙動を制御する。

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

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

管理端末４０は、オペレータが各種情報を入力し、確認する機器である。管理端末は、パーソナルコンピュータ、タブレット等である。管理端末４０は、入力部４１と、表示部４２と、制御部４３と、通信部４４と、を含む。入力部４１は、タッチパネル、マウス、キーボード、マイク等であり、オペレータによる操作を検出する。表示部４２は、画像を表示する。表示部４２は、操作画面や、操作結果画面等、オペレータが移動制御システム１の操作に必要な情報が表示される。制御部４３は、管理端末４０の各部の動作を制御する。通信部４４は、移動制御装置１０の通信部２０と通信を行う。管理端末４０は、捜査情報を移動制御装置１０に送信し、移動制御装置１０で取得した情報、生成した情報を受信する。

ビークル検出装置６０は、対象領域にあるビークルＢの位置を検出する。ビークル検出装置６０は、ビークル検出部６２と、通信部６４とを含む。ビークル検出部６２は、検出対象領域にあるビークルＢを検出する。ビークル検出部６２は、カメラ、ビークルＢと通信を行う通信機器、温度センサ、形状検出センサ等を用いることができる。ビークル検出装置６０は、検出対象領域の各種情報を取得することで、検出対象領域のあるビークルＢを検出する。通信部６４は、移動制御装置１０の通信部２０と通信を行い、検出したビークルの情報を送信する。なお、本実施形態の移動制御システム１は、ビークル検出装置６０を設けたが、ビークルＢで自機の位置を特的できる場合、ビークル検出装置６０は、備えていなくてもい。

次に、図２から図４を用いて、移動制御システム１による複数のビークルの移動を説明する。図２は、ビークルと障害物との関係の一例を示す模式図である。図３は、第１実施形態で移動制御システムが行う処理の流れを示すフローチャートである。図４は、第１実施形態で移動制御システムが行う処理の一例を示す模式図である。

本実施形態の移動制御システム１は、障害物があり、移動可能な領域に制約がある対象領域において、複数のビークルＭをそれぞれの目標位置（指定場所、指定位置）に移動させる。管理端末４０は、障害物の情報及び移動の制約条件の情報を、通信部４４を介して移動制御装置１０に送信する。複数のビークルＢの各々は、位置検出部５１によって取得された位置を示す情報を、通信部５３を介して移動制御装置１０に送信する。また、ビークル検出部６２は、対象領域で検出したビークルＢの情報を、通信部６４を介して移動制御装置１０に送信する。記憶部３２は、複数のビークルＢの各々の位置を示す情報と、各ビークルＢの目標位置Ｔの情報、障害物の情報を累積的に記憶する。演算部３１は、複数のビークルＢの各々の位置を示す情報と、目標位置を示す情報と、障害物の位置情報を含む制約条件に基づいて、複数のビークルＢの各々の移動経路を算出して評価し、評価した結果に基づいて移動経路を決定し、決定した移動経路に基づいて、複数のビークルＢの各々の制御入力を算出し、制御入力を、通信部２０を介して複数のビークルＢに個別に送信する。ここで、制御入力は、単位時間で所定条件を満たしながら複数のビークルＢを各々の移動させるための移動方向及び移動速度を示す情報として機能する。ビークルＢは、制御入力に従って移動するように動力部５４を動作させる。また、記憶部３２は、複数のビークルＢの各々に送信された制御入力を累積的に記憶する。

次に、移動制御装置１０の処理について説明する。移動制御装置１０は、複数のビークルＢで構成されるビークル群の全体でのスループットが向上するように、各ビークルの目標位置への移動経路を最適化する。本実施形態の移動制御装置１０は、ビークルＢの移動の制約条件として、「ビークルＢは、他のビークルＢに距離ｄ_Ａ［ｍ］以上接近しない」、ビークルＢは、障害物１０２に距離ｄ_Ｂ［ｍ］以上接近しない」、ビークルの速度の上下限値、の３つが設定される。ここで、ビークルＢの速度の制限値としては、上限ｖ_ｍａｘと下限ｖ_ｍｉｎを設定する。ビークルＢの速度の上限ｖ_ｍａｘは、ビークルＢの性能や、移動する領域に課せられる制限値等で設定する。ビークルＢは、本実施形態のように地面を走行する場合停止が許容されるため、ｖ_ｍｉｎ＝０とすることができる。ビークルＢが空中また水中を移動し、かつ、ホバリングができない場合、下限ｖ_ｍｉｎは、空中また水中で姿勢を維持できる速度となる。

まず、複数のビークルＢのそれぞれを識別するビークルｍ（ｍ＝１，２，３，・・・，Ｍ）への現在時刻ｋにおける制御入力をｕ_ｍ（ｋ）とおく。ここでＭはビークルの総数を表す。本実施形態では、簡単のため２次元のｘｙ平面上での運動のみとし、制御入力をビークルのｘ軸方向の速度ｕ_ｍｘ（ｋ）とｙ軸方向の速度ｕ_ｍｙ（ｋ）とし、

とする。

また、移動制御装置１０は、モデル予測制御で予測する予測ステップ数（予測ホライズンと呼ばれる）Ｎ_Ｈを含み、未来の経路まで予測する。未来の経路、つまり現在時刻ｋから、予測ステップ数Ｎ_Ｈ後のｋ＋Ｎ_Ｈ−１までの各ステップの制御入力ｕ_ｍは、

となる。

次に、上述した３つの制約条件を、移動経路を算出する制約付き最適化であるモデル予測制御を適用するために数式とする処理について説明する。移動制御装置１０は、予測ステップｋ＝１，・・・，Ｎ_Ｈ全てで制約条件を満たすことを条件として、最適化計算を行う。

制約条件「ビークルは、他のビークルに距離ｄ_Ａ［ｍ］以上接近しない」は、下記式で表すことができる。

ｐ_ｍ（ｋ）＝［ｘ_ｍ（ｋ）ｙ_ｍ（ｋ）］^Ｔおよびｑ_ｍ（ｋ）＝［ｘ´_ｍ（ｋ）ｙ´_ｍ（ｋ）］^Ｔは、それぞれビークルｍとビークルｍに対する目標位置Ｔのｘｙ平面上での座標である。

次に、制約条件「ビークルＢは、障害物に距離ｄ_Ｂ［ｍ］以上接近しない」は、下記式で表すことができる。

ビークルＢと障害物１０２との位置関係は、図２に示すように設定する。図２に示すように、ｘ_ｍｉｎは、障害物の最小ｘ座標、ｘ_ｍａｘは、障害物の最大ｘ座標、ｙ_ｍｉｎは障害物の最小ｙ座標である。ｙ_ｍａｘは障害物の最大ｙ座標である。図２は、１つのビークルと障害物との関係を示しているが、対象領域にある障害物の数は、１つに限らず、複数定義可能である。移動制御装置１０は、障害物の数に比例して、制約条件の数式の数が増える。

次に、「ビークルＢの速度は、上限ｖ_ｍａｘと下限ｖ_ｍｉｎである」は、下記式となる。

ビークルの速度の制約条件の数は、ビークルの数に比例する。また、予測ステップ数Ｎ_Ｈにも比例する。

ここで、制約条件は、モデル予測制御を適用するために、制御入力に関して記載する必要がある。そこで、ｎステップ先のビークルの将来位置を

とし、各ステップのビークルの位置を制約条件に代入する。これにより、制約条件を制御入力に関連する関数とすることができ、モデル予測制御を適用できる。なお、運動モデルには、車両の等価二輪モデルなど非ホロノミックなモデルを用いても良い。

次に、移動制御装置１０は、評価関数Ｊ_ｍ（ｕ_{１・・・，}ｕ_Ｍ）を、

とする。なお、評価関数は、一例でありこれに限定されない。ここでｅ_ｍ（ｋ＋ｎ）∈Ｒ^２×１はｎステップ先の時点におけるビークルの位置予測値と目標位置の偏差である。位置の偏差は、

となる。本実施形態の評価関数Ｊ_ｍ（ｕ_{１・・・，}ｕ_Ｍ）を用いることで、予測ステップ数Ｎ_Ｈ経過後の時点である最終到達時とともに、全予測ステップの地点において目的地との誤差を評価することができる。これにより、ビークルＢの目的地までの距離を最小化することができる。また、評価関数Ｊ_ｍ（ｕ_{１・・・，}ｕ_Ｍ）を用いることで、ビークル全台についてまとめて評価することができ、ビークルの群全体の移動を最適化できる。

移動制御装置１０は、以上のように定義した評価関数と制約条件を用いて制約付き最適化問題

を演算周期毎に解き、最適な制御入力を求める。移動制御装置１０は、算出した制御入力を各ビークルに出力する。

次に、図３を用いて、移動制御装置１０で実行する処理の一例を説明する。移動制御装置１０は、制約条件及び計算周期を取得する（ステップＳ１１）。移動制御装置１０は、管理端末４０に入力された情報を取得し、制約条件を取得する。また、移動制御装置１０は、管理端末４０に入力された情報を取得し、計算周期、つまりビークルＢの移動経路を算出する計算周期（先読ステップの１ステップ分の時間）を取得する。なお、移動制御装置１０は、制約条件を取得しても、制約条件に必要な閾値や障害物の情報を取得し、取得した情報に基づいて制約条件を作成してもよい。また、移動制御装置１０は、ビークルの移動を制御している間に制約条件を更新しない場合、制約条件の取得を一連として実行せず、事前に実行してもよい。

移動制御装置１０は、全ビークルＢの位置の取得、計測を行う（ステップＳ１２）。移動制御装置１０は、通信部２０を介して、ビークルＢから位置検出部５１で計測、検出した位置の情報を取得する。移動制御装置１０は、通信部２０を介して、ビークル検出装置６０で計測したビークルＢの位置情報を取得してもよい。また、移動制御装置１０は、ビークルＢの位置情報に加え、姿勢情報も取得してもよい。

次に、移動制御装置１０は、全目標位置を取得、計測する（ステップＳ１４）。移動制御装置１０は、複数のビークルＢのそれぞれに対して設定される目標位置の情報を取得する。目標位置は、管理端末４０から出力された情報に基づいて取得することができる。管理端末４０から出力される情報としては、目標位置自体の情報や、ビークルで実行する作業の情報としてもよい。ビークルで実行する作業の情報とは、例えば、倉庫で搬送する荷物の情報（荷物の現在位置と、荷物を搬送させる先の位置の情報）である。移動制御装置は、ビークルＢの情報と倉庫で搬送する荷物の情報とに基づいて、荷物の搬送計画と各荷物を搬送させるビークルＢを決定し、ビークルの目標位置を決定する。

移動制御装置１０は、制約条件と、ビークルの位置と、目標位置の情報を取得したら、
制限付き最適化問題を解き、ビークルＭ台分の制御入力を算出する（ステップＳ１６）。移動制御装置１０は、先読ステップ数Ｎ_ＰＨを用いて、制約条件を満たすビークルの移動を算出する。つまり、全ビークルの移動経路の各ステップにおいて、ビークルと目標位置の偏差を評価し、全ビークルの移動経路として、偏差が小さくなる移動経路を算出する。

移動制御装置１０は、算出結果に基づいて、単位時間分のビークルの制御入力を算出する（ステップＳ１８）。単位時間は、先読ステップ数分に対応する時間よりも短い時間である。移動制御装置１０は、実際に制御入力で移動させる時間よりも、（先読ステップ数−単位時間）長い時間の移動経路を評価して、移動経路を算出し、算出結果から単位時間分の移動の制御入力を算出する。移動制御装置１０は、単位時間経過後、時間を更新する（ステップＳ２０）。移動制御装置１０は、処理終了かを判定し（ステップＳ２２）、処理終了ではない（ステップＳ２２でＮｏ）と判定した場合、ステップＳ１１に戻り、処理終了である（ステップＳ２２でＹｅｓ）と判定した場合、本処理を終了する。

移動制御装置１０は、以上のように移動経路を算出することで、図４に示すように、複数の障害物１０２が配置された対象領域１１０で、複数のビークルＢ_１，Ｂ_２，Ｂ_３をそれぞれの目標位置１２０、１２２、１２４に到達させる移動経路１３０、１３２、１３４を算出する。ビークルＢ_１の目標位置１２０までの経路が、移動経路１３０となる。ビークルＢ_２の目標位置１２２までの経路が、移動経路１３２となる。ビークルＢ_３の目標位置１２４までの経路が、移動経路１３４となる。移動経路１３０、１３２、１３４は、予測ステップ毎の位置１４０、１４２が算出される。各予測ステップの位置で制約条件を満たすように、最適化計算を行う。これにより、異なる時間ステップであれば、同じ領域を別のビークルＢが通過することを許容する、時系列を考慮した移動経路を算出することができる。移動制御装置１０は、移動経路１３０、１３２、１３４を算出し、制御入力は、次の単位時間、予測ステップｎ＝０となる時点までの移動を制御する入力を作成し、出力する。

移動制御システム１は、モデル予測制御の考え方を用い、予測ステップをＮ_Ｈとし、制約条件を満たす移動経路を予測（先読）し、各予測ステップでのビークルＢと目標位置の関係において、距離が短いほど、評価が高くなる評価関数を用いて、全ビークルの全予測ステップの位置に関するビークルＢと目標位置の偏差を評価して、移動経路を算出し、
算出した移動経路に基づいて、単位時間分の移動を決定する。

これにより、ビークルが移動した後、次に移動する場合に、ビークルの移動可能な領域の移動で、制約条件を満たす移動ができない、デッドロックが発生することを抑制でき、ビークルの渋滞等が生じることを抑制できる。また、複数の制約条件を同時に加味して、最適化計算を行うことができる。さらに、ビークルが移動できない領域を障害物によって設定することで、移動可能な経路を設定する場合よりも自由度の高い移動経路を算出することができる。また、対象領域の障害物の配置状態が変わった場合も通路の情報等を再作成せずに、制約条件として障害物の情報を更新することで、対象領域の配置に応じた移動経路を算出することができる。

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

本実施形態の移動制御システム１は、ビークルの目標位置までの距離とビークルの移動速度に基づいて、予測ステップ数Ｎ_Ｈを設定する。移動制御システム１は、ビークルＢの現在の位置から目的地までの距離をＬ_Ｇ、ビークルＢの想定される平均速力をｖ_ａｖｅとした場合、目的地までの到達時間Ｔ_Ｇは、Ｔ_Ｇ＝Ｌ_Ｇ／ｖ_ａｖｅで表される。また、予測ステップの１ステップ分の周期をＴｃとする。移動制御システム１は、予測ステップ数Ｎ_Ｈを、Ｎ_Ｈ＞（Ｔ_Ｇ／Ｔ_ｃ）×αを満たす最小の整数とする。αは、目的地値到達までの十分マージンをよるための係数であり１より大きい値（α＞１）である。αは予め設定される。

移動制御システム１は、予測ステップ数Ｎ_Ｈを、Ｎ_Ｈ＞（Ｔ_Ｇ／Ｔ_ｃ）＋Ｎ_αを満たす最小の整数としてもよい。Ｎ_αは、マージン用のステップ数で、０より大きい整数（Ｎ_α＞０）である。

本実施形態の移動制御システム１は、設定される目標位置との距離に基づいて、予測ステップ数Ｎ_Ｈを設定することで、移動経路の算出の精度を低減することなく、演算量が必要以上に多くなることを抑制できる。また、本実施形態の移動制御システム１は、先読ステップ分の時間を、複数のビークルのそれぞれの目標位置に到達するまでにかかる時間よりも長くできる。これにより、複数のビークルのそれぞれの目標位置に到達するまでの複数のビークルの移動経路を算出することができる。具体的には、（計算周期）×（ビークルの移動速度）×（先読ステップのステップ数）＞（ビークルと目標位置との距離）を満たす先読ステップ数を設定することで、目標位置までの経路を算出することができる。これにより、目標位置まで先読した移動経路に基づいて、処理を進めることができる。本実施形態の移動制御システム１は、目標位置にビークルが到達すると評価関数の評価に影響を与えないビークルとなる。このため、予測ステップ数Ｎ_Ｈを変化させない場合でも移動経路は同様の経路が算出される。

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

本実施形態の移動制御システム１は、上記実施形態の評価関数、制約条件に加えて、ビークルの力学的な拘束条件を制約条件として含む。例えば、ビークルとして、タイヤによって駆動、操舵するＡＧＦやＡＧＶを用いる場合、ビークルの機体の旋回半径は、操舵角限界によって制限される。図５に示すビークルＢは、中心２００を通る車軸２０２で接続された、従動輪となる２つの後輪２０４と、操舵輪及び駆動輪となる前輪２０６とを備える３輪のモデルである。前輪２０６の操舵角をφ、中心２００と前輪２０６の軸との距離（ホイールベースの長さ）をＬとし、ｘ軸方向、ｙ軸方向の速度をｘ´、ｙ´、ビークルの速力をｖ、ビークルの姿勢角（ヨー）をθとし、３輪のモデルのビークルＢを等価２輪モデルで近似することで、図５に示す３輪のビークルの駆動方式が以下の式で記述される。

上記運動モデルから、操舵角限界φ_ｌｉｍによるヨーレートの制約条件は、

となる。上記式のｖは、

となる。ｖは、モデル予測による最適化で得られた制御入力で決まる速力となる。移動制御システム１は、第１実施形態、第２実施形態等の制約条件に加え、上記条件を制約条件として、制約付き最適化問題を計算し、制御入力を算出する。

本実施形態の移動制御システム１は、第１実施形態、第２実施形態等の制約条件に加えてビークルの力学的な拘束条件を考慮することにより、非ホロノミックなビークルが制御対象の場合でも、ビークルが移動可能な制御入力を出力することができる。これにより、ビークルでより確実に実行できる移動経路を算出することができ、高い精度でビークルの移動を制御することができる。

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

本実施形態の移動制御システム１は、予測ステップ数Ｎ_Ｈを用いて算出した移動経路の地点間の情報に基づいて、経路の隣接する地点、一例として現時点から次の予測ステップで算出された地点までの経路を並行で計算する。

移動制御システム１は、第１実施形態から第３実施形態と同様に、上記ダイナミクスを用いて、演算周期Ｔ_Ｇで目標位置までの経路を計画する。演算周期Ｔ_Ｇで算出した目標位置までの経路の計画を「グローバル経路計画」とする。グローバル経路計画の各予測ステップで算出した地点を「ウェイポイント」とする。

本実施形態の移動制御システム１は、演算周期（グローバル経路計画周期）Ｔ_Ｇよりも短い演算周期（ローカル経路計画周期）Ｔ_Ｌで動作するローカルシステムを用い、グローバル経路計画のウェイポイント間の移動時の制御入力を作成し、ビークルを制御する。ローカルの演算周期Ｔ_Ｌは、Ｔ_Ｌ＜＜Ｔ_Ｇとしてグローバル経路計画の周期よりも早い周期（短い周期）で演算を行う。ローカルシステムは、グローバル経路計画と同様の制約条件で、次の周期の位置を算出する。ローカルシステムの処理は、移動制御装置１０で実行しても、各ビークルで実行してもよい。移動制御システム１は、ローカルシステムで算出した演算周期Ｔ_Ｌごとの位置情報に基づいて制御入力を算出し、各ビークルの移動を制御する。

移動制御システム１は、ローカル経路計画周期Ｔ_Ｌで目標位置を算出することで、図６に示すように、グローバル経路計画で算出された移動経路３０２の次のウェイポイント１４０までの移動を、ローカルシステムを用いてローカル経路計画周期Ｔ_Ｌ毎に算出された目標位置で接続した移動経路３０４とすることができる。

このように、移動制御システム１は、ローカルシステムを用いることで、予測ステップ数に基づいた移動経路の算出において、周期を短くしてステップ数を増加させずに、制約条件を満たす、より適切で詳細な移動経路に基づいて、制御入力を決定することができる。また、ローカルシステムでは、現在位置から次のウェイポイントへの経路の算出のみ行う、すなわち１ステップ分のみ経路を算出することで、演算量の増加を抑制することができる。

ローカルシステムは、予測ステップ数を設定し、先読を用いた移動経路を算出せず、現時点と次のウェイポイントと、制約条件と、に基づいて、次の演算周期Ｔ_Ｌの位置を算出することが好ましい。これにより、最適化計算時の計算量の増加を抑制することができる。

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

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

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

１ 移動制御システム
１０ 移動制御装置
２０，４４，５３，６４ 通信部
３０，４３，５５ 制御部
３１ 演算部
３２ 記憶部
４０ 管理端末
４１ 入力部
４２ 表示部
５１ 位置検出部
５４ 動力部
６０ ビークル検出装置
６２ ビークル検出部
Ｂ，Ｂ１，Ｂ２，Ｂ３ ビークル
Ｔ，Ｔ１，Ｔ２，Ｔ３ 目標位置

Claims (14)

1. 複数のビークルを移動させる複数のビークルの移動制御方法であって、
   前記複数のビークルが移動する制約条件及び前記ビークルの移動経路を算出する計算周期を取得するステップと、
   前記複数のビークルの各々の位置を取得するステップと、
   前記ビークル毎の目標位置を特定するステップと、
   前記複数のビークルの各々の位置と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出するステップと、
   前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含み、
   前記移動経路を算出するステップは、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する複数のビークルの移動制御方法。
2. 前記制約条件は、前記ビークルの移動可能範囲にある障害物の位置情報と、前記障害物との距離の下限値と、を含む請求項１に記載の複数のビークルの移動制御方法。
3. 前記制約条件は、前記ビークルと他の前記ビークルとの距離の下限値を含む請求項１または請求項２に記載の複数のビークルの移動制御方法。
4. 前記制約条件は、前記ビークル移動速度と制限値を含む請求項１から請求項３のいずれか一項に記載の複数のビークルの移動制御方法。
5. 前記移動経路を算出するステップは、前記ビークルの所定時間の移動の距離及び方向の変動を制限する制限値に基づいて、前記ビークルの移動経路を算出する請求項１から請求項４のいずれか一項に記載のビークルの移動制御方法。
6. 前記ビークルの移動を制御するステップは、前記単位時間後の前記ビークルの位置及び姿勢と、現時点の前記ビークルの位置及び姿勢と、に基づいて、前記単位時間よりも短い単位周期で、前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する請求項１から請求項５のいずれか一項に記載のビークルの移動制御方法。
7. 前記移動経路を算出するステップは、前記先読ステップ分の時間が前記複数のビークルのそれぞれの目標位置に到達するまでにかかる時間よりも長く、前記複数のビークルのそれぞれの目標位置に到達するまでの前記複数のビークルの移動経路を算出する請求項１から請求項６のいずれか一項に記載のビークルの移動制御方法。
8. 前記移動経路を算出するステップは、前記ビークルの移動速度と、前記ビークルと目標位置との距離に基づいて、前記先読ステップのステップ数を設定する請求項１から請求項７のいずれか一項に記載のビークルの移動制御方法。
9. 前記移動経路を算出するステップは、（前記計算周期）×（前記ビークルの移動速度）×（前記先読ステップのステップ数）＞（前記ビークルと目標位置との距離）を満たす先読ステップ数を設定する請求項８に記載のビークルの移動制御方法。
10. 前記移動経路を算出するステップは、前記ビークルの姿勢にも基づいて、前記複数のビークルの移動経路を算出する請求項１から請求項９のいずれか一項に記載のビークルの移動制御方法。
    
11. 複数のビークルの移動を制御する移動制御装置であって、
    前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報を取得し、かつ、前記複数のビークルのそれぞれの目標位置の情報を取得する通信部と、
    前記複数のビークルが移動する制約条件及び前記ビークルの移動経路を算出する計算周期を取得し、前記複数のビークルの各々の位置と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出し、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する制御部と、を含み、
    前記制御部は、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する複数のビークルの移動制御装置。
12. 複数のビークルと、
    前記複数のビークルを移動させる移動制御装置と、を備え、
    前記移動制御装置は、
    前記複数のビークルのそれぞれと通信を行い、前記ビークルの現在の位置の情報を取得し、かつ、前記複数のビークルのそれぞれの目標位置の情報を取得する通信部と、
    前記複数のビークルが移動する制約条件及び前記ビークルの移動経路を算出する計算周期を取得し、前記複数のビークルの各々の位置と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出し、前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御する制御部と、を含み、
    前記制御部は、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する移動制御システム。
13. 複数のビークルを移動させるプログラムであって、
    コンピュータに、
    前記複数のビークルが移動する制約条件及び前記ビークルの移動経路を算出する計算周期を取得するステップと、
    前記複数のビークルの各々の位置を取得するステップと、
    前記ビークル毎の目標位置を特定するステップと、
    前記複数のビークルの各々の位置と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出するステップと、
    前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含み、
    前記移動経路を算出するステップは、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する処理を、実行させるためのプログラム。
14. 複数のビークルを移動させるプログラムを記録したコンピュータ読み取り可能な記録媒体であって、
    コンピュータに、
    前記複数のビークルが移動する制約条件及び前記ビークルの移動経路を算出する計算周期を取得するステップと、
    前記複数のビークルの各々の位置を取得するステップと、
    前記ビークル毎の目標位置を特定するステップと、
    前記複数のビークルの各々の位置と、前記目標位置と、前記制約条件と、に基づいて、前記複数のビークルの先読ステップ分の移動経路を算出するステップと、
    前記移動経路に基づいて、現時点から単位時間後までの前記ビークルの駆動条件を決定し、前記ビークルの移動を制御するステップと、を含み、
    前記移動経路を算出するステップは、前記先読ステップ毎の前記ビークルと目標位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づいて、最適化計算を行い、前記ビークルの移動経路を算出する処理を、を実行させるためのプログラムを記録したコンピュータ読み取り可能な記録媒体。

Images