特開 2024-24555 衝突条件付与装置、方法、プログラム、及び経路生成装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024024555

(43)【公開日】2024-02-22

(54)【発明の名称】衝突条件付与装置、方法、プログラム、及び経路生成装置

(51)【国際特許分類】

   G05D 1/43 20240101AFI20240215BHJP

【ＦＩ】

G05D1/02 S

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2022127476

(22)【出願日】2022-08-09

【新規性喪失の例外の表示】特許法第３０条第２項適用申請有り 令和４年６月３０日 ｈｔｔｐｓ：／／ｉｅｅｅｘｐｌｏｒｅ．ｉｅｅｅ．ｏｒｇ／ｄｏｃｕｍｅｎｔ／９８１１３４４にて公開

(71)【出願人】

【識別番号】000002945

【氏名又は名称】オムロン株式会社

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】笠浦 一海

【テーマコード（参考）】

5H301

【Ｆターム（参考）】

5H301AA03

5H301AA10

5H301BB14

5H301CC03

5H301CC06

5H301LL01

5H301LL02

5H301LL03

5H301LL06

5H301LL11

5H301LL14

(57)【要約】

【課題】ロードマップ上のマルチエージェントの連続的な時間における経路生成を短時間で行うことを可能にする。
【解決手段】抽出部３２が、複数の頂点、及び頂点間を接続する複数の辺を含むロードマップ４０上を複数のエージェントが移動する場合において、エージェント同士が衝突する可能性のある頂点－辺ペア、及び辺－辺ペアを抽出し、算出部３４が、頂点－辺ペアの頂点に一方のエージェントが存在する時刻と、辺の端点から他方のエージェントが移動を開始する時刻との差、及び、辺－辺ペアの一方の辺の端点から一方のエージェントが移動を開始する時刻と、他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、両エージェントの衝突が起こる時間区間を算出し、各ペアに含まれる頂点及び辺の識別情報と時間区間とを対応付けた衝突条件情報をロードマップ４０に付与する。
【選択図】図２

【特許請求の範囲】

【請求項1】

複数の頂点、及び前記頂点間を接続する複数の辺を含むロードマップ上を複数のエージェントが移動する場合において、前記頂点に存在する前記エージェント又は前記辺を移動する前記エージェント同士が衝突する可能性のある前記頂点と前記辺との第１ペア、及び前記エージェント同士が衝突する可能性のある一方の辺と他方の辺との第２ペアを抽出する抽出部と、
前記第１ペアの各々の前記頂点に一方のエージェントが存在する時刻と、前記辺の端点から他方の前記エージェントが移動を開始する時刻との差、及び、前記第２ペアの各々の前記一方の辺の端点から一方のエージェントが移動を開始する時刻と、前記他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、前記一方のエージェントと前記他方のエージェントとの衝突が起こる時間区間を算出する算出部と、
前記第１ペアに含まれる前記頂点及び前記辺の識別情報と前記時間区間とを対応付けた情報、及び前記第２ペアに含まれる２つの前記辺の各々の識別情報と前記時間区間とを対応付けた情報を、前記エージェント同士が衝突する条件として前記ロードマップに付与する付与部と、
を含む衝突条件付与装置。

【請求項2】

前記抽出部は、与えられた点の集合の中で点間の距離が一定以下の点のペアを列挙するアルゴリズム、与えられた点の集合の中で与えられた領域内に存在する点を列挙するアルゴリズム、及び与えられた線分の集合の中で交差している線分のペアを列挙するアルゴリズムを用いて、前記ロードマップに含まれる前記複数の頂点及び前記複数の辺の中から前記第１ペア及び前記第２ペアを抽出する請求項１に記載の衝突条件付与装置。

【請求項3】

前記抽出部は、前記エージェントを直径が所定値の円と仮定した場合、前記ロードマップに含まれる前記複数の頂点及び前記複数の辺から、
辺の端点の少なくとも一方と頂点との距離が前記所定値未満の場合、及び頂点から辺へ引いた垂線の長さが前記所定値未満の場合の少なくとも一方を満たす頂点と辺とのペアを前記第１ペアとして抽出し、
一方の辺の端点の少なくとも一方と他方の辺との距離が前記所定値未満の場合、及び辺同士が交差している場合の少なくとも一方を満たす辺と辺とのペアを前記第２ペアとして抽出する
請求項１に記載の衝突条件付与装置。

【請求項4】

請求項１～請求項３のいずれか１項に記載の衝突条件付与装置により前記エージェント同士が衝突する条件が付与された前記ロードマップ上を移動する複数のエージェントの各々の経路であって、前記経路に含まれる前記頂点に前記エージェントが存在する時刻及び前記辺の端点から前記エージェントが移動を開始する時刻が、前記条件の時間区間に含まれないような経路を生成する生成部を含む経路生成装置。

【請求項5】

前記生成部は、前記エージェントの経路を１つずつ順次生成する場合において、第１エージェントの経路を生成し、次に第２エージェントの経路を生成する際に、前記第１エージェントについて生成された経路に含まれる前記頂点に前記エージェントが存在する時刻及び前記辺の端点から前記エージェントが移動を開始する時刻との差が、前記時間区間に含まれない時刻に、前記第２エージェントが前記頂点に存在するか、前記辺の端点から移動を開始するような経路を生成する請求項４に記載の経路生成装置。

【請求項6】

