JP7471595B2 - ビークルユニット、ビークルの制御方法、及びプログラム - Google Patents

Description

本発明は、ビークルユニット、ビークル、ビークルの制御方法、及びプログラムに関する。

複数のビークルが移動する際に、それぞれのビークルが自身の経路を設定する分散型の制御システムが知られている。例えば特許文献１には、格子状に定義した経路に基づき、ビークル同士が衝突したりデッドロックしたりしないように、それぞれのビークルが経路を設定する旨が記載されている。

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

しかし、ビークルの経路設定には改善の余地があり、ビークルの経路を適切に設定することが求められている。

本開示は、上述した課題を解決するものであり、ビークルの経路を適切に設定可能な、ビークルユニット、ビークル、ビークルの制御方法、及びプログラムを提供することを目的とする。

上述した課題を解決し、目的を達成するために、本開示に係るビークルユニットは、進入禁止場所の位置を示す情報と目的位置の情報とに基づいて、自身が移動する経路であるリーダー用経路を設定する第１ビークルと、前記リーダー用経路に基づいて、自身が移動する経路であるフォロワー用経路を設定する第２ビークルと、を含む。

上述した課題を解決し、目的を達成するために、本開示に係るビークルは、移動を予定する領域内において座標毎に設定されたノードと、前記ノード同士を結ぶエッジとを含む無向グラフ情報を取得する無向グラフ情報取得部と、進入禁止場所を通らず、かつ目的位置に到達可能な前記エッジである経由エッジを抽出し、前記経由エッジに結ばれる前記ノードを経由ノードとして設定する経由ノード設定部と、前記経由ノードに基づき、モデル予測制御で前記リーダー用経路を設定するリーダー用経路算出部と、を含む。

上述した課題を解決し、目的を達成するために、本開示に係るビークルの制御方法は、進入禁止場所の位置を示す情報と目的位置情報とに基づいて、第１ビークルが移動する経路であるリーダー用経路を設定するステップと、前記リーダー用経路に基づいて、第２ビークルが移動する経路であるフォロワー用経路を設定するステップと、を含む。

上述した課題を解決し、目的を達成するために、本開示に係るビークルの制御方法は、移動を予定する領域内において座標毎に設定されたノードと、前記ノード同士を結ぶエッジとを含む無向グラフ情報を取得するステップと、進入禁止場所を通らず、かつ目的位置に到達可能な前記エッジである経由エッジを抽出し、前記経由エッジに結ばれる前記ノードを経由ノードとして設定するステップと、前記経由ノードに基づき、モデル予測制御で移動経路であるリーダー用経路を設定するステップと、を含む。

上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、進入禁止場所の位置を示す情報と目的位置情報とに基づいて、第１ビークルが移動する経路であるリーダー用経路を設定するステップと、前記リーダー用経路に基づいて、第２ビークルが移動する経路であるフォロワー用経路を設定するステップと、を含む、ビークルの制御方法を、コンピュータに実行させる。

上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、移動を予定する領域内において座標毎に設定されたノードと、前記ノード同士を結ぶエッジとを含む無向グラフ情報を取得するステップと、進入禁止場所を通らず、かつ目的位置に到達可能な前記エッジである経由エッジを抽出し、前記経由エッジに結ばれる前記ノードを経由ノードとして設定するステップと、前記経由ノードに基づき、モデル予測制御で移動経路であるリーダー用経路を設定するステップと、を含む、ビークルの制御方法を、コンピュータに実行させる。

本開示によれば、ビークルの経路を適切に設定できる。

図１は、第１実施形態に係るビークル制御システムの模式図である。 図２は、管理システムの模式的なブロック図である。 図３は、第１実施形態に係るリーダービークルの模式的なブロック図である。 図４は、リーダー用経路設定部５４の模式的なモデル図である。 図５は、無向グラフ情報の一例を示す模式図である。 図６は、経由ノードの設定の一例を説明する模式図である。 図７は、リーダー用経路の設定を説明する模式図である。 図８は、第１実施形態に係るフォロワービークルの模式的なブロック図である。 図９は、目標位置設定部の模式的なモデル図である。 図１０は、フォロワー用経路の一例を説明する模式図である。 図１１は、フォーメーション位置及び目的位置を説明する模式図である。 図１２は、同じビークルユニットのリーダービークルとフォロワービークルが移動する際の制御フローを説明するフローチャートである。 図１３は、第２実施形態における経由エッジの抽出の例を説明する模式図である。 図１４は、関数Ｃ_ｂを説明するための模式図である。 図１５は、第３実施形態に係るリーダービークルによって生成されたリーダー用経路の一例を示す模式図である。 図１６は、第４実施形態における経由ノードの抽出方法の一例を説明するための模式図である。 図１７は、第４実施形態における経由ノードの抽出方法の一例を説明するための模式図である。 図１８は、リーダー用経路の更新を説明するための模式図である。 図１９は、リーダー用経路の更新を説明するための模式図である。

以下に添付図面を参照して、本発明の好適な実施形態を詳細に説明する。なお、この実施形態により本発明が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。

（第１実施形態）
（ビークル制御システム）
図１は、第１実施形態に係るビークル制御システムの模式図である。図１に示すように、第１実施形態に係るビークル制御システム１００は、複数のビークル１０を含むビークルユニット１２と、管理システム１４とを含む。ビークル１０は、自動で移動可能な移動体であるが、それに限られず、運転者が操作する有人の移動体であってもよい。ビークル１０は、地面を走行する移動体としてもよいし、空中を飛行する移動体としてもよいし、水中を移動する移動体としてもよい。したがって、ビークル１０は、３次元で移動できる移動体も含むが、以下では説明のため、２次元平面を移動する場合として説明する。２次元平面を移動するビークル１０としては、例えば、ＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）やＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）などが挙げられるが、ビークル１０の種類は任意であってよい。以下、ビークル１０が移動可能な領域、すなわちビークル１０が移動を予定している領域を、領域ＡＲとする。領域ＡＲは、本実施形態では２次元平面であり、水平方向に沿った一方向を、Ｘ方向とし、水平方向に沿った方向であってＸ方向と直交する方向を、Ｙ方向とする。なお、領域ＡＲは、３次元空間であってもよい。

（ビークルユニット）
ビークルユニット１２は、複数のビークル１０で構成されるビークル群であり、複数のビークル１０で構成される１つのチームであるといえる。１つのビークルユニット１２に含まれるビークル１０は、同じ目的をもっており、例えば本実施形態では、同じ目的位置Ｏに移動するという共通の目的を持ったものである。１つのビークルユニット１２に属する各ビークル１０は、所定のフォーメーション、すなわち所定の位置関係をもって、目的位置Ｏまで移動するように、経路が設定される。目的位置Ｏは、領域ＡＲ内におけるビークルユニット１２の目的地である。図１の例では、目的位置Ｏは、１つの座標ではなく所定の面積を占める領域として設定されており、ビークルユニット１２に属するそれぞれのビークル１０が、その目的位置Ｏの領域内を目的地としている。ただし、目的位置Ｏは、領域に限られず、座標として設定されていてもよい。

１つのビークルユニット１２には、ビークル１０として、第１ビークルとしてのリーダービークル１０Ａと、第２ビークルとしてのフォロワービークル１０Ｂとが含まれている。リーダービークル１０Ａは、目的位置Ｏに移動する際に、１つのビークルユニット１２のチームのリーダーとして機能する。フォロワービークル１０Ｂは、リーダービークル１０Ａに追従して移動する。１つのビークルユニット１２においては、リーダービークル１０Ａが１台であり、リーダービークル１０Ａ以外のビークル１０がフォロワービークル１０Ｂとなるように設定されている。図１の例では、１つのビークルユニット１２に５台のビークル１０が属しており、１台がリーダービークル１０Ａで残りの４台がフォロワービークル１０Ｂとなっている。ただし、１つのビークルユニット１２に属するビークル１０の数は任意であり、５台以外の複数台のビークルが属してもよいし、１つのビークルユニット１２に１台のビークル１０のみが属してもよい。１つのビークルユニット１２に１台のビークル１０のみが属する場合には、そのビークル１０は、リーダービークル１０Ａとなる。

なお、本実施形態では、ビークル制御システム１００には、複数のビークルユニット１２（図１の例では３チーム）が含まれているが、ビークル制御システム１００に含まれるビークルユニット１２の数も任意であり、例えば１チームのみが存在してもよい。

（管理システム）
管理システム１４は、ビークル１０を管理するシステムであり、本実施形態では、ビークル１０の目的位置Ｏを設定する。管理システム１４は、本実施形態ではＷＭＳ（Ｗａｒｅｈｏｕｓｅ Ｍａｎａｇｅｍｅｎｔ Ｓｙｓｔｅｍ）であるが、ＷＭＳに限られず任意のシステムであってよい。

図２は、管理システムの模式的なブロック図である。管理システム１４は、コンピュータであり、図２に示すように、通信部２０と、記憶部２２と、制御部２４とを含む。通信部２０は、リーダービークル１０Ａなどの外部の装置と通信を行う通信モジュールであり、例えばアンテナなどである。管理システム１４は、無線通信で通信を行うが、通信方式は任意であってよい。記憶部２２は、制御部２４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）のような主記憶装置と、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）などの外部記憶装置とのうち、少なくとも１つ含む。

制御部２４は、演算装置、すなわちＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）である。制御部２４は、目的位置情報取得部２６を含む。制御部２４は、記憶部２２からプログラム（ソフトウェア）を読み出して実行することで、目的位置情報取得部２６を実現して、その処理を実行する。なお、制御部２４は、１つのＣＰＵによってこの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、目的位置情報取得部２６を、ハードウェア回路で実現してもよい。

目的位置情報取得部２６は、ビークルユニット１２の目的位置Ｏの位置情報を取得する。目的位置情報取得部２６は、例えばビークルユニット１２が実行する作業内容を設定し、作業内容に応じて目的位置Ｏを設定する。ただし、目的位置情報取得部２６による目的位置Ｏの位置情報の取得方法は任意であり、例えばユーザーにより指定されてもよい。目的位置情報取得部２６は、ビークルユニット１２の目的位置Ｏの位置情報を、通信部２０を介して、そのビークルユニット１２に属するリーダービークル１０Ａに送信する。

なお、管理システム１４は必須の構成でなく、例えば、ビークル１０が、より好ましくはリーダービークル１０Ａが、目的位置Ｏの位置情報を設定してもよい。

(ビークルの経路設定)
それぞれのビークル１０は、自身で移動する経路を設定する。すなわち、本実施形態に係るビークル制御システム１００は、管理システム１４がそれぞれのビークル１０の情報を集約して経路を設定するシステムではなく、それぞれのビークル１０が自身の経路を設定する分散型の群知能システムであるといえる。

本実施形態においては、それぞれのビークル１０が経路設定を行うにあたり、以下の前提条件を満足していることが好ましい。
１．同じ目的をもつ（本実施形態では同じ目的位置Ｏとなる）ビークル群を、１つのチーム、すなわち１つのビークルユニット１２とする。
２．１つのビークルユニット１２に属するビークル１０は、所定のフォーメーション（ビークル同士の位置関係）が与えられる。
３．ビークルユニット１２の数をＩとし、１つのビークルユニット１２に属するビークル１０の数をＪとすると、１つのビークルユニット１２には、１台のリーダービークル１０Ａと、Ｊ－１台のフォロワービークル１０Ｂが存在する。
４．リーダービークル１０Ａは、異なるビークルユニット１２に所属するリーダービークル１０Ａ、及び、同じビークルユニット１２に所属するフォロワービークル１０Ｂと、通信可能である。
５．フォロワービークル１０Ｂは、同じビークルユニット１２に所属するリーダービークル１０Ａ、及び、同じビークルユニット１２に所属する他のフォロワービークル１０Ｂと、通信可能である。
６．ビークル１０の位置及び姿勢と速度とは既知であり、計測誤差が所定の許容値以下である。
７．ビークル１０間の通信遅れが、所定の許容値以下である。
８．ビークル１０は、領域ＡＲの地図情報を保有し、障害物などが存在する進入禁止場所の位置（座標）の真値は既知である。
９．ビークル１０は、他のビークル１０と衝突しない。

