(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024152382
(43)【公開日】2024-10-25
(54)【発明の名称】経路計画装置
(51)【国際特許分類】
G05D 1/43 20240101AFI20241018BHJP
【ＦＩ】
G05D1/02 P
【審査請求】未請求
【請求項の数】9
【出願形態】ＯＬ
(21)【出願番号】P 2023066537
(22)【出願日】2023-04-14
(71)【出願人】
【識別番号】000005108
【氏名又は名称】株式会社日立製作所
(74)【代理人】
【識別番号】110002572
【氏名又は名称】弁理士法人平木国際特許事務所
(72)【発明者】
【氏名】石原 新士
(72)【発明者】
【氏名】小谷 匡士
(72)【発明者】
【氏名】金井 政樹
(72)【発明者】
【氏名】杉本 和也
(72)【発明者】
【氏名】成川 理優
(72)【発明者】
【氏名】大塚 敏之
【テーマコード（参考）】
5H301
【Ｆターム（参考）】
5H301AA02
5H301AA10
5H301BB07
5H301BB14
5H301CC03
5H301CC06
5H301CC10
5H301DD07
5H301DD17
5H301GG08
5H301GG14
5H301HH10
5H301LL01
5H301LL02
5H301LL03
5H301LL06
(57)【要約】
【課題】大量のエージェントを効率的に移動させる経路計画をリアルタイムに算出する装置を提供する。
【解決手段】制御可能な移動体であるエージェントの経路を生成する経路計画装置であって、エージェントの位置情報を管理するエージェント管理部、制御不可能な非制御対象の位置情報を取得する環境管理部、エージェント管理部が取得したエージェントの位置情報に基づいて、エージェントの移動目標位置を算出する業務管理部、エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の非制御対象の位置を管理する移動領域管理部、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、移動領域内のエージェントが制約領域に侵入しないようにエージェントの移動目標位置までの経路を算出する経路算出部、及び経路算出部が算出した経路をエージェントに配信する経路配信部を有する。
【選択図】図１
【解決手段】制御可能な移動体であるエージェントの経路を生成する経路計画装置であって、エージェントの位置情報を管理するエージェント管理部、制御不可能な非制御対象の位置情報を取得する環境管理部、エージェント管理部が取得したエージェントの位置情報に基づいて、エージェントの移動目標位置を算出する業務管理部、エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の非制御対象の位置を管理する移動領域管理部、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、移動領域内のエージェントが制約領域に侵入しないようにエージェントの移動目標位置までの経路を算出する経路算出部、及び経路算出部が算出した経路をエージェントに配信する経路配信部を有する。
【選択図】図１
【特許請求の範囲】
【請求項1】
制御可能な移動体であるエージェントの経路を生成する経路計画装置であって、
前記エージェントの位置情報を管理するエージェント管理部、
制御不可能な非制御対象の位置情報を取得する環境管理部、
前記エージェント管理部が取得した前記エージェントの位置情報に基づいて、前記エージェントの移動目標位置を算出する業務管理部、
前記エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の前記非制御対象の位置を管理する移動領域管理部、
前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、
前記移動領域内の前記エージェントが前記制約領域に侵入しないように前記エージェントの前記移動目標位置までの経路を算出する経路算出部、及び
前記経路算出部が算出した経路を前記エージェントに配信する経路配信部を有する、
ことを特徴とする経路計画装置。
前記エージェントの位置情報を管理するエージェント管理部、
制御不可能な非制御対象の位置情報を取得する環境管理部、
前記エージェント管理部が取得した前記エージェントの位置情報に基づいて、前記エージェントの移動目標位置を算出する業務管理部、
前記エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の前記非制御対象の位置を管理する移動領域管理部、
前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、
前記移動領域内の前記エージェントが前記制約領域に侵入しないように前記エージェントの前記移動目標位置までの経路を算出する経路算出部、及び
前記経路算出部が算出した経路を前記エージェントに配信する経路配信部を有する、
ことを特徴とする経路計画装置。
【請求項2】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出する、
ことを特徴とする経路計画装置。
【請求項3】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出し、
前記経路算出部は、前記エージェントの現在位置と目標位置との間の偏差が小さくなるほど評価値が小さくなる評価関数と、前記制約領域管理部が決定した制約条件とを拘束条件として、前記評価関数が演算ごとに前回の演算値よりも小さくなるように前記エージェントの経路を算出する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出し、
前記経路算出部は、前記エージェントの現在位置と目標位置との間の偏差が小さくなるほど評価値が小さくなる評価関数と、前記制約領域管理部が決定した制約条件とを拘束条件として、前記評価関数が演算ごとに前回の演算値よりも小さくなるように前記エージェントの経路を算出する、
ことを特徴とする経路計画装置。
【請求項4】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、前記移動領域内において、前記エージェントの移動目標位置から現在位置までの最短経路を含まない領域を、前記移動領域に含まれる非制御対象の個数の総和以下になるように前記制約領域として算出する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、前記移動領域内において、前記エージェントの移動目標位置から現在位置までの最短経路を含まない領域を、前記移動領域に含まれる非制御対象の個数の総和以下になるように前記制約領域として算出する、
ことを特徴とする経路計画装置。
【請求項5】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、前記移動領域内において該移動領域に含まれる前記非制御対象の個数の総和以下になるように制約領域を算出し、かつ、該制約領域の候補が複数ある場合には面積が最小のものを該制約領域として算出する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、前記移動領域内において該移動領域に含まれる前記非制御対象の個数の総和以下になるように制約領域を算出し、かつ、該制約領域の候補が複数ある場合には面積が最小のものを該制約領域として算出する、
ことを特徴とする経路計画装置。
【請求項6】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、算出した制約領域内に存在する前記非制御対象が移動体を含み、該移動体が前記制約領域から逸脱した、もしくは、事前に設定された時間内に逸脱することが予想される場合に、移動後の該移動体の位置を包含するように制約領域を再計算する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、算出した制約領域内に存在する前記非制御対象が移動体を含み、該移動体が前記制約領域から逸脱した、もしくは、事前に設定された時間内に逸脱することが予想される場合に、移動後の該移動体の位置を包含するように制約領域を再計算する、
ことを特徴とする経路計画装置。
【請求項7】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、算出した制約領域内に存在する前記非制御対象が移動体を含み、該移動体が前記制約領域から逸脱し、かつ最短経路上に存在する場合に、移動後の該移動体の位置を包含するように制約領域を分割する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、算出した制約領域内に存在する前記非制御対象が移動体を含み、該移動体が前記制約領域から逸脱し、かつ最短経路上に存在する場合に、移動後の該移動体の位置を包含するように制約領域を分割する、
ことを特徴とする経路計画装置。
【請求項8】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、前記移動領域内において、前記エージェントの移動目標位置から現在位置までの最短経路中に障害物がある場合には、該障害物を回避する領域を、前記移動領域に含まれる非制御対象の個数の総和以下になるように前記制約領域として算出する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、前記移動領域内において、前記エージェントの移動目標位置から現在位置までの最短経路中に障害物がある場合には、該障害物を回避する領域を、前記移動領域に含まれる非制御対象の個数の総和以下になるように前記制約領域として算出する、
ことを特徴とする経路計画装置。
【請求項9】
請求項１に記載の経路計画装置であって、
前記制約領域管理部は、前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出する際に、
（１）線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（２）線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
（３）非線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（４）非線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
の優先順位で該制約領域を算出する、
ことを特徴とする経路計画装置。
前記制約領域管理部は、前記移動領域内において、該移動領域に含まれる前記非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出する際に、
（１）線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（２）線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
（３）非線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（４）非線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
の優先順位で該制約領域を算出する、
ことを特徴とする経路計画装置。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、複数のエージェントが存在するエリアにおける移動経路計画を生成する経路計画装置に関わる。
【背景技術】
【0002】
物流倉庫の物品搬送や工場の工程間搬送の人手不足を解消するために、移動ロボット（ＡＧＶ：Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ、ＡＭＲ：Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔなど）の導入が進んでいる。このような移動ロボットを導入するには、倉庫や工場内にロボットが移動可能な通路を設定（節点（ノード）と枝（ブランチ／エッジ）からなるグラフなどの作成）が必要になる。この設定が細かいほど、ロボットが選択可能な通路の自由度は高くなるため、より効率的な搬送が可能になる。一方で、設定作業に多くの工数が必要になる。さらに、前記の通路の設定作業は、倉庫や工場のレイアウトが変更するたびに必要になるため、詳細な通路を設定する作業は、倉庫や工場を管理する事業者に大きな負担（エンジニアリングコスト）を求めることになる。
【0003】
また、倉庫や工場では、通路上に物品が仮置きされることもあるため、正確な地図情報を取得すること自体が困難である。なお、この課題は他の産業領域でも共通になり得る。例えば、行動を走行する自動運転車両を扱う場合、道路工事による一時的な道路規制に伴い、パイロンや看板が設置されるといった環境変化が生じるため、正確な地図情報の取得は難しい。また、施工現場や鉱山などで運用される自動化建設機械（油圧ショベル、ホイールローダ、ダンプトラックなど）を扱う場合は、掘削作業に伴い、地図情報が時々刻々と変化する。
【0004】
倉庫や工場内にロボットが１台だけで動作する状況は少ないため、ロボットが、他のロボットや、倉庫や工場内で作業をする作業員と接触しないように走行制御を実行する必要がある。
【0005】
このような課題に対して、特許文献１には、詳細な通路を設計することなく、複数のロボット（特許文献１中ではビークル）が移動する経路を算出する方法が提示されている。さらに、特許文献２には、ロボット（同じく、特許文献２中ではビークル）が移動経路内に存在する障害物（他のロボットや作業員に相当）と接触しないように経路を算出する方法が示されている。
【0006】
より具体的には、特許文献１では、各ロボットが障害物（建物内の柱や壁、自身以外のロボットなど）に接触せず、各ロボットの現在位置と各ロボットの目標位置をできるだけ小さくするようにモデル予測制御の考えに従って、各ロボットに対する制御入力（速度、角速度）を算出する。そして算出された制御入力を積分することで移動経路（位置、姿勢）を算出する。さらに、特許文献２では、ロボットを囲む楕円領域を定義し、この楕円領域の範囲内に障害物が侵入しないように拘束条件を与えた最適化問題を解くことで、ロボットが障害物に接触しない移動経路を算出している。
【先行技術文献】
【特許文献】
【0007】
【特許文献1】特開２０２１－７７０９０号公報
【特許文献2】特開２０２２－１３４９０５号公報
【発明の概要】
【発明が解決しようとする課題】
【0008】
特許文献１によれば、事前に倉庫や工場内に詳細な通路情報を設計せずに、ロボットが目標位置にたどり着く経路を算出することができる。また、全てのロボットのダイナミクスを考慮した移動計画を時系列で算出するため、事前に対象エリアを分割し、その分割区間内に複数のロボットが入らないように制御する排他制御方式に比べて、効率的な移動計画を生成し得る。さらに、特許文献１の技術に、特許文献２の技術を組み合わせることで、対象とするロボットを、対象エリア内の他のロボット、および、障害物と接触させない経路計画を生成できると考える。
【0009】
ただし、特許文献１では、対象エリアに存在する全てのロボットをまとめたダイナミクスを利用した最適化問題を解いている。また、特許文献２でも、ロボットを囲む楕円領域と障害物の関係に対して拘束条件を導出している。このような最適化問題の定式化においては、ロボットの台数、もしくは、障害物の数が増えるにつれて、急速に計算負荷が増加することが知られている。よって、特許文献１と特許文献２の方法を大量のロボットが存在するエリアに適用しようとすると、最適化計算に膨大な時間が必要になり、ロボットの移動計画を立案することが困難になることが予想される。
【0010】
本発明は以上のような課題を解決するためになされたものであり、大量のエージェント（ロボット、ビークル、車両などの総称）を効率的に移動させる経路計画をリアルタイムに算出する装置を提供することを目的としている。
【課題を解決するための手段】
【0011】
上記課題を解決するために、本発明に係る経路計画装置は、制御可能な移動体であるエージェントの経路を生成する経路計画装置であって、エージェントの位置情報を管理するエージェント管理部、制御不可能な非制御対象の位置情報を取得する環境管理部、エージェント管理部が取得したエージェントの位置情報に基づいて、エージェントの移動目標位置を算出する業務管理部、エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の非制御対象の位置を管理する移動領域管理部、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、移動領域内のエージェントが制約領域に侵入しないようにエージェントの移動目標位置までの経路を算出する経路算出部、及び経路算出部が算出した経路をエージェントに配信する経路配信部を有する。
【発明の効果】
【0012】
本発明の上記構成によれば、事前に詳細な地図構築を行うことなく、大量のエージェントを効率的に移動させる経路計画をリアルタイムに算出する装置を提供することが可能になる。
本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。上記した以外の課題、構成および効果については、以下の実施形態の説明により明らかにされる。
本発明に関連する更なる特徴は、本明細書の記述、添付図面から明らかになるものである。上記した以外の課題、構成および効果については、以下の実施形態の説明により明らかにされる。
【図面の簡単な説明】
【0013】
【図1】本発明の経路計画装置の構成を示す機能ブロック図。
【図2】本発明を物流倉庫に適用する場合の状況を説明する図。
【図3】本発明を物流倉庫に適用する場合に利用する地図情報の一例を説明する図。
【図4A】制御対象であるエージェントの構成を説明する図。
【図4B】制御対象であるエージェントの構成を説明する図。
【図5A】本発明における移動領域管理部の出力の一例を説明する図。
【図5B】本発明における移動領域管理部の出力の一例を説明する図。
【図6A】本発明における制約領域管理部の出力の一例を説明する図。
【図6B】本発明における制約領域管理部の出力の一例を説明する図。
【図6C】本発明における制約領域管理部の出力の一例を説明する図。
【図6D】本発明における制約領域管理部の出力の一例を説明する図。
【図6E】本発明における制約領域管理部の出力の一例を説明する図。
【図7A】本発明における制約領域管理部の処理を最短ルートとの関係において説明する図。
【図7B】本発明における制約領域管理部の処理を最短ルートとの関係において説明する図。
【図7C】本発明における制約領域管理部の処理を最短ルートとの関係において説明する図。
【図7D】本発明における制約領域管理部の処理を最短ルートとの関係において説明する図。
【図8A】本発明における制約領域管理部の処理を固定障害物との関係において説明する図。
【図8B】本発明における制約領域管理部の処理を固定障害物との関係において説明する図。
【図9A】本発明における制約領域管理部の処理を移動体との関係において説明する図。
【図9B】本発明における制約領域管理部の処理を移動体との関係において説明する図。
【図9C】本発明における制約領域管理部の処理を移動体との関係において説明する図。
【図10】本発明における制約領域管理部の処理手順を説明するフローチャート。
【図11A】本発明における経路算出部の処理を説明する図。
【図11B】本発明における経路算出部の処理を説明する図。
【図11C】本発明における経路算出部の処理を説明する図。
【図11D】本発明における経路算出部の処理を説明する図。
【図11E】本発明における経路算出部の処理を説明する図。
【図12A】複数台のエージェントに対する経路算出を行う状況を説明する図。
【図12B】複数台のエージェントに対する経路算出を行う状況を説明する図。
【図13】経路計画装置の処理手順を説明するフローチャート。
【図14】本発明を駐車場に適用する場合の駐車場全体の状況を説明する図。
【図15A】本発明を駐車場に適用した場合のエージェントの動作を説明する図。
【図15B】本発明を駐車場に適用した場合のエージェントの動作を説明する図。
【図15C】本発明を駐車場に適用した場合のエージェントの動作を説明する図。
【図15D】本発明を駐車場に適用した場合のエージェントの動作を説明する図。
【図15E】本発明を駐車場に適用した場合のエージェントの動作を説明する図。
【図16A】本発明を施工現場に適用する場合の建設機械の状況を説明する図。
【図16B】本発明を施工現場に適用する場合の状況を説明する図。
【図17】本発明を施工現場に適用する場合の現場全体の状況を説明する図。
【図18A】本発明を施工現場に適用した場合の移動領域管理部の出力の一例。
【図18B】本発明を施工現場に適用した場合の移動領域管理部の出力の一例。
【図19】本発明を施工現場に適用した場合の制約領域管理部の出力の一例。
【発明を実施するための形態】
【0014】
経路計画装置は管理エリア（例えば、倉庫や駐車場）内のすべてのエージェント（ロボット、車両など制御可能な移動体）の移動計画（座標、姿勢、速度などの時系列）を算出し、この経路計画に従って移動するようにエージェントの経路を生成する装置である。
【0015】
以下、本発明の経路計画装置の実施例を、図面を参照しつつ説明する。
【0016】
図１は本発明の経路計画装置の機能ブロック図の一例を簡易化した図である。ここで、本明細書においては、本発明に係る経路計画装置が適用される状況をまず説明し、当該状況との関係において経路計画装置の各機能部の説明を行う。
【0017】
図２は、本実施例に係る経路計画装置が適用される物流倉庫の全体図を示す。本図においては、複数のエージェント（Ａ１００ａ～Ａ１００ｃ）と作業員（Ａ１０１ａ～Ａ１０１ｃ）、そして、非自動運転のフォークリフトＡ１０２が存在する状況を想定する。エージェントは本実施例の経路計画装置が制御可能な自律ロボットである。
【0018】
なお、エージェントＡ１００、作業員Ａ１０１、フォークリフトＡ１０２の数が変化しても問題はない。また、フォークリフトが自動運転式であれば、エージェントＡ１００として扱っても良い。
【0019】
図２中グレーでハッチングされている棚Ａ１０３は固定されているが、破線で囲まれているパレット置き場Ａ１０４は、フォークリフトのオペレータが空きスペースを見つけてパレットを下すため、パレット位置が常に同じになるとは限らない。このため、倉庫内の正確な地図情報を取得するには、パレット位置が変化するたびに地図情報を更新する必要がある。さらに、倉庫内には作業員（Ａ１０１ａ～Ａ１０１ｃ）が仮置きした段ボールＡ１０５もあるため、これらも含めて地図情報をリアルタイムかつ正確に更新するには大きな計算コストが必要になる。このため、本実施例では、静的な地図として、倉庫形状と棚位置Ａ１０３ａ～Ａ１０３ｃのみ記録している図３のような簡易地図を利用する。この簡易地図に記録されている棚の位置は障害物として扱われる。本発明の経路計画装置は、この簡易地図に記載のある障害物にエージェントＡ１００が接触しないように走行経路を作成する。
【0020】
図２の物流倉庫には、監視センサＡ１０６が複数備えられており、倉庫内のエージェント（Ａ１００ａ～Ａ１００ｃ）の位置、作業員（Ａ１０１ａ～Ａ１０１ｃ）の位置、フォークリフトＡ１０２の位置、パレット置き場Ａ１０４のパレット位置、仮置きされた段ボールＡ１０５を監視している。監視センサＡ１０６は場内に設置されている無線通信装置Ａ１０７を介して、管理サーバＡ１０８と通信が可能である。管理サーバＡ１０８は倉庫内にあっても良いし、倉庫の外に配置されていても良い。本実施例の経路計画装置は演算のためのＣＰＵ及びメモリを備えた管理サーバＡ１０８によって実行される。管理サーバＡ１０８で算出された経路計画が無線通信装置Ａ１０７を介して、エージェント（Ａ１００ａ～Ａ１００ｃ）へと配信される。
【0021】
エージェントＡ１００は図４Ａに示すように、差動二輪型の移動ロボットＡ１００であることを想定する。移動ロボットには車輪（ホイール）Ａ１００１の回転数を検出するエンコーダや車体の回転速度、加速度を取得する慣性センサ：ＩＭＵ（Ｉｎｅｒｔｉａ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）などのセンサが備え付けられている。
【0022】
エージェントＡ１００の制御動作を説明するために、簡略化した機能ブロック図を図４Ｂに示す。エージェントＡ１００はＢｌｕｅｔｏｏｔｈ（登録商標）やＷｉ－Ｆｉ（登録商標）などの無線通信を可能とする通信部Ａｇ０１を備えており、無線通信装置Ａ１０７を介して、管理サーバＡ１０８との通信が可能である。
【0023】
エージェントＡ１００は、前述の複数のセンサＡｇ０２を組み合わせて自身の位置（２次元平面上でのｘ、ｙ座標）、および、方位を算出する自己位置演算部Ａｇ０３を備えている。なお、自己位置演算部はＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）として知られている技術によって実現可能であるため、詳細な説明は省略する。
【0024】
エージェントＡ１００は管理サーバＡ１０８から受信した経路計画、および、自己位置演算部Ａｇ０３で算出した自身の位置に基づいて、エージェントのアクチュエータ（左右の車輪のモータ）を制御するための追従制御部Ａｇ０４を備える。
【0025】
追従制御部Ａｇ０４で算出した制御指令はアクチュエータ制御部Ａｇ０５を介してアクチュエータを実際に制御することになる。アクチュエータ制御部Ａｇ０５はモータ制御用のＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｃｏｎｔｒｏｌｌｅｒ）に相当する。
【0026】
自己位置演算部Ａｇ０３で算出した位置情報は、通信部Ａｇ０１、および、無線通信装置Ａ１０７を介して、管理サーバＡ１０８に提供することも可能である。このような構成にしておけば、作業員Ａ１０１の背後にエージェントＡ１００が移動したことによって、監視センサＡ１０６の死角に入った場合でも、エージェントＡ１００の位置情報を管理サーバＡ１０８に提供することができる。
【0027】
再び、図１に戻り、図２の物流倉庫を対象とした、経路計画装置の各機能ブロックについて詳細に説明する。経路計画装置は、図１に示すように業務管理部Ｓ００１、エージェント管理部Ｓ００２、環境管理部Ｓ００３、移動領域管理部Ｓ００４、制約領域管理部Ｓ００５、経路算出部Ｓ００６、及び経路配信部Ｓ００７を備えている。
【0028】
業務管理部Ｓ００１は、エージェント管理部Ｓ００２が取得したエージェントＡ１００の位置情報に基づいて、エージェントの移動目標位置を算出するとともに、倉庫内のエージェントＡ１００に対して、タスクの管理を行う。なお、本発明におけるタスクとは、例えば、物流倉庫内のエージェントが物品を回収し、所定の棚の前まで運ぶ、一連の動作を意味する。なお、物品の積込みや積み下ろしは場内の作業員Ａ１０１が実行するため、本発明では、エージェントが任意の地点Ａから任意の地点Ｂへと移動する、というタスクの一部の動作のみを管理することになる。よって、業務管理部Ｓ００１は、少なくとも、各エージェントＡ１００に対して、目標位置Ｔｇを算出する機能が備わっていれば良い。なお、エージェントＡ１００ａには目標位置Ｔｇａ、エージェントＡ１００ｂには目標位置Ｔｇｂのように、目標位置Ｔｇはエージェント毎に与えられる点に注意されたい。
【0029】
エージェント管理部Ｓ００２は、倉庫内のエージェントＡ１００の位置情報を管理するとともに、タスクの実行状況を管理する。エージェントＡ１００の現在位置は各エージェントが自己位置演算部Ａｇ０３で算出した自己位置、もしくは、環境センサＡ１０６が観測した位置に基づいて管理される。また、タスクの実行状況とは、エージェントに目標位置Ｔｇが与えられた後に、その目標位置ＴｇにエージェントＡ１００が到達しているかどうかの判断に相当する。エージェントＡ１００が目標位置Ｔｇに到達したらタスクが完了したと判断され、業務管理部Ｓ００１から次のタスクが割り付けられる。一方、エージェントＡ１００が目標位置Ｔｇに到達していない場合は、タスク継続中と判断され、該当するエージェントには次のタスクは提供されない。
【0030】
環境管理部Ｓ００３は、制御不可能な非制御対象の位置情報を取得する。つまり、環境管理部Ｓ００３は倉庫内に配置されている環境センサＡ１０６で検出したセンサデータに基づいて、倉庫内のエージェントＡ１００、作業員Ａ１０１、フォークリフトＡ１０２、パレット位置Ａ１０４、仮置きされた段ボールＡ１０５の位置を算出する。なお、各対象の位置の時間変化から移動体（エージェント、作業員、フォークリフトの総称）の移動速度を算出することも可能である。また、環境センサＡ１０６で取得した点群、もしくは、画像情報から対象が移動体か非移動体（パレット、段ボールの総称）を判別することも可能である。
【0031】
移動領域管理部Ｓ００４は、エージェントＡ１００が移動可能な移動領域ＭＡを決定し、かつ、該移動領域内の非制御対象の位置を管理する。つまり、移動領域管理部Ｓ００４は業務管理部Ｓ００１が算出したエージェント目標位置Ｔｇと、エージェント管理部Ｓ００２が算出したエージェント位置を囲むようにエージェントが移動可能な移動領域ＭＡを定義するとともに移動領域ＭＡ内の非制御対象の位置を管理する。例として、それぞれのエージェントの初期位置と目標位置Ｔｇを囲むように長方形の領域によって定義された移動領域ＭＡを図５Ａに示す。各エージェントの移動計画は対応する移動領域内で計算される。このように移動領域を分割する理由は、移動計画の計算を高速化するためであり、詳細については後述する。
【0032】
なお、図５Ａでは、移動領域ＭＡを長方形で定義しているが、移動領域ＭＡの決定方法はこの形状に限定されるものではない。また、図５Ｂのように、各エージェントの移動領域が重複して定義されても良いし、同一の移動領域が割り当てられても良い。さらに、計算リソースが十分にある場合には、移動領域ＭＡを倉庫全域としても良い。
【0033】
制約領域管理部Ｓ００５は、移動領域ＭＡ内において、該移動領域に含まれる非制御対象の個数の総和以下の制約領域を算出する。具体的には、制約領域管理部Ｓ００５は移動領域管理部Ｓ００４が算出した移動領域ＭＡ内の非制御対象（エージェントＡ１００以外の移動体、非移動体の総称）の位置情報に従って、エージェントが侵入してはいけない領域である制約領域ＬＡを定義・算出する。
【0034】
エージェントＡ１００ａの移動領域ＭＡａが図６Ａのように与えられた場合の制約領域の一例として図６Ｂを与えることができる。図６Ｂでは、図中左上にある段ボールＡ１０５ａと作業員Ａ１０１ａをまとめた領域を制約領域ＬＡ１とし、さらに、図中右下にある段ボールＡ１０５ｂと作業員Ａ１０１ｂをまとめた領域を制約領域ＬＡ２として与えている。このような領域は数１のような不等式条件で定義することができる。
【0035】
【数1】
【0036】
数１の上式は制約領域ＬＡ１の領域を示し、下式は制約領域ＬＡ２の領域を示している。
【0037】
なお、制約領域はこのような表現に限定されるものではない、例えば、図６Ｃに示すように、円形の制約領域を定義することもできる。円形の領域であれば、数２のような不等式条件で制約領域ＬＡｉを表現できる。添え字ｉは制約領域の番号を示している。
【0038】
【数2】
【0039】
数２は円の内側を表現する不等式で制約領域を定義しているが、円の代わりに楕円の方程式を利用しても良い。さらに、図６Ｄに示すように複雑な形状の制約領域であっても、数３に示すように基底関数ｆｉ（ｘ、ｙ）と係数ｇｉの線形和によって近似表現することができることが広く知られている。そして本実施例においては、制約領域管理部Ｓ００５が算出する制約領域は、移動領域に含まれる非制御対象の個数の総和以下の基底関数の線形和を用いて算出されることが特徴である。
【0040】
【数3】
【0041】
また、このような線形和で表現すると、係数ｇｉを効率的に算出できるという利点がある。例えば、このような領域分割はサポートベクターマシン（Ｓｕｐｐｏｒｔ Ｖｅｃｔｏｒ Ｍａｃｈｉｎｅ、ＳＶＭ）などのクラスタリング技術を利用することで実現することができる。なお、数３の表現は数１、数２を含む、より一般的な表現であると言える。
【0042】
制約領域管理部Ｓ００５が算出した制約領域ＬＡに侵入しないようにエージェントの経路計画を行うため、制約領域ＬＡが広くなりすぎるとエージェントが移動できる経路がなくなってしまう可能性がある。よって、制約領域ＬＡは障害物を囲み、かつ、その領域の面積が小さくなるように設定されることが望ましい。例えば、図６Ｅの実線で区画された領域（移動領域ＭＡａ中実線ＡＬ１より図中において上側の領域及び実線ＡＬ２より図中において下側の領域）も、点線で区画された領域（移動領域ＭＡａ中点線ＤＬ１より図中において上側の領域及び点線ＤＬ２より図中において下側の領域）もともに障害物を分離することができているが、実線で区画された領域のほうが制約領域ＬＡが狭いため、こちらを採用することが望ましい。即ち、制約領域の候補が複数ある場合には、その面積が最小であるものを制約領域として算出することが好ましい。
【0043】
各演算周期において、制約領域を更新する際には、数１のように直線でエリアを区分していた制約条件を数２のように円でエリアを区分する方式に変更しても良い。これについて、図６Ａの状況に新たに段ボールＡ１０５ｃが加えられた図７Ａの状況を例に説明する。図７Ｂに示すように、図６Ｂ～６Ｄのいずれの方法を利用しても制約領域ＬＡ１、ＬＡ２のいずれの領域にも含まれない位置に段ボールＡ１０５ｃが配置されている状況では、段ボールＡ１０５ｃのみ囲むように制約領域ＬＡ３を設ける。このように個別の非制御対象に対して制約領域ＬＡを定める条件の例は、エージェントＡ１００の現在位置と目標位置Ｔｇを結ぶ最短ルート候補ＱＲＣと制約領域ＬＡ’とが重複領域を有さないこと、として図７Ｃのように表現することができる。換言すると、制約領域管理部Ｓ００５は、移動領域ＭＡ内において、エージェントの移動目標位置から現在位置までの最短経路を含まない領域を、移動領域ＭＡに含まれる非制御対象の個数の総和以下になるように制約領域として算出する。なお、最短ルート候補ＱＲＣとは、図７Ｄに示すように、二次元空間における最短距離を含む領域として定義しても良い。
【0044】
さらに、図８Ａに示すように、移動領域ＭＡ内に、柱や壁のように動かない障害物ＢＲが存在する場合、最短ルートは現空間における最短距離ではなく、図８Ｂに示すように、ＲＲＴ（Ｒａｐｉｄｌｙ ｅｘｐｌｏｒｉｎｇ ｒａｎｄｏｍ ｔｒｅｅ）などの探索ベースの手法で算出される、障害物ＢＲを避けた経路ＱＲを最短ルートとして扱っても良い。
【0045】
なお、図８Ｂに示した通り、ＲＲＴを利用するだけでもロボットの移動経路を算出することは可能であるが、ロボットのダイナミクス（加減速や角速度の限界値を含む）を十分に考慮出来ないため、実際にこの経路に従ってエージェントが動作できるとは限らないことに注意されたい。
【0046】
さらに、このような個別の制約領域ＬＡの定義を許容する条件は、移動領域ＭＡ内の非制御対象の個数の総和に対して、設定される制約領域の総和が少なくなることを満足する必要がある。このような条件を設ける理由は、後述の経路算出部Ｓ００６の説明において、補足する。
【0047】
制約領域ＬＡを定義する処理は、移動領域ＭＡが定義されたときに実行される。この時算出された制約領域ＬＡ内に非制御対象のうち非移動体しか存在しない場合は、その制約領域ＬＡは移動領域ＭＡが更新されない限り保持される。一方、制約領域ＬＡ内に非制御対象である移動体が存在する場合、事前に設定された制約領域から移動体が逸脱した、もしくは、逸脱することが予測された時点で制約領域ＬＡの更新を実行する。
【0048】
換言すると、制約領域管理部Ｓ００５は、算出した制約領域ＬＡ内に存在する非制御対象が移動体を含み、かつ該移動体が制約領域ＬＡから逸脱した、もしくは、事前に設定された時間内に逸脱することが予想される場合に、移動後の該移動体の位置を包含するように制約領域ＬＡを再計算することが好ましい。
【0049】
例えば、図９Ａのように、制約領域ＬＡ１ａ、ＬＡ２ａが設定された状況を想定する。作業員Ａ１０１ａ、Ａ１０１ｂは自身の作業状況に応じて倉庫内を移動するため、図９Ａで与えられた制約領域ＬＡ１ａ、ＬＡ２ａの外側へと移動する。制約領域からの逸脱が確認されると、図９Ｂのように新しい制約領域ＬＡ１ｂ、ＬＡ２ｂがそれぞれ定義される。同様に、制約領域ＬＡ１ｂ、ＬＡ２ｂから作業員Ａ１０１が移動すると、制約領域が再度更新される。ここで、図９Ｃに示したような状況に作業員Ａ１０１ｂが移動すると、制約領域ＬＡ２ｃは、制約領域ＬＡ２ｂ内に存在していた２つの段ボールのみを囲む領域として定義される。
【0050】
なお、以上の処理は作業員Ａ１０１が制約領域から実際に逸脱する前に実行しても良い。たとえば、環境管理部Ｓ００３で管理している作業員Ａ１０１の位置情報の変化を利用して、作業員Ａ１０１の移動速度ｖａ１を算出し、所定時間ｔ１後に距離ｖａ１×ｔ１だけ離れた位置が制約領域から逸脱することが分かった時点で制約領域を更新する形態でも良い。このような方法を採用すると、エージェントＡ１００が作業員Ａ１０１と接触する可能性をさらに抑制することができる。
【0051】
制約領域管理部Ｓ００５の処理手順を図１０のフローチャートを用いて詳細に説明する。Ｆｃ１０１にて、移動領域管理部Ｓ００４から移動領域ＭＡの更新があったか、もしくは、既に与えられている移動領域ＭＡ内に作業員が新たに入ってくる、または仮置きの段ボールＡ１０５が追加されるなど環境管理部Ｓ００３から得られる環境状況の変化の有無を確認する。いずれかの更新が確認された場合（ＹＥＳ）、Ｆｃ１０２へと遷移する。いずれの更新も確認されない場合（Ｎｏ）には、そのまま更新が確認されるまで待機する。Ｆｃ１０２では、制約領域管理部Ｓ００５が、与えられた環境情報（非制御対象の位置情報）と移動領域ＭＡを用いて、制約領域ＬＡの算出を行う。制約領域ＬＡの算出が終わるとＦｃ１０３へと遷移する。
【0052】
Ｆｃ１０３では、Ｆｃ１０２で算出された複数の制約領域ＬＡｉについて、各制約領域内に非制御対象のうち移動体が含まれるかを判断する。移動体が含まれない場合（ＮＯ）、Ｆｃ１０４へと遷移する。一方、移動体が含まれる場合（ＹＥＳ）、Ｆｃ１０５へと遷移する。Ｆｃ１０４に遷移した場合、該当する制約領域ＬＡｉ内には移動体が含まれていないため、環境変化がない限り、その制約領域ＬＡｉを変更する必要がない。よって、制約領域ＬＡｉを保持する。
【0053】
Ｆｃ１０５に遷移した場合、該当する制約領域ＬＡｉ内には移動体が含まれているため、移動体の移動状況に応じて制約領域ＬＡｉの見直しが必要になる。事前に算出された制約領域ＬＡｉから移動体が逸脱していない場合（ＮＯ）、Ｆｃ１０４へと遷移する。一方、事前に算出された制約領域ＬＡｉから移動体が逸脱する場合（ＹＥＳ）、Ｆｃ１０６へと遷移する。なお、上述の通り、移動体の逸脱判定は実際に移動体が制約領域から逸脱した場合だけでなく、移動体が制約領域から逸脱することが予想された場合にしても良い。
【0054】
Ｆｃ１０６に遷移すると、事前に与えられた制約領域ＬＡｉから逸脱した移動体がエージェントＡ１００の現在位置と目標位置Ｔｇをつなぐ最短ルート候補上にいるかを判断する。移動体が最短ルート候補上にいない場合（ＮＯ）、Ｆｃ１０７へ遷移する。一方、移動体が最短ルート候補上にいる場合（ＹＥＳ）、Ｆｃ１０８へ遷移する。
【0055】
Ｆｃ１０７に遷移すると、移動後の移動体位置を含むように制約領域ＬＡｉを修正する。Ｆｃ１０８に遷移すると、それまで移動体を囲んでいた制約領域ＬＡｉを分割し、移動後の移動体位置を囲むようにして新たな制約領域ＬＡｊ（ｊ≠ｉ）として与える。換言すると、制約領域管理部Ｓ００５は、算出した制約領域内に存在する非制御対象が移動体を含み、かつ該移動体が制約領域から逸脱し、かつ最短経路上に存在する場合に、移動後の該移動体の位置を包含するように制約領域を分割する。さらに、移動体が逸脱したことで、制約領域ＬＡｉが囲んでいる領域が不用意に広くなっている場合は、合わせて、制約領域ＬＡｉの修正も行うことが望ましい。以上の処理は、経路計画装置の演算周期ごとに実行されることが望ましい。以上が制約領域管理部Ｓ００５の果たす機能である。
【0056】
再び図１を参照する。経路算出部Ｓ００６は、移動領域ＭＡ内のエージェントＡ１００が制約領域ＬＡに侵入しないようにエージェントＡ１００の目標位置Ｔｇまでの経路を算出する。つまり、経路算出部Ｓ００６は制約領域管理部Ｓ００５が算出した制約領域ＬＡに基づいて、各エージェントＡ１００が目標位置Ｔｇに向かうための経路算出を行う。
【0057】
エージェントＡ１００が図４Ａに示すような差動二輪タイプのロボットの場合、その運動方程式は数４で与えることができる。
【0058】
【数4】
【0059】
ここで、ｘ、ｙはロボットのｘ座標、ｙ座標、θはロボットの方位（向き）であり、ｖはロボットの移動速度、ωはロボットの角速度である。ここで目標位置Ｔｇの座標を（ｘｇ、ｙｇ）としてこれらをまとめたベクトルをｒ＝［ｘｇ，ｙｇ］Ｔ（Ｔはベクトルの転置を意味する）と定義する。また、数４を演算周期Δｔで離散化したときのｘ、ｙ座標をまとめたものを数５における数式５ａのベクトルｐで定義する。
【0060】
【数5】
【0061】
この時、エージェントＡ１００を目標位置へと移動させる問題は、数６における数式６ａによって表されるモデル予測制御として表現することができる。なお、ベクトルｅ（ｋ）は数６における数式６ｂによって定義される現在位置ｐ（ｋ）から目標位置ｒへの偏差ベクトルである。
【0062】
【数6】
【0063】
また、ベクトルｕは制御入力のベクトルであり、ｕ（ｋ）＝［ｖ（ｋ），ω（ｋ）］Ｔである。さらに、Ｓ、Ｑ、Ｒは重み行列であり、Ｎｐは予測ホライズンである。モデル予測制御では、数式６ａの評価関数Ｊを最小化するように制御入力ｕ（ｋ）を算出する。このような定式化は制御問題として一般的なものであり、この定式化自体には特別な工夫は無いため、制御入力ｕ（ｋ）の算出方法の詳細については省略する。
【0064】
制御入力ｕ（ｋ）を算出できれば、数４を積分することで各時刻における位置、方位を算出でき、これらを繋ぎ合わせたものを経路として利用することが可能である。なお、目標位置Ｔｇに姿勢情報もが含まれている場合、つまり、目標座標（ｘｇ，ｙｇ）と目標姿勢θｇを統合したベクトルをｒ＝［ｘｇ，ｙｇ，θｇ］Ｔとして、エージェントの位置、姿勢をまとめたベクトルｐ（数５における数式５ｂ）に変更すればよい。
【0065】
なお、モデル予測制御には、拘束条件を陽に扱うことができるという利点がある。例えば、図４Ａの差動二輪ロボットの場合、ロボットの移動速度ｖ、および、旋回角速度ωは走行モータの左右輪の回転速度ωｌ、ωｒに依存して上限値、下限値が決まる。このようなアクチュエータの動作特性を拘束条件として組み込むことで、ロボットが実際に追従可能な経路を生成できるようになる。また、数式６ａの評価関数Ｊには制御入力ｕが含まれているため、評価関数Ｊを最小化するような制御入力ｕを算出する場合、制御入力ｕそのものを小さくする、つまり、消費エネルギを抑制する効果も期待できる。
【0066】
さて、数６において、Ｓ、Ｑを対角行列として、対角要素の値を等しい値に設計すると、数式６ａの第１項と第２項はエージェントＡ１００と目標位置Ｔｇを結ぶ直線距離に重みをかけた値として考えることができる。よって、数６を最小化する制御入力ｕ（ｋ）を算出し、この制御入力を積分した場合に算出される経路は２次元空間上の最短経路になる（図１１Ａ）。このように、数６を最小化するように制御入力を算出すると、詳細な地図情報を利用せずに効率的な移動経路を算出できるというメリットがある。そして、経路を複数回計算する場合、計算ごとにその演算値が小さくなるように計算することで、計算ごとにより正確な経路を算出することが可能になる。
【0067】
換言すると、制約領域管理部Ｓ００５は、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の基底関数の線形和を用いて制約領域を算出する。そして経路算出部Ｓ００６は、エージェントの現在位置と目標位置との間の偏差が小さくなるほど評価値が小さくなる評価関数Ｊ（数式６（ａ））と、制約領域管理部Ｓ００５が決定した制約条件（数３参照）とを拘束条件として、評価関数が演算ごとに前回の演算値よりも小さくなるようにエージェントの経路を算出する。
【0068】
ただし、図１１ＢのようにエージェントＡ１００ａとその目標位置Ｔｇａとの間に障害物ＢＲがある場合、数６を最小化するように算出した経路を利用すると、エージェントが障害物に衝突してしまう。
【0069】
このような状況を回避するには、図１１Ｃに示すように、エージェントＡ１００ａを囲む領域と障害物ＢＲを囲む領域が交差しないことを、最適制御問題の制約条件として付与する方法が考えられる。具体的には、エージェントＡ１００ａを囲む領域、障害物ＢＲを囲む領域をそれぞれ半径ｒａ、ｒｏの円で与えると、数７を拘束条件として付与することで図１１Ｄのような障害物ＢＲを回避する経路を生成することができる。すなわち、制約領域管理部Ｓ００５は、移動領域内において、エージェントの移動目標位置Ｔｇから現在位置までの最短経路中に障害物がある場合には、該障害物を回避する領域を、移動領域に含まれる非制御対象の個数の総和以下になるように制約領域として算出する。
【0070】
【数7】
【0071】
なお、上記の説明では、各領域を円で定義しているが、円以外で領域を定義しても良い。つまり、各領域を楕円や正方形で定義しても同様の効果を得ることができる。
【0072】
以上のような処理はエージェントと障害物がそれぞれ１つなので、拘束条件が１つで済んでいる。しかし、移動領域ＭＡ内に障害物がｎｏ個あると、１台のエージェントがそれぞれの障害物に接触しないための拘束条件はｎｏ個必要になる（障害物が４個の場合の例として図１１Ｅを示す）。さらに、エージェントの台数がｎａ台になると、全てのエージェントがｎｏ個の障害物と接触しない条件としては、ｎａ×ｎｏ個の拘束条件が必要になり、各エージェント同士もぶつからないように制御するには、さらに拘束条件の数を増やす必要がある。前述の通り、最適化問題を解くにあたっては、考慮すべき拘束条件の数を増やすほど計算負荷が急激に増加することが知られている。
【0073】
上記問題に対して本実施例では、移動領域管理部Ｓ００４において、各移動領域ＭＡに存在するエージェントＡ１００の数を抑制し、さらに、制約領域管理部Ｓ００５において、エージェントが侵入してはいけない領域である制約領域ＬＡを与えることで、個別の拘束条件を考えることなく、障害物（作業員Ａ１０１や段ボールＡ１０５を含む）とエージェントＡ１００が接触しない走行経路を算出できる。
【0074】
上記の説明をより具体化する。図６Ａに示した状況での経路計画を行う場合、特別の工夫を行わずに経路計画を行うと、２人の作業員及び４個の段ボールが存在するため数８のように、拘束条件として６本の数式を考える必要がある。
【0075】
【数8】
【0076】
なお、数８における［ｓ．ｔ．］は［ｓｕｂｊｅｃｔ ｔｏ］の略記であり、［ｓ．ｔ．］以降の条件のもと最適化問題を解くことを意味している。一方、図６Ｂのように、制約領域を直線で区分すると最適化問題は数９で与えることができる。
【0077】
【数9】
【0078】
数９の拘束条件は２本の数式になるため、もともとの最適化問題（数８）よりも高速に解くことができる。さらに、数８の拘束条件は非線形であるのに対して、数９の拘束条件は線形であるため、計算負荷をさらに抑制することができる。
【0079】
また図６Ｃのように、拘束領域を円で区分すると最適化問題は数１０で与えることができる。
【0080】
【数10】
【0081】
数１０も数９と同様に、拘束条件は２本になるため、もともとの最適化問題（数８）よりも高速に解くことができる。ただし、拘束条件が非線形になるため、図６ｂのような区分よりは計算時間を要する。図６Ｄのような区分を利用するとさらに拘束条件が複雑になるため、さらに計算が必要になる。よって、制約領域管理部Ｓ００５はできるだけ簡易な数式を用いて制約領域ＬＡを定義することが望ましい。
【0082】
より具体的には、拘束条件の数式が非線形であるよりも線形であることが好ましく、さらに拘束条件を規定する基底関数の総和が事前に設定した許容値よりも少ないことが好ましい。
【0083】
これをまとめると、制約領域管理部Ｓ００５は、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の基底関数の線形和を用いて前記制約領域を算出する際に、
（１）線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（２）線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
（３）非線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（４）非線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
の優先順位で該制約領域を算出することが好ましい。
（１）線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（２）線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
（３）非線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
（４）非線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
の優先順位で該制約領域を算出することが好ましい。
【0084】
これまでは、移動領域ＭＡにエージェントＡ１００が１台しかいない状況での経路算出部Ｓ００６の処理内容を説明した。しかし経路算出部Ｓ００６は図５Ｂのように、同一の移動領域内に２台以上のエージェントが存在する場合にも同様の演算が可能である。
【0085】
例えば、図１２Ａに示すように移動領域ＭＡ内にエージェントＡ１００が２台いる状況であっても、制約領域管理部Ｓ００５にて障害物を囲むように制約領域ＬＡ１が図１２Ｂのように与えられれば、１台目のエージェントＡ１００ａについては数１１、２台目のエージェントＡ１００ｂについては数１２の最適化問題を解くことによって経路計画を行うことができる。
【0086】
【数11】
【0087】
【数12】
【0088】
なお、数１１と数１２に共通する数式はエージェントＡ１００ａとエージェントＡ１００ｂが接触しないための拘束条件である。エージェント数が３台以上になったとしても、同様の定式化によって、経路計画を行うことができる。
【0089】
再び図１に戻り、経路配信部Ｓ００７は、経路算出部Ｓ００６で算出した経路を各エージェントに配信するために、無線通信装置Ａ１０７との通信を行う。なお、既に、経路が与えられているエージェントについては経路変更が無い限り、経路配信を行う必要はない。エージェントＡ１００は無線通信装置Ａ１０７から経路を受信すると、その経路に従って追従制御を実行する。
【0090】
これまでに説明した経路計画装置Ａ１０８が実行する処理手順を図１３のフローチャートを用いて説明する。まず、Ｆｃ２０１でエリア内情報取得を行う。この処理はエージェント管理部Ｓ００２と環境管理部Ｓ００３による処理であり、エージェントＡ１００、もしくは、監視センサＡ１０６から提供されるエージェントＡ１００の位置、作業員Ａ１０１の位置、パレット置き場Ａ１０４のパレット位置、仮置きされた段ボールＡ１０５の位置などを更新する作業に相当する。エージェント管理部Ｓ００２と環境管理部Ｓ００３の実行順序はどちらが先でも良いし、並行に実行されても良い。Ｆｃ２０１が終了すると、Ｆｃ２０２へと遷移する。
【0091】
Ｆｃ２０２では、タスクが割り付けられていないエージェントＡ１００が存在することを確認する。タスク割り付けが無いエージェントがある場合（ＹＥＳ）はＦｃ２０３へと遷移する。タスク割り付けが無いエージェントが無い場合（ＮＯ）、つまり、すべてのエージェントにタスクが割り付けられている場合はＦｃ２０４へと遷移する。
【0092】
Ｆｃ２０３では、エージェントＡ１００にタスク割り付けを行う。タスクが割り付けられることでエージェントの目標位置Ｔｇが確定する。Ｆｃ２０３の処理が完了すると、Ｆｃ２０４へと遷移する。以上、Ｆｃ２０２とＦｃ２０３は業務管理部Ｓ００１による処理である。
【0093】
Ｆｃ２０４では、エージェントＡ１００の現在位置と目標位置Ｔｇを囲むように移動領域ＭＡを算出する。なお、複数のエージェントＡ１００の現在位置と目標位置Ｔｇが類似する場合には、図５Ｂのように、複数のエージェントを含む移動領域ＭＡを定義しても良い。この処理は、移動領域管理部Ｓ００４によるものである。移動領域の算出が完了すると、Ｆｃ２０５に遷移する。
【0094】
Ｆｃ２０５では、Ｆｃ２０１で取得したエリア内情報を利用して、各移動領域ＭＡ内における制約領域ＬＡを算出する。この処理は、制約領域管理部Ｓ００５によるものである。なお、Ｆｃ２０５の処理の詳細は、図１０のフローチャートに従う。制約領域の算出が完了すると、Ｆｃ２０６へと遷移する。
【0095】
Ｆｃ２０６では、Ｆｃ２０３～Ｆｃ２０５までに算出された目標位置Ｔｇ、制約領域ＬＡを利用してエージェントの移動経路を算出する。この処理は、経路算出部Ｓ００６によるものである。経路の算出が完了すると、Ｆｃ２０７へと遷移する。
【0096】
Ｆｃ２０７では、Ｆｃ２０６で算出した経路をエージェントへと配信する処理を行う。この機能は、経路配信部Ｓ００７によるものである。経路の配信が完了すると、Ｆｃ２０８へと遷移する。
【0097】
Ｆｃ２０８では、それぞれのエージェントＡ１００のタスクが完了した、つまり、全てのエージェントＡ１００が目標位置Ｔｇに到達したかを確認する。タスクが完了した場合（ＹＥＳ）は処理を完了し、所定時間経過後に再度処理を開始する（Ｆｃ２０１に戻る）。一方、タスクが完了していない場合（ＮＯ）、Ｆｃ２０９へと遷移する。
【0098】
Ｆｃ２０９では、タスク実行中に作業者Ａ１０１などの位置が変更していないかを確認するために再度エリア内情報を確認する。この処理はＦｃ２０１と同様である。
【0099】
以上、本実施例によれば、作業領域内に複数のエージェントや非制御対象が存在する場合であっても、適宜制約領域を設定して経路算出のための拘束条件を少なくすることで、大量のエージェントを効率的に移動させる経路計画をリアルタイムに算出することが可能になっている。
【0100】
以上では本発明を物流倉庫に適用した場合を説明したが、以下では本発明を駐車場における経路計画に適用した場合の具体的な実施例を説明する。図１４に本発明を適用する駐車場の例を示す。
【0101】
対象とする駐車場は２つの車道に挟まれた施設の駐車場であり、それぞれの車道から駐車場への出入り口を備えているものとする。また、本実施形態の駐車場を利用する自動車は通信機能を有する自動運転車両、ＣＡＶ（Ｃｏｎｎｅｃｔｅｄ Ａｕｔｏｎｏｍｏｕｓ Ｖｅｈｉｃｌｅ）であることを前提する。本実施例では、ＣＡＶがエージェントＡ１００に相当する。
【0102】
上述した実施例では非制御対象の移動体として、物流倉庫内で働く作業員Ａ１０１を想定していたが、本実施形態では駐車場から施設まで移動する歩行者Ａ１０１がこれに相当する。また、仮置きされた段ボールＡ１０５に相当するものとして、駐車エリア以外に一時停止している車両Ａ１０５が挙げられる。また、駐車場全体を確認できるように監視カメラＡ１０６が配置されているのは上述した実施例と同様である。
【0103】
駐車場（管理エリア）に入場したＣＡＶはエリア内の無線通信装置Ａ１０７を介して、管理サーバＡ１０８と通信を行い、サーバから与えられる経路計画に従って駐車スペースへと移動する。ＣＡＶが駐車場内の空きスペースｂｉ（ｉ＝１、２、３）へと移動することがタスクに相当する。なお、ここでは、車両を空きスペースｂｉへの誘導に関する説明のみを行うが、駐車場から退場する車両に関しても、車道までの誘導を行うことも可能である。
【0104】
管理サーバＡ１０８は駐車場内の空きスペースｂｉを管理しており、入場してきた各車両を空きスペースｂｉに誘導する。なお、図１４では管理サーバＡ１０８を施設内に配置しているが、駐車場内の無線通信装置Ａ１０７との通信が可能な場所ならばどこに配置しても良い。
【0105】
駐車場内の空きスペースへの誘導の具体例としてＣＡＶ１：Ａ１００ａ、ＣＡＶ２：Ａ１００ａが入場する状況を例にとり、経路計画装置Ａ１０８の動作を図１５Ａ～１５Ｅを用いて説明する。
【0106】
ＣＡＶ１、ＣＡＶ２が管理エリアに入場すると、管理サーバＡ１０８との通信を開始する。管理サーバＡ１０８に実装された業務管理部Ｓ００１は、各ＣＡＶの現在位置と空きスペースｂｉ、位置関係に従って、目標位置Ｔｇを決定する。ＣＡＶ１、ＣＡＶ２の目標位置が空きスペースｂ１、ｂ２に設定されたとすると、移動領域管理部Ｓ００４によって図１５Ｂのように移動エリアＭＡ１、ＭＡ２が設定される。駐車場内の監視カメラが取得した情報から環境管理部Ｓ００３が歩行者Ａ１０１、一時停止中の車両Ａ１０５、駐車中の車両Ａ１０５の位置を計算し、これらの情報に基づいて、制約領域管理部Ｓ００５で図１５Ｃのように制約領域ＬＡ１ａ、ＬＡ１ｂ、ＬＡ２を算出する。
【0107】
制約領域の算出が完了すると、経路算出部Ｓ００６によって、制約領域に接触しないようにＣＡＶの移動経路が算出される。しかし、図１５Ｄに示すように、ＣＡＶ１は移動経路上に制約領域ＬＡ１ａが存在するため、目標位置ｂ１までの経路を生成することはできない。ただし、制約領域ＬＡ１ａ内の障害物は歩行者と一時停止中の車両であるため、時間経過とともに移動していくと予測される。結果として、図１５Ｅのように、ＣＡＶ１がｂ１まで移動する経路を生成することが可能になる。
【0108】
上述した２つの実施例は平面を移動するエージェントの経路計画を扱っているが、本発明は平面を移動するエージェント以外にも適用することができる。
【0109】
本発明を施工現場における経路計画に適用した場合の具体的な実施形態を説明する。
図１６Ａ、１６Ｂに本発明を適用する施工現場の例を示す。図１６Ａは本実施例のエージェントＡ１００に相当する油圧ショベルの初期姿勢を上空から描いたもの、図１６Ｂは油圧ショベルの最終姿勢を側面から描いたものである。本発明の経路計画装置は油圧ショベルの作業機位置を初期姿勢からダンプトラックＤＴの上空に設けた目標位置まで移動させる３次元軌跡を提供する。
図１６Ａ、１６Ｂに本発明を適用する施工現場の例を示す。図１６Ａは本実施例のエージェントＡ１００に相当する油圧ショベルの初期姿勢を上空から描いたもの、図１６Ｂは油圧ショベルの最終姿勢を側面から描いたものである。本発明の経路計画装置は油圧ショベルの作業機位置を初期姿勢からダンプトラックＤＴの上空に設けた目標位置まで移動させる３次元軌跡を提供する。
【0110】
図１７の施工現場の全体概要図に示す通り、既述の実施例と同様に、施工現場には監視カメラＡ１０６が設置されており、無線通信装置Ａ１０７を介して、管理サーバＡ１０８によって現場内の状況を常時監視できている。なお、制御対象の油圧ショベルが１台しかない状況であれば、油圧ショベルの動作を制御するコントローラをサーバ代わりにしても良い。
【0111】
施工現場内には油圧ショベルＡ１００とダンプトラックＤＴ以外に、油圧ショベルでは対応が難しい細かい掘削作業を行ったり、施工現場の進捗状況を監視するために複数の作業員Ａ１０１がいる状況がありえる。
【0112】
サーバとして実装されている本実施例の経路計画装置Ａ１０８は、油圧ショベルＡ１００が図１６Ａの姿勢に示された、掘削動作完了後の初期姿勢になったことをエージェント管理部Ｓ００２によって判断し、環境管理部Ｓ００３によって計算したダンプトラックＤＴの位置に基づいて、業務管理部Ｓ００１によってダンプトラックＤＴの荷台の上に目標位置Ｔｇを設定する。
【0113】
本実施例では、油圧ショベルＡ１００がフロント作業機のみを操作することを想定しているため、移動領域管理部Ｓ００４は油圧ショベルＡ１００のフロント作業機が動作可能な範囲を図１８Ａ、１８Ｂに示すように決定する。油圧ショベルＡ１００は昇降動作だけでなく旋回動作も実現できるため、図１８Ａ及び図１８Ｂのハッチング部分に示すように移動領域ＭＡも３次元領域になる。
【0114】
同様に、本実施例では、制約領域管理部Ｓ００５が算出する制約領域ＬＡは、例えば、図１９のように、ダンプトラックと作業員を囲む直方体としての領域になる。なお、制約領域の形状が直方体に限定されないことは言うまでもない。
【0115】
経路算出部Ｓ００６では、図１９の制約領域内に油圧ショベルのフロント作業機が侵入しないように制約条件を与えてモデル予測制御を解くことによって、フロント作業機の移動経路を算出できる。
【0116】
以上、物流倉庫、駐車場と施工現場を例に本発明の実施例を詳細に述べたが、本発明の適用先がこれらの場合に限定されないことは言うまでもない。たとえば、港湾における搬送車両の移動経路生成、テーマパーク内を移動するロボットの経路生成などにも活用できる。
【0117】
以上で説明した本発明の実施例によれば、以下の作用効果を奏する。
（１）本発明に係る経路計画装置は、制御可能な移動体であるエージェントの経路を生成する経路計画装置であって、エージェントの位置情報を管理するエージェント管理部、制御不可能な非制御対象の位置情報を取得する環境管理部、エージェント管理部が取得したエージェントの位置情報に基づいて、エージェントの移動目標位置を算出する業務管理部、エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の非制御対象の位置を管理する移動領域管理部、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、移動領域内のエージェントが制約領域に侵入しないようにエージェントの移動目標位置までの経路を算出する経路算出部、及び経路算出部が算出した経路をエージェントに配信する経路配信部を有する。
（１）本発明に係る経路計画装置は、制御可能な移動体であるエージェントの経路を生成する経路計画装置であって、エージェントの位置情報を管理するエージェント管理部、制御不可能な非制御対象の位置情報を取得する環境管理部、エージェント管理部が取得したエージェントの位置情報に基づいて、エージェントの移動目標位置を算出する業務管理部、エージェントが移動可能な移動領域を決定し、かつ、該移動領域内の非制御対象の位置を管理する移動領域管理部、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の制約領域を算出する制約領域管理部、移動領域内のエージェントが制約領域に侵入しないようにエージェントの移動目標位置までの経路を算出する経路算出部、及び経路算出部が算出した経路をエージェントに配信する経路配信部を有する。
【0118】
上記構成により、制約領域を導入することにより演算負荷のかかる拘束条件の数を減らし、大量のエージェントを効率的に移動させる経路計画をリアルタイムに算出する方法を提供することが可能になる。
【0119】
（２）制約領域管理部は、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の基底関数の線形和を用いて制約領域を算出する。これにより、拘束条件をより簡素化した高速演算を実現することが可能になる。
【0120】
（３）制約領域管理部は、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の基底関数の線形和を用いて制約領域を算出し、経路算出部は、エージェントの現在位置と目標位置との間の偏差が小さくなるほど評価値が小さくなる評価関数と、制約領域管理部が決定した制約条件とを拘束条件として、評価関数が演算ごとに前回の演算値よりも小さくなるようにエージェントの経路を算出する。これにより、演算回数を重なるごとに正確な経路を算出することが可能になる。
【0121】
（４）制約領域管理部は、移動領域内において、エージェントの移動目標位置から現在位置までの最短経路を含まない領域を、移動領域に含まれる非制御対象の個数の総和以下になるように制約領域として算出する。これにより、移動目標位置から現在位置までの最短経路が制約領域として算出され、エージェントが該最短経路を通過できなくなってしまう恐れを防止できる。
【0122】
（５）制約領域管理部は、移動領域内において該移動領域に含まれる非制御対象の個数の総和以下になるように制約領域を算出し、かつ、該制約領域の候補が複数ある場合には面積が最小のものを該制約領域として算出する。これにより、エージェントが移動可能な移動領域の面積を最大限確保することが可能になり、採用できる経路の選択肢を多数確保することが可能になる。
【0123】
（６）制約領域管理部は、算出した制約領域内に存在する非制御対象が移動体を含み、該移動体が制約領域から逸脱した、もしくは、事前に設定された時間内に逸脱することが予想される場合に、移動後の該移動体の位置を包含するように制約領域を再計算する。これにより、リアルタイムでエージェントの経路を修正することが可能になる。
【0124】
（７）制約領域管理部は、算出した制約領域内に存在する非制御対象が移動体を含み、該移動体が制約領域から逸脱し、かつ最短経路上に存在する場合に、移動後の該移動体の位置を包含するように制約領域を分割する。これにより、（４）と同様に、エージェントが最短経路を通過できなくなる恐れをリアルタイムに排除することが可能になる。
【0125】
（８）制約領域管理部は、移動領域内において、エージェントの移動目標位置から現在位置までの最短経路中に障害物がある場合には、該障害物を回避する領域を、移動領域に含まれる非制御対象の個数の総和以下になるように制約領域として算出する。これにより、エージェントが障害物と衝突してしまう経路を生成することを防止できる。
【0126】
（９）制約領域管理部は、移動領域内において、該移動領域に含まれる非制御対象の個数の総和以下の基底関数の線形和を用いて制約領域を算出する際に、
［１］線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
［２］線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
［３］非線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
［４］非線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
の優先順位で該制約領域を算出する。本発明においてはこのような優先順位に従うことにより経路の最適化を迅速に行うことが可能になるものである。
［１］線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
［２］線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
［３］非線形、かつ、基底関数の総和が事前に設定した許容値よりも少ない
［４］非線形、かつ、基底関数の総和が事前に設定した許容値よりも多い
の優先順位で該制約領域を算出する。本発明においてはこのような優先順位に従うことにより経路の最適化を迅速に行うことが可能になるものである。
【0127】
なお、本発明は、上記の実施例に限定されるものではなく、様々な変形が可能である。例えば、上記の実施例は、本発明を分かりやすく説明するために詳細に説明したものであり、本発明は、必ずしも説明した全ての構成を備える態様に限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能である。また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、削除したり、他の構成を追加・置換したりすることが可能である。
【符号の説明】
【0128】
Ａ１００ エージェント、Ａ１０１ 作業員（非制御対象）、Ａ１０５ 段ボール（非制御対象）、ＬＡ 制約領域、ＭＡ 移動領域、Ｓ００１ 業務管理部、Ｓ００２ エージェント管理部、Ｓ００３ 環境管理部、Ｓ００４ 移動領域管理部、Ｓ００５ 制約領域管理部、Ｓ００６ 経路算出部、Ｓ００７ 経路配信部、Ｔｇ 目標位置
【図1】
【図2】
【図3】
【図4A】
【図4B】
【図5A】
【図5B】
【図6A】
【図6B】
【図6C】
【図6D】
【図6E】
【図7A】
【図7B】
【図7C】
【図7D】
【図8A】
【図8B】
【図9A】
【図9B】
【図9C】
【図10】
【図11A】
【図11B】
【図11C】
【図11D】
【図11E】
【図12A】
【図12B】
【図13】
【図14】
【図15A】
【図15B】
【図15C】
【図15D】
【図15E】
【図16A】
【図16B】
【図17】
【図18A】
【図18B】
【図19】