抽出部が、複数の頂点、及び前記頂点間を接続する複数の辺を含むロードマップ上を複数のエージェントが移動する場合において、前記頂点に存在する前記エージェント又は前記辺を移動する前記エージェント同士が衝突する可能性のある前記頂点と前記辺との第１ペア、及び前記エージェント同士が衝突する可能性のある一方の辺と他方の辺との第２ペアを抽出し、
算出部が、前記第１ペアの各々の前記頂点に一方のエージェントが存在する時刻と、前記辺の端点から他方の前記エージェントが移動を開始する時刻との差、及び、前記第２ペアの各々の前記一方の辺の端点から一方のエージェントが移動を開始する時刻と、前記他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、前記一方のエージェントと前記他方のエージェントとの衝突が起こる時間区間を算出し、
付与部が、前記第１ペアに含まれる前記頂点及び前記辺の識別情報と前記時間区間とを対応付けた情報、及び前記第２ペアに含まれる２つの前記辺の各々の識別情報と前記時間区間とを対応付けた情報を、前記エージェント同士が衝突する条件として前記ロードマップに付与する
衝突条件付与方法。

【請求項7】

コンピュータを、
複数の頂点、及び前記頂点間を接続する複数の辺を含むロードマップ上を複数のエージェントが移動する場合において、前記頂点に存在する前記エージェント又は前記辺を移動する前記エージェント同士が衝突する可能性のある前記頂点と前記辺との第１ペア、及び前記エージェント同士が衝突する可能性のある一方の辺と他方の辺との第２ペアを抽出する抽出部、
前記第１ペアの各々の前記頂点に一方のエージェントが存在する時刻と、前記辺の端点から他方の前記エージェントが移動を開始する時刻との差、及び、前記第２ペアの各々の前記一方の辺の端点から一方のエージェントが移動を開始する時刻と、前記他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、前記一方のエージェントと前記他方のエージェントとの衝突が起こる時間区間を算出する算出部、及び、
前記第１ペアに含まれる前記頂点及び前記辺の識別情報と前記時間区間とを対応付けた情報、及び前記第２ペアに含まれる２つの前記辺の各々の識別情報と前記時間区間とを対応付けた情報を、前記エージェント同士が衝突する条件として前記ロードマップに付与する付与部
として機能させるための衝突条件付与プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、衝突条件付与装置、衝突条件付与方法、衝突条件付与プログラム、及び経路生成装置に関する。

【背景技術】

【0002】

一つの環境内を、大きさを持つ複数のエージェントが互いに衝突することなくそれぞれの出発点からそれぞれの目的点へと移動する経路を探索する問題は、ＭＡＰＦ（Multi-Agent Path Finding）と呼ばれる。一般的なＭＡＰＦ問題では、環境内でエージェントが移動可能な経路を表現したグリッドグラフ又はロードマップ上で、各エージェントの移動を離散的な時間の下で決定する。

【0003】

ＭＡＰＦ問題に関する技術として、例えば、走行路の競合、隣接したノードを考慮し、複数の無人車が効率よく目的ノードへ移動するための動作及び経路を計画する運行管理制御装置が提案されている（特許文献１）。この装置では、各無人車の移動経路及びノード予約シーケンスと、搬送物の位置や搬送先などと、各無人車の現在位置、移動方向などと、走行路上の各ノードの座標と、その接続関係およびコストなどと、ペトリネットモデルのデータとがメモリに記憶される。また、この装置は、メモリに記憶された情報を用いて、無人車の最適な走行経路、動作順序を決定する。

【0004】

また、例えば、複数の走行路を含む走行路ネットワークを走行する複数の移動体に対する運行計画を行う運航計画システムが提案されている（特許文献２）。このシステムは、複数の移動体の位置情報と、走行路ネットワークの構造を表す走行路構造情報と、走行路ネットワークにおける複数の指定領域について、複数の移動体に対してそれぞれ通過する１つ以上の指定領域の順序を指定した複数の経路計画と、に基づき、走行路で移動体同士の競合が発生しないように、複数の移動体が走行路を走行するタイミングを指定した複数の走行タイミング計画を生成する。

【0005】

また、シングルエージェントについて、移動障害物がある場合の経路探索を連続的な時間の下で行うＳＩＰＰ（Safe Interval Path Planning）という手法が提案されている（非特許文献１）。

【先行技術文献】

【特許文献】

【0006】

【特許文献1】特開平１０－３１２２１７号公報

【特許文献2】特開２０２０－１４９３７０号公報

【非特許文献】

【0007】

【非特許文献1】Mike Phillips and Maxim Likhachev, "SIPP: Safe Interval Path Planning for Dynamic Environments ", Computer Science 2011 IEEE International Conference on Robotics and Automation, 9 May 2011.

【発明の概要】

【発明が解決しようとする課題】

【0008】

しかしながら、各エージェントの移動を離散的な時間の下で決定する場合、時間を全てのエージェントに共通のターンに分け、エージェントの行動はターン毎に決定されることになる。これによりエージェント間の衝突を避けるための制限が大きくなり、経路探索が困難になる場合があるという問題がある。

【0009】

離散的な時間におけるＭＡＰＦ問題を、ＳＩＰＰのような連続的な時間を扱う手法と組み合わせることで、ＭＡＰＦ問題を連続的な時間の場合に拡張することも考えられる。しかし、非グリッドグラフであるロードマップにＳＩＰＰを適用する場合、エージェント同士の衝突判定が計算上のボトルネックになる。なぜなら、ＳＩＰＰを適用するためには、ロードマップ上の全ての移動障害物と、どのタイミングで、どの箇所で衝突が起こり得るかを事前に把握しておく必要がある。そのため、マルチエージェントの場合、何らかの優先度順に１つずつエージェントの経路を決定していくことになる。この場合、エージェントの数が増えると、膨大な計算が必要となり、ＭＡＰＦ問題の解である各エージェントの経路を、現実的な時間で生成することが困難となる。