また、ビークル１０がＡＧＦやＡＧＶなどの地上を走行する移動体である場合には、以下の前提条件が追加される。
１０．ビークル１０は、非ホノロミックなシステムであり、真横に移動できない。
１１．ビークル１０は、旋回半径及び速度に制約があり、上下限の範囲外では動作できない。

なお、リーダービークル１０Ａとフォロワービークル１０Ｂとは、１つのビークルユニット１２で適切に協調して動作するために、経路の設定方法が異なる。以下、リーダービークル１０Ａとフォロワービークル１０Ｂとの具体的な構成について説明する。

（リーダービークル）
図３は、第１実施形態に係るリーダービークルの模式的なブロック図である。図３に示すように、リーダービークル１０Ａは、制御装置３０と、通信部３２と、位置検出部３４と、動力部３６とを有する。

（通信部）
通信部３２は、外部の装置と通信を行う通信モジュールであり、例えばアンテナなどである。リーダービークル１０Ａは、無線通信で通信を行うが、通信方式は任意であってよい。リーダービークル１０Ａは、通信部３２を介して、管理システム１４と通信して、情報の送受信を行う。リーダービークル１０Ａは、通信部３２を介して、同じビークルユニット１２に所属するフォロワービークル１０Ｂと通信して、情報の送受信を行う。リーダービークル１０Ａは、通信部３２を介して、異なるビークルユニット１２に所属するリーダービークル１０Ａと通信して、情報の送受信を行う。本実施形態では、リーダービークル１０Ａは、異なるビークルユニット１２に所属するフォロワービークル１０Ｂとは、直接通信を行わない。

（位置検出部）
位置検出部３４は、リーダービークル１０Ａの位置及び姿勢、すなわち自己位置及び自己姿勢を検出する。リーダービークル１０Ａの位置とは、本実施形態においては、領域ＡＲ内においてリーダービークル１０Ａが位置している座標を指し、リーダービークル１０Ａの姿勢とは、リーダービークル１０Ａが向いている方向を指す。位置検出部３４は、任意の方法で位置及び姿勢を検出してよいが、例えば、位置検出部３４の具体的構成例として、グローバル・ポジショニング・システム（ＧＰＳ：Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）等の測位システムを利用して位置を検出するための測位装置が挙げられる。また例えば、位置検出部３４は、所定の起点に対する位置を検出する慣性航法装置であってもよい。また例えば、位置検出部３４は、レーザ光により位置及び姿勢を検出するものであってもよい。この場合例えば、領域ＡＲに反射体が設けられており、位置検出部３４が反射体に向けてレーザ光を照射し、反射体から反射したレーザ光を検出することで、位置及び姿勢が検出できる。

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

（制御装置）
制御装置３０は、リーダービークル１０Ａの動作を制御する装置である。制御装置３０は、コンピュータであり、記憶部４０と制御部４２とを備える。記憶部４０は、制御部４２の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。記憶部４０が記憶する制御部４２用のプログラムは、制御装置３０が読み取り可能な記録媒体に記憶されていてもよい。

制御部４２は、演算装置、すなわちＣＰＵである。制御部４２は、目的位置情報取得部５０と、自己位置情報取得部５２と、リーダー用経路設定部５４と、情報送信部５６と、移動制御部５８とを含む。制御部４２は、記憶部４０からプログラム（ソフトウェア）を読み出して実行することで、目的位置情報取得部５０と自己位置情報取得部５２とリーダー用経路設定部５４と情報送信部５６と移動制御部５８とを実現して、それらの処理を実行する。なお、制御部４２は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、これらの処理を実行してもよい。また、目的位置情報取得部５０と自己位置情報取得部５２とリーダー用経路設定部５４と情報送信部５６と移動制御部５８との少なくとも一部を、ハードウェア回路で実現してもよい。

（目的位置情報取得部）
目的位置情報取得部５０は、リーダービークル１０Ａの目標とする移動先である目的位置Ｏの位置情報を取得する。リーダービークル１０Ａの目的位置Ｏは、そのリーダービークル１０Ａが所属するビークルユニット１２の目的位置Ｏともいえる。目的位置情報取得部５０は、通信部３２を介して、管理システム１４から、目的位置Ｏの位置情報を取得する。

（自己位置情報取得部）
自己位置情報取得部５２は、リーダービークル１０Ａ自身の位置及び姿勢の情報を取得する。自己位置情報取得部５２は、位置検出部３４を制御して、リーダービークル１０Ａ自身の位置情報（座標情報）及び姿勢情報（向きを示す情報）を取得する。以下、位置情報及び姿勢情報を合わせて、適宜、位置姿勢情報と記載する。自己位置情報取得部５２は、所定時間毎に、逐次、リーダービークル１０Ａ自身の位置姿勢情報を取得する。また、自己位置情報取得部５２は、位置検出部３４を用いて、他のビークル１０との相対距離を算出してもよい。相対距離を算出して経路生成に用いることで、通信遅れをカバーすることができる。

（リーダー用経路設定部）
リーダー用経路設定部５４は、リーダービークル１０Ａ用の移動経路であるリーダー用経路ＲＡを設定する。リーダー用経路設定部５４は、次の式（１）、（２）を達成可能なように、リーダー用経路ＲＡを設定して、リーダービークル１０Ａへの制御入力を算出する。なお、以降の説明は、ビークル１０がＡＧＦやＡＧＶなどの地上を走行する移動体である場合を例としたものであり、ビークル１０は、上述のように地面を走行する移動体に限られない。以下の説明における式などは、ビークル１０の種類に応じて適宜設定されてよい。

ここで、ｉはビークルユニット１２の識別番号であり、ｉ＝１、２、・・・Ｉとなり、Ｉはビークルユニット１２の総数である。また、ｊは１つのビークルユニット１２でのビークル１０の識別番号であり、ｊ＝１、２、・・・Ｊとなり、Ｊは１つのビークルユニット１２でのビークル１０の総数である。リーダービークル１０Ａの識別番号を１とする。また、ｋは離散時間であり、ｋ＝０、１、２、・・・となる。また、ｐ_ｉｊ（ｋ）は、識別番号がｉのビークルユニット１２に属する、識別番号がｊのビークル１０（以降、ビークル１０_ｉｊとする）の、位置（座標）を指す。ｑ_ｉｊは、ビークル１０_ｉｊの目的位置Ｏの座標である。また、Λは、ビークル１０同士の間で接近を許容する距離の最小値である。

また、ビークル１０_ｉｊの位置であるｐ_ｉｊ（ｋ）と、ビークル１０_ｉｊの位置姿勢であるｐ´_ｉｊ（ｋ）は、次の式（３）、（４）で表される。なお、ｘ_ｉｊ（ｋ）は、ビークル１０_ｉｊのＸ方向における位置（座標）であり、ｙ_ｉｊ（ｋ）は、ビークル１０_ｉｊのＹ方向における位置（座標）であり、θ_ｉｊ（ｋ）は、ビークル１０_ｉｊのヨー角方向の姿勢角である。Ｔは転置を示す。

また、ビークル１０_ｉｊの進行方向の速度をｖ_ｉｊ（ｋ）とし、ビークル１０_ｉｊの前輪舵角をφ_ｉｊ（ｋ）とすると、ビークル１０_ｉｊへの制御入力は、速度であるｖｒｅｆ_ｉｊ（ｋ）と、前輪舵角であるφｒｅｆ_ｉｊ（ｋ）となる。以下適宜、ビークル１０_ｉｊへの制御入力を、式（５）に示すｕｒｅｆ_ｉｊ（ｋ）とする。

図４は、リーダー用経路設定部５４の模式的なモデル図である。図４に示すモデルＭ１Ａは、リーダー用経路ＲＡを設定して、リーダービークル１０Ａへの制御入力を算出するモデルである。図４に示すモデルＭ２Ａは、リーダービークル１０Ａの運動モデルである。図４に示すように、モデルＭ２Ａは、制御入力であるｕｒｅｆ_ｉｊ（ｋ）が入力値として入力されたら、ビークル１０_ｉｊが制御入力に基づき移動することで、位置姿勢であるｐ´_ｉｊ（ｋ）を出力値として出力する。リーダービークル１０ＡのモデルＭ２Ａとしては、ビークル１０がＡＧＦやＡＧＶなどの地上を走行する移動体である場合を例にした場合、例えば、次の式（６）－（８）に示す等価二輪モデルを用いてよい。

なお、ｘ´_ｉｊ（ｋ）、ｙ´_ｉｊ（ｋ）、θ´_ｉｊ（ｋ）は、それぞれ、ｘ_ｉｊ（ｋ）、ｙ_ｉｊ（ｋ）、θ_ｉｊ（ｋ）の時間微分値を指す。また、Ｌは、ビークル１０_ｉｊのホイールベース長を指す。運動モデルは、このような等価二輪モデルに限られず、ビークルの種類に応じたモデルを適用してよい。

図４に示すように、モデルＭ１Ａは、位置姿勢であるｐ´_ｉｊ（ｋ）が入力値として入力されたら、リーダー用経路ＲＡを設定して、リーダービークル１０Ａへの制御入力であるｕｒｅｆ_ｉｊ（ｋ）を出力値として出力する。モデルＭ１Ａは、以下の式（９）のように表される。

Ｕ_ｉｊ（ｋ）は、計画経路であるｐ´_ｉｊ（ｋ＋１）、ｐ´_ｉｊ（ｋ＋２）、・・・ｐ´_ｉｊ（ｋ＋Ｎ）に対応した、ビークル１０_ｉｊへの制御入力の系列を表す。Ｕ_ｉｊ（ｋ）は、次の式（１０）のように定義される。

なお、ｆ_ｉｊは評価関数であり、ｇ_ｉｊは制約条件である。

（リーダー用経路の設定）
リーダー用経路設定部５４によるリーダー用経路ＲＡの設定方法について、以下、より具体的に説明する。リーダー用経路設定部５４は、進入禁止場所の位置を示す情報と目的位置Ｏの位置情報とに基づいて、リーダービークル１０Ａが移動する経路であるリーダー用経路ＲＡを設定する。進入禁止場所とは、例えば障害物が存在する場所など、領域ＡＲ内において、ビークル１０の進入が禁止されたり、ビークル１０の進入が不可能だったりする場所を指し、予め位置が設定されている。また、リーダー用経路設定部５４は、後述するフォロワービークル１０Ｂが生成するフォロワー用経路ＲＢを参照することなく、リーダー用経路ＲＡを設定する。すなわち、リーダービークル１０Ａは、フォロワー用経路ＲＢを入力値に用いることなく、言い換えればフォロワー用経路ＲＢを考慮することなく、リーダー用経路ＲＡを設定する。

より具体的には、リーダー用経路設定部５４は、無向グラフ情報取得部５４Ａと、経由ノード設定部５４Ｂと、リーダー用経路算出部５４Ｃとを含む。

（無向グラフ情報取得部）
図５は、無向グラフ情報の一例を示す模式図である。無向グラフ情報取得部５４Ａは、無向グラフ情報を取得する。図５の例に示すように、無向グラフ情報は、領域ＡＲ内の座標毎に設定されたノードｎｄと、一対のノードｎｄを結ぶ線であるエッジｅｄとを含む。言い換えれば、無向グラフ情報は、領域ＡＲ内におけるそれぞれのノードｎｄの位置情報（座標情報）と、ノードｎｄ同士を結ぶエッジｅｄの位置情報（座標情報）とを含む情報であるといえる。なお、エッジｅｄは直線であるが、直線であることに限られない。

無向グラフ情報取得部５４Ａは、無向グラフ情報を予め生成して記憶部４０に記憶させておき、リーダー用経路ＲＡを設定する際には、予め作成した無向グラフ情報を読み出すことで、無向グラフ情報を取得する。以下、無向グラフ情報の生成方法について説明する。