【0010】

本発明は、上記の点に鑑みてなされたものであり、ロードマップ上のマルチエージェントの連続的な時間における経路生成を短時間で行うことを可能にすることを目的とする。

【課題を解決するための手段】

【0011】

上記目的を達成するために、本発明に係る衝突条件付与装置は、複数の頂点、及び前記頂点間を接続する複数の辺を含むロードマップ上を複数のエージェントが移動する場合において、前記頂点に存在する前記エージェント又は前記辺を移動する前記エージェント同士が衝突する可能性のある前記頂点と前記辺との第１ペア、及び前記エージェント同士が衝突する可能性のある一方の辺と他方の辺との第２ペアを抽出する抽出部と、前記第１ペアの各々の前記頂点に一方のエージェントが存在する時刻と、前記辺の端点から他方の前記エージェントが移動を開始する時刻との差、及び、前記第２ペアの各々の前記一方の辺の端点から一方のエージェントが移動を開始する時刻と、前記他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、前記一方のエージェントと前記他方のエージェントとの衝突が起こる時間区間を算出する算出部と、前記第１ペアに含まれる前記頂点及び前記辺の識別情報と前記時間区間とを対応付けた情報、及び前記第２ペアに含まれる２つの前記辺の各々の識別情報と前記時間区間とを対応付けた情報を、前記エージェント同士が衝突する条件として前記ロードマップに付与する付与部と、を含んで構成される。

【0012】

また、前記抽出部は、与えられた点の集合の中で点間の距離が一定以下の点のペアを列挙するアルゴリズム、与えられた点の集合の中で与えられた領域内に存在する点を列挙するアルゴリズム、及び与えられた線分の集合の中で交差している線分のペアを列挙するアルゴリズムを用いて、前記ロードマップに含まれる前記複数の頂点及び前記複数の辺の中から前記第１ペア及び前記第２ペアを抽出してもよい。

【0013】

また、前記抽出部は、前記エージェントを直径が所定値の円と仮定した場合、前記ロードマップに含まれる前記複数の頂点及び前記複数の辺から、辺の端点の少なくとも一方と頂点との距離が前記所定値未満の場合、及び頂点から辺へ引いた垂線の長さが前記所定値未満の場合の少なくとも一方を満たす頂点と辺とのペアを前記第１ペアとして抽出し、一方の辺の端点の少なくとも一方と他方の辺との距離が前記所定値未満の場合、及び辺同士が交差している場合の少なくとも一方を満たす辺と辺とのペアを前記第２ペアとして抽出してもよい。

【0014】

また、上記目的を達成するために、本発明に係る経路生成装置は、上記の衝突条件付与装置により前記エージェント同士が衝突する条件が付与された前記ロードマップ上を移動する複数のエージェントの各々の経路であって、前記経路に含まれる前記頂点に前記エージェントが存在する時刻及び前記辺の端点から前記エージェントが移動を開始する時刻が、前記条件の時間区間に含まれないような経路を生成する生成部を含んで構成される。

【0015】

また、前記生成部は、前記エージェントの経路を１つずつ順次生成する場合において、第１エージェントの経路を生成し、次に第２エージェントの経路を生成する際に、前記第１エージェントについて生成された経路に含まれる前記頂点に前記エージェントが存在する時刻及び前記辺の端点から前記エージェントが移動を開始する時刻との差が、前記時間区間に含まれない時刻に、前記第２エージェントが前記頂点に存在するか、前記辺の端点から移動を開始するような経路を生成してもよい。

【0016】

また、上記目的を達成するために、本発明に係る衝突条件付与方法は、抽出部が、複数の頂点、及び前記頂点間を接続する複数の辺を含むロードマップ上を複数のエージェントが移動する場合において、前記頂点に存在する前記エージェント又は前記辺を移動する前記エージェント同士が衝突する可能性のある前記頂点と前記辺との第１ペア、及び前記エージェント同士が衝突する可能性のある一方の辺と他方の辺との第２ペアを抽出し、算出部が、前記第１ペアの各々の前記頂点に一方のエージェントが存在する時刻と、前記辺の端点から他方の前記エージェントが移動を開始する時刻との差、及び、前記第２ペアの各々の前記一方の辺の端点から一方のエージェントが移動を開始する時刻と、前記他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、前記一方のエージェントと前記他方のエージェントとの衝突が起こる時間区間を算出し、付与部が、前記第１ペアに含まれる前記頂点及び前記辺の識別情報と前記時間区間とを対応付けた情報、及び前記第２ペアに含まれる２つの前記辺の各々の識別情報と前記時間区間とを対応付けた情報を、前記エージェント同士が衝突する条件として前記ロードマップに付与する方法である。

【0017】

また、上記目的を達成するために、本発明に係る衝突条件付与プログラムは、コンピュータを、複数の頂点、及び前記頂点間を接続する複数の辺を含むロードマップ上を複数のエージェントが移動する場合において、前記頂点に存在する前記エージェント又は前記辺を移動する前記エージェント同士が衝突する可能性のある前記頂点と前記辺との第１ペア、及び前記エージェント同士が衝突する可能性のある一方の辺と他方の辺との第２ペアを抽出する抽出部、前記第１ペアの各々の前記頂点に一方のエージェントが存在する時刻と、前記辺の端点から他方の前記エージェントが移動を開始する時刻との差、及び、前記第２ペアの各々の前記一方の辺の端点から一方のエージェントが移動を開始する時刻と、前記他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、前記一方のエージェントと前記他方のエージェントとの衝突が起こる時間区間を算出する算出部、及び、前記第１ペアに含まれる前記頂点及び前記辺の識別情報と前記時間区間とを対応付けた情報、及び前記第２ペアに含まれる２つの前記辺の各々の識別情報と前記時間区間とを対応付けた情報を、前記エージェント同士が衝突する条件として前記ロードマップに付与する付与部として機能させるためのプログラムである。

【発明の効果】

【0018】

本発明に係る衝突条件付与装置、方法、プログラム、及び経路生成装置によれば、ロードマップ上のマルチエージェントの連続的な時間における経路生成を短時間で行うことを可能にすることができる。

【図面の簡単な説明】

【0019】

【図1】経路生成装置のハードウェア構成を示すブロック図である。

【図2】経路生成装置の機能構成の例を示すブロック図である。

【図3】ロードマップの一例を示す図である。

【図4】ロードマップのデータ構成の一例を示す図である。

【図5】頂点－辺ペア及び辺－辺ペアを説明するための図である。

【図6】衝突条件の時間区間を説明するための図である

【図7】衝突条件付きロードマップのデータ構成の一例を示す図である。

【図8】生成される経路の一例を示す図である。

【図9】時間区間を含む衝突条件を用いた経路生成を説明するための図である。

【図10】衝突条件に時間区間を含まない場合の経路生成を説明するための図である。

【図11】デッドロックが生じている例を示す図である。

【図12】衝突条件付与処理の流れを示すフローチャートである。

【図13】経路生成処理の流れを示すフローチャートである。

【図14】本手法と比較手法とを比較した実験結果を示すグラフである。

【発明を実施するための形態】

【0020】

以下、本発明の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

【0021】

図１は、本実施形態に係る経路生成装置１０のハードウェア構成を示すブロック図である。図１に示すように、経路生成装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１２、メモリ１４、記憶装置１６、入力装置１８、出力装置２０、記憶媒体読取装置２２、及び通信Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）２４を有する。各構成は、バス２６を介して相互に通信可能に接続されている。

【0022】

記憶装置１６には、後述する衝突条件付与処理及び経路生成処理を実行するための経路生成プログラムが格納されている。ＣＰＵ１２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ１２は、記憶装置１６からプログラムを読み出し、メモリ１４を作業領域としてプログラムを実行する。ＣＰＵ１２は、記憶装置１６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。

【0023】

メモリ１４は、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置１６は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

【0024】

入力装置１８は、例えば、キーボードやマウス等の、各種の入力を行うための装置である。出力装置２０は、例えば、ディスプレイやプリンタ等の、各種の情報を出力するための装置である。出力装置２０として、タッチパネルディスプレイを採用することにより、入力装置１８として機能させてもよい。記憶媒体読取装置２２は、ＣＤ（Ｃｏｍｐａｃｔ Ｄｉｓｃ）－ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）－ＲＯＭ、ブルーレイディスク、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の各種記憶媒体に記憶されたデータの読み込みや、記憶媒体に対するデータの書き込み等を行う。通信Ｉ／Ｆ２４は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0025】

次に、本実施形態に係る経路生成装置１０の機能構成について説明する。図２は、経路生成装置１０の機能構成の例を示すブロック図である。図２に示すように、経路生成装置１０は、機能構成として、衝突条件付与部３０と、生成部３８とを含む。衝突条件付与部３０は、さらに、抽出部３２と、算出部３４と、付与部３６とを含む。衝突条件付与部３０は、本発明の「衝突条件付与装置」の一例である。各機能構成は、ＣＰＵ１２が記憶装置１６に記憶された経路生成プログラムを読み出し、メモリ１４に展開して実行することにより実現される。

【0026】

衝突条件付与部３０は、ロードマップ４０を取得する。図３に、ロードマップ４０の一例を示す。図３に示すように、ロードマップ４０は、複数の頂点、及び頂点間を接続する複数の辺を含む。図３において、黒点が頂点、頂点間を接続する線分が辺である。図３に示すように、ロードマップ４０は、グリッドグラフとは異なり、各辺の長さは共通ではなく、各頂点は二次元平面で任意の位置をとることができ、また、辺同士が交差する場合も許容する平面グラフである。

【0027】

図４に、ロードマップ４０のデータ構成の一例を示す。図４の例では、ロードマップ４０は、頂点情報テーブル４２と、辺情報テーブル４４とを含む。頂点情報テーブル４２には、各頂点の識別情報である頂点ＩＤと、各頂点の位置情報とが対応付けて記憶されている。辺情報テーブル４４には、各辺の識別情報である辺ＩＤと、各辺の２つの端点に相当する頂点の頂点ＩＤとが対応付けて記憶されている。

【0028】

衝突条件付与部３０は、取得したロードマップ４０に衝突条件（詳細は後述）を付与して、衝突条件付きロードマップ５０として記憶する。以下、ロードマップ４０に衝突条件を付与する機能構成である、衝突条件付与部３０の抽出部３２、算出部３４、及び付与部３６の各々について説明する。

【0029】

抽出部３２は、ロードマップ４０上を複数のエージェントが移動する場合において、頂点に存在するエージェント又は辺を移動するエージェント同士が衝突する可能性のある箇所を抽出する。抽出部３２は、衝突する可能性のある個所として、図５に示すような頂点と辺とのペア、及び辺と辺とのペアを抽出する。この頂点と辺とのペアは、本発明の「第１ペア」の一例であり、辺と辺とのペアは、本発明の「第２ペア」の一例である。以下では、頂点と辺とのペアを「頂点－辺ペア」、辺と辺とのペアを「辺－辺ペア」と表記する。