無向グラフ情報取得部５４Ａは、領域ＡＲの地図情報を取得する。領域ＡＲの地図情報とは、領域ＡＲ内の進入禁止場所の位置情報や、ビークル１０が移動可能な通路などの位置情報を含んだ情報である。領域ＡＲの地図情報は、既知であり予め設定されている。無向グラフ情報取得部５４Ａは、領域ＡＲの地図情報に基づいて、領域ＡＲ内の座標毎にノードｎｄを設定する。無向グラフ情報取得部５４Ａは、例えば、所定間隔毎に、領域ＡＲ内の全域にわたってノードｎｄを設定してよい。そして、無向グラフ情報取得部５４Ａは、設定したノードｎｄのうちから、一対のノードｎｄを抽出して、抽出した一対のノードｎｄを結ぶ線分を定義する。そして、無向グラフ情報取得部５４Ａは、領域ＡＲの地図情報から読み出した進入禁止場所の位置情報と、一対のノードｎｄを結ぶ線分の位置情報とから、一対のノードｎｄを結ぶ線分の少なくとも一部分が、進入禁止場所の領域内に位置するかを判断する。言い換えれば、無向グラフ情報取得部５４Ａは、一対のノードｎｄを結ぶ線分の座標と進入禁止場所の座標とが重なるかを判断する。無向グラフ情報取得部５４Ａは、一対のノードｎｄを結ぶ線分の少なくとも一部分が進入禁止場所の領域内に位置する場合、その線分をエッジｅｄとして設定しない。一方、無向グラフ情報取得部５４Ａは、一対のノードｎｄを結ぶ線分の少なくとも一部分が進入禁止場所の領域内に位置しない場合、すなわち一対のノードｎｄを結ぶ線分の全区間が進入禁止場所の領域外に位置する場合、その線分をエッジｅｄとして設定する。無向グラフ情報取得部５４Ａは、設定したノードｎｄから抽出できる一対のノードｎｄの全ての組み合わせについて、同様の処理を行って、エッジｅｄを設定する。なお、図５の例では、ノードｎｄ１と他のノードｎｄを結ぶ線分のうち、進入禁止場所ＡＲ１を通らない線分が、エッジｅｄとして設定され（図５の実線）、進入禁止場所ＡＲ１を通る線分が、エッジｅｄとして設定されていない（図５の破線）ことを示している。

なお、上述のように、無向グラフ情報取得部５４Ａは、領域ＡＲ内の全域にわたって所定間隔毎にノードｎｄを設定してもよいが、ノードｎｄの設定方法はそれに限られず任意である。例えば、無向グラフ情報取得部５４Ａは、領域ＡＲ内のうちで、進入禁止場所ＡＲ１の周縁（例えば頂点など）に位置にノードｎｄを設定してもよい。この場合、ノードｎｄは、進入禁止場所ＡＲ１の周縁以外の場所に設定されていなくてもよいし、進入禁止場所ＡＲ１の周縁以外の場所にも設定されてもよい。

無向グラフ情報取得部５４Ａは、このようにしてノードｎｄとエッジｅｄを抽出して、ノードｎｄとエッジｅｄの位置情報を、無向グラフ情報として設定する。従って、無向グラフ情報には、進入禁止場所を通るエッジｅｄが含まれず、進入禁止場所を通らないエッジｅｄが含まれることとなる。なお、エッジｅｄの位置情報には、エッジｅｄの長さを示す情報も含まれる。

（経由ノード設定部）
経由ノード設定部５４Ｂは、無向グラフ情報に基づき、経由ノードｎｄａを設定する。図６は、経由ノードの設定の一例を説明する模式図である。経由ノード設定部５４Ｂは、無向グラフ情報に含まれるエッジｅｄのうちから、目的位置Ｏに到達可能なエッジｅｄを、経由エッジｅｄａとして抽出する。より具体的には、経由ノード設定部５４Ｂは、リーダービークル１０Ａ又はビークルユニット１２の初期位置から、目的位置Ｏまでを結ぶエッジｅｄを、経由エッジｅｄａとして抽出する。経由エッジｅｄａは、目的位置Ｏに応じて、１つのエッジｅｄとなる場合もあるし、ノードｎｄを介して連結する複数のエッジｅｄとなる場合もある。

本実施形態では、経由ノード設定部５４Ｂは、エッジｅｄを通って目的位置Ｏまで到達するまでの距離が最短となるように、経由エッジｅｄａを抽出する。言い換えれば、経由ノード設定部５４Ｂは、エッジｅｄを通って目的位置Ｏまで到達可能な経路が複数ある場合には、それらの経路のうち、距離が最も短い経路を構成するエッジｅｄを、経由エッジｅｄａとして抽出する。すなわち、経由ノード設定部５４Ｂは、エッジｅｄの長さ（すなわちノードｎｄ間の距離）に重み付けをして、その重み付けに基づき、目的位置Ｏまで到達するまでのエッジｅｄの合計長さが最短となるように、経由エッジｅｄａを抽出する。経由ノード設定部５４Ｂは、例えばダイクストラ法などのグラフ探索により最短経路を算出することにより、経由エッジｅｄａを抽出してよい。

経由ノード設定部５４Ｂは、経由エッジｅｄａに結ばれているノードｎｄを、経由ノードｎｄａとして設定する。経由ノードｎｄａは目的位置Ｏまでの経由エッジｅｄａに結ばれたポイントであるため、経由ノード設定部５４Ｂは、経由エッジｅｄａや経由ノードｎｄａを抽出することで、目的位置Ｏまでのおおまなか経路を設定しているといえる。図６は、初期位置がノードｎｄ１と重なり、目的位置Ｏがノードｎｄ４と重なった場合の経由ノードｎｄａの例を示している。図６の例では、ノードｎｄ１とノードｎｄ２とを結ぶエッジｅｄ、ノードｎｄ２とノードｎｄ３とを結ぶエッジｅｄ、ノードｎｄ３とノードｎｄ４とを結ぶエッジｅｄとが、目的位置Ｏまでの最短となる経路なので、それらのエッジｅｄ（図６の実線部分）が、経由エッジｅｄａとされ、それらのエッジｅｄに結ばれるノードｎｄ１、ｎｄ２、ｎｄ３、ｎｄ４が、経由ノードｎｄａとして抽出されている。

なお、以上の説明では、無向グラフ情報取得部５４Ａが無向グラフ情報を生成した時点で、進入禁止場所ＡＲ１を通るエッジｅｄを除外しており、経由ノード設定部５４Ｂが、進入禁止場所ＡＲ１を通らないエッジｅｄのなかから、経由エッジｅｄａを選定していた。ただし、進入禁止場所ＡＲ１を通るエッジｅｄを除外するタイミングはそれに限られず、経由ノード設定部５４Ｂは、進入禁止場所ＡＲ１を通らず、かつ目的位置Ｏに到達可能なエッジｅｄを、経由エッジｅｄａとして選定すればよい。例えば、無向グラフ情報取得部５４Ａは、一対のノードｎｄを通る線分の全てをエッジｅｄとして選定し、経由ノード設定部５４Ｂは、それらのエッジｅｄのうちから、進入禁止場所ＡＲ１を通らないエッジｅｄを選定して、選定したエッジｅｄから、上記と同様の方法で、経由エッジｅｄａ及び経由ノードｎｄａを選定してよい。

（リーダー用経路算出部）
図７は、リーダー用経路の設定を説明する模式図である。リーダー用経路算出部５４Ｃは、経由ノードｎｄａに基づき、リーダー用経路ＲＡを設定する。リーダー用経路算出部５４Ｃは、経由ノードｎｄａに基づき、モデル予測制御（ＭＰＣ:Model Predictive Control）でリーダー用経路ＲＡを設定する。より詳しくは、リーダー用経路算出部５４Ｃは、経由ノードｎｄａを通るように、すなわち経由ノードｎｄａをウェイポイントとして、モデル予測制御を用いてリーダー用経路ＲＡを設定する。リーダー用経路算出部５４Ｃは、進入禁止場所ＡＲ１を通らない経由エッジｅｄａに基づいて設定された経由ノードｎｄａをウェイポイントとするため、リーダービークル１０Ａが進入禁止場所ＡＲ１に進入することを抑制できる。また、リーダー用経路算出部５４Ｃは、最短経路となる経由エッジｅｄａに基づいて設定された経由ノードｎｄａをウェイポイントとするため、リーダー用経路ＲＡが長くなることを抑制して、作業効率を向上できる。

リーダー用経路算出部５４Ｃは、経由ノードｎｄａ毎に、リーダー用経路ＲＡを設定する。すなわち、図７に示すように、リーダー用経路算出部５４Ｃは、初期位置（図７の例ではノードｎｄ１）から、次の経由ノードであるノードｎｄ２までのリーダー用経路ＲＡ１を、モデル予測制御を用いて算出する。なお、次の経由ノードとは、前の経由ノードよりも目的位置Ｏに近く、かつ、前の経由ノードｎｄａに対して経由エッジｅｄａで結ばれている経由ノードであり、言い換えれば、リーダービークル１０Ａの次のウェイポイントとなる経由ノードを指す。リーダービークル１０Ａは、算出されたリーダー用経路ＲＡ１に沿って、ノードｎｄ２に向けて移動する。リーダービークル１０Ａがノードｎｄ２に対して所定距離範囲内に近づいたら、リーダー用経路算出部５４Ｃは、次のウェイポイントをノードｎｄ３に切り替えて、ノードｎｄ３を通るリーダー用経路ＲＡ２を、モデル予測制御を用いて算出する。同様に、リーダービークル１０Ａがノードｎｄ３に対して所定距離範囲内に近づいたら、リーダー用経路算出部５４Ｃは、次のウェイポイントをノードｎｄ４に切り替えて、ノードｎｄ４を通るリーダー用経路ＲＡ３を、モデル予測制御を用いて算出する。このようにして、目的位置Ｏであるノードｎｄ４までのリーダー用経路ＲＡが、経由ノードｎｄａ毎に順次設定されて、リーダービークル１０Ａは目的位置Ｏに適切に到着できる。なお、本実施形態における所定距離は、ゼロとして設定されている。従って、リーダー用経路算出部５４Ｃは、リーダービークル１０Ａがウェイポイントである経由ノードｎｄａに到着したこと（距離がゼロとなったこと）をトリガとして、ウェイポイントを次の経由ノードｎｄａに切り替えて、到着した経由ノードｎｄａから次の経由ノードｎｄａまでの経路を、リーダー用経路ＲＡとして更新設定する。ただし、所定距離はゼロに限られず、例えば後述の第５実施形態に示すように、任意の長さに設定されてよい。

リーダー用経路算出部５４Ｃは、このようにしてリーダー用経路ＲＡを設定して、例えばリーダー用経路ＲＡとリーダービークル１０Ａの現在の位置姿勢に基づき、設定したリーダー経路ＲＡに沿って移動可能な制御入力を算出する。

なお、リーダービークル１０Ａはビークルユニット１２全体の経路を決める役割を有するため、次のウェイポイントまでビークルユニット１２全体が到達できるように、モデル予測制御に基づいてリーダー用経路ＲＡを算出するための評価関数であるｆ_ｉ１は、次の式（１１）のように定義されることが好ましい。また、リーダー用経路ＲＡを算出するための制約条件であるｇ_ｉ１は、上述の式（６）－（８）に加えて、次の式（１２）のように制御入力の上限が設定されてもよい。

（情報送信部）
図３に示す情報送信部５６は、通信部３２を介して、リーダービークル１０Ａ自身の情報や自身が所属するビークルユニット１２に関する情報を送信する。情報送信部５６は、自身が所属するビークルユニット１２のフォロワービークル１０Ｂに、リーダービークル１０Ａ自身の位置姿勢情報と、リーダー用経路設定部５４に設定されたリーダー用経路ＲＡの位置情報と、フォーメーション情報とを送信する。フォーメーション情報とは、同じビークルユニット１２に所属するビークル１０が維持することが望ましいフォーメーションを示す情報であり、より詳しくは、リーダービークル１０Ａに対するフォロワービークル１０Ｂの望ましい相対位置を示す情報といえる。フォーメーション情報は、フォロワービークル１０Ｂ毎に設定される。すなわち、リーダービークル１０Ａに対するフォロワービークル１０Ｂの望ましい相対位置は、フォロワービークル１０Ｂ毎に設定されて、フォロワービークル１０Ｂ毎に異なる。フォーメーション情報は、任意に設定されてよい。例えば、フォーメーション情報は、予め設定されていてもよいし、リーダービークル１０Ａが設定してもよい。