【0030】

図５の頂点－辺ペアでは、Ａ１は、ある頂点に存在するエージェント、Ａ２は、ある辺の端点から移動を開始するエージェントを表している。この場合、エージェントＡ２が辺に沿って移動することにより、ある頂点に存在するエージェントＡ１と衝突する可能性があるため、このような頂点－辺ペアが衝突する可能性のある個所として抽出される。また、図５の辺－辺ペアでは、Ａ１は、ある辺の端点から移動を開始するエージェント、Ａ２は、他の辺の端点から移動を開始するエージェントを表している。この場合、エージェントＡ１及びＡ２がそれぞれ辺に沿って移動することにより衝突する可能性があるため、このような辺－辺ペアが衝突する可能性のある個所として抽出される。

【0031】

具体的には、抽出部３２は、以下のようなアルゴリズムを用いて、ロードマップ４０に含まれる複数の頂点及び複数の辺の中から、頂点－辺ペア及び辺－辺ペアを抽出する。
（１）与えられた点の集合の中で点間の距離が一定以下の点のペアを列挙するアルゴリズム
（２）与えられた点の集合の中で与えられた領域内に存在する点を列挙するアルゴリズム
（３）与えられた線分の集合の中で交差している線分のペアを列挙するアルゴリズム

【0032】

上記（１）については、Fixed -Radius Near Neighbors Algorithm等が知られている。このアルゴリズムは、点をグリッド上のバケットに分け、隣接又は同じバケットにある点のペアについて距離を計算するというものである。このアルゴリズムの時間計算量はＯ（Ｖ＋Ｍ）である。ただし、Ｖは点の総数、Ｍは見つけられるペアの数である。

【0033】

上記（２）についてはPartition tree等が知られている。このアルゴリズムは、事前に点の集合をうまく再帰的に分割することで領域探索を楽にするものである。この他にも、上記（２）のアルゴリズムとしては、事前計算は重いが一度のクエリが軽いものなど、種々のアルゴリズムがある。

【0034】

上記（３）については、Bentley-Ottmann Algorithm等が知られている。このアルゴリズムは平面走査しながら走査線と交わっている線分を管理し、交差を検出するものである。このアルゴリズムの時間計算量はＯ（（Ｅ＋Ｍ）ｌｏｇＥ）である。ただし、Ｅは線分の総数、Ｍは見つけられるペアの数である。

【0035】

より具体的には、抽出部３２は、ロードマップ４０が二次元平面上にあり、辺は全て線分で、エージェントは全て同じ半径ｒの円形であるという仮定の下、下記の方法により頂点－辺ペア及び辺－辺ペアを抽出する。なお、ロードマップ４０において、頂点を多くとれば曲線は線分で近似できる。また、エージェントは衝突条件を厳しくするために大きめの円で形を近似してもよい。したがって、上記仮定は実用上問題にならないと考えられる。

【0036】

抽出部３２は、（ｉ）辺の端点の少なくとも一方と頂点との距離が２ｒ未満の場合、及び（ｉｉ）頂点から辺へ引いた垂線の長さが２ｒ未満の場合の少なくとも一方を満たす頂点と辺とのペアを頂点－辺ペアとして抽出する。（ｉ）を満たすペアは上記（１）のアルゴリズムを適用することで、（ｉｉ）を満たすペアは上記（２）のアルゴリズムを適用することで効率的に抽出することができる。

【0037】

また、抽出部３２は、一方の辺の端点の少なくとも一方と他方の辺との距離が２ｒ未満の場合、及び（ｉｉ）辺同士が交差している場合の少なくとも一方を満たす辺と辺とのペアを辺－辺ペアとして抽出する。（ｉ）を満たすペアは上記（２）のアルゴリズムを適用することで、（ｉｉ）を満たすペアは上記（３）のアルゴリズムを適用することで効率的に抽出することができる。

【0038】

算出部３４は、抽出部３２により抽出された頂点－辺ペア及び辺－辺ペアの各々について、その頂点に存在するエージェント又は辺を移動するエージェント同士の衝突が起こる時間的な条件を算出する。

【0039】

具体的には、算出部３４は、頂点－辺ペアの頂点に一方のエージェントが存在する時刻と、辺の端点から他方のエージェントが移動を開始する時刻との差について、一方のエージェントと他方のエージェントとの衝突が起こる時間区間を算出する。また、算出部３４は、辺－辺ペアの一方の辺の端点から一方のエージェントが移動を開始する時刻と、他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、一方のエージェントと他方のエージェントとの衝突が起こる時間区間を算出する。

【0040】

例えば、図６に示すように、辺Ｅ１の端点を時刻ｔ１にエージェントＡ１が出発し、辺Ｅ２の端点を時刻ｔ２にエージェントＡ２が出発するとする。図６の例の場合、辺Ｅ１上を移動するエージェントＡ１と、辺Ｅ２上を移動するエージェントＡ２とが衝突する可能性がある箇所は辺Ｅ１と辺Ｅ２とが交差する付近（図６中の網掛部分）である。また、エージェントＡ１とエージェントＡ２とが衝突するか否かは、時刻ｔ１と時刻ｔ２との差に依存する。例えば、図６の例で、エージェントＡ１及びＡ２が同じ速度で移動する場合、ｔ１とｔ２とがほぼ同時刻であれば、エージェントＡ１とエージェントＡ２とは衝突しない、すなわち、両エージェントはすれ違える。一方、ｔ１よりｔ２の方が所定時間後であり、この時間差により、両エージェントが衝突する可能性がある箇所に同時に到達する場合には、衝突が発生する。