（移動制御部）
移動制御部５８は、動力部３６を制御して、リーダービークル１０Ａの移動を制御する。移動制御部５８は、リーダー用経路設定部５４が算出した制御入力に基づいて動力部３６を制御することで、リーダービークル１０Ａをリーダー用経路ＲＡに沿って移動させる。

（フォロワービークル）
図８は、第１実施形態に係るフォロワービークルの模式的なブロック図である。図８に示すように、フォロワービークル１０Ｂは、制御装置７０と、通信部７２と、位置検出部７４と、動力部７６とを有する。

（通信部）
通信部７２は、外部の装置と通信を行う通信モジュールであり、例えばアンテナなどである。フォロワービークル１０Ｂは、無線通信で通信を行うが、通信方式は任意であってよい。フォロワービークル１０Ｂは、通信部７２を介して、同じビークルユニット１２に所属するリーダービークル１０Ａ、及び同じビークルユニット１２に所属する他のフォロワービークル１０Ｂと通信して、情報の送受信を行う。本実施形態では、フォロワービークル１０Ｂは、異なるビークルユニット１２に所属するビークル１０や、管理システム１４とは、直接通信を行わない。

（位置検出部）
位置検出部７４は、フォロワービークル１０Ｂの位置及び姿勢、すなわち自己位置及び自己姿勢を検出する。位置検出部７４の構成は、リーダービークル１０Ａの位置検出部３４と同様であるため、説明を省略する。

（動力部）
動力部７６は、フォロワービークル１０Ｂを移動させる動力として機能する。動力部３６の具体的構成は、フォロワービークル１０Ｂの運用形態に応じるが、リーダービークル１０Ａの動力部３６と同様であるため、説明を省略する。

（制御装置）
制御装置７０は、フォロワービークル１０Ｂの動作を制御する装置である。制御装置７０は、コンピュータであり、記憶部８０と制御部８２とを備える。記憶部８０は、制御部８２の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。記憶部８０が記憶する制御部８２用のプログラムは、制御装置７０が読み取り可能な記録媒体に記憶されていてもよい。

制御部８２は、演算装置、すなわちＣＰＵである。制御部８２は、自己位置情報取得部９０と、情報取得部９２と、目標位置設定部９４と、移動制御部９６とを含む。制御部８２は、記憶部８０からプログラム（ソフトウェア）を読み出して実行することで、自己位置情報取得部９０と情報取得部９２と目標位置設定部９４と移動制御部９６とを実現して、それらの処理を実行する。なお、制御部８２は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、これらの処理を実行してもよい。また、自己位置情報取得部９０と情報取得部９２と目標位置設定部９４と移動制御部９６との少なくとも一部を、ハードウェア回路で実現してもよい。

（自己位置情報取得部）
自己位置情報取得部９０は、フォロワービークル１０Ｂ自身の位置及び姿勢の情報を取得する。自己位置情報取得部９０は、位置検出部７４を制御して、フォロワービークル１０Ｂ自身の位置姿勢情報を取得する。自己位置情報取得部９０は、所定時間毎に、逐次、フォロワービークル１０Ｂ自身の位置姿勢情報を取得する。また、自己位置情報取得部９０は、位置検出部７４を用いて、他のビークル１０との相対距離を算出してもよい。相対距離を算出して経路生成に用いることで、通信遅れをカバーすることができる。

（情報取得部）
情報取得部９２は、通信部７２を介して、同じビークルユニット１２に所属するリーダービークル１０Ａと通信して、リーダービークル１０Ａから情報を取得する。情報取得部９２は、同じビークルユニット１２に所属するリーダービークル１０Ａから、リーダービークル１０Ａの位置姿勢情報と、リーダー用経路ＲＡの位置情報と、フォーメーション情報とを取得する。情報取得部９２は、自身のフォロワービークル１０Ｂに対して設定されたフォーメーション情報を、すなわち自身のフォロワービークル１０Ｂのリーダービークル１０Ａに対する望ましい相対位置を、取得する。情報取得部９２は、フォーメーション情報として、すなわちリーダービークル１０Ａに対するフォロワービークル１０Ｂの相対位置の情報として、リーダービークル１０Ａからの望ましい相対距離ｄ_ｊ（図１１参照）と、リーダービークル１０Ａに対する望ましい相対角度Ψ_ｊ（図１１参照）とを取得する。また、情報取得部９２は、通信部７２を介して、同じビークルユニット１２に所属する他のフォロワービークル１０Ｂと通信して、情報を取得する。情報取得部９２は、同じビークルユニット１２に所属する他のフォロワービークル１０Ｂの位置情報を取得する。

（目標位置設定部）
目標位置設定部９４は、同じビークルユニット１２に属するリーダービークル１０Ａのリーダー経路ＲＡに基づいて、フォロワービークル１０Ｂ用の移動経路であるフォロワー用経路ＲＢを設定する。そして、目標位置設定部９４は、フォロワー用経路ＲＢに基づいて、フォロワービークル１０Ｂの目標位置ＯＭ及び目標姿勢を設定して、目標位置ＯＭ及び目標姿勢に基づき、フォロワービークル１０Ｂへの制御入力を算出する。目標位置ＯＭは、フォロワービークル１０Ｂが目標とする移動先の位置であり、最終的な移動先である目的位置Ｏに至るまでの中途の目的地であるといえる。目標姿勢は、目標位置ＯＭにおいてフォロワービークル１０Ｂがとるべき姿勢を指す。目標位置設定部９４は、上述の式（１）、（２）を達成可能なように、フォロワー用経路ＲＢを設定して、フォロワービークル１０Ｂへの制御入力を算出する。

図９は、目標位置設定部の模式的なモデル図である。図９に示すモデルＭ１Ｂは、フォロワー用経路ＲＢを設定して、フォロワービークル１０Ｂへの制御入力を算出するモデルである。図９に示すモデルＭ２Ｂは、フォロワービークル１０Ｂの運動モデルである。本実施形態では、モデルＭ２Ｂは、リーダービークル１０ＡのモデルＭ２Ａと同様であるため、説明を省略する。

図９に示すように、モデルＭ１Ｂは、フォロワービークル１０Ｂ自身の位置姿勢であるｐ´_ｉｊ（ｋ）と、同じビークルユニット１２に属するリーダービークル１０Ａの位置姿勢であるｐ´_ｉ１（ｋ＋１）と、同じビークルユニット１２に属する他のフォロワービークル１０Ｂの位置姿勢であるｐ´_ｉｍ（ｋ）とが入力値として入力されたら、フォロワー用経路ＲＢ、目標位置ＯＭ及び目標姿勢を設定して、フォロワービークル１０Ｂへの制御入力であるｕｒｅｆ_ｉｊ（ｋ）を出力値として出力する。モデルＭ１Ｂは、リーダービークル１０ＡのモデルＭ１Ａと同様に、上述の式（９）のように表されるが、評価関数ｆ_ｉｊと制約条件ｇ_ｉｊとが、リーダービークル１０Ａとは異なる。

目標位置設定部９４によるフォロワー用経路ＲＢ、目標位置ＯＭ及び目目標姿勢の設定方法について、以下、より具体的に説明する。目標位置設定部９４は、フォロワー用経路設定部９４Ａと、フォーメーション算出部９４Ｂと、目標位置算出部９４Ｃとを含む。

（フォロワー用経路設定部）
図１０は、フォロワー用経路の一例を説明する模式図である。フォロワー用経路設定部９４Ａは、同じビークルユニット１２に所属するリーダービークル１０Ａと追従するように、情報取得部９２が取得したリーダー用経路ＲＡに基づき、フォロワー用経路ＲＢを設定する。フォロワー用経路設定部９４Ａは、リーダー用経路ＲＡから所定距離離れるように、フォロワー用経路ＲＢを設定する。より詳しくは、図１０に示すように、フォロワー用経路設定部９４Ａは、リーダー用経路ＲＡと、自身のフォロワービークル１０Ｂに対して予め設定された所定距離δ_ｉｊとから、リーダー用経路ＲＡから、リーダー用経路ＲＡに直交する方向に所定距離δ_ｉｊだけ離れた経路を、フォロワー用経路ＲＢとして決定する。フォロワー用経路ＲＢは、リーダー用経路ＲＡに対して所定距離δ_ｉｊ離れつつ、平行な経路となる。フォロワー用経路ＲＢがリーダー用経路ＲＢから離れる所定距離δ_ｉｊは、フォロワービークル１０Ｂ毎に予め設定されており、フォロワービークル１０Ｂ毎に異なる方向又は長さとなる。所定距離δ_ｉｊは、任意に設定されてよいが、そのフォロワービークル１０Ｂのフォーメーション情報において設定されたリーダービークル１０Ａとの相対距離に近い値（相対距離に対して所定の数値範囲内）に設定されることが好ましい。

すなわち、フォロワー用経路設定部９４Ａは、フォロワー用経路ＲＢを、次の式（１３）－（１５）により求める。

なお、式（１３）－（１５）においては、ｊは１でなく、ｎ＝１、２、・・・Ｎである。

（フォーメーション算出部）
図１１は、フォーメーション位置及び目的位置を説明する模式図である。フォーメーション算出部９４Ｂは、情報取得部９２が取得したフォーメーション情報に基づき、すなわちリーダービークル１０Ａからの相対距離ｄ_ｊと、リーダービークル１０Ａに対する相対角度Ψ_ｊとに基づき、フォロワービークル１０Ｂのフォーメーション位置ＦＭを算出する。フォーメーション位置ＦＭとは、ビークルユニット１２のフォーメーションを維持するための、フォロワービークル１０Ｂの望ましい位置を指す。すなわち、フォーメーション情報がリーダービークル１０Ａに対するフォロワービークル１０Ｂの相対位置であるのに対し、フォーメーション位置ＦＭは、リーダービークル１０Ａに対する相対位置ではなく、領域ＡＲの座標系におけるフォロワービークル１０Ｂの位置（座標）といえる。フォーメーション算出部９４Ｂは、情報取得部９２が取得したリーダービークル１０Ａの現在の位置から、相対角度Ψ_ｊの方向に、相対距離ｄ_ｊだけ離れた位置を、フォーメーション位置ＦＭとして算出する。

すなわち、フォーメーション位置ＦＭを式（１６）とした場合、フォーメーション算出部９４Ｂは、次の式（１７）、（１８）により、フォーメーション位置ＦＭを算出する。なお、ｘｆ_ｉｊ（ｋ）は、フォーメーション位置ＦＭのＸ方向における座標であり、ｙｆ_ｉｊ（ｋ）は、フォーメーション位置ＦＭのＹ方向における座標である。

（目標位置算出部）
目標位置算出部９４Ｃは、フォロワー用経路設定部９４Ａが設定したフォロワー用経路ＲＢと、フォーメーション算出部９４Ｂが算出したフォーメーション位置ＦＭとに基づき、目標位置ＯＭ及び目標姿勢を設定する。目標位置算出部９４Ｃは、フォロワー用経路ＲＢ上の位置（座標）のうち、フォーメーション位置ＦＭに最も近い位置を、すなわちフォーメーション位置ＦＭに最も近いフォロワー用経路ＲＢ上の位置を、目標位置ＯＭとして設定する。そして、目標位置算出部９４Ｃは、例えば、目標位置ＯＭに到達した際のフォロワービークル１０Ｂの姿勢（進行方向の向き）が、フォロワー用経路ＲＢに沿うように、目的姿勢を設定する。このように目標位置ＯＭを設定することで、できるだけフォーメーションを維持しつつ、大回りなどによって経路が長くなってしまうことを抑制できる。

すなわち、目標位置ＯＭのインデクスは、次の式（１９）のように表される。

そのため、次の式（２０）を目標位置ＯＭ及び目標姿勢とすると、目標位置ＯＭ及び目標姿勢は、次の式（２１）－（２３）のように設定される。

目標位置算出部９４Ｃは、このようにして目標位置ＯＭ及び目標姿勢を設定して、目標位置ＯＭ及び目標姿勢に基づき、目標位置ＯＭ及び目標姿勢に到達可能なフォロワービークル１０Ｂの制御入力を算出する。なお、フォロワービークル１０Ｂは、目標位置算出部９４Ｃは、リーダービークル１０Ａがリーダー用経路ＲＡを更新する度に、フォロワー用経路ＲＢ、フォーメーション位置ＦＭ、目標位置ＯＭ及び目標姿勢を更新する。

なお、フォロワービークル１０Ｂの式（９）での評価関数ｆ_ｉｊは、次の式（２４）のように設定されることが好ましい。これにより、フォロワービークル１０Ｂの制御入力が算出できる。なお、フォロワービークル１０Ｂの予測ホライズンは１である。フォロワービークル１０Ｂは、リーダー用経路ＲＡに追従するのみであるため、先読みして経路を設定する必要はなく、予測ホライズンは１で十分となる。このように予測ホライズンを１とすることで、演算負荷を低減できる。ただし、フォロワービークル１０Ｂの予測ホライズンは１に限定されない。また、ηｘ、ηｙ、ηｚは重み係数である。

また、フォロワービークル１０Ｂの式（９）での制約条件ｇ_ｉｊは、次の式（２５）のように設定されることが好ましい。これにより、リーダービークル１０Ａとフォロワービークル１０Ｂとが接近した際に、リーダービークル１０Ａはフォロワービークル１０Ｂを回避しようとせず（動作を変えず）、フォロワービークル１０Ｂがリーダービークル１０Ａを回避する動作をとることが可能となる。これにより、両者が同時に回避動作を行う際に生じるデッドロックを回避できる。

なお、フォロワー用経路ＲＢは、リーダー用経路ＲＡに基づいて設定されているため、進入禁止場所ＡＲ１を避けるように設定されるが、フォロワービークル１０Ｂは、例えばリーダー用経路ＲＡの位置情報に加えて、進入禁止場所ＡＲ１の位置情報も用いることで、進入禁止場所ＡＲ１を通らないようにフォロワー用経路ＲＢを設定してよい。また、フォロワービークル１０Ｂは、リーダー用経路ＲＡの位置情報に加えて、同じビークルユニット１２の他のフォロワービークル１０Ｂの位置情報を用いて、同じビークルユニット１２の他のフォロワービークル１０Ｂに衝突しないように、フォロワー用経路ＲＢを設定してよい。なお、ここでの、他のフォロワービークル１０Ｂに衝突しないようなフォロワー用経路ＲＢは、後述の第３実施形態における障害物回避とは異なる処理に基づいて設定される。ここでのフォロワー用経路ＲＢは、進入禁止場所ＡＲ１の位置情報と、同じビークルユニット１２の他のフォロワービークル１０Ｂの位置情報とに基づき、フォーメーションやオフセット量を適切に設計することで、設定される。

（移動制御部）
移動制御部９６は、動力部７６を制御して、フォロワービークル１０Ｂの移動を制御する。移動制御部５８は、目標位置算出部９４Ｃが算出した制御入力に基づいて動力部３６を制御することで、フォロワービークル１０Ｂを目的位置ＯＭ及び目的姿勢に向けて移動させる。

（演算の他の例）
ここで、式（９）のような非線形制約付き最適化問題の求解には、公知技術であるＳＱＰ（Sequential Quadratic Programming）法等の収束計算を利用できる。しかし収束計算は演算時間が長く，予測ホライズンを大きく設定した数十メートル規模の経路計画では，リアルタイムでの経路計画ができない可能性がある。その場合は、リアルタイム計算に特化した公知技術であるＣ／ＧＭＲＥＳ法を用いてリーダー用経路ＲＡの生成を行ってもよい。Ｃ／ＧＭＲＥＳ法では、最適解が時間的に連続に変化すると仮定し、その変化を追跡することで解を更新する。制御入力である解をＵ（ｔ）とし、状態量をｐ（ｔ）とすると、最適化問題の停留条件を満たすオイラー・ラグランジュ方程式は、次の式（２６）のように表される。

Ｆ（Ｕ，ｐ，ｔ）は、評価関数、状態方程式、制約条件からなる式である。オイラー・ラグランジュ方程式の誤差が時間とともに滑らかに減衰すると仮定して、次の式（２７）のように変形されて（ζ＞０）、Ｕ´（ｔ）についてさらに変形して、次の式（２８）のようになる。

そして、式（２８）をＵ´（ｔ）について解いて時間微分することで、未知変数Ｕ（ｔ）が得られる。方程式を解く際にはＧＭＲＥＳ（Generalized Minimal Residual Method）法と呼ばれる連立方程式解法を用いている。このアルゴリズムにおいては、各演算周期で上記の方程式を一回だけ解いて解を更新し、最適化のための収束計算を行わないことで、計算の高速化を実現している。ただし、初回の解を生成するときのみ、ニュートン法を用いてｔ＝０における最適解Ｕ(0)を求める。Ｃ／ＧＭＲＥＳ法は、高速に解を更新できる利点がある一方で、ＳＱＰ法のように不等式制約を陽に考慮できないため、制御入力（速度・前輪舵角）の上下限制約は、スラック変数を用いて等式制約へ変換する必要がある。

（制御フロー）
次に、同じビークルユニット１２のリーダービークル１０Ａとフォロワービークル１０Ｂが移動する際の制御フローを説明する。図１２は、同じビークルユニットのリーダービークルとフォロワービークルが移動する際の制御フローを説明するフローチャートである。図１２に示すように、リーダービークル１０Ａは、無向グラフ情報に基づいて、経由ノードｎｄａを設定する（ステップＳ１０）。より詳しくは、リーダービークル１０Ａは、無向グラフ情報取得部５４Ａにより、ノードｎｄと、進入禁止場所ＡＲ１を通らないエッジｅｄとを含む無向グラフ情報を読み出す。そして、リーダービークル１０Ａは、経由ノード設定部５４Ｂにより、目的位置Ｏまでの経路が最短となるエッジｅｄを経由エッジｅｄａとして抽出し、経由エッジｅｄａに結ばれているノードｎｄを経由ノードｎｄａとして抽出する。次に、リーダービークル１０Ａは、次の経由ノードｎｄａまでのリーダー用経路ＲＡを設定する（ステップＳ１２）。すなわち、リーダービークル１０Ａは、リーダー用経路算出部５４Ｃにより、次のウェイポイントとなる経由ノードｎｄａを通るように、モデル予測制御によってリーダー用経路ＲＡを設定する。リーダービークル１０Ａは、情報送信部５６により、同じビークルユニット１２に所属するフォロワービークル１０Ｂに、リーダー用経路ＲＡの情報、自身の位置情報、及びフォーメーション情報を送信する（ステップＳ１４）。リーダービークル１０Ａは、移動制御部５８により、リーダー用経路ＲＡに沿って移動する（ステップＳ１６）。

その後、目的位置Ｏに到達していない場合（ステップＳ１８；Ｎｏ）、リーダービークル１０Ａは、自身が次の経由ノードｎｄａに対して所定距離範囲内に位置しているかを判断し（ステップＳ２０）、次の経由ノードｎｄａに対して所定距離範囲内に位置している場合（ステップＳ２０；Ｙｅｓ）、次の経由ノードｎｄａを、さらに先の経由ノードｎｄａに更新して（ステップＳ２２）、ステップＳ１２に戻り、更新した経由ノードｎｄａまでのリーダー用経路ＲＡを生成して、移動を続ける。すなわち、リーダービークル１０Ａは、目指している経由ノードｎｄａに対して所定距離範囲内まで到達する度に、目指している経由ノードｎｄａを更新してリーダー用経路ＲＡを生成する。なお、次の経由ノードｎｄａに対して所定距離範囲内に位置していない場合（ステップＳ２０；Ｎｏ）、ステップＳ１６に戻って移動を続ける。

一方、フォロワービークル１０Ｂは、自身の位置情報と、他のフォロワービークル１０Ｂの位置情報とを取得し（ステップＳ３０）、同じビークルユニット１２のリーダービークル１０Ａから、リーダー用経路ＲＡの情報、リーダービークル１０Ａの位置情報、及びフォーメーション情報を取得する（ステップＳ３２）。ステップＳ３０、Ｓ３２の実行順は任意である。フォロワービークル１０Ｂは、これらの情報を取得したら、リーダー用経路ＲＡに基づいて、フォロワー用経路ＲＢを設定し（ステップＳ３４）、フォロワー用経路ＲＢとフォーメーション情報とに基づき、目標位置ＯＭ及び目的姿勢を設定する（ステップＳ３６）。具体的には、フォロワービークル１０Ｂは、フォロワー用経路設定部９４Ａにより、リーダー用経路ＲＡから所定距離離れるように、フォロワー用経路ＲＢを設定し、フォーメーション算出部９４Ｂにより、リーダービークル１０Ａの位置及びフォーメーション情報に基づき、フォーメーションを維持するためのフォーメーション位置ＦＭを算出する。そして、フォロワービークル１０Ｂは、フォーメーション位置ＦＭに最も近いリーダー用経路ＲＡの位置を、目標位置ＯＭとして設定する。

フォロワービークル１０Ｂは、移動制御部９６により、リーダービークル１０Ａに追従して、目標位置ＯＭに向けて移動する（ステップＳ４０）。フォロワービークル１０Ｂは、目的位置Ｏに到達しない場合（ステップＳ４２；Ｎｏ）、ステップＳ３０に戻り、目標位置ＯＭを更新しつつ、リーダービークル１０Ａに追従した移動を続ける。一方、リーダービークル１０Ａやフォロワービークル１０Ｂは、目的位置Ｏに到達したら（ステップＳ１８；Ｙｅｓ、ステップＳ４２；Ｙｅｓ）、本処理を終了する。

（効果）
以上説明したように、本実施形態においては、１つのビークルユニット１２において、リーダービークル１０Ａが、進入禁止場所ＡＲ１を通らないようにリーダー用経路ＲＡを設定してリーダー用経路ＲＡを移動しつつ、フォロワービークル１０Ｂが、リーダー用経路ＲＡに基づいてフォロワー用経路ＲＢを設定して、フォロワー用経路ＲＢに基づいて移動する。このように、本実施形態においては、リーダービークル１０Ａが主体となって経路を生成し、フォロワービークル１０Ｂは、リーダービークル１０Ａの経路に基づいて自身の経路を生成して、リーダービークル１０Ａに追従する。そのため、ビークルユニット１２によると、複数のビークル１０を適切に協調して移動させることが可能となる。また、本実施形態においては、リーダービークル１０Ａが、進入禁止場所ＡＲ１を通らないようにリーダー用経路ＲＡを設定するため、それぞれのビークル１０が自身の経路を生成する分散型制御の場合においても、障害物への衝突を抑制することが可能となる。

また、リーダービークル１０Ａは、無向グラフ情報を用いておおまかな経路を生成した後、すなわち経由ノードｎｄａを設定した後、モデル予測制御によって詳細なリーダー用経路ＲＡを生成する。リーダービークル１０Ａは、このように無向グラフとモデル予測制御を組み合わせることで、経路を適切に生成することが可能となる。

（第２実施形態）
次に、第２実施形態について説明する。第２実施形態に係るビークルユニット１２は、他のビークルユニット１２のリーダービークル１０Ａが経由ノードｎｄａとして予約したノードｎｄを用いずに、リーダー用経路ＲＡを生成する点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。