【0041】

このような状況を考慮して、算出部３４は、時刻ｔ１と時刻ｔ２との時間差（ｔ１－ｔ２）が時間区間ａ～ｂの範囲内のときに、両エージェントが衝突するという場合の時間区間ａ～ｂを算出する。算出部３４は、例えば、Collision Detection for Agents in Multi-Agent Pathfinding（参考文献１）の手法を適用して、時間区間ａ～ｂを算出する。
参考文献１：Thayne T. Walker, Nathan R. Sturtevant, "Collision Detection for Agents in Multi-Agent Pathfinding", moarXiv:1908.09707v3 [cs.RO], 15 Nov 2019.

【0042】

付与部３６は、頂点－辺ペアに含まれる頂点の頂点ＩＤ及び辺の辺ＩＤと、その頂点－辺ペアについて算出部３４で算出された時間区間とを含むタプルを衝突条件情報として生成する。同様に、付与部３６は、辺－辺ペアに含まれる２つの辺の各々の辺ＩＤと、その辺－辺ペアについて算出部３４で算出された時間区間とを含むタプルを衝突条件情報として生成する。そして、付与部３６は、生成したタプルの集合を、ロードマップ４０に付与し、衝突条件付きロードマップ５０として、経路生成装置１０の所定の記憶領域に記憶する。

【0043】

図７に、衝突条件付きロードマップ５０のデータ構成の一例を示す。図７の例では、衝突条件付きロードマップ５０は、図４に示すロードマップ４０のデータ構成に衝突条件４６が付与されたものとなっている。なお、図７において、衝突条件の１つであるタプル（Ｅ１，Ｅ２，ａ＜ｔ１－ｔ２＜ｂ）は、辺Ｅ１及びＥ２からなる辺－辺ペアの辺Ｅ１及び辺Ｅ２の各々の端点をエージェントが出発する時刻ｔ１と時刻ｔ２との差の時間区間がａ～ｂであることを表している。

【0044】

生成部３８は、衝突条件付きロードマップ５０上を移動する複数のエージェントの各々の経路を生成する。具体的には、生成部３８は、経路に含まれる頂点にエージェントが存在する時刻及び辺の端点からエージェントが移動を開始する時刻が、衝突条件の時間区間に含まれないような経路を生成する。例えば、生成部３８は、図８に示すように、エージェント毎に、出発点となる頂点から目的点となる頂点までに経由する各頂点と、その頂点から次の頂点へ向けて出発する時刻との組み合わせの系列を経路として生成する。

【0045】

より具体的には、生成部３８は、エージェントの経路を１つずつ順次生成する場合において、第１エージェントの経路を生成し、次に第２エージェントの経路を生成する。生成部３８は、例えば、Ａ＊アルゴリズムを用いて各エージェントの経路を生成する。この場合、生成部３８は、出発点から、ある頂点を通って目的点まで辿り着く場合の最短経路を探索し、既に最短の到達時刻が確定している頂点Ｖ及び頂点Ｖから出ている辺を選択する。生成部３８は、選択した頂点Ｖ及び頂点Ｖから出ている辺が含まれる衝突条件を、衝突条件付きロードマップ５０に付与された衝突条件から取得する。該当する衝突条件が存在する場合、生成部３８は、先に生成された他のエージェントの経路のうち、選択した辺の端点を出発する時刻を取得する。生成部３８は、取得した衝突条件に含まれる時間区間を制約として、頂点Ｖの出発時刻を調整し、辺を辿った先にある頂点への到達時刻の候補を求める。生成部３８は、この処理を、目的点への最短の到達時刻が確定するまで続け、探索の履歴をもとに目的点から最適な辺を逆に辿ることで経路を生成する。

【0046】

例えば、図９に示すように、エージェントＡ１について、辺Ｅ１を経由する経路であって、辺Ｅ１の端点である頂点Ｖ１から時刻τ１１に移動を開始して辺Ｅ１に沿って移動する経路が生成されているとする。次に、エージェントＡ２の経路の生成において、ある辺から頂点Ｖ２に到達し、頂点Ｖ２を端点とする辺Ｅ２が選択されたとする。また、辺Ｅ１と辺Ｅ２とは、辺－辺ペアとして抽出されており、（Ｅ１，Ｅ２，ａ＜ｔ１－ｔ２＜ｂ）という衝突条件が付与されているとする。この場合、生成部３８は、ａ＜τ１１－τ２１＜ｂを満たさないような時刻τ２１に頂点Ｖ２を出発して辺Ｅ２を移動する経路を生成する。生成部３８は、時間区間を満たさないような時刻τ２１が存在しない場合、又は、そのような時刻τ２１を採用した場合に頂点Ｖ２でのエージェントの待ち時間が所定時間以上になる場合には、頂点Ｖ２を端点とする他の辺を選択し直す。また、１つ前の辺にさかのぼって次の辺を選択し直してもよい。

【0047】

なお、生成部３８は、各エージェントの経路の生成のアルゴリズムとして、出発点から順次、次の辺を選択することで経路を生成するアルゴリズムを採用してもよい。この場合も、辺を選択する際に、衝突条件に基づく出発時刻の調整を行えばよい。

【0048】