第２実施形態においては、リーダービークル１０Ａは、抽出済みノードの情報を取得する。抽出済みノードとは、他のビークルユニット１２のリーダービークル１０Ａによって経由ノードｎｄａとして抽出されているノードｎｄである。すなわち、リーダービークル１０Ａは、無向グラフ情報に含まれているノードｎｄのうちの、どのノードｎｄが他のビークルユニット１２のリーダービークル１０Ａによって経由ノードｎｄａとして抽出済みであるかの情報を、取得する。リーダービークル１０Ａは、他のビークルユニット１２のリーダービークル１０Ａと通信して、他のビークルユニット１２のリーダービークル１０Ａから、直接抽出済みノードの情報を取得してもよいし、管理システム１４から抽出済みノードの情報を取得してもよい。

そして、リーダービークル１０Ａは、抽出済みノード以外のノードｎｄから、経由ノードｎｄａを抽出する。具体的には、リーダービークル１０Ａは、経由ノード設定部５４Ｂによって経由エッジｅｄａを抽出する際に、無向グラフ情報に含まれているノードｎｄから、抽出済みノードを除外し、無向グラフ情報に含まれているエッジｅｄから、抽出済みノードに接続されているエッジｅｄを除外する。従って、経由エッジｅｄａを抽出する際の無向グラフ情報には、抽出済みノード以外のノードｎｄと、抽出済みノードに接続されたエッジｅｄ以外のエッジｅｄとが、残ることになる。リーダービークル１０Ａは、抽出済みノードに接続されたエッジｅｄ以外のエッジｅｄのうちから、第１実施形態と同様の方法で、経由エッジｅｄａを抽出して、経由ノードｎｄａを抽出する。

このように、リーダービークル１０Ａは、他のビークルユニット１２によって予約済みのノードｎｄを経由ノードｎｄａとして使用しないため、他のビークルユニット１２との衝突やデッドロックを適切に抑制できる。

また、リーダービークル１０Ａは、経由ノードｎｄａの抽出が完了したら、自身が抽出した経由ノードｎｄａの情報を、抽出済みノードの情報として、他のビークルユニット１２のリーダービークル１０Ａに送信する。リーダービークル１０Ａは、抽出済みノードの情報を、他のビークルユニット１２のリーダービークル１０Ａに直接送信してもよいし、管理システム１４に送信してもよい。リーダービークル１０Ａは、抽出済みノードの情報の送信が完了してから、移動を開始する。リーダービークル１０Ａは、目的位置Ｏまで到着したら、自身が抽出した経由エッジｅｄａを抽出済みエッジから除外する旨の情報を、他のビークルユニット１２のリーダービークル１０Ａに送信する。これにより、他のリーダービークル１０Ａに使用されていないエッジｅｄが、経由エッジｅｄａとして選択できなくなることが抑制できる。

図１３は、第２実施形態における経由エッジの抽出の例を説明する模式図である。図１３の例では、１つのビークルユニット１２のリーダービークル１０Ａ１は、ノードｎｄ１に位置しており、目的位置Ｏがノードｎｄ４となっている。そして、他の１つのビークルユニット１２のリーダービークル１０Ａ２は、ノードｎｄ２に位置しており、目的位置Ｏがノードｎｄ６となっている。そして、リーダービークル１０Ａ１が先に経由エッジｅｄａを抽出した場合を例にして説明する。

図１３のステップＳ９０に示すように、最初に、リーダービークル１０Ａ１が、ノードｎｄ１、ｎｄ３、ｎｄ４を、経由ノードｎｄａとして抽出して、ノードｎｄ１、ｎｄ３、ｎｄ４が抽出済みノードである旨の情報を、リーダービークル１０Ａ２に送信する。

図１３のステップＳ９２に示すように、その後、リーダービークル１０Ａ２が、経由ノードｎｄａを抽出する。この際、ノードｎｄ１、ｎｄ３、ｎｄ４が抽出済みノードであるため、リーダービークル１０Ａ２は、ノードｎｄ１、ｎｄ３、ｎｄ４に結ばれたエッジｅｄを除いたエッジｅｄから、経由エッジｅｄａを抽出して、経由ノードｎｄａを抽出する。図１３の例では、リーダービークル１０Ａ２は、ノードｎｄ２、ｎｄ５、ｎｄ８、ｎｄ７、ｎｄ６を、経由ノードｎｄａとして抽出している。

このように、図１３の例では、他のビークルユニット１２が通過予定のノードｎｄを他のビークルユニット１２に通過させないため、ビークルユニット１２同士の接近を発生しにくくすることができる。

（第３実施形態）
次に、第３実施形態について説明する。第３実施形態に係るリーダービークル１０Ａは、リーダー用経路ＲＡを設定する際に、リーダー用経路ＲＡが進入禁止場所ＡＲ１を通らないように演算を行う点で、第１実施形態とは異なる。第３実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。なお、第３実施形態は、第２実施形態にも適用可能である。

第１実施形態においては、進入禁止場所ＡＲ１に基づき作成された無向グラフ情報を用いてリーダー用経路ＲＡを生成するため、進入禁止場所ＡＲ１内の障害物と交差するリーダー用経路ＲＡが生成されることはない。ただし、例えば狭隘部などを走行する場合には、障害物と接触するリーダー用経路ＲＡが生成される可能性がある。それに対し、第３実施形態に係るリーダービークル１０Ａは、以降のように評価関数にバリア関数を用いてリーダー用経路ＲＡを生成することで、障害物と接触するリーダー用経路ＲＡが生成される可能性をより低減することが可能となる。以下、具体的に説明する。

第３実施形態においては、式（９）で示したモデルＭ１Ａに対して、リーダー用経路ＲＡを生成する際の評価関数ｆ_ｉ１を、次の式（２９）のように定義する。

ここで、式（２９）の評価関数の第２項が、壁などの静止障害物を回避するためのバリア関数を表し、Ｂは重み係数を表す。また、関数Ｃ_ｂは、進入禁止場所ＡＲ１の端点同士を結ぶベクトルａ_ｉと、進入禁止場所ＡＲ１の端点からビークル１０の位置を結ぶベクトルｂｉを用いて、次の式（３０）のように設定される。

ここで、ξは定数パラメータ、Ｎ_Ｒは進入禁止場所ＡＲ１の端点の個数であり、矩形である場合は４となる。図１４は、関数Ｃ_ｂを説明するための模式図である。図１４に示すように進入禁止場所ＡＲ１が矩形である場合には、ｂ_ｉ×ａ_ｉ＝（ｘ－ｘ_ｉ）×（ｙ_ｉ＋１－ｙ_ｉ）・（ｘ_ｉ＋１－ｘ_ｉ）となる。すなわち、関数Ｃ_ｂは、ベクトルの外積の符号を用いた内外判定を示す関数であるといえる。

図１５は、第３実施形態に係るリーダービークルによって生成されたリーダー用経路の一例を示す模式図である。図１５の例に示すように、ノードｎｄ０、ｎｄ１、ｎｄ２、ｎｄ３、ｎｄ４、ｎｄ５、ｎｄ６を通る経路において、ノードｎｄ２、ｎｄ３、ｎｄ４を通る経路は、進入禁止場所ＡＲ１に挟まれた狭隘部を通る経路となる。そのため、無向グラフ情報を用いた場合でも、リーダー用経路が、図１５の破線に示すように、進入禁止場所ＡＲ１に少しだけ進入してしまうおそれがある。それに対し、第３実施形態においては、式（２９）のような評価関数を用いて、関数Ｃ_ｂなどで経路が進入禁止場所ＡＲ１内に入らないことを判定基準として、リーダー用経路ＲＡを生成する。そのため、図１５の実線に示すように、リーダー用経路ＲＡが進入禁止場所ＡＲ１内を通ることが抑制できる。

なお、以上の説明では、リーダー用経路ＲＡを作成する際に式（２９）の評価関数を用いるとしたが、フォロワービークル１０Ｂも、同様の評価関数を用いて、フォロワー用経路ＲＢを生成してもよい。これにより、フォロワー用経路ＲＢについても、進入禁止場所ＡＲ１内を通ることを適切に抑制できる。

（第４実施形態）
次に、第４実施形態について説明する。第４実施形態に係るビークルユニット１２は、他のビークルユニット１２のリーダービークル１０Ａの直近のウェイポイントとなる経由ノードｎｄａとして予約したノードｎｄを除外して、リーダー用経路ＲＡを生成する点で、第１実施形態とは異なる。第４実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。なお、第４実施形態は、第３実施形態にも適用可能である。

図１６及び図１７は、第４実施形態における経由ノードの抽出方法の一例を説明するための模式図である。第４実施形態においては、リーダービークル１０Ａは、直近ノードの情報を取得する。直近ノードとは、他のビークルユニット１２のリーダービークル１０Ａによって、次（直後）のウェイポイントとなる経由ノードｎｄａとして選択されているノードｎｄであり、言い換えれば、他のビークルユニット１２によって抽出された経由ノードｎｄａのうち、他のビークルユニット１２の直近の移動先のノードｎｄであるといえる。すなわち、リーダービークル１０Ａは、無向グラフ情報に含まれているノードｎｄのうちの、どのノードｎｄが他のビークルユニット１２によって直近の移動先のノードｎｄとして選択されているかの情報を、取得する。

また、リーダービークル１０Ａは、直近ノードの情報と共に、直近エッジの情報も取得する。直近エッジとは、他のビークルユニット１２が抽出した直近ノード（直近の移動先のノードｎｄ）と、直近ノードの１つ前のノードｎｄ（直近の移動先のノードｎｄに移動する際に移動元となるノードｎｄ）とを結ぶエッジｅｄである。直近エッジとは、他のビークルユニット１２が使用中のエッジｅｄであるともいえる。すなわち、リーダービークル１０Ａは、無向グラフ情報に含まれているエッジｅｄのうちの、どのエッジｅｄが他のビークルユニット１２によって使用中であるかの情報を、取得する。リーダービークル１０Ａは、他のビークルユニット１２のリーダービークル１０Ａと通信して、他のビークルユニット１２のリーダービークル１０Ａから直接、直近ノード及び直近エッジの情報を取得してもよいし、管理システム１４から直近ノード及び直近エッジの情報を取得してもよい。

リーダービークル１０Ａは、直近ノード以外のノードｎｄから、経由ノードｎｄａを抽出する。また、リーダービークル１０Ａは、直近エッジ以外のエッジｅｄから、経由エッジｅｄａを抽出する。具体的には、リーダービークル１０Ａは、経由ノード設定部５４Ｂによって経由エッジｅｄａを抽出する際に、無向グラフ情報に含まれているノードｎｄから、直近ノードを除外し、無向グラフ情報に含まれているエッジｅｄから、直近エッジを除外する。従って、経由エッジｅｄａを抽出する際の無向グラフ情報には、直近ノード以外のノードｎｄと、直近エッジ以外のエッジｅｄとが、残ることになる。

図１６を例にすると、１つのビークルユニット１２のリーダービークル１０Ａ１は、ノードｎｄ１に位置しており、目的位置Ｏがノードｎｄ４となっている。そして、他の１つのビークルユニット１２のリーダービークル１０Ａ２は、ノードｎｄ２に位置しており、目的位置Ｏがノードｎｄ６となっている。そして、リーダービークル１０Ａ１が先に移動開始する場合を例にして説明する。ステップＳ１００、Ｓ１０２に示すように、リーダービークル１０Ａ１は、ノードｎｄ１、ｎｄ３、ｎｄ４を、経由ノードｎｄａとして抽出する。そして、リーダービークル１０Ａ１は、ノードｎｄ３が直近ノードであり、ノードｎｄ１とノードｎｄ３を結ぶエッジが直近エッジである旨の情報を、リーダービークル１０Ａ２に送信してから、直近ノードであるノードｎｄ３への移動を開始する。