ここで、図１０を参照して、衝突条件に時間区間が含まれない場合であって、離散的な時間の下で、グリッドグラフを用いて経路生成を行う場合について説明する。ある時刻ｔにおけるエージェントＡ１及びエージェントＡ２の移動方向が矢印で示す方向の場合、両エージェントの衝突を回避するため、図１０の網掛で示す箇所が衝突する可能性のある個所として、経路生成の際の制約となる。

【0049】

また、特許文献１に記載の発明のように、ペトリネットモデルを用いた経路計画等でも、衝突箇所に時間的条件を示さず、少しでも衝突している箇所は、同時に使うことを禁止している。これらは、衝突の可能性を実際より大きく見積もっており、経路生成の計算コストの増加や、図１１に示すようなデッドロックの原因になりうる。

【0050】

一方、本実施形態では、衝突条件に時間区間を含めることで、衝突の可能性がある箇所において、同時に使うことが禁止される部分は、図６に示すように、必要最小限の範囲とすることができる。そのため、探索される解（経路）の空間が増え、解が見つかる機会が増加することで、経路生成の成功率が上昇すると共に、経路生成のための計算コストを削減できる。

【0051】

次に、本実施形態に係る経路生成装置１０の作用について説明する。図１２は、経路生成装置１０のＣＰＵ１２により実行される衝突条件付与処理の流れを示すフローチャートである。また、図１３は、経路生成装置１０のＣＰＵ１２により実行される経路生成処理の流れを示すフローチャートである。ＣＰＵ１２が記憶装置１６から経路生成プログラムを読み出して、メモリ１４に展開して実行することにより、ＣＰＵ１２が経路生成装置１０の各機能構成として機能し、図１２に示す衝突条件付与処理、及び図１３に示す経路生成処理が実行される。以下、衝突条件付与処理、及び経路生成処理の各々について詳述する。

【0052】

まず、図１２に示す衝突条件付与処理について説明する。

【0053】

ステップＳ１０で、抽出部３２が、ロードマップ４０を取得する。次に、ステップＳ１２で、抽出部３２が、ロードマップ４０上を複数のエージェントが移動する場合において、頂点に存在するエージェント又は辺を移動するエージェント同士が衝突する可能性のある箇所として、頂点－辺ペア及び辺－辺ペアを抽出する。

【0054】

次に、ステップＳ１４で、算出部３４が、頂点－辺ペアの頂点に一方のエージェントが存在する時刻と、辺の端点から他方のエージェントが移動を開始する時刻との差について、両エージェントの衝突が起こる時間区間を算出する。また、算出部３４が、辺－辺ペアの一方の辺の端点から一方のエージェントが移動を開始する時刻と、他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、両エージェントの衝突が起こる時間区間を算出する。

【0055】

次に、ステップＳ１６で、付与部３６が、頂点－辺ペアの頂点ＩＤ及び辺の辺ＩＤ、及び、辺－辺ペアの２つの辺ＩＤの各々と、算出された時間区間とを含むタプルを衝突条件情報として生成する。そして、付与部３６が、生成したタプルの集合を、ロードマップ４０に付与し、衝突条件付きロードマップ５０として、経路生成装置１０の所定の記憶領域に記憶し、衝突条件付与処理は終了する。

【0056】

次に、図１３に示す経路生成処理について説明する。ここでは、各エージェントにエージェントＩＤとして、Ａｉ（ｉ＝１，２，・・・，Ｎ、Ｎはエージェントの個数）が付与されているものとする。

【0057】

ステップＳ３０で、生成部３８が、エージェントＩＤを特定する変数ｉに１を設定する。次に、ステップＳ３２のループ処理で、衝突条件付きロードマップ５０を用いて、エージェントＡｉの出発点から、ある頂点を通って目的点まで辿り着く場合の最短経路を探索する。詳細には、ステップＳ３４で、生成部３８が、既に最短の到達時刻が確定している頂点Ｖ及び頂点Ｖから出ている辺を選択する。

【0058】

次に、ステップＳ３６で、生成部３８が、選択した頂点Ｖ及び頂点Ｖから出ている辺が含まれる衝突条件を、衝突条件付きロードマップ５０に付与された衝突条件から取得する。また、生成部３８が、先に生成された他のエージェントの経路のうち、取得した衝突条件に含まれる辺の端点の出発時刻ｔ１を取得する。

【0059】

次に、ステップＳ３８で、生成部３８が、頂点ＶをエージェントＡｉが出発する出発時刻ｔ２を、取得した他のエージェントの出発時刻ｔ１との差が、取得した衝突条件に含まれる時間区間外となるように調整し、辺を辿った先にある頂点への到達時刻の候補を求める。調整可能な時刻が存在しない場合には、ステップＳ３４に戻り、生成部３８が頂点Ｖ及び頂点Ｖから出ている辺を選択し直す。生成部３８は、上記ステップＳ３４～Ｓ３８の処理を、目的点への最短の到達時刻が確定するまで繰り返す。目的点への最短の到達時刻が確定した場合は、ステップＳ３２のループ処理を終了し、ステップＳ４０へ移行する。ステップＳ４０では、生成部３８が、探索の履歴をもとに目的点から最適な辺を逆に辿ることで経路を生成する。

【0060】

次に、ステップＳ４２で、生成部３８が、ｉがエージェントの個数であるＮとなったか否かを判定する。ｉ＝Ｎの場合には、ステップＳ４６へ移行し、ｉ≠Ｎの場合には、ステップＳ４４へ移行する。ステップＳ４４では、生成部３８が、ｉを１インクリメントして、ステップＳ３２に戻る。ステップＳ４６では、生成部３８が、上記ステップＳ４０で生成された各エージェントの経路を出力し、経路生成処理は終了する。

【0061】

以上説明したように、本実施形態に係る経路生成装置は、衝突条件付与部が、ロードマップ上を複数のエージェントが移動する場合において、エージェント同士が衝突する可能性のある頂点－辺ペア、辺－辺ペアを抽出する。そして、衝突条件付与部が、頂点－辺ペアの頂点に一方のエージェントが存在する時刻と、辺の端点から他方のエージェントが移動を開始する時刻との差について、両エージェントの衝突が起こる時間区間を算出する。また、衝突条件付与部が、辺－辺ペアの一方の辺の端点から一方のエージェントが移動を開始する時刻と、他方の辺の端点から他方のエージェントが移動を開始する時刻との差について、両エージェントの衝突が起こる時間区間を算出する。そして、衝突条件付与部が、各ペアに含まれる頂点及び辺のＩＤと算出した時間区間とを対応付けた衝突条件をロードマップに付与する。このように、衝突の可能性がある箇所に時間区間を含む衝突条件が付与されたロードマップを事前に用意することで、ロードマップ上のマルチエージェントの連続的な時間における経路生成を短時間で行うことができる。

【0062】

また、衝突条件に時間区間を含めることで、衝突する箇所を最小限に見積もることができるため、探索される解（経路）の空間が増え、解が見つかる機会が増加することで、経路生成の成功率が上昇すると共に、経路生成のための計算コストを削減できる。

【0063】

現状においては、環境をグリッドに分け、グリッドに特化したプランニングアルゴリズムを用いるか、又は、人の手で作られたロードマップや、何らかの手法により得られたロードマップを用い、ＣＣＢＳ（Continuous Conflict Based Search）のような衝突判定のみを必要とするアルゴリズムにより経路生成を行う手法が考えられる。前者の手法は、環境の形状がグリッドで表現し難い場合には、適用が困難である。また、後者の手法として一般的に知られているアルゴリズムには時間計算量の現実的な保証がないため、経路生成に時間がかかる場合がある。

【0064】

本実施形態のようにロードマップに対して、時間区間を含む衝突条件を付与することで、様々なロードマップに対して、ＰＳＩＰＰ（Prioritized Safe Interval Path Planning）のような、時間コストの小さいアルゴリズムを用いて経路を得ることができる。

【0065】

また、本実施形態では、衝突条件付与部が、衝突の可能性がある箇所を抽出する際に、既存のアルゴリズムを用いて、効率的に該当箇所である頂点－辺ペア及び辺－辺ペアを抽出することができる。

【0066】

ここで、本実施形態の手法（以下、「本手法」という）と比較手法とを比較した実験結果について説明する。本実験では、障害物のある環境上でよく知られた方法ＰＲＭ（Probabilistic Road Map）で構築されたロードマップ上のＭＡＰＦ問題を解いた。また、既存手法であるＣＣＢＳ、及び時間区間を含む衝突条件が付与されていないロードマップを用いたＰＳＩＰＰを比較手法とした。

【0067】

図１４は、経路生成（プランニング）の成功率のグラフであり、横軸はエージェント数である。また、左のグラフは、障害物がある環境での実験結果、右のグラフは障害物がない環境での実験である。比較手法では、数十台のプランニングが限界であったのに対し、本手法では１００台以上のエージェントについてもプランニングに成功した。さらに、障害物のない環境では、２０００台近いエージェントについても、３０秒の時間制限の下でプランニングに成功した。

【0068】

本発明は、工場や倉庫など、決められた場所の中で、多数のロボットが物品の運搬や見回り、清掃等の作業のため移動を行う状況において、それらのロボットの安全な経路を短時間で計算することに応用できる。

【0069】

なお、上記実施形態では、経路生成装置の中に衝突条件付与部を含む構成について説明したが、衝突条件付与部の機能構成を含む衝突条件付与装置と、生成部を含む経路生成装置とを、それぞれ異なる装置で実現してもよい。この場合、衝突条件付与装置から出力される衝突条件付きロードマップは、ＰＳＩＰＰ以外の手法を適用した経路生成装置で利用されてもよいし、経路生成以外の用途で使用されてもよい。

【0070】

また、上記実施形態では、全てのエージェントのサイズが同一である仮定で説明したが、エージェントのサイズはそれぞれ異なっていてもよい。この場合、頂点－辺ペア及び辺－辺ペアを抽出する際に用いるエージェントの半径ｒとして、サイズが最大のエージェントの外接円の半径を用いればよい。

【0071】

また、エージェントの速度も等速でなくてもよく、エージェント毎かつ辺毎に、ある決められた速度制限及び加速度制限の下で加減速を行うようにしてもよい。この場合、エージェントの加減速を考慮して、衝突条件に含まれる時間区間との比較を行うようにすればよい。

【0072】

また、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した衝突条件付与処理及び経路生成処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、衝突条件付与処理及び経路生成処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

【0073】

また、上記実施形態では、経路生成プログラムが記憶装置に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、ブルーレイディスク、ＵＳＢメモリ等の記憶媒体に記憶された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

【符号の説明】

【0074】

１０ 経路生成装置
１２ ＣＰＵ
１４ メモリ
１６ 記憶装置
１８ 入力装置
２０ 出力装置
２２ 記憶媒体読取装置
２４ 通信Ｉ／Ｆ
２６ バス
３０ 衝突条件付与部
３２ 抽出部
３４ 算出部
３６ 付与部
３８ 生成部
４０ ロードマップ
４２ 頂点情報テーブル
４４ 辺情報テーブル
４６ 衝突条件
５０ 衝突条件付きロードマップ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】