また、リーダービークル１０Ａ２は、直近ノード（図１６ではノードｎｄ３）に結ばれている直近エッジ以外のエッジｅｄである周辺エッジｅｄｓに対して、待ち時間に応じた重み付けを行う。待ち時間とは、リーダービークル１０Ａ２が、周辺エッジｅｄｓの使用を開始するまでに、すなわち周辺エッジｅｄｓに接続されているノードｎｄへの移動を開始するまでに待機する時間を指す。図１６の例では、Ｓ１０２に示すように、周辺エッジｅｄｓは、ノードｎｄ３－ｎｄ２間のエッジｅｄ２、ノードｎｄ３－ｎｄ４間のエッジｅｄ４、ノードｎｄ３－ｎｄ６間のエッジｅｄ６であり、直近ノードと対になるノードｎｄは、ノードｎｄ２、ｎｄ４、ｎｄ６となる。直近ノードと対になるノードｎｄ２、ｎｄ４、ｎｄ６には、リーダービークル１０Ａ１の次の移動先となるノードが含まれているため、リーダービークル１０Ａ２は、直近ノードと対になるノードに移動するまでに待機が必要となる場合もある。そのため、周辺エッジｅｄｓに対して重み付けを行うことで、待ち時間も考慮した適切な経路設計が可能となる。

待ち時間に応じた重み付けは、例えば、リーダービークル１０Ａ１の次の次の移動先（直近ノードに対して次の移動先）となるノードｎｄの情報に基づき設定される。すなわち、リーダービークル１０Ａ１の次の次の移動先（２つ先の移動先）となるノードｎｄと結ばれている周辺エッジｅｄｓについては、待ち時間が長くなるように設定されて、リーダービークル１０Ａ１の次の次の移動先となるノードｎｄと結ばれていない周辺エッジｅｄｓについては、待ち時間が短くなるように設定される。リーダービークル１０Ａ２は、リーダービークル１０Ａ１の次の次の移動先となるノードｎｄの情報を取得し、その情報に基づき、周辺エッジｅｄｓについて、待ち時間に応じた重み付けを行う。図１６の例では、リーダービークル１０Ａ１の次の次の移動先は、ノードｎｄ４である。そのため、リーダービークル１０Ａ２は、リーダービークル１０Ａ１の次の移動先であるノードｎｄ３（直近ノード）に対する周辺エッジｅｄｓであるノードｎｄ２、ｎｄ４、ｎｄ６のうちで、リーダービークル１０Ａ１の次の次の移動先であるノードｎｄ４に接続されるエッジｅｄ４について、待ち時間が長くなるように重み付けを行う。

リーダービークル１０Ａ２は、エッジｅｄの長さの重み付けと、待ち時間に応じた重み付けとに基づき、直近エッジ以外のエッジｅｄから、経由エッジｅｄａを抽出する。リーダービークル１０Ａ２は、エッジｅｄの長さの重み付けと、待ち時間に応じた重み付けとから、目的位置Ｏまでの到達時間が早くなるように、経由エッジｅｄａを抽出する。なお、待ち時間の重み付けをゼロとした場合には、目的位置Ｏまでの距離が最短となることで、目的位置Ｏまでの到達時間が早くなるので、的位置Ｏまでの距離が最短となるような経由エッジｅｄａが抽出される。図１６の例では、ステップＳ１０２に示すように、リーダービークル１０Ａ２は、エッジｅｄの長さの重み付けと、待ち時間に応じた重み付けとに基づき、直近エッジであるノードｎｄ１とノードｎｄ３を結ぶエッジ以外のエッジｅｄのうちから、ノードｎｄ２－ノードｎｄ４間のエッジｅｄと、ノードｎｄ４－ノードｎｄ７間のエッジｅｄと、ノードｎｄ７－ノードｎｄ６間のエッジｅｄとを、経由エッジｅｄａとする。そして、リーダービークル１０Ａ２は、ノードｎｄ２、ｎｄ４、ｎｄ７、ｎｄ６を、経由ノードｎｄａとする。

その後、ステップＳ１０２に示すように、リーダービークル１０Ａ２は、ノードｎｄ４が直近ノードである旨の情報をリーダービークル１０Ａ１に送信してから、直近ノードであるノードｎｄ４への移動を開始する。以降の処理は、これまでと同様に行われるため、説明を省略する。

このように、第４実施形態においては、他のリーダービークル１０Ａ１の直近の移動先となるノードｎｄのみを、リーダービークル１０Ａ２の移動を禁止するノードｎｄとするため、ビークル１０同士の衝突を抑制しつつ、移動を禁止するノードｎｄが増加することを抑えて、ビークル群全体での経路を最適化することが可能となる。すなわち、例えば図１７に示すように、リーダービークル１０Ａ１の経由ノードｎｄａをノードｎｄ１、ｎｄ３、ｎｄ４とし、リーダービークル１０Ａ２の経由ノードｎｄａをノードｎｄ２、ｎｄ４、ｎｄ７、ｎｄ６とすることが可能となり、時間差をおいてノードｎｄ４を共有して、経路を最適化できる。また、周辺エッジｅｄｓについて待ち時間で重み付けすることで、他のチームが利用するノードｎｄに対して迂回か待機かを選択することができ、ビークル群全体での経路をより適切に最適化することができる。

（第５実施形態）
次に、第５実施形態について説明する。第５実施形態においては、リーダービークル１０Ａは、次のウェイポイントである経由ノードｎｄａに到着する前に、ウェイポイントを更新して次のリーダー用経路ＲＡを設定する点で、第１実施形態とは異なる。第５実施形態において第１実施形態と構成が共通する箇所は、説明を省略する。なお、第５実施形態は、第２実施形態から第４実施形態にも適用可能である。

図１８及び図１９は、リーダー用経路の更新を説明するための模式図である。第１実施形態においては、リーダー用経路算出部５４Ｃは、図１８のステップＳ１１０、Ｓ１１２に示すように、リーダービークル１０Ａがウェイポイントであるノードｎｄ１に到着したら、ウェイポイントを次のノードｎｄ２に切り替えて、到着したノードｎｄ１から次のノードｎｄ２までの経路を、リーダー用経路ＲＡとして更新設定する。その後も同様に、ステップＳ１１４に示すように、リーダービークル１０Ａがノードｎｄ２に到着したら、ウェイポイントを次のノードｎｄ３に切り替えて、到着したノードｎｄ２から次のノードｎｄ３までの経路を、リーダー用経路ＲＡとして更新設定する。この場合、図１８に示すように、大回りとなってしまい経路が長くなってしまうおそれがある。

それに対し、第５実施形態においては、リーダー用経路算出部５４Ｃは、図１９のステップＳ１２０、Ｓ１２２に示すように、リーダービークル１０Ａがウェイポイントであるノードｎｄ１に対して所定距離ｄの範囲内に到着したら、ウェイポイントを次のノードｎｄ２に切り替えて、現在地から次のノードｎｄ２までの経路を、リーダー用経路ＲＡとして更新設定する。その後も同様に、ステップＳ１２４に示すように、リーダービークル１０Ａがノードｎｄ２に対して所定距離ｄの範囲内に到着したら、ウェイポイントを次のノードｎｄ３に切り替えて、現在地からから次のノードｎｄ３までの経路を、リーダー用経路ＲＡとして更新設定する。この場合、図１９に示すように、大回りとなることを抑制して、スループットを向上できる。なお、所定距離ｄは、任意に設定してよく、例えば一定の値に設定されていてもよいし、ノードｎｄ間の距離に対する割合に基づき、ノードｎｄごとに動的に設定してもよいし、環境レイアウトとしてノードｎｄごとに定義してもよい。

（効果）
以上説明したように、本実施形態に係るビークルユニット１２は、進入禁止場所ＡＲ１の位置を示す情報と目的位置Ｏの情報とに基づいて、自身が移動する経路であるリーダー用経路ＲＡを設定するリーダービークル（第１ビークル）１０Ａと、リーダー用経路ＲＡに基づいて、自身が移動する経路であるフォロワー用経路ＲＢを設定するフォロワービークル１０Ｂ（第２ビークル）１０Ｂと、を含む。本実施形態に係るビークルユニット１２は、リーダービークル１０Ａが主体となって経路を生成し、フォロワービークル１０Ｂは、リーダービークル１０Ａの経路に基づいて自身の経路を生成して、リーダービークル１０Ａに追従する。そのため、ビークルユニット１２によると、複数のビークル１０を適切に協調して移動させることが可能となる。また、本実施形態においては、リーダービークル１０Ａが、進入禁止場所ＡＲ１を通らないようにリーダー用経路ＲＡを設定するため、それぞれのビークル１０が自身の経路を生成する分散型制御の場合においても、障害物への衝突を抑制することが可能となる。このように、本実施形態に係るビークルユニット１２は、ビークルの経路を適切に設定することができる。

リーダービークル１０Ａは、フォロワー用経路ＲＢを参照することなくリーダー用経路ＲＡを設定する。本実施形態に係るビークルユニット１２は、リーダービークル１０Ａがフォロワービークル１０Ｂを考慮することなく経路を生成するため、ビークル毎に適切に役割を配分して、複数のビークル１０を適切に協調して移動させることが可能となる。

リーダービークル１０Ａは、移動を予定する領域ＡＲ内において座標毎に設定されたノードｎｄと、ノードｎｄ同士を結ぶエッジｅｄとを含む無向グラフ情報を取得する無向グラフ情報取得部５４Ａと、進入禁止場所ＡＲ１を通らず、かつ目的位置Ｏに到達可能なエッジｅｄである経由エッジｅｄａを抽出し、経由エッジｅｄａに結ばれるノードｎｄを経由ノードｎｄａとして設定する経由ノード設定部５４Ｂと、経由ノードｎｄａに基づきリーダー用経路ＲＡを設定するリーダー用経路算出部５４Ｃと、を含む。このリーダービークル１０Ａは、無向グラフ情報を用いて生成したおおまかな経路に基づいて、リーダー用経路ＲＡを生成する。リーダービークル１０Ａは、このように経路生成することで、経路を適切に生成することが可能となる。

経由ノード設定部５４Ｂは、エッジｅｄを通って目的位置Ｏまで到達するまでの距離が最短となるように、経由エッジｅｄａを抽出する。リーダービークル１０Ａは、このように経由エッジｅｄａを抽出することで、経路を適切に生成することが可能となる。

経由ノード設定部５４Ｂは、他のビークルユニット１２によって抽出された経由ノードｎｄａの情報を取得し、他のビークルユニット１２によって抽出された経由ノードｎｄａ以外のノードｎｄから、経由ノードｎｄａを抽出する。リーダービークル１０Ａは、このように他のビークルユニット１２によって選択済みのノードｎｄを使用しないことで、ビークル１０同士の衝突やデッドロックを適切に抑制できる。

経由ノード設定部５４Ｂは、他のビークルユニット１２によって抽出された経由ノードｎｄａのうち、他のビークルユニット１２の直近の移動先のノードｎｄである直近ノードの情報を取得し、他のビークルユニット１２の直近ノードに結ばれたエッジｅｄ（周辺エッジｅｄｓ）について、そのエッジｅｄの通行が開始可能になるまでの待ち時間の情報を取得し、目的位置Ｏまでの距離と、待ち時間とに基づき、経由エッジｅｄａを抽出する。リーダービークル１０Ａは、このように経由エッジｅｄａを抽出することで、待ち時間も考慮した適切な経路設計が可能となる。

リーダー用経路算出部５４Ｃは、経由ノードｎｄａに基づき、モデル予測制御でリーダー用経路ＲＡを設定する。このリーダービークル１０Ａは、無向グラフ情報を用いたおおまかな経路生成と、モデル予測制御による経路生成とを組み合わせる。リーダービークル１０Ａは、このように経路生成することで、経路を適切に生成することが可能となる。

リーダー用経路算出部５４Ｃは、経由ノードｎｄａを通るように、リーダー用経路ＲＡを設定する。リーダービークル１０Ａは、このように経路生成することで、経路を適切に生成することが可能となる。

リーダー用経路算出部５４Ｃは、経由ノードｎｄａ毎に、リーダー用経路ＲＡを設定する。リーダービークル１０Ａは、このように経由ノードｎｄａ毎に経路生成することで、経路を適切に生成することが可能となる。

リーダー用経路算出部５４Ｃは、リーダービークル１０Ａが、経由ノードｎｄａとして設定された第１経由ノードに対して所定距離ｄの範囲内に到達したら、次の経由ノードｎｄａである第２経由ノードまでのリーダー用経路ＲＡを設定する。リーダービークル１０Ａは、このように経由ノードｎｄａ毎に経路生成することで、経路を適切に生成することが可能となる。

リーダー用経路算出部５４Ｃは、リーダー用経路ＲＡが進入禁止場所ＡＲ１を通らないように、リーダー用経路ＲＡを設定する。リーダーダービークル１０Ａは、このように経由ノードｎｄａ毎に経路生成することで、例えば狭隘部を通る場合にも進入禁止場所ＡＲ１に進入してしまうことが抑制されて、経路を適切に生成することが可能となる。

フォロワービークル１０Ｂは、リーダー用経路ＲＡから所定距離離れるように、フォロワー用経路ＲＢを設定する。フォロワービークル１０Ｂは、このようにフォロワー用経路ＲＢを設定することで、リーダービークル１０Ａに衝突しないように、リーダービークル１０Ａに追従することができる。

フォロワービークル１０Ｂは、リーダービークル１０Ａに対するフォロワービークル１０Ｂの望ましい相対位置を示すフォーメーション情報と、リーダービークル１０Ａの位置情報とを取得する情報取得部９２と、フォーメーション情報及びリーダービークル１０Ａの位置に基づき、フォロワービークル１０Ｂの望ましい位置であるフォーメーション位置ＦＭを算出するフォーメーション位置算出部９４Ｂと、フォロワー用経路ＲＢ及びフォーメーション位置ＦＭに基づき、目標位置ＯＭを設定する目標位置算出部９４Ｃと、を含む。フォロワービークル１０Ｂは、このように目的位置ＯＭを設定するため、リーダービークル１０Ａに対するフォーメーションを出来るだけ維持しつつ、適切な経路で移動することができる。

目標位置算出部９４Ｃは、フォーメーション位置ＦＭに最も近いフォロワー用経路ＲＢ上の位置を、目標位置ＯＭとして設定する。フォロワービークル１０Ｂは、このように目的位置ＯＭを設定するため、リーダービークル１０Ａに対するフォーメーションを出来るだけ維持しつつ、大回りなど経路が長くなってしまうことを抑制できる。

第１ビークル１０Ａは、他のビークルユニット１２の第１ビークル１０Ａと情報の送受信を行う。第１ビークル１０Ａは、他のビークルユニット１２の第１ビークル１０Ａと情報の送受信を行うことで、他のビークルユニット１２との衝突やデッドロックなどを抑制できる。

本実施形態に係るビークル１０（リーダービークル１０Ａ）は、移動を予定する領域ＡＲ内において座標毎に設定されたノードｎｄと、ノードｎｄ同士を結ぶエッジｅｄとを含む無向グラフ情報を取得する無向グラフ情報取得部５４Ａと、進入禁止場所ＡＲ１を通らず、かつ目的位置Ｏに到達可能なエッジｅｄである経由エッジｅｄａを抽出し、経由エッジｅｄａに結ばれるノードｎｄを経由ノードｎｄａとして設定する経由ノード設定部５４Ｂと、経由ノードｎｄａに基づき、モデル予測制御で移動経路であるリーダー用経路ＲＡを設定するリーダー用経路算出部５４Ｃと、を含む。このビークル１０は、無向グラフ情報を用いたおおまかな経路生成と、モデル予測制御による経路生成とを組み合わせる。リーダービークル１０Ａは、このように経路生成することで、経路を適切に生成することが可能となる。

本実施形態に係るビークル１０の制御方法は、進入禁止場所ＡＲ１の位置を示す情報と目的位置Ｏの情報とに基づいて、リーダービークル１０Ａが移動する経路であるリーダー用経路ＲＡを設定するステップと、リーダー用経路ＲＡに基づいて、フォロワービークル１０Ｂが移動する経路であるフォロワー用経路ＲＢを設定するステップと、を含む。本実施形態に係る制御方法によると、ビークルの経路を適切に設定することができる。

本実施形態に係るビークル１０の制御方法は、移動を予定する領域ＡＲ内において座標毎に設定されたノードｎｄと、ノードｎｄ同士を結ぶエッジｅｄとを含む無向グラフ情報を取得するステップと、進入禁止場所ＡＲ１を通らず、かつ目的位置Ｏに到達可能なエッジｅｄである経由エッジｅｄａを抽出し、経由エッジｅｄａに結ばれるノードｎｄを経由ノードｎｄａとして設定するステップと、経由ノードｎｄａに基づき、モデル予測制御で移動経路であるリーダー用経路ＲＡを設定するステップと、を含む。本実施形態に係る制御方法によると、ビークルの経路を適切に設定することができる。

本実施形態に係るプログラムは、進入禁止場所ＡＲ１の位置を示す情報と目的位置Ｏの情報とに基づいて、リーダービークル１０Ａが移動する経路であるリーダー用経路ＲＡを設定するステップと、リーダー用経路ＲＡに基づいて、フォロワービークル１０Ｂが移動する経路であるフォロワー用経路ＲＢを設定するステップと、含むビークルの制御方法を、コンピュータに実行させる。本実施形態に係るプログラムによると、ビークルの経路を適切に設定することができる。

本実施形態に係るプログラムは、移動を予定する領域ＡＲ内において座標毎に設定されたノードｎｄと、ノードｎｄ同士を結ぶエッジｅｄとを含む無向グラフ情報を取得するステップと、進入禁止場所ＡＲ１を通らず、かつ目的位置Ｏに到達可能なエッジｅｄである経由エッジｅｄａを抽出し、経由エッジｅｄａに結ばれるノードｎｄを経由ノードｎｄａとして設定するステップと、経由ノードｎｄａに基づき、モデル予測制御で移動経路であるリーダー用経路ＲＡを設定するステップと、を含むビークルの制御方法を、コンピュータに実行させる。本実施形態に係るプログラムによると、ビークルの経路を適切に設定することができる。

以上、本発明の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。

１０ ビークル
１０Ａ リーダービークル（第１ビークル）
１０Ｂ フォロワービークル（第２ビークル）
１２ ビークルユニット
５０ 目的位置情報取得部
５２、９０ 自己位置情報取得部
５４ リーダー用経路設定部
５６ 情報送信部
５８、９６ 移動制御部
９２ 情報取得部
９４ 目標位置設定部
ＡＲ１ 進入禁止場所
ＲＡ リーダー用経路
ＲＢ フォロワー用経路

Claims (14)

1. 進入禁止場所の位置を示す情報と目的位置の情報とに基づいて、自身が移動する経路であるリーダー用経路を設定する第１ビークルと、
   前記リーダー用経路に基づいて、自身が移動する経路であるフォロワー用経路を設定する第２ビークルと、
   を含み、
   前記第２ビークルは、
   前記リーダー用経路から所定距離離れるように、前記フォロワー用経路を設定するフォロワー用経路設定部と、
   前記第１ビークルに対する前記第２ビークルの望ましい相対位置を示すフォーメーション情報と、前記第１ビークルの位置情報とを取得する情報取得部と、
   前記フォーメーション情報及び前記第１ビークルの位置に基づき、前記第２ビークルの望ましい位置であるフォーメーション位置を算出するフォーメーション位置算出部と、
   前記フォロワー用経路及び前記フォーメーション位置に基づき、前記フォロワー用経路のうちで前記フォーメーション位置に最も近い位置を、目標位置として設定する目標位置算出部と、を含む、
   ビークルユニット。
2. 前記第１ビークルは、前記フォロワー用経路を参照することなく前記リーダー用経路を設定する、請求項１に記載のビークルユニット。
3. 前記第１ビークルは、
   移動を予定する領域内において座標毎に設定されたノードと、前記ノード同士を結ぶエッジとを含む無向グラフ情報を取得する無向グラフ情報取得部と、
   前記進入禁止場所を通らず、かつ前記目的位置に到達可能な前記エッジである経由エッジを抽出し、前記経由エッジに結ばれる前記ノードを経由ノードとして設定する経由ノード設定部と、
   前記経由ノードに基づき前記リーダー用経路を設定するリーダー用経路算出部と、を含む、請求項１又は請求項２に記載のビークルユニット。
4. 前記経由ノード設定部は、前記エッジを通って前記目的位置まで到達するまでの距離が最短となるように、前記経由エッジを抽出する、請求項３に記載のビークルユニット。
5. 前記経由ノード設定部は、他のビークルユニットによって抽出された前記経由ノードの情報を取得し、他のビークルユニットによって抽出された前記経由ノード以外のノードから、前記経由ノードを抽出する、請求項３又は請求項４に記載のビークルユニット。
6. 前記経由ノード設定部は、
   他のビークルユニットによって抽出された前記経由ノードのうち、他のビークルユニットの直近の移動先のノードである直近ノードの情報を取得し、
   他のビークルユニットの前記直近ノードに結ばれたエッジについて、そのエッジの通行が開始可能になるまでの待ち時間の情報を取得し、
   前記目的位置までの距離と、前記待ち時間とに基づき、前記経由エッジを抽出する、請求項３から請求項５のいずれか１項に記載のビークルユニット。
7. 前記リーダー用経路算出部は、前記経由ノードに基づき、モデル予測制御で前記リーダー用経路を設定する、請求項３から請求項６のいずれか１項に記載のビークルユニット。
8. 前記リーダー用経路算出部は、前記経由ノードを通るように、前記リーダー用経路を設定する、請求項７に記載のビークルユニット。
9. 前記リーダー用経路算出部は、前記経由ノード毎に、前記リーダー用経路を設定する、請求項７又は請求項８に記載のビークルユニット。
10. 前記リーダー用経路算出部は、前記第１ビークルが、前記経由ノードとして設定された第１経由ノードに対して所定距離範囲内に到達したら、次の経由ノードである第２経由ノードまでの前記リーダー用経路を設定する、請求項９に記載のビークルユニット。
11. 前記リーダー用経路算出部は、前記リーダー用経路が前記進入禁止場所を通らないように、前記リーダー用経路を設定する、請求項７から請求項１０のいずれか１項に記載のビークルユニット。
12. 前記第１ビークルは、他のビークルユニットの前記第１ビークルと情報の送受信を行う、請求項１から請求項１１のいずれか１項に記載のビークルユニット。
13. 進入禁止場所の位置を示す情報と目的位置情報とに基づいて、第１ビークルが移動する経路であるリーダー用経路を設定するステップと、
    前記リーダー用経路に基づいて、第２ビークルが移動する経路であるフォロワー用経路を設定するステップと、
    を含み、
    前記フォロワー用経路を設定するステップは、
    前記リーダー用経路から所定距離離れるように、前記フォロワー用経路を設定するステップと、
    前記第１ビークルに対する前記第２ビークルの望ましい相対位置を示すフォーメーション情報と、前記第１ビークルの位置情報とを取得するステップと、
    前記フォーメーション情報及び前記第１ビークルの位置に基づき、前記第２ビークルの望ましい位置であるフォーメーション位置を算出するステップと、
    前記フォロワー用経路及び前記フォーメーション位置に基づき、前記フォロワー用経路のうちで前記フォーメーション位置に最も近い位置を、目標位置として設定するステップと、を含む、
    ビークルの制御方法。
14. 進入禁止場所の位置を示す情報と目的位置情報とに基づいて、第１ビークルが移動する経路であるリーダー用経路を設定するステップと、
    前記リーダー用経路に基づいて、第２ビークルが移動する経路であるフォロワー用経路を設定するステップと、
    を含む、ビークルの制御方法を、コンピュータに実行させ、
    前記フォロワー用経路を設定するステップは、
    前記リーダー用経路から所定距離離れるように、前記フォロワー用経路を設定するステップと、
    前記第１ビークルに対する前記第２ビークルの望ましい相対位置を示すフォーメーション情報と、前記第１ビークルの位置情報とを取得するステップと、
    前記フォーメーション情報及び前記第１ビークルの位置に基づき、前記第２ビークルの望ましい位置であるフォーメーション位置を算出するステップと、
    前記フォロワー用経路及び前記フォーメーション位置に基づき、前記フォロワー用経路のうちで前記フォーメーション位置に最も近い位置を、目標位置として設定するステップと、を含む、
    プログラム。

Images