JP6633467B2 - 行動制御システム、行動制御方法、プログラム - Google Patents

Description

本発明は、複数の制御対象物の行動を制御する技術に関する。例えば、複数のロボットを、開始位置における隊列形成状態から協調して移動させ、障害物を回避させ、目標位置で隊列形成をさせるための各ロボットの行動計画を求めるロボット協調制御技術に関する。

近年、多数の自律移動ロボットを効率的に制御にするための研究が活発に行われている。その任務内容は、人の入れない箇所の監視、物品の搬送などさまざまであるが、多数のロボットの協調動作による隊列形成を効率的に行わせるための技術が求められており盛んに研究が行われている（例えば、非特許文献１参照）。多数のロボットによる効率的な隊列形成を実現するには、それぞれのロボットの配置、動作順序などを事前に計画することが重要である。このような計画においては、当然ながら、複数のロボットが動作する実環境における障害物の存在や経路の形状なども十分に考慮しなければならない。

このような計画計算を行うための効果的な手法の一つとして、マルコフ決定過程における動的計画法や強化学習の手法があり、さまざまな研究が行われている（例えば、非特許文献２参照）。

また、ロボットの隊列制御の中でも、ロボット同士が互いに接したままの状態で、アメーバのように全体で移動を行うという仮定の下でのロボット隊列制御においては、ロボット同士の相対的な位置関係から、各ロボットの絶対位置の決定が可能であるという利点と、付加的な位置計測用の装備を必要としないという利点があり、そのようなロボットの研究もおこなわれている。例えば、非特許文献３に示すものでは任意の矩形形状隊列から他の矩形形状隊列までの隊列制御が示されている。

また、非特許文献４に示す研究に至る一連の研究や非特許文献５では、ある隊列から他の隊列に変化する隊列制御が示されている。

非特許文献６に示す研究に示されている手法では、複数の立方体形状のロボット同士での面せん断動作(あるロボットが、他のロボットと接した状態で、接する面上をスライド移動する動作)によるロボットの隊列変形が扱われている。

M.Shimizu, A.Ishiguro, T.Kawakatsu, Y.Masubuchi, "Coherent Swarming from Local Interaction by Exploiting Molecular Dynamics and Stokesian Dynamics Methods", Proceeaings of the 2003 IEEE/RSJ International Conference on intelligent Robots and Systems, Las Veqas, pp.1614-1619, October 2003. Y.Wang, C.W.de Silva, "Multi-Robot Box-pushing: Single-Agent Q-Learning vs. Team Q-Learning", Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, pp.3694-3699, October 2006. A.Becker, G.Habibi, J.Werfel, M.Rubenstein, and J.McLurkin, "Massive Uniform Manipulation: Controlling Large Populations of Simple Robots with a Common Input Signal", Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Japan, pp.520-527, November, 2013. Stanton Wong1 and Jennifer Walter "Deterministic Distributed Algorithm for Self-Reconfiguration of Modular Robots from Arbitrary to Straight Chain Configurations", Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany, pp.537-543, May 6-10, 2013. Michael Rubenstein, Alejandro Cornejo, Radhika Nagpal, "Programmable self-assembly in a thousand-robot swarm", SCIENCE, 2014, Vol. 345, Issue 6198, pp.795-799. H. Kawano, "Complete Reconfiguration Algorithm for Sliding Cube- shaped Modular Robots with only Sliding Motion Primitive", in Proc.2015 IEEE/RSJ Int. Conf. Intelligent Robots and Systems, Hamburg, Germany, pp.3276-3283, Sep., 2015.

しかしながら、非特許文献１の手法では、流体力学的な特性をロボット動作に組み込む手法を用いて群ロボットの動作を制御しており、低い計算負荷での制御を可能にしている利点があるが、任意の形状の隊列形成をすることができるとは限らない。

また、非特許文献２の手法のように、マルコフ決定過程における動的計画法や強化学習を使用してこのような計画を行おうとすると、単体のロボットを使用する場合に比べて複数のロボットを使用する場合には、その計算に要する時間や計算機の記憶容量がロボットの数に対して指数関数的に増大してしまう。その主たる原因となるのが、探索計算のためのマルコフ状態空間内の状態数の莫大な増加である。非特許文献２では、検証された強化学習の手法では、ロボット数の増加に伴い、指数関数的に計算負荷が増加するという、マルコフ状態空間内の爆発問題への解決策は示されていない。

また、非特許文献１，２の手法ともに、付加的な位置計測用の装備を必要とする。

また、非特許文献３では、各時刻でロボットに与えられる動作命令が皆同じ方向であるという条件を考慮しており、付加的な位置計測用の装備を必要としないが、その実現には障害物の存在を必要としている。また、一度の動作における各ロボットの移動誤差から発生する隊列の崩れの問題も解決できていない。

非特許文献４の手法においては、一度、線形隊列への変換をしなければならず、可能な隊列形成動作そのものへの制約が大きい。

非特許文献５の手法においては、開始隊列と目標隊列が共有する点がなければならないことや、障害物を考慮していないなどの問題点がある。

非特許文献６の手法においては、障害物の存在は考慮しているが、各ロボット個々の位置を隊列内で指定する方式のヘテロジニアス（Heterogeneous）隊列制御は実現できていない。

このような現状に鑑みて、本発明では、多数のロボットの存在を考慮しつつも、計画計算に必要な計算時間や計算機の記憶容量を少ないものに低減可能で、かつ、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ障害物のある環境にて変形動作を行うことを可能とし、ヘテロジニアス隊列制御を実現することができる、ロボット協調隊列形成技術を提供することを目的とする。

本発明の一態様は、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、開始位置の集合に配置されたp台の制御対象物を所定の入口位置を含む目標位置の集合に移動させるための行動制御を行う行動制御システムであって、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれR個の位置単位からなる一塊の任意の形状を成し、前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、p台の前記制御対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ３つ以上の方向において当該制御対象物単位を構成する他の制御対象物と隣接し、制御対象物単位がその制御対象物単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数に基づいて制御され、静止するか、または、３次元空間上の第一〜第六方向の何れかのN×N×Q個の位置からなる位置単位に移動するように制御されるものとし、前記価値関数が記憶される記憶部と、複数の制御対象物単位が成すある隊列をGaとし、複数の制御対象物単位が成す他の隊列をGbとし、隊列Gbに存在し隊列Gaに存在しない制御対象物単位を頭部制御対象物単位とし、隊列Gaに存在し隊列Gbに存在しない制御対象物単位を尾部制御対象物単位とし、前記開始位置の集合に配置されたp台の前記制御対象物を前記目標位置の集合に移動させる際に、頭部制御対象物単位が移動の先頭を務めるように、前記価値関数を用いてある時刻の隊列Gaに対して隊列Gbを決定する移動先隊列決定用動作計画部と、頭部制御対象物単位の位置にM×N×Q台の制御対象物が位置するように各制御対象物の動作を制御する各制御対象物動作用動作計画部と、制御対象物の前記目標位置の集合の中での現在の位置を現在位置とし、制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、前記目標位置の集合の中に２つのボイドを生成するボイド生成部と、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、前記ボイド生成部がボイドを生成する際に前記目標位置の集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、前記待避制御対象物を各々が存在していた前記目標位置の集合の中の元の位置に戻すことにより、前記２つのボイドを消去するボイド消去部とを含み、前記価値関数は、ある位置単位から前記所定の入口位置を含む位置単位までのマンハッタン距離を用いて得られ、前記各制御対象物動作用動作計画部は、隊列Gaの中で最もマンハッタン距離が小さい制御対象物単位を制御対象物単位Hとし、制御対象物単位Hから頭部制御対象物単位へ移動する方向をahとし、尾部制御対象物単位に隣接し、尾部制御対象物単位よりもマンハッタン距離が小さい制御対象物単位を制御対象物単位T'とし、尾部制御対象物単位から制御対象物単位T'へ移動する方向をatとし、制御対象物単位Hから頭部制御対象物単位へ制御対象物を移動させる際に、制御対象物単位H内に発生したボイドの位置と、他の制御対象物単位内に発生したボイドの位置とが同じになるように、ボイドを移動させるものとし、方向ahと方向atに基づき、制御対象物単位Hと頭部制御対象物単位との間、及び、尾部制御対象物単位と制御対象物単位T'との間において隣接状態を維持するように、制御対象物単位Hにおいてボイドを発生させる。

本発明の一態様は、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、開始位置の集合に配置されたp台の制御対象物を所定の入口位置を含む目標位置の集合に移動させるための行動制御を行う行動制御システムであって、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれR個の位置単位からなる一塊の任意の形状を成し、前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、p台の前記制御対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ３つ以上の方向において当該制御対象物単位を構成する他の制御対象物と隣接し、制御対象物単位がその制御対象物単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数に基づいて制御され、静止するか、または、３次元空間上の第一〜第六方向の何れかのN×N×Q個の位置からなる位置単位に移動するように制御されるものとし、前記価値関数が記憶される記憶部と、複数の制御対象物単位が成すある隊列をGaとし、複数の制御対象物単位が成す他の隊列をGbとし、隊列Gbに存在し隊列Gaに存在しない制御対象物単位を頭部制御対象物単位とし、隊列Gaに存在し隊列Gbに存在しない制御対象物単位を尾部制御対象物単位とし、前記開始位置の集合に配置されたp台の前記制御対象物を前記目標位置の集合に移動させる際に、頭部制御対象物単位が移動の先頭を務めるように、前記価値関数を用いてある時刻の隊列Gaに対して隊列Gbを決定する移動先隊列決定用動作計画部と、頭部制御対象物単位の位置にM×N×Q台の制御対象物が位置するように各制御対象物の動作を制御する各制御対象物動作用動作計画部と、制御対象物の前記目標位置の集合の中での現在の位置を現在位置とし、制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、所定の場合に前記目標位置の集合の中に２つのボイドを生成するボイド生成部と、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、前記ボイド生成部がボイドを生成する際に前記目標位置の集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、すべての制御対象物が各々の目標位置に到達している場合に、前記待避制御対象物を各々が存在していた前記目標位置の集合の中の元の位置に戻すことにより、前記２つのボイドを消去する最終ボイド消去部とを含み、前記価値関数は、ある位置単位から前記所定の入口位置を含む位置単位までのマンハッタン距離を用いて得られ、前記各制御対象物動作用動作計画部は、隊列Gaの中で最もマンハッタン距離が小さい制御対象物単位を制御対象物単位Hとし、制御対象物単位Hから頭部制御対象物単位へ移動する方向をahとし、尾部制御対象物単位に隣接し、尾部制御対象物単位よりもマンハッタン距離が小さい制御対象物単位を制御対象物単位T'とし、尾部制御対象物単位から制御対象物単位T'へ移動する方向をatとし、制御対象物単位Hから頭部制御対象物単位へ制御対象物を移動させる際に、制御対象物単位H内に発生したボイドの位置と、他の制御対象物単位内に発生したボイドの位置とが同じになるように、ボイドを移動させるものとし、方向ahと方向atに基づき、制御対象物単位Hと頭部制御対象物単位との間、及び、尾部制御対象物単位と制御対象物単位T'との間において隣接状態を維持するように、制御対象物単位Hにおいてボイドを発生させ、前記ボイド生成部がボイドを生成する所定の場合とは、前記ボイド生成部の実行が１回目であるか、または、２回目以降の場合において前記現在位置にいる制御対象物と前記目標位置にいる制御対象物のいずれかが前記２つの待避制御対象物と一致するときである。

本発明に拠れば、多数のロボットの存在を考慮しつつも、計画計算に必要な計算時間や計算機の記憶容量を少ないものに低減可能で、かつ、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ障害物のある環境にて変形動作を行うことを可能とし、ヘテロジニアス隊列制御を実現することができる。

ロボットの移動を説明するための図。 図２Ａは開始位置の集合を説明するための図、図２Ｂは目標位置の集合を説明するための図。 尾部ロボット及び頭部ロボットを説明するための図。 頭部ロボットの位置にいた一つのボイドが、尾部ロボットの位置に移動していく様子を示す図。 ロボット単位を説明するための図。 図６Ａ及び図６Ｂはボイド発生条件を説明するための図。 ロボット単位内でのロボット位置を説明するための図。 ボイド移動制御を説明するための図。 図９Ａ及び図９Ｂはボイド発生条件を説明するための図。 図１０Ａ及び図１０Ｂはボイド発生条件を説明するための図。 図１１Ａ及び図１１Ｂはボイド発生条件を説明するための図。 図１２Ａ及び図１２Ｂはボイド発生条件を説明するための図。 ロボット位置入れ替え制御の制約を説明するための図。 各個ロボット位置入れ替え制御の様子を示す図。 分割されたパスの様子を示す図。 線要素でのロボット位置入れ替えの様子を示す図。 角要素でのロボット位置入れ替えの様子を示す図。 Void_Moveで用いる変数を示す図（その１）。 Void_Moveで用いる変数を示す図（その２）。 待避ロボットが他のロボットへの接続を維持できない位置の例を示す図。 第一実施形態に係る行動制御システムの機能ブロック図。 第一実施形態に係る行動制御システムの処理フロー（ホモジニアス隊列制御）の例を示す図。 第一実施形態に係る行動制御システムの処理フロー（ロボット位置入れ替え制御）の例を示す図。 多数のロボットが協調して開始位置における隊列形成状態から移動を行い、目標位置での隊列形成を行う任務を説明するための図。 第二実施形態に係る行動制御システムの機能ブロック図。 第二実施形態に係る行動制御システムの処理フロー（ロボット位置入れ替え制御）の例を示す図。

以下、本発明の実施形態について説明する。なお、以下の説明に用いる図面では、同じ機能を持つ構成部や同じ処理を行うステップには同一の符号を記し、重複説明を省略する。

＜第一実施形態＞
まず、行動制御システム及び方法の理論的背景について説明する。以下、行動制御の対象である制御対象物が、ロボットである場合を例に挙げて説明するが、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。

[問題設定]
多数のロボットが協調して開始位置における隊列形成状態から、各ロボットが接した状態を維持しつつ移動を行い、目標位置での隊列形成を行う任務は、例えば図１に例示するような、互いに接する面同士をスライドさせて移動していくことが可能な立方体型のロボットの使用を想定する。図２に示すように、壁で区切られた部屋（ただし図中、壁を省略する）においての開始位置から目標位置まで複数のロボットの移動によって実現するものである。

ロボットについては、例えば図１に示すように、ロボットの周囲縦横高さ方向（以下「上下左右前後方向」ともいう）６マスのうち一つに他のロボットが存在している状態を維持しながら移動をするものとする。この手法では１つのロボット自身が、一台のロボットのサイズ分の距離を移動することで、一回の動作の移動量を正確に測ることができるというメリットがある。また、一つの面を共有する隣り合うロボットとの相対的な位置を計測しあうことで、ロボットの群れ全体の中での各ロボットの位置も容易に知ることができる。このため、ロボットの移動量の誤差によって、隊列が崩れるといった問題を起こしにくい。また、複数のロボットを連結したように、同時に複数のロボットを移動させていくことが可能である。

なお、ロボットは、隣の位置に他のロボットが存在しているか否か、障害物があるか否か、そして、自身が目標位置上にいるかどうかを知ることができるものとする。

任務を行うロボットは、p台（p≧16=8×2）であり、各ロボットは、隣接するロボットと一面以上を共有しつつ、三次元空間におけるX-Y-Z軸方向に移動可能とする。図１の各立方体は、それぞれのロボットの位置を示すものである。各立方体にはロボットは一台しか存在することができない。それぞれのロボットは、移動しようとする方向に障害物か他のロボットがある場合には、静止をするものと仮定する。なお、ロボットが存在しうる立方体状の空間をマス、または、格子ともいう。図２において、グレーに塗りつぶされたマスはロボットが存在する位置を示し、実線で囲まれた白抜きのマスは障害物が存在する位置を示す。図２Ａのロボットが存在する位置はロボットの開始位置の集合を示し、図２Ｂのロボットが存在する位置はロボットの目標位置の集合を示す。目標位置の集合で表される領域を目標隊列エリアともいう。このように、各開始位置及び各目標位置は、それぞれ縦横高さ方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、ロボットの開始位置及び目標位置での隊列形状はそれぞれ一塊の任意の形状である。

[ロボットの座標設定]
それぞれのロボットi（iはロボット番号を表すi=0,1,2,3,…,p-1）の初期位置を（Xr0[i],Yr0[i],Zr0[i]）とし、目標位置を（Xre[i],Yre[i],Zre[i]）とするとき、本問題は、初期位置に配置されたロボットが、目標位置まで移動するための行動計画を求めることと定義できる。目標位置（Xre[i],Yre[i],Zre[i]）の集合をGとする。

[任務空間の定義]
iをロボット番号としたとき、ロボットiの各状態（ロボットの位置と行動）は離散値で表現される。部屋をX,Y,Zの直交座標系からなる3次元空間で表すと、X軸、Y軸、Z軸をそれぞれ離散化表現した値により各位置を表現する。つまり、部屋（３次元空間）は格子で区切られ、各格子が各位置に対応する。また、各格子において、障害物の「ある／なし」が予め設定されている。

[ロボット動作の定義]
また、行動主体は部屋に配置されている各ロボットとなる。ロボットi（iはロボット番号）の行動aは、静止、縦横高さ方向への１格子分の移動、の計７種類のうちのいずれかを取る。例えば、a∈{0,1,2,3,4,5,6}として、
0: 静止
1: 三次元空間内でX軸正方向に１格子だけ移動する
2: 三次元空間内でY軸正方向に１格子だけ移動する
3: 三次元空間内でX軸負方向に１格子だけ移動する
4: 三次元空間内でY軸負方向に１格子だけ移動する
5: 三次元空間内でZ軸正方向に１格子だけ移動する
6: 三次元空間内でZ軸負方向に１格子だけ移動する
とする。

[探索計算上の問題点]
このような任務環境における状態空間は、ロボット数×3の次元数の状態を持ち、かつ選択可能な行動数は、ロボットの行動（＝7通り）のロボット数乗だけ存在する。例えば、ロボット数が50で、部屋の縦横高さ方向の格子数がそれぞれ20であるとすれば状態数は20の150乗個にもなり、探索計算に要する資源の量は膨大なものとなる。さらにロボット数が1台増えるごとに、その状態数は8000倍増加していくことになる。本実施形態の[問題設定]の項で説明したように、ロボット同士が接しているという拘束条件を取り入れる場合、ロボットのお互いの移動を考慮したうえで探索計算行わなければならないために、根本的な計算量の削減は難しく、複数ロボットを使用する場合の大きな問題となっている。

[二つの動作計画の導入]
そこで、本実施形態では、これらの計算負荷の問題を解決するための方策の一つとして、ロボットの動作決定における探索計算を二つに分けることとする。ここでそのための準備として、ある隊列Gaをなしているロボットが、次になすべき隊列の形状としてGbを考える。隊列Gaと隊列Gbの間には、隊列Gaにて存在し、隊列Gbにて存在しない位置にあるロボットと、隊列Gbにて存在し隊列Gaにて存在しない位置にあるロボットがある。前者を尾部ロボットとし、後者を頭部ロボットとする。本実施形態では、ちょうど、頭部ロボット位置にあるロボットが、ロボット群の移動において先頭を務め、尾部ロボットが、移動の最後尾を務めるロボットになるように、隊列Gaと隊列Gbを定義する。

この定義のもと、一つ目の探索計算は、隊列Gaをなすロボットの群が、障害物を回避しつつも、目標位置まで移動するためには、次の隊列Gbとしてどの隊列を選択すればよいかを決定するための（図３）動作計画計算（移動先隊列決定用動作計画）である。この探索計算は、事実上、頭部ロボット位置に移動するロボットとして、隊列Gaのどのロボットを選択するかと、隊列Gaにおける尾部ロボットを決定するための計算である。

二つ目の探索計算は、ロボットが互いに接した位置関係を維持しつつも、隊列Gaから隊列Gbへ変形する際の各ロボットの動作を決定するための動作計画計算（各ロボット動作用動作計画）である。これは、事実上、移動先隊列決定用動作計画により決定済みの頭部ロボット及び尾部ロボットに対し、尾部ロボットの位置からロボットを追い出し、隊列Gbにおける頭部ロボットの位置へロボットを誘導するための探索計算である。

本実施形態では、これらの探索計算を、目標位置の集合G内に一点の入口位置を設定し、その入口位置からの各ロボット位置の全体ロボット構造内でのマンハッタン距離を計算することで行う。

[ボイド制御]
これらの二つの探索計算の計算負荷の軽減のため、本実施形態では、ボイド制御の考え方を導入する。まず、ここでいうボイドとは、あるロボットが別の位置に移動した後に、元いた位置にできる空隙のことである。別の言い方をすると、ボイドとは、ロボットの移動する方向と反対の方向に移動する仮想的な存在である。こうした群ロボットの隊列形成問題においては、複数のロボットの動作に着目するがゆえに、その探索計算量が爆発してしまうが、視点を変えて、ボイドの動きに着目すれば、多数のロボットの動作計画の問題を単一のボイドの動作計画として考えることができ、探索計算負荷の軽減に適している。

まず、移動先群決定用動作計画において、ロボット群の移動の先頭を務める頭部ロボットの動作について考える。本実施形態では、目標位置の集合Gに一か所の入口位置Peを設けて、他の位置からは頭部ロボットがGに入ることはできないこととする。また、一度G内に入ったすべてのロボットは、二度とG外には出られないこととする。この条件のもと、G外にいる頭部ロボットが入口位置Peに至るまでのロボットの経路決定問題は、一台のロボットを入口位置Peまで誘導するための探索問題を解けばよく、得られた解を頭部ロボットの選択と誘導のために使用すればよい。目標位置の集合G内の頭部ロボットの動作については、ボイドの動きに着目して、ボイドの誘導について同様のことを行う。すなわち、目標位置の集合G内の頭部ロボットに望まれる動作とは、適切に頭部ロボットが入口位置Peから遠ざかり、目標位置の集合G内の各場所に散らばっていきつつ目標位置の集合Gを埋めていくことであるが、これをボイドの動きとしてみれば、ロボットが入口位置Peから、目標位置の集合G内に入るたびに、ボイドはそれに伴って、入口位置Peから目標位置の集合Gの外に出ていくのである。このとき、目標位置の集合G内のボイドは、すべて一点の入口位置Peに向かって移動していくのが理想的な動作である。つまり、目標位置の集合G内の頭部ロボットを目標位置の集合Gに適切に散らばるように誘導するためには、目標位置の集合G内に位置する各ボイドが入口位置Peに至るための経路を探索計算すればよく、そうして求められたボイドの動作を頭部ロボットを動かすことで実現するようにすればよい。この考え方ならば、すべてのロボットが、一点、入口位置Peを目指して動作するための動作計画を一回行えばよいので、計算負荷が劇的に少なくなる。

続いて、隊列Gaを維持しているロボット群が、隊列Gbに変形することを考える。このとき、ロボット群が隊列Gaから隊列Gbに変形する動作とは、頭部ロボットの位置にいた一つのボイドが、尾部ロボットの位置に移動していくプロセスとしてとらえることができる（図３及び図４参照）。このようなボイドの動作は、頭部ロボット位置をスタート位置とし、尾部ロボット位置をゴール位置とした、一つのボイドの動作計画によって計算可能であり、計算負荷も小さい。この計算において考慮すべきボイドの移動の拘束条件としては、ボイドが移動する際に、ロボットが存在する位置をたどっていくという制限を設けるだけでよい。

[8マスロボット単位の導入]
さらに、本実施形態では、図５に示すように、８つの田の字状に隣接したロボットを一つの単位とし（ロボット単位）、ロボットは、この田の字型のロボット単位を維持しつつ移動を行うとする。言い換えると、８台毎に１つのロボット単位を構成し、１つのロボット単位を構成する８台のロボットはそれぞれ３つの方向において１つのロボット単位を構成する他のロボットと隣接した状態を維持しつつ移動を行う。このロボット単位の集団は、互いにロボット単位ごとに一面を共有し、接しながら移動をするように制御される。

このような８つのロボットを一つの単位とした移動を行う理由としては、このような状態で移動を行う限り、各ロボット単位の中のいずれのロボットが一台のみ欠けても、各ロボット単位はお互いに一つの面で接しあう位置関係を崩さずに済むからである。すなわちこれは、隊列形態の維持を考量しなければならない各ロボット動作用動作計画において、ロボット同士の接続を考慮するための計算負荷を軽減することにつながるからである。各ロボット単位内のボイドが１つ以内であれば、全ロボット単位内にボイドが存在してもよいことになるので、一度に複数のボイドをロボット群の中に並列で存在させて、ロボットの隊列変形動作を高速化することも可能である。

先に述べた移動先隊列決定用動作計画は、８台のロボットによるロボット単位がなす群れの移動について行う。先に述べた頭部ロボット、尾部ロボットは、それぞれ８台のロボットで構成される頭部ロボット単位、尾部ロボット単位となる。各ロボット動作用動作計画は、頭部ロボット単位に含まれる８つのロボットの位置に、ロボットを誘導し、尾部ロボット単位の位置に含まれるロボットから、８つのロボットを追い払うための動作を探索計算する。

ここでは８台のロボットがなすロボット単位が一つのマスの単位（本実施形態では、以下、この単位を「マス単位」または「位置単位」ともいう）であるとし、一つのマス単位を一状態として状態空間を組む。ロボット単位の位置を（Xr_unit[j],Yr_unit[j], Zr_unit[j]）（j=0,1,2,…j_max-1）としたとき、そのロボット単位jに所属するロボットをi1,i2,i3,i4,i5,i6,i7,i8とすれば、
Xri1 = 2 ×Xr_unit[j]
Yri1 = 2 ×Yr_unit[j]
Zri1 = 2 ×Zr_unit[j]
Xri2 = 2 ×Xr_unit[j] + 1
Yri2 = 2 ×Yr_unit[j]
Zri2 = 2 ×Zr_unit[j]
Xri3 = 2 ×Xr_unit[j]
Yri3 = 2 ×Yr_unit[j] + 1
Zri3 = 2 × Zr_unit[j]
Xri4 = 2 ×Xr_unit[j] + 1
Yri4 = 2 ×Yr_unit[j] + 1
Zri4 = 2 ×Zr_unit[j]
Xri5 = 2 ×Xr_unit[j]
Yri5 = 2 ×Yr_unit[j]
Zri5 = 2 ×Zr_unit[j] + 1
Xri6 = 2 ×Xr_unit[j] + 1
Yri6 = 2 ×Yr_unit[j]
Zri6 = 2 ×Zr_unit[j] + 1
Xri7 = 2 ×Xr_unit[j]
Yri7 = 2 ×Yr_unit[j] + 1
Zri7 = 2 ×Zr_unit[j] + 1
Xri8 = 2 ×Xr_unit[j] + 1
Yri8 = 2 ×Yr_unit[j] + 1
Zri8 = 2 ×Zr_unit[j] + 1
である。

頭部ロボット単位決定用の開始位置の集合をSU、目標位置の集合をGUとする。開始位置の集合SU,目標位置の集合GUは、ロボット単位と同様に8つのロボット位置が一単位となったマスで構成される。GU内には一つの入口位置単位PeUが設けられ、各ロボットは、PeU内の位置を経由してGU内に入る。なお、移動先隊列決定用動作計画にて使用するロボット単位の総数は、開始位置の集合SU及び目標位置の集合GU内のロボット単位の数と同じでなければならない。よって、ロボットの全体数pを8の倍数とし、開始位置の集合SU及び目標位置の集合GUにおいて、すべてのロボット単位は8台のロボットが充填され、端数のロボットは生じないものとする。また目標位置の集合GU及び開始位置の集合SUはそれぞれR(R=p/8)個の位置単位からなる一塊の任意の形状を成す。

[移動先隊列決定用動作計画[Next_Formation_Decision]]
本実施形態では、ロボットが動作を開始する前にあらかじめ、２つの動作計画のために使用する任務空間内の各位置単位の入口位置単位PeUからのマンハッタン距離の計算を行う。そのために、まず、任務空間内の各位置単位（X,Y,Z）にて、入口位置単位PeUからの各位置単位へのマンハッタン距離δ[X][Y][Z]を以下の計算手続きで求める。なお、以下において、説明を簡単にするため「位置単位」を単に「位置」ともいい、「入口位置単位PeU」を単に「入口位置Pe」ともいう。

[マンハッタン距離δの計算]
(1)各位置（X,Y,Z）において、next[a][X][Y][Z]を用意し、隣接する行動aの方向に障害物があるか、もしくは位置（X,Y,Z）が入口位置Pe以外の目標位置の集合GU内の位置の場合に、隣接する行動aの方向の位置が目標位置の集合GUの外であるか、もしくは位置（X,Y,Z）が目標位置の集合GU外の場合に、隣接する行動aの方向の位置が入口位置Pe以外の目標位置の集合GU内の位置である場合には、next[a][X][Y][Z]←0とし、それ以外の場合はnext[a][X][Y][Z]←１とする。すなわち、next[a][X][Y][Z]=0は位置（X,Y,Z）において行動aを実行できないことを、next[a][X][Y][Z]=1は位置（X,Y,Z）において行動aを実行できることを意味する。
(2)状態空間内の各位置（X,Y,Z）のマンハッタン距離δ[X][Y][Z]を状態空間内の格子数より大きな値s_maxに初期化する。
(3)入口位置Peのマンハッタン距離δを0に初期化する。
(4)入口位置Pe以外の、各位置（X,Y,Z）の全行動aについて、next[a][X][Y][Z]の値が0ではない場合の、行動aによって位置(X,Y,Z)から移動した先の位置（X’,Y’,Z’）でのマンハッタン距離δ[X’][Y’][Z’]を調べ、その最小値に１を加えた値が、現在のδ[X][Y][Z]よりも小さい場合は、その値をδ[X][Y][Z]に代入する。
(5)上述の(4)の処理にて、δ[X][Y][Z]値の更新がなくなるまで、(4)を繰り返す。

以上の処理で計算したマンハッタン距離δ[X][Y][Z]を使用し、続いて開始位置の集合SUと目標位置の集合GUをつなぐパスPを決定する。パスPの決定は以下の処理によってなされる。パスPをなす位置をp[ip](ip=0,1,2,3…)とする。

[パスPの決定]
(1)開始位置の集合SU内にて、最もマンハッタン距離δ[X][Y][Z]値の小さな位置をPsとする。
(2)位置Psに隣接し、位置Psにおけるマンハッタン距離δよりも１だけ小さなマンハッタン距離δを持つ開始位置の集合SUの外の位置をパスPの始点p[0]とする。
(3)ip←0とする。
(4)位置p[ip]に隣接する位置の中で、位置p[ip]におけるマンハッタン距離δよりも１だけ小さなマンハッタン距離δを持つ位置をp[ip+1]とする。もし、p[ip+1]が入口位置Peに一致しないならば、ipをインクリメントし、(4)を繰りかえす。一致する場合は(5)へ移行する。
(5)目標位置の集合GU及びパスPの何れにも含まれないすべての位置のマンハッタン距離δをs_maxに再設定する。
(6)開始位置の集合SU内のすべての位置(X,Y,Z)の全行動aについて、next[a][X][Y][Z]の値が0ではない場合の、行動aによって位置(X,Y,Z)から移動した先の位置（X’,Y’,Z’）でのマンハッタン距離δ[X’][Y’][Z’]を調べ、その最小値に１を加えた値が、現在のδ[X][Y][Z]よりも小さい場合は、その値をδ[X][Y][Z]に代入する。
(7)上述の(6)の処理にて、δ[X][Y][Z]値の更新がなくなるまで、(6)を繰り返す。
(8)目標位置の集合GU内のすべての点のマンハッタン距離δの符号を負にする。この処理により、目標位置の集合GU内では、入口位置Peより離れるほどマンハッタン距離δが小さくなる。

以上の処理で計算されたマンハッタン距離δは、SU+P+GUの中のみでのロボットの移動を考慮したマンハッタン距離の値となる。マンハッタン距離δを使用して、ある隊列Gaをなすロボット単位の群が、つぎに移動すべき隊列Gbの頭部ロボットを決定する方法は以下のとおりである。本処理を、頭部決定処理Head_Robot_Decisionとする。なお、以上の処理で計算されたマンハッタン距離δが価値関数に相当し、ロボット単位がそのロボット単位の現在の位置単位において各行動を取ったときの適切さを表す。具体的には、マンハッタン距離δが小さくなる方向に移動するようにロボット単位を制御する。よって、価値関数（ロボット単位がそのロボット単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数）は、ある位置単位から入口位置単位までのマンハッタン距離を用いて得られると言える。

[Head_Robot_Decision]
(1)各ロボット単位の位置（Xr_unit[j],Yr_unit[j],Zr_unit[j]）において、位置（Xr_unit[j],Yr_unit[j],Zr_unit[j]）に隣接し、かつロボットが存在しておらず、かつパスP及び目標位置の集合GUの何れかに属する位置の中で、マンハッタン距離δ[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]の値より1小さい、マンハッタン距離δをもつ位置を求め、その位置のマンハッタン距離δをδ_neighbor_max[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]とする。
(2)全ロボット単位中で最大のδ_neighbor_max[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]の値を持つロボット単位H(なお、ロボット単位Hの位置を（Xrh,Yrh,Zrh）とする)を選択し、ロボット単位Hに隣接し、かつロボットが存在しておらず、かつパスP及び目標位置の集合GUの何れかに属する位置の中で、マンハッタン距離δがロボット単位Hのそれより１小さいマンハッタン距離δをもつ位置を頭部ロボット単位Dの位置とする。ロボット単位Hから頭部ロボット単位Dの位置に向かう行動の方向をa_headとする。

以上の処理では、(i)目標位置の集合GU外にしかロボット単位が存在しないときに、ロボットの群れがパスPを通して入口位置Peに近づいていくか、もしくは、(ii)目標位置の集合GU内にロボット単位が存在するときには、入口位置Peに近い位置から優先的にロボットを埋めていけるように、頭部ロボット単位Dを選択している。

続いて、尾部ロボット単位位置を決定するための手法Tail_Robot_Decisionを述べる。

[Tail_Robot_Decision]
(1)各ロボット単位位置（Xr_unit[j],Yr_unit[j],Zr_unit[j]）において、
δ'[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]
=δ[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]-δ[Xrh][Yrh][Zrh]
を計算する。なお、δ[Xrh][Yrh][Zrh]はロボット単位Hのマンハッタン距離であり、δ'[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]は、ロボット単位Hの位置から各ロボット単位位置（Xr_unit[j],Yr_unit[j],Zr_unit[j]）までのマンハッタン距離を表す。
(2)各ロボット単位位置（Xr_unit[j],Yr_unit[j],Zr_unit[j]）において、隣接するすべてのロボット単位N（なお、ロボット単位Nの位置を(Xrn,Yrn,Zrn)とする）について、δ'[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]≧δ'[Xrn][Yrn][Zrn]を満たし、かつ、δ'[Xr_unit[j]][Yr_unit[j]][Zr_unit[j]]≧１であるロボット単位の何れか一つを尾部ロボット単位Tとして選択する。つまり、尾部ロボット単位Tは、隣接するすべてのロボット単位N以上にロボット単位Hから離れており、かつ、ロボット単位Hから１マス単位以上離れている（ロボット単位Hと尾部ロボット単位Tとのマンハッタン距離が１マス単位以上離れている）。

以上の処理では、隊列Gaより取り払ったとしても、隊列Gaの連続性を失わないロボット単位を尾部ロボット単位として設定している。

以上の処理をまとめて、以下のように、移動先隊列決定用動作計画処理（Next_Formation_Decision）が行われる。
(1)頭部決定処理Head_Robot_Decisionを行う。
(2)尾部ロボット決定処理Tail_robot_Decisionを行う。
(3)上述の(2)にて特定した尾部ロボット単位Tを、現在の隊列Gaから削除し、隊列Gaに頭部ロボット単位Dを追加し、隊列Gbとする。

[各ロボット動作決定用動作計画]
移動先隊列決定用動作計画において頭部ロボット単位Dと尾部ロボット単位Tが決定され、次の隊列Gbが決定されたので、現隊列Gaから次隊列Gbへの変形を各ロボットの移動によって実現するための探索計算を各ロボット動作決定用動作計画にて行う。

本実施形態では、８つのロボットを組にしたロボット単位を維持しつつロボット群が移動を行うようにしているので、ボイド制御を行う際の、各ロボットの接続維持を考える際には、尾部ロボット単位Tと頭部ロボット単位D付近でのボイドの動きにさえ気を配れば、それ以外は、ボイドが各ロボット単位内に１つ以内であるようにすればよい。

以下、各ロボット動作決定用動作計画の動作について述べる。ロボット動作決定用動作計画においては、ロボット単位Hから頭部ロボット単位Dへロボット単位を移動させるたびに、８つのボイドが発生するので、まずは、それら発生した各々のボイドについてそのボイドがロボット単位Hから尾部ロボット単位Tまでたどるべき経路を計算する。

ここで、ロボット単位H内にて発生させる８つの各ボイドの位置と順序については、注意を要する。後述するボイド制御においては、一度の動作において同方向の2ステップの動作でボイドを移動させる(ロボットから見れば、一度の動作において2台のロボットを同方向に同時に移動させる)ことを行うが、そのようにすることで、ロボット単位H内で発生したボイドのH内での位置と、ボイドが移動する道中のロボット単位内でのボイドの位置を同じにしたままボイドを移動させることができる。すなわち、ボイドが尾部ロボット単位にて外に追い出されるまで、ボイドはそれが含まれる各ロボット単位内での位置を変えないことになるが、そのような条件において、ロボット単位Hとロボット単位Dにおける非接続と、尾部ロボット単位とその他のロボット単位とでの非接続を起こさないようにしなければならない。その方法について以下に述べる。

ロボット単位Hで発生したボイドが、ロボット単位内を通って、尾部ロボット単位Tの一つ手前のロボット単位T’に到達するとする。この時、ロボット単位Hからロボット単位Dへ向かう行動の方向をah（=a_head）、ロボット単位Tからロボット単位T’へ向かう行動の方向をatとする。なお、at、ahはそれぞれ行動aの移動方向に対応しており、例えばat=1,2,3,4,5,6のときそれぞれX軸正方向、Y軸正方向、X軸負方向、Y軸負方向、Z軸正方向、Z軸負方向を意味する。ロボット単位T’に到達したボイドの位置からatの逆向きにたどっていけるTとT’内のロボットをすべて、atの向きに１ステップ移動させることで、ボイドをロボット隊列の外に追い出すことを考える。このとき、H内でのボイド発生位置と順序は、ロボット単位Hからah方向にロボットが押し出されるとき、「それ以前にHから押し出されたロボットと、今押し出されるロボットの押し出された後の位置が、互いに押し出す方向に垂直な方向で接する位置関係にある」ようであれば、それ以降に押し出されるロボットの位置がどの位置であろうとも（voidがどこに発生しようとも）、Hからのロボットの押し出しにおいて非接続は生じないことと、ロボット単位Tからat方向にロボットがT’に吸収されるとき、「T’とTが接する面であって、T側の面上にあるロボットが少なくとも２つあって、それらが互いに接しているならば、T’内のどの位置にvoidがが発生しても、Tからのロボットの吸収において非接続は生じないこと、を利用する。簡単に言えば、HとDの接する面であって、D側の面、TとT’の接する面であって、T側の面上に、二つ以上のロボットが互いに接する状態で存在しているか、そのvoidの発生によってそうなるならば、voidの位置はどこでもよく、逆にそうでないなら、voidの位置は、Dに接しておらずかつTに接していない位置に限られる、ということである(図６参照)。条件は、以下のとおりである。

以下の条件に見合うように決定される。（ボイドがロボット単位HからTに至るまでの軌道は、後述の[Void_Unit_Trajectory_Decision]で計算される。）

ロボット単位H内でのローカル位置を以下のように定義する。（図７）
H0(0,1,0), H1(1,1,0), H2(0,1,1), H3(1,1,1),H4(0,0,0), H5(1,0,0), H6(0,0,1), H7(1,0,1)

さらにk番目(k=1,2,3,4,5,6,7,8)に発生するボイドのロボット単位H内ローカル位置をvoid[k]とするとき以下の条件（以下、「ボイド発生条件」ともいう）をvoid[k]が満たすようにすればよい。

なお、ボイド発生条件は、ahとatに基づき、ロボット単位Hと頭部ロボット単位Dとの間、及び、尾部ロボット単位Tとロボット単位T'との間において隣接状態を維持するためのロボット単位Hにおけるボイドの発生位置の条件とも言える。例えば、図８のように、ボイドを発生させると尾部ロボット単位Tとロボット単位T'との間において隣接状態を維持することができない。

[ボイド発生条件]
(1)void[8]の位置(8番目に発生するボイドのロボット単位H内ローカル位置(以下、ボイドの発生位置ともいう))は、atに平行な座標軸の座標値において、atが正方向であれば１,負方向であれば０でなければならない。例えば、at=1のとき(ロボット単位Tからロボット単位T'へ向かう行動の方向がX軸正方向のとき)、void[8]の(ローカル位置の)X座標は１でなければならない。

(2)（void[7]とvoid[8]の位置をatに垂直な面に投影したときの位置(以下、投影位置ともいう)を示す二つの座標成分は、一つだけの座標成分が同じである） or
(void[7]とvoid[8]の位置をatに垂直な面に投影したときの面内位置を示す二つの座標成分が、二つの座標成分とも異なる値であり、かつ、atに平行な座標軸の座標値において、void[7]とvoid[8]では同じ値であり、かつ、(i)void[6]の位置をatに垂直な面に投影したときの面内位置が、void[7]とvoid[8]の位置をatに垂直な面に投影したときの面内位置と異なる位置であるか、または(ii)void[6]の位置が、void[7]またはvoid[8]と同じ位置であり、かつvoid[5]の位置が、void[6]の対角の位置（全座標値が異なる位置）以外の位置である。) or
(void[7]とvoid[8]の位置が同じ位置であり、かつ、void[6]の位置の座標値のうちすくなくとも一つが、void[8]の座標値に等しい。それらvoid[6]の位置のうち、さらに、void[6]を atに垂直な面に投影したときの面内位置が、void[8]と対角の位置のときは、void[5]の位置はvoid[8]の対角の位置（全座標値が異なる位置）以外の位置である。)
でなければならない。

(3)void[1]の位置(1番目のボイド発生位置)は、ahに平行な座標軸の座標値において、ahが正方向であれば0,負方向であれば1でなければならない。例えば、ah1のとき(ロボット単位Hからロボット単位Dへ向かう行動の方向がX軸正方向のとき)、void[1]のX座標は0でなければならない。

(4)（void[1]とvoid[2]の位置をahに垂直な面に投影したときの面内位置を示す二つの座標成分は、一つだけの座標成分が同じである） or
(void[1]とvoid[2]の位置をahに垂直な面に投影したときの面内位置を示す二つの座標成分とも異なる値であり、かつ、ahに平行な座標軸の座標値において、void[1]とvoid[2]では同じ値であり、かつ、(i)void[3]の位置をahに垂直な面に投影したときの面内位置が、void[1]とvoid[2]の位置をahに垂直な面に投影したときの面内位置と異なる位置であるか、または、(ii)void[3]の位置が、void[1]と(または)void[2]と同じ位置であり、かつvoid[4]の位置が、void[3]の対角の位置（全座標値が異なる位置）以外の位置である。) or
(void[1]とvoid[2]の位置が同じ位置であり、かつ、void[3]の位置の座標値のうちすくなくとも一つが、void[1]の座標値に等しい。それらvoid[3]の位置のうち、さらに、void[3]を ahに垂直な面に投影したときの面内位置が、void[1]と対角の位置のときは、void[4]は、void[1]の対角の位置（全座標値が異なる位置）以外の位置である。)
でなければならない。

(5)(ahに垂直な面とatに垂直な面にvoid[k]の各位置を投影したときに、それぞれの面内に投影された各void[k]の面内位置と同じ面内位置に投影される他のvoid[l](l!=k)が一つだけ存在しなければならない。

ボイド発生条件の意味するところを、図９から図１２を用いて簡単に説明する。

ボイド発生条件の(3)(4)は、ロボット単位H内のロボットが頭部ロボット単位Dに移動する際に、接続状態を維持するための条件である。例えば、ah=1のとき(ロボット単位Hからロボット単位Dへ向かう行動の方向がX軸正方向のとき)、void[1]のX座標は0でない場合、先頭のロボットが接続状態を維持できない(図９Ａ及び図９Ｂ参照)。この状態をボイド発生条件の(3)により防ぐ。

ボイド発生条件の(4)は、図１０Ａの状態から、void[2]、場合によってはvoid[3]の発生位置を特定し、押し出されるロボットが接続状態を維持できるようにしている。

ボイド発生条件の(4)の一つ目の条件では、void[1]が発生したときに移動したロボットと、void[2]が発生するときに移動するロボットが接続状態を維持する(図１０Ｂ)。そのため、ahの方向におけるロボットの接続はあってもなくてもよい。

ボイド発生条件の(4)の二つ目の条件では、void[1]が発生したときに移動したロボットと、void[2]が発生するときに移動するロボットとが接続状態にない代わりに、ahの方向においてロボットの接続状態を維持する(図１１Ａ)。また、(i)ではvoid[1]及びvoid[2]が発生したときに移動したロボットと、void[3]が発生するときに移動するロボットが接続状態を維持する(図１１Ｂ)。そのため、ahの方向におけるロボットの接続はあってもなくてもよい。(ii)ではvoid[3]をvoid[1]またはvoid[2]と同じ位置とすることで、ahの方向においてロボットの接続状態を維持する(図１２Ａ)。

同様に、尾部ロボット単位T内のロボットがロボット単位T'に移動する際に、接続状態を維持するための条件がボイド発生条件(1)(2)である。

ボイド発生条件(5)は、ロボットが頭部ロボット単位Dから飛び出さないようにするための条件である(図１２Ｂ参照)。また、既に尾部ロボット単位Tにロボットが存在しない位置にボイドを移動させないための条件である。

例えば、以上の(1)〜(5)を満たすvoid[k]を決定する簡単な方法として、以下の方法[Decision_Void_Birth]、[Decision_Void_Birth_2]がある。何れの方法であっても、上述のボイド発生条件を満たす。

[Decision_Void_Birth]
(1)If ah＝2 or 3 or 6 then void[1]=H7, void[2]=H2
(2)If ah＝4 or 1 or 5 then void[1]=H0, void[2]=H5
(3)If at＝1 or 2 or 5 then void[7]=H6, void[8]=H3
(4)If at＝3 or 4 or 6 then void[7]=H1, void[8]=H4
(5)If at＝5 or 6 then void[6]=H0 or void[6]=H5 or void[6]=H2 or void[6]=H7
(6)If ah＝5 or 6 then void[3]=H6 or void[3]=H3 or void[3]=H1 or void[3]=H4
(7)その他のvoid[k]をまだvoid[1],void[2],void[7],void[8],void[3],void[6]にまだ使用されていない位置に割り当てる。

以上の方法は、(5)(6)を適用するat、ahの向きがZ軸方向である場合のものであり、void[1]とvoid[2]及び,void[7]とvoid[8]がXY平面内にて対角の位置になっている。(5)(6)を適用するat、ahの向きをX軸、Y軸にした場合も同様の手法を得ることができる。

以上の方法で得たvoid[1]〜void[8]とah,atの値の例を以下の表(表中、vk(k=1,2,…,8)はvoid[k]を意味する)に示す。

以上[Decision_Void_Birth]に示したvoid[k]の割り振り方は、ロボット単位H内の各ロボット位置が二度以上ボイドの発生位置とならない、いわば重複のない場合の割り振り方である(void[1], void[2], void[3], void[4], void[5], void[6], void[7], void[8]が、すべて異なる位置である割り振り方は、(5)の条件を満たす。） が、当然、重複を許した割り振り方も許される。そのような場合については、例えば、以下の方法[Decision_Void_Birth_2]を用いることができる。

[Decision_Void_Birth_2]
(1)void[1],void[2]の位置を同じ位置とし、ともに、ahに平行な座標軸の座標値において、ahが正方向であれば0,負方向であれば1とする。
(2)void[7],void[8]の位置を同じ位置とし、ah、atの方向への投影位置において、void[1],void[2]と重ならない位置のうち一つを、void[7],void[8]の同じ位置とし、ともに、atに平行な座標軸の座標値において、atが正方向であれば1,負方向であれば0とする。
(3)void[3],void[4]の位置を同じ位置とし、ah、atの方向への投影位置において、void[1],void[2], void[7],void[8]と重ならず、ロボット単位立方体内で、void[1],void[2]の位置の対角にない位置をvoid[3],void[4]とする。
(4)void[5],void[6]の位置を同じ位置とし、ah、atの方向への投影位置において、void[1],void[2],void[3],void[4], void[7],void[8]と重ならず、ロボット単位立方体内で、void[7],void[8]の位置の対角にない位置をvoid[5],void[6]とする。

ロボット単位Hから頭部ロボット単位Dへの移動においては、ロボット単位H内の位置void[k]にあるロボットが、そのロボットからah方向にたどっていけるロボットをすべて、ah方向にスライドすることで、ロボットが移動する。そのため、ロボット単位H内にて発生するボイドの位置は、void[k]の位置となる。ロボットが開始位置の集合SUから目標位置の集合GUへの隊列形成動作を開始してからの発生ボイドの通し番号をiv(=0,1,2,3…)で管理し、フル充填されたロボット単位Hから頭部ロボット単位Dへのロボットの移動が開始されたときに、それまでに発生しているボイドの最後のボイドの通し番号をiv_prevとすれば、iv_prevは、(8の倍数-1)である。ロボット単位Hから頭部ロボット単位Dへの移動で発生するk番目(k=1,2,3,4,5,6,7,8)のボイドは、通し番号では、iv_prev+k番目のボイドである。

各ボイドの軌道を( void_trj_x[iv_prev+k][t], void_trj_y[iv_prev+k][t] , void_tr
j_z[iv_prev+k][t] ) (t=0,1,2,3…)で表現する。

各ボイドの軌道計算処理（Void_Trajectory_Decision）は、以下の手順で行われる。

[Void_Trajectory_Decision]
(1)ロボット単位H内、各ボイドの発生位置を[ボイド発生条件]により決定する。例えば、[Decision_Void_Birth]または[Decision_Void_Birth_2]を実行する。
(2)各ボイドの発生位置を各ボイドの軌道の終点void_end[iv_prev+k]とする。
(3)上述の(1)で発生位置を設定した８つのボイドがロボット単位Hの位置から尾部ロボット単位Tの位置までたどるべき位置単位の道筋を(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu]) (tu=0,1,2,3…)とし、後述するVoid_Unit_Trajectory_Decisionにて(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])を決定する。ここで、(void_unit_trj_x[0], void_unit_trj_y[0], void_unit_trj_z[0])をTとする。tuの値は、Tを始点０とし、Hに近づくにつれて大きくなるようにボイドの動きと逆向きに設定する。
(4)(void_unit_trj_x[1], void_unit_trj_y[1], void_unit_trj_z[1])をT'とする。T'は尾部ロボット単位Tに隣接するロボット単位である。
(5)ロボット単位T'内のローカル位置を、各ボイドk(k=1,2,…,8)の目標位置に設定する。なお、ロボット単位Hにおけるボイドの発生位置void[k]に対応する、ロボット単位T'内のローカル位置を各ボイドkの目標位置に設定する。
(6)上述の(5)で決定された各ボイドの目標位置から、ロボット単位Hの位置内のボイド発生位置に至る軌道を(7)以下の処理で設定する。
(7)it←0とする。( void_trj_x[iv_prev+k][0], void_trj_y[iv_prev+k][0] , void_trj_z[iv_prev+k][0] )を上述の(5)で決定されたボイドkの目標位置とする。
(8)(void_unit_trj_x[2+it/2-1], void_unit_trj_y[2+it/2-1], void_unit_trj_z[2+it/2-1])がロボット単位Hの位置に一致していないならば、(void_unit_trj_x[2+(it)/2-1], void_unit_trj_y[2+(it)/2-1], void_unit_trj_z[2+(it)/2-1])から (void_unit_trj_x[2+it/2], void_unit_trj_y[2+it/2], void_unit_trj_z[2+it/2])へ移動する方向に、( void_trj_x[iv_prev+k][it], void_trj_y[iv_prev+k][it] , void_trj_z[iv_prev+k][it] )から１ステップ移動した位置を( void_trj_x[iv_prev+k][it+1], void_trj_y[iv_prev+k][it+1] , void_trj_z[iv_prev+k][it+1] )とし、２ステップ移動した位置を( void_trj_x[iv_prev+k][it+2], void_trj_y[iv_prev+k][it+2] , void_trj_z[iv_prev+k][it+2] )とし、itを2回インクリメントする。(void_unit_trj_x[2+it/2-1], void_unit_trj_y[2+it/2-1], void_unit_trj_z[2+it/2-1])がロボット単位Hの位置に一致するまで(8)を繰り返す。(void_unit_trj_x[2+it/2-1], void_unit_trj_y[2+it/2-1], void_unit_trj_z[2+it/2-1])がロボット単位Hの位置に一致しているならば、軌道内点数trj_num[iv_prev+k]←itとして終了する。

Void_Unit_Trajectory_Decisionの処理は以下のとおりである。

[Void_Unit_Trajectory_Decision]
(1)尾部ロボット単位Tの位置を(void_unit_trj_x[0], void_unit_trj_y[0], void_unit_trj_z[0])とする。tu←１とし、尾部ロボット単位Tよりも小さなマンハッタン距離δを持つ隣接のロボット単位の位置を(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])とする。このロボット単位(void_unit_trj_x[1], void_unit_trj_y[1], void_unit_trj_z[1])の位置が、ロボット単位T'に相当する。
(2)もし、(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])が入口位置Peでもロボット単位Hでなければ、tuをインクリメントし、位置(void_unit_trj_x[tu-1], void_unit_trj_y[tu-1], void_unit_trj_z[tu-1])にあるロボット単位よりも小さなマンハッタン距離δを持つ隣接のロボット単位の位置を(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])とする。(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])が入口位置Peまたはロボット単位Hの位置になるまで(2)を繰り返す。
もし、(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])が入口位置Pe(Hでなく)の場合は、(3)へ移行する。
もし、(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])がロボット単位Hの位置の場合は、Void_Unit_Trajectory_Decisionを終了する。
(3)ロボット単位Hの位置を(void_unit_trj_rev_x[0], void_unit_trj_rev_y[0], void_unit_trj_rev_z[0])とする。tr←１とし、ロボット単位Hよりも大きなマンハッタン距離δを持つ隣接のロボット単位の位置を(void_unit_trj_rev_x[tr], void_unit_trj_rev_y[tr], void_unit_trj_rev_z[tr])とする。
(4)もし、(void_unit_trj_rev_x[tr], void_unit_trj_rev_y[tr], void_unit_trj_rev_z[tr])が入口位置Peでなければ、trをインクリメントし、位置(void_unit_trj_rev_x[tr-1], void_unit_trj_rev_y[tr-1], void_unit_trj_rev_z[tr-1]) にあるロボット単位よりも大きなマンハッタン距離δを持つ隣接のロボット単位の位置を(void_unit_trj_rev_x[tr], void_unit_trj_rev_y[tr], void_unit_trj_rev_z[tr])とする。(void_unit_trj_rev_x[tr], void_unit_trj_rev_y[tr], void_unit_trj_rev_z[tr])が入口位置Peになるまで(4)を繰り返す。
もし、(void_unit_trj_rev_x[tr], void_unit_trj_rev_y[tr], void_unit_trj_rev_z[tr])が入口位置Peの場合は、(5)へ移行する。
(5)it=1,…,trとして、(void_unit_trj_x[tu+it], void_unit_trj_y[tu+it], void_unit_trj_z[tu+it])←(void_unit_trj_rev_x[tr-it], void_unit_trj_rev_y[tr-it], void_unit_trj_rev_z[tr-it])とする。

この処理により、(void_unit_trj_x[tu], void_unit_trj_y[tu], void_unit_trj_z[tu])(tu=0,1,2,…)にボイドの尾部ロボット単位Tからロボット単位Hまでロボット単位の経路を格納することができる。

[ボイド移動制御]
以下、上記の各ロボット動作決定用動作計画を使用して、ボイドをロボット単位Hの位置から尾部ロボット単位Tの位置に移動させる制御方法を示す。図８に示すように、ロボット単位H内の頭部ロボット単位Dに接する位置のロボットと、ロボット単位T'内の尾部ロボット単位Tに接する位置にあるロボット位置に空白が生じないように、ロボット単位Hでのボイド発生と尾部ロボット単位Tでのボイド追い出しをしていることで、ロボット群全体の接続は保たれている。なお、本実施形態では、ロボット単位Hとロボット単位T'が一致しても構わない。本制御方法で、各ロボット単位内において、ボイドが一つ以内に収まるように制御されている。

以下を目標位置の集合GUが満たされるまで、繰り返す。

[Void_Control]
(1) （iv_prev +1）が全ロボット数に等しくないならば、Void_Trajectory_Decisionを実行する。trj_cnt[iv_prev+1],…, trj_cnt[iv_prev+8]に初期値(例えば-1)を設定する。iv_prevの値を8回インクリメントする。等しいならば（３）へ
(2)以下をvoid[iv_prev]がロボット単位Hの外に出るまで繰り返す。
発生番号の大きいもの（iv=iv_prev）から以下の処理をする。
(2-i)trj_cnt[iv]=-10のとき→void[iv]について何もしない。
(2-ii)trj_cnt[iv]< trj_num[iv](前述の通り、変数trj_numはロボット単位Hから尾部ロボット単位Tまでの軌道内点数を表す) かつtrj_cnt[iv]≧0のとき→
i= trj_cnt[iv]からtrj_num[iv]まで以下を行う：
位置nv(void_trj_x[trj_num[iv]-i-1],void_trj_y[trj_num[iv]-i-1], void_trj_z[trj_num[iv]-i-1])が含まれるロボット単位Nvに他のvoidがないこと、自分より発生番号の小さいvoidが、すべてロボット単位Nvよりもマンハッタン距離δの大きいロボット単位に所属していることを確認する。確認できたなら、nvの位置にあるロボットを位置(void_trj_x[trj_num[iv]-i],void_trj_y[trj_num[iv]-i],void_trj_z[trj_num[iv]-i])に移動させる。trj_cnt[iv]をインクリメントする。確認できなくなるまで処理を繰り返し、確認できないならなにもせず、ループ終了する。
(2-iii)trj_cnt[iv]＝trj_num[iv]のとき→
現在のvoid[iv]の位置から、aｔの方向の逆方向に隣接していて、たどれるロボットをすべて、aｔ方向に1ステップ移動させて、trj_cnt[iv]←‐10とする。
(2-iv)trj_cnt[iv]=‐1のとき→
ロボット単位Hが８つのロボットで満たされているならば、void[k]の発生位置にあるロボットと、そのロボットに行動ahの方向で隣接しているロボット、さらにその先にahの方向で隣接しているロボットがあればそのロボットを皆まとめて、ahの方向に1ステップ移動させる。trj_cnt[iv]←0とする。満たされてない場合なにもしない。
(3)発生番号の大きいもの（iv=iv_prev）から以下の処理をする。
(3-i)trj_cnt[iv]=-10のとき→void[iv]について何もしない。
(3-ii)trj_cnt[iv]< trj_num[iv](前述の通り、変数trj_numはロボット単位Hから尾部ロボット単位Tまでの軌道内点数を表す) かつtrj_cnt[iv]≧0のとき→
i= trj_cnt[iv]からtrj_num[iv]まで以下を行う：
位置nv(void_trj_x[trj_num[iv]-i-1],void_trj_y[trj_num[iv]-i-1], void_trj_z[trj_num[iv]-i-1])が含まれるロボット単位Nvに他のvoidがないこと、自分より発生番号の小さいvoidが、すべてロボット単位Nvよりもマンハッタン距離δの大きいロボット単位に所属していることを確認する。確認できたなら、nvの位置にあるロボットを位置(void_trj_x[trj_num[iv]-i],void_trj_y[trj_num[iv]-i],void_trj_z[trj_num[iv]-i])に移動させる。trj_cnt[iv]をインクリメントする。確認できなくなるまで処理を繰り返し、確認できないならなにもせず、ループ終了する。
(3-iii)trj_cnt[iv]＝trj_num[iv]のとき→
現在のvoid[iv]の位置から、aｔの方向の逆方向に隣接していて、たどれるロボットをすべて、aｔ方向に1ステップ移動させて、trj_cnt[iv]←‐10とする。

以上の手法によるロボットの隊列形成は、2個以上のロボット単位で構成される任意の形状の開始位置の集合SU、目標位置の集合GUのロボット群に適用可能である。

[ヘテロジニアス隊列制御]
しかしながら、上記隊列制御では、各ロボットを同一の属性とみなした場合のホモジニアス（Homogeneous）隊列制御までしか完了しておらず、各ロボットの個々の属性を別のものとして扱うヘテロジニアス隊列制御は、完了していない。ここで、別の言い方をすると、ヘテロジニアス隊列制御とは、各ロボットiをそれぞれ目標位置（Xre[i],Yre[i],Zre[i]）に到達させる隊列制御のことである。ヘテロジニアス隊列制御を完了させるためには、以下に述べる各ロボット位置の入れ替え制御を行う必要がある。

[ロボット位置入れ替え制御]
前述のホモジニアス隊列制御が完了している時点（ロボットがなす全体の形状は目標位置の集合Gと同一であるが、各ロボットの位置が目標位置に到達している保証はない状態）における各ロボットjの位置を（Xrh[j],Yrh[j],Zrh[j]）とすると、ロボット位置の入れ替え制御とは、（Xrh[j],Yrh[j],Zrh[j]）= （Xre[i],Yre[i],Zre[i]）となるiとjの組を特定し、ロボットiを現状のロボットjの位置に移動させることである。すなわち、ロボットiの移動先ロボット位置をd(i)としたとき、d(i)=jであり、すべてのロボットiを現状のロボットd(i)の位置に移動させていく制御に他ならない。

このロボット位置入れ替え制御において重要なことは、前述のホモジニアス隊列制御においてロボットが占有可能であった領域以外の領域にロボットを存在させることなく、各ロボット位置入れ替えを行わなければならないことである。すなわち、本ロボット位置入れ替え制御において、開始位置の集合G、パスP、目標位置の集合Gを合わせた領域以外にロボットを移動させてはならない。

本ロボット位置入れ替え制御を開始する時点では、ロボット全体でGと同じ形状をなしている。ここで、GはPと接する一つのロボット単位の面を必ず持つので、その面をSsとする。また、Ssに接するP内のロボット単位位置をMsとする（図１３参照）。GとMsのみで占められる空間内ですべてのロボットの位置を入れ替えることができれば、ロボット位置入れ替え制御は、どのようなホモジニアスロボット隊列変形制御後にも適用可能であるといえる。もちろん、ロボット単位位置Ms以外にも目標位置の集合Gに接していて障害物等に占有されていないオープンな空間が一ロボット単位分確保されている場合（以下、このようなオープンなロボット単位位置をMopenという）は、ロボット単位位置Ms以外のロボット単位位置Mopenと目標位置の集合Gで占められる空間内でロボット位置入れ替え制御を行ってもよい。

各ロボットiが現状のロボットiの位置からロボットd(i)の位置へ移動し、ロボットiに追い出されたロボットd(i)がロボットiが元あった位置まで移動する動作を一つの単位とし、各個ロボット位置入れ替え制御とする。

ロボット位置入れ替え制御は、初めに任意のロボットiを選択し、ロボットiをロボットd(i)の位置と入れ替える各個ロボット位置入れ替え制御を実行し、その後にロボットd(i)をロボットd(d(i))の位置と入れ替える各個ロボット位置入れ替え制御を実行し、その後にロボットd(d(i))をロボットd(d(d(i)))の位置と入れ替える各個ロボット位置入れ替え制御を実行するという形で各個ロボット位置入れ替え制御を繰り返し実行することで行われる。この繰り返しは、各個ロボット位置入れ替え動作を開始する時点で、i＝d(d(d(d(…d(i)))))となるまで行われる。そして、この各個ロボット位置入れ替え制御の繰り返しを、位置の入れ替えの済んでいないロボットがなくなるまで、さらに繰り返して実行することで、ロボット位置入れ替え制御は完了する。図１４に示すロボット位置入れ替え制御処理手順Robot_Position_Permutationは以下の通りである。

［Robot_Position_Permutation］
(1)すべてのロボットiについて変数p(i)=0とする（pは、ロボット位置入れ替え済み判定フラグである）。tp=0とし、Record_Historyを実行する（ただし、tpは時刻カウンタとする）。
(2)i=d(i)となるすべてのロボットiについて、p(i)=1とする。
(3)すべてのiについてp(i)=1となるまで、(4)〜(6)を繰り返す。
(4)p(i)=0である最小のiを選択する。
(5)i=d(i)でないうちは、(6)を繰り返す。
(6)入れ替え元のロボットorigin←i, 入れ替え先のロボットdestination←d(i)とし、Each_Robot_Position_Exchangeを実行する。

Record_Historyについては、後述するが、時刻カウンタがtpである時点（現時点）における各ロボットiの位置を記録する処理である。ここでは、tp=0における各ロボットiの位置、つまり初期状態を記録している。

また、Each_Robot_Position_Exchangeについても、後述するが、各個ロボット位置入れ替え制御を行う処理である。

[各個ロボット位置入れ替え制御]
各個ロボット位置入れ替え制御において重要なことは、入れ替え前後において入れ替え元のロボットoriginの位置と入れ替え先のロボットdestinationの位置が入れ替わる以外、その他のロボットの位置は変化せず保持されることが保証されていることである。

このような動作を保証するために、各個ロボット位置入れ替え制御においては、まず、はじめにG内に二つのボイドを発生させ、その後、発生させた二つのボイドを移動させつつ二つのボイドによって供給される空隙空間を使用してロボットの位置入れ替えを行う。また、二つのボイドを発生させるために、G内にある二つのロボットをGの外に追い出す制御を行う。

このような各個ロボット位置入れ替え制御処理手順Each_Robot_Position_Exchangeは、以下の通りである。

[Each_Robot_Position_Exchange]
(1)Calculate_Path_From_Origin_To_Destinationを実行する。
(2)Void_Generationを実行する。
(3)Robot_Exchangeを実行する。
(4)Void_UnGenerationを実行する。

(1)のCalculate_Path_From_Origin_To_Destinationでは、G内におけるロボットoriginの位置からロボットdestinationの位置までをつなぐ経路を計算する（以下、入れ替え経路生成という）。(2)のVoid_Generationでは、G内に二つのボイドを生成する（以下、ボイド生成という）。(3)のRobot_Exchangeでは、ロボットoriginの位置とロボットdestinationの位置の入れ替えを行う（以下、入れ替え制御という）。(4)のVoid_UnGenerationでは、ボイド生成にてG外に追い出された二つのロボットをG内の元の位置に戻す（以下、ボイド消去という）。

[入れ替え経路生成]
入れ替え経路生成処理手順Calculate_Path_From_Origin_To_Destinationは、以下の通りである。

[Calculate_Path_From_Origin_To_Destination]
(1) Calculate_Perm_Manhattanを実行する。
(2) Calculate_Perm_Pathを実行する。
(3) Divide_Pathを実行する。

以下、Calculate_Perm_Manhattan、Calculate_Perm_Path、Divide_Pathの３つの処理手順について説明する。まず、Calculate_Perm_Manhattanについて説明する。

Calculate_Perm_Manhattan は、ロボットdestinationからの各ロボットiのマンハッタン距離δp[i]を計算するものである。

[Calculate_Perm_Manhattan]
(1)G内にある各ロボットiの位置（Xr[i],Yr[i],Zr[i]）において、next_p[a][i]を用意し、隣接する行動aの方向に他のロボットがある場合には、next_p[a][i]←1とし、それ以外の場合はnext_p[a][i]←0とする。
(2)各ロボットiの位置のマンハッタン距離δp[i]をG内の格子数より大きな値s_maxに初期化する。
(3)ロボットdestinationの位置のマンハッタン距離δp[destination]を0に初期化する。
(4)すべてのロボットiと行動aについて、next_p[a][i]の値が0ではない場合の、行動aによってロボットiの位置から移動した先の位置にあるロボットjのマンハッタン距離δp[j]を調べ、その最小値に１を加えた値が、現在のδp[i]よりも小さい場合は、その値をδp[i]に代入する。
(5)上述の(4)の処理にて、δp[i]値の更新がなくなるまで、(4)を繰り返す。

次に、Calculate_Perm_Pathについて説明する。Calculate_Perm_Pathは、ロボットoriginからロボットdestinationに至るパスを計算するものである。

[Calculate_Perm_Path]
(1)ロボットoriginからロボットdestinationに至るパスに含まれるロボット数を格納する変数をtrj_numとし、値を0に初期化する。パス中のロボット番号を格納する変数をp_trj[t](t=0…)とし、p_trj[0]=originとする。また、t=0とする。
(2) p_trj[t]＝destinationでないとき、ロボットp_trj[t]から各行動aによって移動した先にある隣接ロボットj’のうち、δp[j’]の値が、δp[t]‐1となるロボットj’を選択し、p_trj[t+1]=j’とする。ロボットｊ’に至る行動をa’として、a_trj[t]=a’とする。ｔをインクリメントする。
(3) p_trj[t]＝destination となるまで、(2)を繰り返す。p_trj[t]＝destinationのとき、trj_num=t+1、a_trj[trj_num-1]=a_trj[trj_num-2]として終了する。

最後に、Divide_Pathについて説明する。Divide_Pathは、Calculate_Perm_Pathで計算したパスを直線成分ごとに分割する（図１５参照）。なお、図１５では、線要素1、線要素2、角要素1、角要素2、角要素3の記号は省略している。

[Divide_Path]
(1)直線パスの格納変数path_perm_x[t],path_perm_y[t], path_perm_z[t]とする。また、パス中の角の番号およびその数を格納する変数をそれぞれcorner[],corner_numとする。path_perm_x[0]←Xr[p_trj[0]]、path_perm_y[0]←Yr[p_trj[0]]、path_perm_z[0]←Zr[p_trj[0]]とし、perm_num←1 、t←0、corner_num←0とする。t<trj_num-1の間、(2)を繰り返す。
(2) a_trj[t]= a_trj[t+1]のとき、path_perm_x[perm_num]←Xr[p_trj[t+1]]、path_perm_y[perm_num]←Yr[p_trj[t+1]]、path_perm_z[perm_num]←Zr[p_trj[t+1]]とし、perm_num、ｔをインクリメントする。そうでないとき、corner[corner_num]←perm_numとし、corner_numをインクリメントする。tをインクリメントしてから、path_perm_x[perm_num]←Xr[p_trj[t+1]]、path_perm_y[perm_num]←Yr[p_trj[t+1]]、path_perm_z[perm_num]←Zr[p_trj[t+1]]とし、perm_numをインクリメントする。

[ボイド生成]
生成した二つのボイド位置を(void_perm_x[0], void_perm_y[0], void_perm_z[0])、(void_perm_x[1], void_perm_y[1], void_perm_z[1])とし、ボイドを生成した結果G外に出されたロボットをロボットout_0, out_1とする。

以下、G内に二つのボイドを生成するボイド生成処理手順Void_Generationについて説明する。Void_Generationは、後述するMove_ModuleとRecord_Historyを用いて実現される。Move_Module(i, vx, vy, vz)はロボットiをX方向、Y方向、Z方向のうち１つの方向に所定の量（vx, vy, vz）だけ移動させるものであり、Record_Historyは、先述した通り、時刻カウンタtpにおける各ロボットiの位置を記録するものである。

[Void_Generation]
(1)G内の他のロボット単位位置もしくは障害物に接していないロボット単位の面を一つ選択し、Ssとする。Ssを持つロボット単位をMsとする。Ssに接するGに属する（以下、簡単のため、Ssに属するということにする）４つのロボットをim[0], im[1], im[2], im[3]とする。Ssが向いている方向（ロボット単位位置Msの中から外に向くSsの法線の方向）をasとする。なお、asは1〜6の値を取り、その意味は［ロボット動作の定義］に準ずるものとする。例えば、as=1は、三次元空間内でX軸正方向を意味するものとする。 (void_perm_x[0], void_perm_y[0], void_perm_z[0])、(void_perm_x[1], void_perm_y[1], void_perm_z[1])をG外の値とする。t_void←0とする。なお、t_voidは、後述するUpdate_Void_Navigationで使用される変数である。
(2)Ssに属するロボットim[i’]のうち、ロボットim[i’]から方向asと逆向きに接する位置にあるロボットをjm[i’]としたとき、ロボットim[i’],jm[i’]のどちらもがパス(path_perm_x[t], path_perm_y[t], path_perm_z[t]）に含まれないものを選び、ロボットout_0,out_1とする。ロボットout_0,out_1の位置を変数(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0]), (void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])に格納する。
(3)asの値に従って以下の制御をロボットout_0,out_1に対して行う。
as=1のとき:
(3.1)Move_Module(out_0, 1, 0, 0)、Move_Module(out_1, 1, 0, 0)を実行し、Record_Historyを実行する。
(3.2)If Zr[out_0] % 2 = 1,
Then, Move_Module(out_0, 0, 0, -1)を実行する。
Else, Move_Module(out_0, 0, 0, 1)を実行する。
(3.3)Record_Historyを実行する。
(3.4)Move_Module(out_1, 1, 0, 0)を実行する。
as=2のとき:
(3.1)Move_Module(out_0, 0, 1, 0)、Move_Module(out_1, 0, 1, 0)を実行し、Record_Historyを実行する。
(3.2)If Xr[out_0] % 2 = 1,
Then, Move_Module(out_0, -1, 0, 0)を実行する。
Else, Move_Module(out_0, 1, 0, 0)を実行する。
(3.3)Record_Historyを実行する。
(3.4)Move_Module(out_1, 0, 1, 0) を実行する。
as=3のとき:
(3.1)Move_Module(out_0, -1, 0, 0)、Move_Module(out_1, -1, 0, 0)を実行し、Record_Historyを実行する。
(3.2)If Zr[out_0] % 2 = 1,
Then, Move_Module(out_0, 0, 0, -1)を実行する。
Else, Move_Module(out_0, 0, 0, 1)を実行する。
(3.3)Record_Historyを実行する。
(3.4)Move_Module(out_1, -1, 0, 0)を実行する。
as=4のとき:
(3.1)Move_Module(out_0, 0, -1, 0)、Move_Module(out_1, 0, -1, 0)を実行し、Record_Historyを実行する。
(3.2)If Xr[out_0] % 2 == 1,
Then, Move_Module(out_0, -1, 0, 0)を実行する。
Else, Move_Module(out_0, 1, 0, 0)を実行する。
(3.3)Record_Historyを実行する。
(3.4)Move_Module(out_1, 0, -1, 0)を実行する。
as=5のとき:
(3.1)Move_Module(out_0, 0, 0, 1)、Move_Module(out_1, 0, 0, 1)を実行し、Record_Historyを実行する。
(3.2)If Yr[out_0] % 2 == 1,
Then, Move_Module(out_0, 0, -1, 0)を実行する。
Else, Move_Module(out_0, 0, 1, 0)を実行する。
(3.3)Record_Historyを実行する。
(3.4)Move_Module(out_1, 0, 0, 1)を実行する。
as=6のとき:
(3.1)Move_Module(out_0, 0, 0, -1)、Move_Module(out_1, 0, 0, -1)を実行し、Record_Historyを実行する。
(3.2)If Yr[out_0] % 2 == 1,
Then, Move_Module(out_0, 0, -1, 0)を実行する。
Else, Move_Module(out_0, 0, 1, 0)を実行する。
(3.3)Record_Historyを実行する。
(3.4)Move_Module(out_1, 0, 0, -1)を実行する。
(4)Record_Historyを実行する。
(5)(void_perm_x[0], void_perm_y[0], void_perm_z[0])、(void_perm_x[1], void_perm_y[1], void_perm_z[1])に(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0]), (void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])の値を格納する。つまり、生成する二つのボイドの位置は、(2)で特定したロボットout_0,out_1の位置と一致する。
(6)ロボットout_0の位置を(escape_cube_buffer_x[0], escape_cube_buffer_y[0], escape_cube_buffer_z[0])に、ロボットout_1の位置を(escape_cube_buffer_x[1], escape_cube_buffer_y[1], escape_cube_buffer_z[1])に、バッファリングする。つまり、(escape_cube_buffer_x[0], escape_cube_buffer_y[0], escape_cube_buffer_z[0])、(escape_cube_buffer_x[1], escape_cube_buffer_y[1], escape_cube_buffer_z[1])には、ロボットout_0, out_1を待避させた位置が格納される。

[Move_Module(i, vx, vy, vz)]
(1)ロボットiをX方向にvx,Y方向にvy,Z方向にvzだけ移動させる。ただし、vx,vy,vzのうち一つの成分のみ非零値とする。
(2)二つのボイドの位置(void_perm_x[0], void_perm_y[0], void_perm_z[0])、(void_perm_x[1], void_perm_y[1], void_perm_z[1])がG内の値であるとき、ロボットiの移動に伴い、対応するボイドの位置を移動させる。

[Record_History]
(1)(permutation_x[tp][i], permutation_y[tp][i], permutation_z[tp][i])に各ロボットiの現時点の位置を格納し、tpをインクリメントする。
(2)movable_num←0とする。なお、movable_numは、後述するAdd_Movableで使用される変数である。

[入れ替え制御]
ロボットoriginの位置とロボットdestinationの位置を入れ替える制御は、
(α)ロボットoriginをロボットdestinationの位置まで移動させる。
(β)ロボットdestinationをロボットoriginの元の位置まで移動させる。
の二つのステップからなる。(α)の移動に伴い、ロボットoriginとロボットdestination以外のロボットも移動を余儀なくされ、それらのロボットの位置も変化してしまうが、(β)の移動に伴ってそれらの位置はすべて元に戻される。

先述の通り、Divide_Pathによって、分割されたパスは、図１５に示すようになっている。p_trj[t]に格納されているのは、ロボットoriginとロボットdestinationをつなぐパスに含まれるt番目のロボットの番号である。また、Divide_Pathによりパスから角にあるロボットを取り除いた、パスのｔ番目の位置が(path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されている。corner[j]は、j+1番目の直線部分の開始点番号となっている。

（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置をcorner[j]等を用いて線要素、最終線要素、角要素に分けることができる。

（線要素j）
j=0のとき、t=0〜corner[0]-1の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置。
0< j< corner_numのとき、t = corner[j-1]〜corner[j]-1の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置。
j=corner_numかつcorner[j] < perm_num-2のとき、t=corner[j-1]〜perm_num-2の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置。

（最終線要素）
t = perm_num-2〜perm_num-1の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置

（角要素j）
t=corner[j]-1〜corner[j]の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置

以下、第１の入れ替え制御処理手順Robot_Exchange_1、第２の入れ替え制御処理手順Robot_Exchange_2について説明する。まず、Robot_Exchange_1について説明する。Robot_Exchange_1は、先述の(α)ロボットoriginをロボットdestinationの位置まで移動させる制御を実現するものであり、ロボットoriginをそれが存在する線要素、角要素、最終線要素において、それぞれの要素の終点位置にあるロボットとロボットoriginの位置を入れ替えていくことでなされる。実際の制御方法は、以下の通りである。

[Robot_Exchange_1]
(1)i_l, t←0とし、カウンタi_c=0〜corner_num-1のすべてのi_cの値について、以下の制御を実行する。
If t != corner[i_c]-1,
Then, (角要素の始点にロボットoriginがない)
Line_Perm(i_l, 0)を実行し、線要素i_lの始点と終点の位置にあるロボットの位
置を入れ替える。
Corner_Perm(i_c, 0) を実行し、角要素i_cの始点と終点の位置にあるロボットの位
置を入れ替える。t←corner[i_c]とする。i_lをインクリメントする。
Else, (すでに角要素の始点にロボットoriginがある)
Corner_Perm(i_c, 0)を実行し、角要素i_cの始点と終点の位置にあるロボットの
位置を入れ替える。t←corner[i_c]とする。
//最後の部分は残り2マスだけ分離して入れ替える
(2)以下の制御を実行する。
If corner[corner_num-1] != perm_num-1,
Then, If corner[corner_num-1] != perm_num-2,
Then,
Line_Perm(i_l, 0)を実行する。
Last_Line_Permを実行し最終線要素の始点と終点の位置にあるロボットの位置
を入れ替える。
Else,
Last_Line_Permを実行し最終線要素の始点と終点の位置にあるロボットの位置
を入れ替える。

なお、Line_Perm及びCorner_Permの第２引数はいずれも0となっているが、これはフラグ(_flg)を示すものであり、Robot_Exchange_2の中でLine_Perm及びCorner_Permが呼び出されるときは、1に設定される。

次に、Robot_Exchange_2について説明する。Robot_Exchange_2は、先述の(β)ロボットdestinationをロボットoriginが元あった位置まで移動させる制御を実現するものであり、ロボットdestinationをそれが存在する線要素、角要素において、それぞれの要素の始点位置にあるロボットとロボットdestinationの位置を入れ替えていくことでなされる。実際の制御方法は、以下の通りである。

[Robot_Exchange_2]
(1) If corner[corner_num-1] != perm_num-1 && corner[corner_num-1] != perm_num-2, Then, Line_Perm(i_l, 1)を実行する。i_lをデクリメントする。
(2) カウンタi_c=corner_num-1〜0のすべてのi_cの値について、以下の制御を実行する。
(2.1)If i_c > 0 Then,
If corner[i_c-1] != corner[i_c]-1,
Then, (角要素の終点にロボットdestinationがない)
If perm_num-1 > corner[i_c], Then,
Corner_Perm(i_c, 1)を実行する。
Line_Perm(i_l, 1)を実行する。
t←corner[i_c]とし、i_lをデクリメントする。
Else,
If perm_num-1 > corner[i_c],
Then, (角要素が最終の要素の場合、帰りに入れ替えはしない。)
Corner_Perm(i_c, 1)を実行する。
t←corner[i_c]とする。
Else,
If 0 != corner[i_c]-1, Then,
If perm_num-1 > corner[i_c], Then,
Corner_Perm(0, 1)を実行する。
Line_Perm(0, 1)を実行する。
Else,
If perm_num-1 > corner[i_c], Then,
Corner_Perm(0, 1) を実行する。

以上を統合して、全ロボットの位置を入れ替えていくための入れ替え制御処理手順Robot_Exchangeは以下のように処理される。

[Robot_Exchange]
(1)Robot_Exchange_1を実行する。
(2)Robot_Exchange_2を実行する。

Robot_Exchange_1やRobot_Exchange_2では、線要素や角要素でのロボット位置入れ替えにLine_PermやCorner_Permを利用した。以下、[線要素でのロボット位置入れ替え]及び[角要素でのロボット位置入れ替え]でその詳細について説明する。

[線要素でのロボット位置入れ替え]
線要素におけるロボット位置の入れ替えは、線要素に隣接するロボットをバディロボット（以下、単にバディともいう）として使用して図１６に示すように行われる。バディは、線要素がX軸方向向きのときはY方向の隣接ロボットを、線要素がY軸方向向きのときはZ方向の隣接ロボットを、線要素がZ軸方向向きのときはX方向の隣接ロボットを線要素内のロボット数から２つ少ない数使用する。

線要素内でのロボット位置の入れ替え開始時にはまず、二つのボイドの位置を線要素開始位置と線要素の二つ目の位置に隣接する位置に移動させる。二つのボイドの位置もバディと同様に、線要素がX軸方向向きのときはY方向の隣接位置を、線要素がY軸方向向きのときはZ方向の隣接位置を、線要素がZ軸方向向きのときはX方向の隣接位置を２つ使用する。線要素内ロボット、ボイド、バディロボットはいずれも同一平面内に存在する。（つまり、線要素がX軸方向のときはXY面内、線要素がY軸方向のときはYZ面内、線要素がZ軸方向のときはZX面内に存在する。）

線要素内のロボット数をnm+2とすると、2*nm+6回の入れ替え動作で、始点位置と終点位置にあるロボット位置の入れ替えが終了する。最終線要素におけるロボット位置の入れ替えは、nm=0の場合であり、同様に行われる。この入れ替え動作を一度行うと、バディ位置にあったロボットの位置と始点、終点以外の線要素内ロボットの位置が入れ替わってしまうが、入れ替え動作をもう一度行うことで、これらのロボット位置はもとの位置に戻る。すなわち、ロボットoriginがロボットdestinationの位置まで移動する(α)の制御と、ロボットoriginがロボットdestinationの位置まで移動する(β)の制御において、一度ずつ同一の線要素での入れ替え動作が行われるので、(α)、(β)の制御を通じて、始点位置と終点位置以外にあるロボットの位置に変更はないことになる。なお、nm=0の場合、すなわち最終線要素の入れ替えの場合においては、最終線要素での入れ替え制御は(α)でしか行われないが、もともと始点と終点以外のロボット位置以外はボイド位置しかないので問題ない。

線要素での入れ替え制御の処理は以下の通りである。
[Line_Perm(i_l, _flg)]
(1)線要素i_lの始点位置番号を_s, 終点位置番号を_gとし、Line_Perm_By_Position(_s, _g, _flg)を実行する。

[Last_Line_Perm]
(1) _flg=0として、Line_Perm_By_Position(perm_num-2 perm_num-1, _flg)を実行する。

[Line_Perm_By_Position(_s, _g, _flg)]
(1)線要素の向きがX方向のときdirection←1とし、線要素の向きがY方向のときdirection←2とし、線要素の向きがZ方向のときdirection←3とする。
(2)入れ替え動作開始時の二つのボイド目標位置 (l_x[0], l_y[0], l_z[0])← (path_perm_x[_s], path_perm_y[_s], path_perm_z[_s])、(l_x[1], l_y[1], l_z[1])← (path_perm_x[_s+1], path_perm_y[_s+1], path_perm_z[_s+1])とする。
(3)directionの値により以下を実行する（線要素の向きによるボイド目標位置設定に相当する）。
(3.1)direction=1 のとき：
If l_y[1]が奇数, Then, l_y[0], l_y[1]をデクリメントする。_p_l←-1とする。
Else, l_y[0], l_y[1]をインクリメントする。_p_l←1とする。
(3.2)direction=2 のとき：
If l_z[1]が奇数, Then, l_z[0], l_z[1]をデクリメントする。_p_l←-1とする。
Else, l_z[0], l_z[1]をインクリメントする。_p_l←1とする。
(3.3)direction=3 のとき：
If l_x[1]が奇数, Then, l_x[0], l_x[1]をデクリメントする。_p_l←-1とする。
Else, l_x[0], l_x[1]をインクリメントする。_p_l←1とする。
(4)If _flg=0, Then,
Navigate_Void(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])を実行する。
Else,
Reverse_Void_To(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])を実行する。
(5)Before_Care(_s, _g, direction, _p_l)を実行する。
(6)Rotation_in_Line(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], direction, _p_l)を実行する。

なお、Navigate_Void, Reverse_Void_Toについては［ボイドの制御］にて、Before_Careについては［待避ロボットの制御］にて後述する。

[Rotation_in_Line(_s, _g, _x0, _y0, _z0, _x1, _y1, _z1, _d, _p_l)]
(1)index_rot[r](r=0,1,…,2*nm+1)を線要素内ロボットとバディロボットに設定する通し番号とし（図１６参照）、r=nm〜2*nm+1において、index_rot[r]の値を(path_perm_x[_s + (r - nm)], path_perm_y[_s + (r - nm)], path_perm_z[_s + (r - nm)])にあるロボットの番号に設定する(線要素内ロボット通し番号に相当する)。
(2)_vx←0, _vy←0, _vz←0とし、_d=1のときは、_vyに_p_lの値を加算する。_d=2のときは、_vzに_p_lの値を加算する。_d=3のときは、_vxに_p_lの値を加算する。
(3) r=0〜nm-1において、index_rot[r]の値を(path_perm_x[_g - r] + _vx, path_perm_y[_g - r] + _vy, path_perm_z[_g - r] + _vz)にあるロボットの番号に設定する(バディロボット通し番号に相当する)。
(4) _d=1のときは、Rotation_X_Line(_p_l)を実行する。_d=2のときは、Rotation_Y_Line(_p_l)を実行する。_d=3のときは、Rotation_Z_Line(_p_l)を実行する。

以下、Rotation_X_Line、Rotation_Y_Line、Rotation_Z_Lineについて、図１６を参照しながら、説明する。なお、Add_MovableとEvacuating_Cube_Controlは、Move_Moduleで実行しようとしているロボット移動が二つの待避ロボットout_0、out_1の他のロボットからのディスコネクションを引き起こさないかチェックし、引き起こす場合には待避ロボットの位置を変更して、ディスコネクションを回避する機能を実現するものである。詳細は[待避ロボットの制御]で説明する。

[Rotation_X_Line(_p_l)]
(1)線要素が正方向のとき_g_p_l←1とする。線要素が負方向のとき_g_p_l←-1とする。
(2)Add_Movable(index_rot[nm], 0, _p_l, 0)を実行する。Evacuating_Cube_Controlを実行する。
(3)Move_Module(index_rot[nm], 0, _p_l, 0)を実行する。Record_Historyを実行する(図１６動作(a))。
(4)r=nm+1〜2*nm+1のすべてのrについて、Add_Movable(index_rot[r], -_g_p_l, 0, 0)を実行する。その後、Evacuating_Cube_Controlを実行する。
(5)r=nm+1〜2*nm+1のすべてのrについてMove_Module(index_rot[r], -_g_p_l, 0, 0)を実行する。Record_Historyを実行する(図１６動作(b))。
(6)Add_Movable(index_rot[nm], _g_p_l, 0, 0)を実行する。Evacuating_Cube_Controlを実行する。
(7)Move_Module(index_rot[nm], _g_p_l, 0, 0)を実行する。Record_Historyを実行する(図１６動作(c))。
(8)カウンタj=1〜nmまで以下を繰り返す。
(8.1)Add_Movable(index_rot[nm+j], 0, _p_l, 0)を実行する。
Add_Movable(index_rot[j-1], 0, -_p_l, 0)を実行する。
Evacuating_Cube_Controlを実行する。
(8.2)Move_Module(index_rot[nm+j], 0, _p_l, 0)を実行する。
Move_Module(index_rot[j-1], 0, -_p_l, 0)を実行する。
Record_Historyを実行する(図１６動作(d))。
(8.3)r=nm+j+1〜2*nm+1のすべてのrにおいて、
Add_Movable(index_rot[r], -_g_p_l, 0, 0)を実行する。
r=0〜j-1のすべてのrにおいて、
Add_Movable(index_rot[r], -_g_p_l, 0, 0)を実行する。
r=j〜nm+jのすべてのrにおいて、
Add_Movable(index_rot[r], _g_p_l, 0, 0)を実行する。
Evacuating_Cube_Control(movable_num)を実行する。
(8.4)r=nm+j+1〜2*nm+1のすべてのrにおいて
Move_Module(index_rot[r], -_g_p_l, 0, 0)を実行する。
r=0〜j-1のすべてのrにおいて
Move_Module(index_rot[r], -_g_p_l, 0, 0) を実行する。
r=j〜nm+jのすべてのrにおいて
Move_Module(index_rot[r], _g_p_l, 0, 0) を実行する。
Record_Historyを実行する(図１６動作(e))。
(9)Add_Movable(index_rot[nm], 0, -_p_l, 0)を実行する。Evacuating_Cube_Controlを実行する。
(10)Move_Module(index_rot[nm], 0, -_p_l, 0)を実行する。Record_Historyを実行する(図１６動作(f))。
(11)r=nm+1〜2*nmのすべてのrにおいて、Add_Movable(index_rot[r], _g_p_l, 0, 0)を実行したのち、Evacuating_Cube_Controlを実行する。
(12)r=nm+1〜2*nmのすべてのrにおいて、Move_Module(index_rot[r], _g_p_l, 0, 0)を実行した後、Record_Historyを実行する(図１６動作(g))。

[Rotation_Y_Line(_p_l)]
(1)線要素が正方向のとき_g_p_l←1とする。線要素が負方向のとき_g_p_l←-1とする。
(2)Add_Movable(index_rot[nm], 0, 0, _p_l)を実行する。Evacuating_Cube_Controlを実行する。
(3)Move_Module(index_rot[nm], 0, 0, _p_l)を実行する。Record_Historyを実行する(図１６動作(a))。
(4)r=nm+1〜2*nm+1のすべてのrについて、Add_Movable(index_rot[r], 0, -_g_p_l, 0)を実行する。その後、Evacuating_Cube_Controlを実行する。
(5)r=nm+1〜2*nm+1のすべてのrについてMove_Module(index_rot[r], 0, -_g_p_l, 0)を実行する。Record_Historyを実行する(図１６動作(b))。
(6)Add_Movable(index_rot[nm], 0, _g_p_l, 0)を実行する。Evacuating_Cube_Controlを実行する。
(7)Move_Module(index_rot[nm], 0, _g_p_l, 0)を実行する。Record_Historyを実行する(図１６動作(c))。
(8)カウンタj=1〜nmまで以下を繰り返す。
(8.1)Add_Movable(index_rot[nm+j], 0, 0, _p_l)を実行する。
Add_Movable(index_rot[j-1], 0, 0, -_p_l)を実行する。
Evacuating_Cube_Controlを実行する。
(8.2)Move_Module(index_rot[nm+j], 0, 0, _p_l)を実行する。
Move_Module(index_rot[j-1], 0, 0, -_p_l)を実行する。
Record_Historyを実行する(図１６動作(d))。
(8.3)r=nm+j+1〜2*nm+1のすべてのrにおいて、
Add_Movable(index_rot[r], 0, -_g_p_l, 0)を実行する。
r=0〜j-1のすべてのrにおいて、
Add_Movable(index_rot[r], 0, -_g_p_l, 0)を実行する。
r=j〜nm+jのすべてのrにおいて、
Add_Movable(index_rot[r], 0, _g_p_l, 0)を実行する。
Evacuating_Cube_Control(movable_num)を実行する。
(8.4)r=nm+j+1〜2*nm+1 のすべてのrにおいて、
Move_Module(index_rot[r], 0, -_g_p_l, 0)を実行する。
r=0〜j-1のすべてのrにおいて、
Move_Module(index_rot[r], 0, -_g_p_l, 0)を実行する。
r=j〜nm+jのすべてのrにおいて、
Move_Module(index_rot[r], 0, _g_p_l, 0)を実行する。
Record_Historyを実行する。(図１６動作(e))。
(9)Add_Movable(index_rot[nm], 0, 0, -_p_l)を実行する。Evacuating_Cube_Controlを実行する。
(10)Move_Module(index_rot[nm], 0, 0, -_p_l)を実行する。Record_Historyを実行する(図１６動作(f))。
(11)r=nm+1〜2*nmのすべてのrにおいて、Add_Movable(index_rot[r], 0, _g_p_l, 0)を実行したのち、Evacuating_Cube_Controlを実行する。
(12)r=nm+1〜2*nmのすべてのrにおいて、Move_Module(index_rot[r], 0, _g_p_l, 0) を実行した後、Record_Historyを実行する(図１６動作(g))。

[Rotation_Z_Line(_p_l)]
(1)線要素が正方向のとき_g_p_l←1とする。線要素が負方向のとき_g_p_l←-1とする。
(2)Add_Movable(index_rot[nm], _p_l, 0, 0)を実行する。Evacuating_Cube_Controlを実行する。
(3)Move_Module(index_rot[nm], _p_l, 0, 0)を実行する。Record_Historyを実行する(図１６動作(a))。
(4)r=nm+1〜2*nm+1のすべてのrについて、Add_Movable(index_rot[r], 0, 0, -_g_p_l)を実行する。その後、Evacuating_Cube_Controlを実行する。
(5)r=nm+1〜2*nm+1のすべてのrについて、Move_Module(index_rot[r], 0, 0, -_g_p_l)を実行する。Record_Historyを実行する(図１６動作(b))。
(6)Add_Movable(index_rot[nm], 0, 0, _g_p_l)を実行する。Evacuating_Cube_Controlを実行する。
(7)Move_Module(index_rot[nm], 0, 0, _g_p_l)を実行する。Record_Historyを実行する(図１６動作(c))。
(8)カウンタj=1〜nmまで以下を繰り返す。
(8.1)Add_Movable(index_rot[nm+j], _p_l, 0, 0)を実行する。
Add_Movable(index_rot[j-1], -_p_l, 0, 0)を実行する。
Evacuating_Cube_Controlを実行する。
(8.2)Move_Module(index_rot[nm+j], _p_l, 0, 0)を実行する。
Move_Module(index_rot[j-1], -_p_l, 0, 0)を実行する。
Record_Historyを実行する(図１６動作(d))。
(8.3)r=nm+j+1〜2*nm+1のすべてのrにおいて、
Add_Movable(index_rot[r], 0, 0, -_g_p_l)を実行する。
r=0〜j-1のすべてのrにおいて、
Add_Movable(index_rot[r], 0, 0, -_g_p_l)を実行する。
r=j〜nm+jのすべてのrにおいて、
Add_Movable(index_rot[r], 0, 0, _g_p_l)を実行する。
Evacuating_Cube_Control(movable_num)を実行する。
(8.4)r=nm+j+1〜2*nm+1のすべてのrにおいて、
Move_Module(index_rot[r], 0, 0, -_g_p_l)を実行する。
r=0〜j-1のすべてのrにおいて、
Move_Module(index_rot[r], 0, 0, -_g_p_l)を実行する。
r=j〜nm+jのすべてのrにおいて、
Move_Module(index_rot[r], 0, 0, _g_p_l)を実行する。
Record_Historyを実行する(図１６動作(e))。
(9)Add_Movable(index_rot[nm], -_p_l, 0, 0) を実行する。Evacuating_Cube_Controlを実行する。
(10)Move_Module(index_rot[nm], -_p_l, 0, 0)を実行する。Record_Historyを実行する(図１６動作(f))。
(11)r=nm+1〜2*nmのすべてのrにおいて、Add_Movable(index_rot[r], 0, 0, _g_p_l)を実行したのち、Evacuating_Cube_Controlを実行する。
(12) r=nm+1〜2*nmのすべてのrにおいて、Move_Module(index_rot[r], 0, 0, _g_p_l)を実行した後、Record_Historyを実行する(図１６動作(g))。

[角要素でのロボット位置入れ替え]
角要素でのロボット位置の入れ替えの動作を図１７に示す。角要素に属する二つのロボット位置の両方に接する位置にボイドのうち一つが存在し、もう一つのボイドは角要素に属する二つのロボット位置が属する面に垂直な方向に他方のボイドに接している。角要素の入れ替え動作においては、入れ替え動作の前後で入れ替え対象のロボット以外の位置は変化しない。角要素でのロボット位置入れ替え処理は以下の通りである。

[Corner_Perm(i_c, _flg)]
(1)_s←corner[i_c]-1, _g←corner[i_c]とする。
(2)If path_perm_x[_s] = path_perm_x[_g] ,Then,
_d←1とする。
If path_perm_x[_s]は奇数, Then, _p_l = -1, Else _p_l = 1.
If path_perm_y[_s] = path_perm_y[_g] ,Then,
_d←2とする。
If path_perm_y[_s]は奇数, Then, _p_l = -1, Else _p_l = 1.
If path_perm_z[_s] = path_perm_z[_g] ,Then,
_d←3とする。
If path_perm_z[_s]は奇数, Then, _p_l = -1, Else _p_l = 1.
(3)入れ替え動作開始時の二つのボイド目標位置 (l_x[0], l_y[0], l_z[0]), (l_x[1], l_y[1], l_z[1])について、_dの値の違いにより、以下のように設定する。
_d=1のとき：
(3.1) l_x[0]←path_perm_x[_s]、l_x[1]←l_x[0]+_p_lとする。
(3.2) 位置(l_x[0], path_perm_y[_s], path_perm_z[_g])にロボットが存在しているか、もしくは、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかが存在している場合には、l_y[0]←path_perm_y[_s], l_z[0]←path_perm_z[_g]とする。
(3.3) (3.2)においてl_y[0], l_z[0]の値が設定されなかった場合、位置(l_x[0], path_perm_y[_g], path_perm_z[_s])にロボットが存在しているか、もしくは、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかが存在している場合には、l_y[0]←path_perm_y[_g], l_z[0]←path_perm_z[_s]とする。
(3.4)l_y[1]←l_y[0], l_z[1]←l_z[0]とする。
_d=2のとき：
(3.1) l_y[0]←path_perm_y[_s]、l_y[1]←l_y[0]+_p_lとする。
(3.2) 位置(path_perm_x[_s], l_y[0], path_perm_z[_g])にロボットが存在しているか、もしくは、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかが存在している場合には、l_x[0]←path_perm_x[_s], l_z[0]←path_perm_z[_g]とする。
(3.3) (3.2)においてl_x[0], l_z[0]の値が設定されなかった場合、位置(path_perm_x[_g], l_y[0], path_perm_z[_s])にロボットが存在しているか、もしくは、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかが存在している場合には、 l_x[0]←path_perm_x[_g], l_z[0]←path_perm_z[_s]とする。
(3.4)l_x[1]←l_x[0], l_z[1]←l_z[0]とする。
_d=3のとき：
(3.1) l_z[0]←path_perm_z[_s]、l_z[1]←l_z[0]+_p_lとする。
(3.2) 位置(path_perm_x[_s], path_perm_y[_g], l_z[0])にロボットが存在しているか、もしくは、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかが存在している場合には、l_x[0]←path_perm_x[_s], l_y[0]←path_perm_y[_g]とする。
(3.3) (3.2)においてl_x[0], l_y[0]の値が設定されなかった場合、位置(path_perm_x[_g], path_perm_y[_s], l_z[0])にロボットが存在しているか、もしくは、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかが存在している場合には、l_x[0]←path_perm_x[_g], l_y[0]←path_perm_y[_s]とする。
(3.4)l_x[1]←l_x[0], l_y[1]←l_y[0]とする。
(4)If _flg = 0, Then,
Navigate_Void(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])を実行する。
Else,
Reverse_Void_To(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])を実行する。
(5)At_Corner(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)を実行する。

[At_Corner(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)]
(1)_d=1のとき、At_Corner_X( _s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)を実行する。
_d=2のとき、At_Corner_Y(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)を実行する。
_d=3のとき、At_Corner_Z(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)を実行する。

[At_Corner_X(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)]
(1) 位置(path_perm_x[_s], path_perm_y[_s], path_perm_z[_s])にあるロボットの番号をi_sとし、位置(path_perm_x[_g], path_perm_y[_g], path_perm_z[_g])にあるロボットの番号をi_gとする。(_sx, _sy, _sz)←(Xr[i_s], Yr[i_s], Zr[i_s]), (_gx, _gy, _gz)←(Xr[i_g], Yr[i_g], Zr[i_g])とする。
(2)Add_Movable(i_s, 0, l_y[0]-_sy, l_z[0]-_sz)を実行し、Evacuating_Cube_Controlを実行する。
(3)Move_Module(i_s, 0, l_y[0]-_sy, l_z[0]-_sz)を実行する。Record_Historyを実行する(図１７動作(a))。
(4)Add_Movable(i_s, _p_l, 0, 0)を実行する。Evacuating_Cube_Controlを実行する。
(5)Move_Module(i_s, _p_l, 0, 0)を実行する。Record_Historyを実行する(図１７動作(b))。
(6)Add_Movable(i_g, 0, l_y[0]-_gy, l_z[0]-_gz)を実行する。Evacuating_Cube_Controlを実行する。
(7)Move_Module(i_g, 0, l_y[0]-_gy, l_z[0]-_gz)を実行する。Record_Historyを実行する(図１７動作(c))。
(8)Add_Movable(i_g, 0, _sy-l_y[0], _sz-l_z[0])を実行する。Evacuating_Cube_Controlを実行する。
(9)Move_Module(i_g, 0, _sy-l_y[0], _sz-l_z[0])を実行する。Record_Historyを実行する(図１７動作(d))。
(10)Add_Movable(i_s, -_p_l, 0, 0)を実行する。Evacuating_Cube_Controlを実行する。
(11)Move_Module(i_s, -_p_l, 0, 0)を実行する。Record_Historyを実行する(図１７動作(e))。
(12)Add_Movable(i_s, 0, _gy-l_y[0], _gz-l_z[0])を実行する。Evacuating_Cube_Controlを実行する。
(13)Move_Module(i_s, 0, _gy-l_y[0], _gz-l_z[0])を実行する。Record_Historyを実行する(図１７動作(f))。

[At_Corner_Y(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)]
(1) 位置(path_perm_x[_s], path_perm_y[_s], path_perm_z[_s])にあるロボットの番号をi_sとし、位置(path_perm_x[_g], path_perm_y[_g], path_perm_z[_g])にあるロボットの番号をi_gとする。(_sx, _sy, _sz)←(Xr[i_s], Yr[i_s], Zr[i_s]), (_gx, _gy, _gz)←(Xr[i_g], Yr[i_g], Zr[i_g])とする。
(2) Add_Movable(i_s, l_x[0]-_sx, 0, l_z[0]-_sz)を実行し、Evacuating_Cube_Controlを実行する。
(3)Move_Module(i_s, l_x[0]-_sx, 0, l_z[0]-_sz)を実行する。Record_Historyを実行する(図１７動作(a))。
(4)Add_Movable(i_s, 0, _p_l, 0)を実行する。Evacuating_Cube_Controlを実行する。
(5)Move_Module(i_s, 0, _p_l, 0)を実行する。Record_Historyを実行する(図１７動作(b))。
(6)Add_Movable(i_g, l_x[0]-_gx, 0, l_z[0]-_gz)を実行する。Evacuating_Cube_Controlを実行する。
(7)Move_Module(i_g, l_x[0]-_gx, 0, l_z[0]-_gz)を実行する。Record_Historyを実行する(図１７動作(c))。
(8)Add_Movable(i_g, _sx-l_x[0], 0, _sz-l_z[0]) を実行する。Evacuating_Cube_Controlを実行する。
(9)Move_Module(i_g, _sx-l_x[0], 0, _sz-l_z[0]) を実行する。Record_Historyを実行する(図１７動作(d))。
(10)Add_Movable(i_s, 0, -_p_l, 0)を実行する。Evacuating_Cube_Controlを実行する。
(11)Move_Module(i_s, 0, -_p_l, 0)を実行する。Record_Historyを実行する(図１７動作(e))。
(12)Add_Movable(i_s, _gx-l_x[0], 0, _gz-l_z[0])を実行する。Evacuating_Cube_Controlを実行する。
(13)Move_Module(i_s, _gx-l_x[0], 0, _gz-l_z[0])を実行する。Record_Historyを実行する(図１７動作(f))。

[At_Corner_Z(_s, _g, l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1], _d, _p_l)]
(1) 位置(path_perm_x[_s], path_perm_y[_s], path_perm_z[_s])にあるロボットの番号をi_sとし、位置(path_perm_x[_g], path_perm_y[_g], path_perm_z[_g])にあるロボットの番号をi_gとする。(_sx, _sy, _sz)←(Xr[i_s], Yr[i_s], Zr[i_s]), (_gx, _gy, _gz)←(Xr[i_g], Yr[i_g], Zr[i_g])とする。
(2)Add_Movable(i_s, l_x[0]-_sx, l_y[0]-_sy, 0)を実行し、Evacuating_Cube_Controlを実行する。
(3)Move_Module(i_s, l_x[0]-_sx, l_y[0]-_sy, 0)を実行する。Record_Historyを実行する(図１７動作(a))。
(4)Add_Movable(i_s, 0, 0, _p_l)を実行する。Evacuating_Cube_Controlを実行する。
(5)Move_Module(i_s, 0, 0, _p_l)を実行する。Record_Historyを実行する(図１７動作(b))。
(6)Add_Movable(i_g, l_x[0]-_gx, l_y[0]-_gy, 0)を実行する。Evacuating_Cube_Controlを実行する。
(7)Move_Module(i_g, l_x[0]-_gx, 0, l_y[0]-_gy, 0)を実行する。Record_Historyを実行する(図１７動作(c))。
(8)Add_Movable(i_g, _sx-l_x[0], _sy-l_y[0], 0)を実行する。Evacuating_Cube_Controlを実行する。
(9)Move_Module(i_g, _sx-l_x[0], _sy-l_y[0], 0)を実行する。Record_Historyを実行する(図１７動作(d))。
(10)Add_Movable(i_s, 0, 0, -_p_l)を実行する。Evacuating_Cube_Controlを実行する。
(11)Move_Module(i_s, 0, 0, -_p_l)を実行する。Record_Historyを実行する(図１７動作(e))。
(12)Add_Movable(i_s, _gx-l_x[0], _gy-l_y[0], 0)を実行する。Evacuating_Cube_Controlを実行する。
(13)Move_Module(i_s, _gx-l_x[0], 0, _gy-l_y[0], 0)を実行する。Record_Historyを実行する(図１７動作(f))。

[ボイドの制御]
線要素または角要素でのロボット位置入れ替えが終わり、次の要素内でのロボット入れ替えを開始するときに、二つのボイド位置を次の要素内でのロボット入れ替え開始時の初期位置に移動させる必要がある。Robot_Exchange_1を実行する間に、ボイドの移動に伴って、いくつかのロボットの位置は変化するが、Robot_Exchange_2を実行する際にそれらはもとに戻されるように制御される。ボイドの制御は、先述のNavigate_VoidとReverse_Void_Toによって行われる。Robot_Exchange_1を実行する間のボイドの制御は、Navigate_Voidで行い、Robot_Exchange_2を実行する間のボイドの制御は、Navigate_Voidにて記録されたボイド動作の履歴データを逆再生する形で行う。ボイドの移動するパスは、ロボット位置入れ替えパス上のすべての位置(path_perm_x[], path_perm_y[], path_perm_z[])を通らないように設定される。

[Navigate_Void(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])]
(1)((void_perm_x[0], void_perm_y[0], void_perm_z[0]) = (l_x[0], l_y[0], l_z[0]) && (void_perm_x[1], void_perm_y[1], void_perm_z[1]) = (l_x[1], l_y[1], l_z[1])) || ((void_perm_x[0], void_perm_y[0], void_perm_z[0]) = (l_x[1], l_y[1], l_z[1]) && (void_perm_x[1], void_perm_y[1], void_perm_z[1]) = (l_x[0], l_y[0], l_z[0]))でないとき（つまり、二つのボイド位置が二つの目標位置(l_x[], l_y[], l_z[])の両方に一致していないとき）、(2)以下を実行する。
(2)Void_Manhattanを実行し、現ボイド位置からの各ロボット位置のマンハッタン距離を計算する。
(3)Void_Move(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])を実行し、ボイドを移動させる。

Void_Manhattanは、現ボイド位置からの各ロボット位置のマンハッタン距離δv[i]を計算するものである。

[Void_Manhattan]
(1)G内にある各ロボットiの位置（Xr[i],Yr[i],Zr[i]）において、next_p[a][i]を用意し、隣接する行動aの方向に他のロボットがあるか、二つの現ボイド位置(void_perm_x[], void_perm_y[], void_perm_z[])のいずれかがある場合には、next_p[a][i]←1とし、それ以外の場合はnext_p[a][i]←0とする。
(2)各ロボットiの位置のマンハッタン距離δv[i]をG内の格子数より大きな値s_maxに初期化する。
(3) すべてのロボットiと行動aについて、next_p[a][i]=1のとき、行動aによってロボットiの位置から移動した先の位置にあるロボットjのマンハッタン距離δv[j]を調べ、その最小値に1を加えた値が、現在のδp[i]よりも小さいときは、その値をδv[i]に代入する。また、それに優先して、行動aによってロボットiの位置から移動した先の位置にボイドがある場合は、1をδv[i]に代入する。
(4)上述の(3)の処理にて、δv[i]値の更新がなくなるまで、(3)を繰り返す。

Void_Moveでは、２つのケースに分けてボイドをボイド目標位置まで移動させる（図１８、図１９参照）。

[Void_Move(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])]
(1)ボイド目標位置(l_x[0], l_y[0], l_z[0])、(l_x[1], l_y[1], l_z[1])の両方にロボットが存在するとき、それらのロボットのうち、δvの値の大きい方を_g_0、小さい方を_g_1とし、(2)〜(7)を実行する(各変数の説明は図１８を参照)。
(2)ロボット_g_0からロボット_g_1の位置を経て、現ボイド位置に至るパスに含まれるロボット数を格納する変数をtrj_numとし、値を2に初期化する。 パス中のロボット番号を格納する変数をp_trj[t](t=0,…)とし、p_trj[0]=_g_0, p_trj[1]=_g_1とする。t←2とする。
(3) δv[p_trj[t-1]]＝1でないとき、ロボットp_trj[t-1]から各行動aによって移動した先にある隣接ロボットj’のうち、δv[j’]の値がδv[p_trj[t-1]]‐1となるロボットj’を選択し、p_trj[t]=j’とする。ｔをインクリメントする。(3)を繰り返す。δv[p_trj[t-1]]＝1のとき、trj_num=tとして終了する。
(4)t=2〜trj_num+1のすべてのtにおいて、(_px[t], _py[t], _pz[t])←(Xr[p_trj[trj_num-(t-1)], Yr[p_trj[trj_num-(t-1)], Zr[p_trj[trj_num-(t-1)]])とする。
(5)If ボイド位置(void_perm_x[0], void_perm_y[0], void_perm_z[0])が位置(_px[2], _py[2], _pz[2])に隣接している, Then,
(_px[1], _py[1], _pz[1])←(void_perm_x[0], void_perm_y[0], void_perm_z[0])、
(_px[0], _py[0], _pz[0])←(void_perm_x[1], void_perm_y[1], void_perm_z[1])とする。
(6) If ボイド位置(void_perm_x[1], void_perm_y[1], void_perm_z[1])が位置(_px[2], _py[2], _pz[2])に隣接している, Then,
(_px[1], _py[1], _pz[1])←(void_perm_x[1], void_perm_y[1], void_perm_z[1])、
(_px[0], _py[0], _pz[0])←(void_perm_x[0], void_perm_y[0], void_perm_z[0])とする。
(7) t=trj_num-1〜0において、以下を繰り返す。
j=0〜1において、以下を繰り返す。
(7.1)(_xd, _yd, _zd)←(Xr[p_trj[t]], Yr[p_trj[t]], Zr[p_trj[t]])。
(7.2)Add_Movable(p_trj[t],
_px[(trj_num-1)-t+(1-j)]-_px[(trj_num-1)-t+(2-j)],
_py[(trj_num-1)-t+(1-j)]-_py[(trj_num-1)-t+(2-j)],
_pz[(trj_num-1)-t+(1-j)]-_pz[(trj_num-1)-t+(2-j)] )を実行する。
Evacuating_Cube_Controlを実行する。
(7.3)Move_Module(p_trj[t],
_px[(trj_num-1)-t+(1-j)]-_px[(trj_num-1)-t+(2-j)],
_py[(trj_num-1)-t+(1-j)]-_py[(trj_num-1)-t+(2-j)],
_pz[(trj_num-1)-t+(1-j)]-_pz[(trj_num-1)-t+(2-j)])を実行する。
Record_Historyを実行する。
(7.4)ロボットp_trj[t]の動作aをUpdate_Void_Navigation(_p_trj[t], a)にて
記録する。
(8)ボイド目標位置(l_x[0], l_y[0], l_z[0])、(l_x[1], l_y[1], l_z[1])のどちらか一方のみにロボットが存在するとき、そのロボットを_g_0とし、(9)〜(15)を実行する(各変数の説明は図１９を参照)。
(9)ボイド目標位置と一致している方の現ボイド位置をバッファ(_xd[0],_yd[0],_zd[0])に格納し、一致していない方の現ボイド位置をバッファ(_xd[1],_yd[1],_zd[1])に格納する。
(10)Add_Movable(_g_0, _xd[0]-Xr[_g_0], _yd[0]-Yr[_g_0], _zd[0]-Zr[_g_0])を実行する。Evacuating_Cube_Controlを実行する。
(11)Move_Module(_g_0, _xd[0]-Xr[_g_0], _yd[0]-Yr[_g_0], _zd[0]-Zr[_g_0])を実行する。Record_Historyを実行する。
(12)ロボット_g_0の動作aをUpdate_Void_Navigation(_g_0, a)にて記録する。
(13)Add_Movable(_g_0, _xd[1]-Xr[_g_0], _yd[1]-Yr[_g_0], _zd[1]-Zr[_g_0])を実行する。Evacuating_Cube_Controlを実行する。
(14)Move_Module(_g_0, _xd[1]-Xr[_g_0], _yd[1]-Yr[_g_0], _zd[1]-Zr[_g_0])を実行する。Record_Historyを実行する。
(15) ロボット_g_0の動作aをUpdate_Void_Navigation(_g_0, a)にて記録する。

[Update_Void_Navigation(i, a)]
(1)行動aと逆方向の向きの行動をa_invとする。
(2)void_navi_dir[t_void]←a_inv, void_navi_num[t_void]←iとする。
(3)t_voidをインクリメントする。

[Reverse_Void_To(l_x[0], l_y[0], l_z[0], l_x[1], l_y[1], l_z[1])]
(1)((void_perm_x[0], void_perm_y[0], void_perm_z[0]) = (l_x[0], l_y[0], l_z[0]) && (void_perm_x[1], void_perm_y[1], void_perm_z[1]) = (l_x[1], l_y[1], l_z[1])) || ((void_perm_x[0], void_perm_y[0], void_perm_z[0]) = (l_x[1], l_y[1], l_z[1]) && (void_perm_x[1], void_perm_y[1], void_perm_z[1]) = (l_x[0], l_y[0], l_z[0]))になるまで（すなわち、二つのボイド位置が、二つの目標位置(l_x[], l_y[], l_z[])の両方に一致するまで）、Reverse_Void_Moveを繰り返し実行する。

Reverse_Void_Moveでは、(2)-(4)でボイド動作の履歴を移動時間ステップ分逆再生する処理を実行する。なお、変数t_void及びvoid_navi_dir[]は、Update_Void_Navigationで用いた変数である。

[Reverse_Void_Move]
(1)t_voidをデクリメントする。
(2)
void_navi_dir[t_void]=1のとき:
Add_Movable(void_navi_num[t_void], 1, 0, 0)を実行する。
void_navi_dir[t_void]=3のとき:
Add_Movable(void_navi_num[t_void], -1, 0, 0)を実行する。
void_navi_dir[t_void]=2のとき:
Add_Movable(void_navi_num[t_void], 0, 1, 0)を実行する。
void_navi_dir[t_void]=4のとき:
Add_Movable(void_navi_num[t_void], 0, -1, 0)を実行する。
void_navi_dir[t_void]=5のとき:
Add_Movable(void_navi_num[t_void], 0, 0, 1)を実行する。
void_navi_dir[t_void]=6のとき:
Add_Movable(void_navi_num[t_void], 0, 0, -1)を実行する。
(3)Evacuating_Cube_Controlを実行する。
(4)
void_navi_dir[t_void]=1のとき:
Move_Module(void_navi_num[t_void], 1, 0, 0)を実行する。
void_navi_dir[t_void]=3のとき:
Move_Module(void_navi_num[t_void], -1, 0, 0)を実行する。
void_navi_dir[t_void]=2のとき:
Move_Module(void_navi_num[t_void], 0, 1, 0)を実行する。
void_navi_dir[t_void]=4のとき:
Move_Module(void_navi_num[t_void], 0, -1, 0)を実行する。
void_navi_dir[t_void]=5のとき:
Move_Module(void_navi_num[t_void], 0, 0, 1)を実行する。
void_navi_dir[t_void]=6のとき:
Move_Module(void_navi_num[t_void], 0, 0, -1)を実行する。
(5)Record_Historyを実行する。

[ボイド消去]
Robot_Exchangeの終了後に実行されるボイド消去処理手順Void_UnGenerationでは、G外に待避してあったロボットout_0とout_1がG内に戻される。Void_UnGenerationの実行前に、ボイドの位置は、ボイドが生成された直後の位置に戻されている。ボイド消去におけるロボットの動作は、ほぼ生成時のときの逆再生動作になるが、Robot_Exchangeを実行中に、待避ロボットの位置が変化している場合もあるので、厳密には常にそうとは限らない。

なお、(escape_cube_buffer_x[0], escape_cube_buffer_y[0], escape_cube_buffer_z[0])、(escape_cube_buffer_x[1], escape_cube_buffer_y[1], escape_cube_buffer_z[1])は、それぞれVoid_Generationにおいて待避させたロボットout_0の位置、ロボットout_1の位置である。

[Void_UnGeneration]
(1)ボイドの位置(void_perm_x[], void_perm_y[], void_perm_z[])をG外のいずれかの位置の値に設定する。
(2)(escape_cube_buffer_x[0], escape_cube_buffer_y[0], escape_cube_buffer_z[0])がロボットout_0の位置に、(escape_cube_buffer_x[1], escape_cube_buffer_y[1], escape_cube_buffer_z[1])がロボットout_1の位置に等しくない場合は、待避ロボットをボイド生成時の位置に戻すために、以下を実行する。
(2.1)as＝1 || as=3のとき(asはVoid_Generationで定義したもの)：
If Yr[out_0] % 2 = 1, Then,
Move_Module(out_0, 0, -1, 0)を実行する。
Move_Module(out_1, 0, -1, 0)を実行する。
Record_Historyを実行する。
Else,
Move_Module(out_0, 0, 1, 0)を実行する。
Move_Module(out_1, 0, 1, 0)を実行する。
Record_Historyを実行する。
(2.2) as=2 || as=4のとき：
If Zr[out_0] % 2 = 1, Then,
Move_Module(out_0, 0, 0, -1)を実行する。
Move_Module(out_1, 0, 0, -1)を実行する。
Record_Historyを実行する。
Else,
Move_Module(out_0, 0, 0, 1)を実行する。
Move_Module(out_1, 0, 0, 1)を実行する。
Record_Historyを実行する。
(2.3) as=5 || as=6のとき:
If Xr[out_0] % 2 = 1, Then,
Move_Module(out_0, -1, 0, 0)を実行する。
Move_Module(out_1, -1, 0, 0)を実行する。
Record_Historyを実行する。
Else,
Move_Module(out_0, 1, 0, 0)を実行する。
Move_Module(out_1, 1, 0, 0)を実行する。
Record_Historyを実行する。
(3) 待避ロボットのGへの格納を以下の処理により行う。
(3.1)as=1のとき:
Move_Module(out_1, -1, 0, 0)を実行する。
Record_Historyを実行する。
If Zr[out_0] % 2 = 1, Then,
Move_Module(out_0, 0, 0, -1)を実行する。
Else,
Move_Module(out_0, 0, 0, 1)を実行する。
Record_Historyを実行する。
Move_Module(out_0, -1, 0, 0)を実行する。
Move_Module(out_1, -1, 0, 0)を実行する。
Record_Historyを実行する。
(3.2)as=2のとき:
Move_Module(out_1, 0, -1, 0)を実行する。
Record_Historyを実行する。
If Xr[out_0] % 2 = 1、Then,
Move_Module(out_0, -1, 0, 0)を実行する。
Else,
Move_Module(out_0, 1, 0, 0)を実行する。
Record_Historyを実行する。
Move_Module(out_0, 0, -1, 0)を実行する。
Move_Module(out_1, 0, -1, 0)を実行する。
Record_Historyを実行する。
(3.3)as=3のとき:
Move_Module(out_1, 1, 0, 0)を実行する。
Record_Historyを実行する。
If Zr[out_0] % 2 = 1, Then,
Move_Module(out_0, 0, 0, -1)を実行する。
Else,
Move_Module(out_0, 0, 0, 1)を実行する。
Record_Historyを実行する。
Move_Module(out_0, 1, 0, 0)を実行する。
Move_Module(out_1, 1, 0, 0)を実行する。
Record_Historyを実行する。
(3.4)as=4のとき:
Move_Module(out_1, 0, 1, 0)を実行する。
Record_Historyを実行する。
If Xr[out_0] % 2 = 1, Then,
Move_Module(out_0, -1, 0, 0)を実行する。
else,
Move_Module(out_0, 1, 0, 0)を実行する。
Record_Historyを実行する。
Move_Module(out_0, 0, 1, 0)を実行する。
Move_Module(out_1, 0, 1, 0)を実行する。
Record_Historyを実行する。
(3.5)as=5のとき:
Move_Module(out_1, 0, 0, -1)を実行する。
Record_Historyを実行する。
If Yr[out_0] % 2 = 1, Then,
Move_Module(out_0, 0, -1, 0)を実行する。
Else,
Move_Module(out_0, 0, 1, 0)を実行する。
Record_Historyを実行する。
Move_Module(out_0, 0, 0, -1)を実行する。
Move_Module(out_1, 0, 0, -1)を実行する。
Record_Historyを実行する。
(3.6)as=6のとき:
Move_Module(out_1, 0, 0, 1)を実行する。
Record_Historyを実行する。
If Yr[out_0] % 2 = 1, Then,
Move_Module(out_0, 0, -1, 0)を実行する。
Else,
Move_Module(out_0, 0, 1, 0)を実行する。
Record_Historyを実行する。
Move_Module(out_0, 0, 0, 1)を実行する。
Move_Module(out_1, 0, 0, 1)を実行する。
Record_Historyを実行する。

[待避ロボットの制御]
Robot_Exchange実行中にロボットを移動させるMove_Module処理を呼び出す前の段階で、そのMove_Moduleで実行しようとしているロボット移動が、二つの待避ロボットout_0、out_1の他のロボットからのディスコネクションを引き起こさないかチェックし、引き起こす場合には、待避ロボットの位置を変更して、ディスコネクションを回避する機能を、前述の通り、Add_MovableとEvacuating_Cube_Controlで実装する。

待避ロボットは常に二つのロボットが接した状態であり、また常に面Ssに接している。待避ロボットの動作は、二つのロボットの接続している向きに垂直、かつasに垂直な方向への動作に限られる。なお、Ssとasについては、Void_Generationの(1)で定義したものである。

[Add_Movable(i, _vx, _vy, _vz)]
(1)これから動作しようとしているロボット番号を格納する変数movable_cubes[]、これから動作しようとするロボットの数を格納する変数movable_numとし、movable_cubes [movable_num]←iとする。
(2)これから動作しようとしているロボットの移動方向を格納する変数の値を設定する。すなわち、movable_vx[movable_num]←_vx, movable_vy[movable_num]←_vy, movable_vz[movable_num]←_vzとする。
(3)movable_numをインクリメントする。

[Evacuating_Cube_Control]
(1)ロボットout_0かロボットout_1にasの逆の方向で接している（面Ssを介して接している）ロボットがあるかチェックする。そのようなロボットがある場合、そのロボットの番号のすべてがこれから動作しようとしているロボット（movable_cubes[im], im=0,1,…,movable-1）に含まれるかどうかをチェックし、含まれないものがある場合は終了する。すべて含まれる場合は、ロボットout_0,out_1のいずれかに接していて、かつ、movable_num個のロボットmovable_cubes[]に含まれる最多で２つのロボットをロボットmovable_cubes[im0], movable_cubes[im1]とする。
(2)ロボットmovable_cubes[im0], movable_cubes[im1]について、ロボットmovable_cubes[im0], movable_cubes[im1]が、それぞれ（movable_vx[im0], movable_vy[im0], movable_vz[im0]）, (movable_vx[im1], movable_vy[im1], movable_vz[im1]）が示すベクトル量だけ移動した後も、ロボットout_0,out_1のいずれかに接しているかどうかをチェックする。ロボットmovable_cubes[im0], movable_cubes[im1]のうち、少なくとも一つが接しているならば、終了する。
(3)ロボットmovable_cubes[im0], movable_cubes[im1]に、それぞれベクトル（-movable_vx[im0], -movable_vy[im0], -movable_vz[im0]）, (-movable_vx[im1], -movable_vy[im1], -movable_vz[im1]）の指す方向で接しているmovable_cubes[]に属するロボットがあった場合、それらをロボットmovable_cubes[entail_im0], movable_cubes[entail_im1]とし、ロボットmovable[im0]とロボットmovable_cubes[entail_im0]の移動方向が同じとき、すなわち（movable_vx[im0], movable_vy[im0], movable_vz[im0]）=（movable_vx[entail_im0], movable_vy[entail_im0], movable_vz[entail_im0]）のときには終了する。ロボットmovable[im1]とロボットmovable_cubes[entail_im1]の移動方向が同じとき、すなわち（movable_vx[im1], movable_vy[im1], movable_vz[im1]）=（movable_vx[entail_im1], movable_vy[entail_im1], movable_vz[entail_im1]）のときには終了する。
(4)面Ssにあるロボットが２つのみであり、それらの位置関係がSs内において対角の位置であるとき、それらのロボットのうち、ロボットout_0にもout_1にも接していないものをロボットtmp_moveとし、接しているものをロボットno_tmp_moveする。ロボットtmp_moveをロボットno_tmp_moveに接し、ロボットout_0, out_1に接しない位置に1ステップ移動させる。（例：as=1,3のときは、Zr[tmp_move]が奇数なら、Z負方向に1ステップ移動させる）。Record_Historyを実行する。
(5)以下の待避動作を行う。
(5.1)as=1,3のとき:
If Yr[out_0]が奇数 Then, ロボットout_0,out_1をY負方向に1ステップ移動。
Else, ロボットout_0,out_1をY正方向に1ステップ移動。
(5.2)as=2,4のとき:
If Zr[out_0]が奇数 Then, ロボットout_0,out_1をZ負方向に1ステップ移動。
Else, ロボットout_0,out_1をZ正方向に1ステップ移動。
(5.3)as=5,6のとき:
If Xr[out_0]が奇数 Then, ロボットout_0,out_1をX負方向に1ステップ移動。
Else, ロボットout_0,out_1をX正方向に1ステップ移動。
(6)Record_Historyを実行する。
(7)(4)にて移動させたロボットtmp_moveがある場合、ロボットtmp_moveを（4）での動作と逆方向に１ステップ移動させる。Record_Historyを実行する。

Before_Careは、線要素でのロボット位置入れ替えの前に（Line_Perm_By_Positionで）、呼び出される処理である。つまり、線要素でのロボット入れ替え時にも、待避ロボットの他ロボットへの接続を維持できる（他のロボットからのディスコネクションを引き起こさないよう）安全な位置への待避ロボットの移動処理を行う。図２０に示す位置（安全でない位置）にロボットout_0,out_1が少なくとも一つ接していなければ、安全である。変数(bad_place_x[], bad_place_y[], bad_place_z[])にロボットout_0,out_1が安全でない位置を格納する。

[Before_Care(_s, _g, _d, _p_l)]
(1)変数(bad_place_x[], bad_place_y[], bad_place_z[]),bad_numを定義する。bad_num ←0とする。
(2) i=0〜_g-_sのすべての値にて、以下を実行する。
(2.1)(bad_place_x[bad_num], bad_place_y[bad_num], bad_place_y[bad_num])←(path_perm_x[_s+i], path_perm_y[_s+i], path_perm_z[_s+i])とする。
bad_numをインクリメントする。
(2.2)
_d=1のとき:（線要素がX向き）
(bad_place_x[bad_num], bad_place_y[bad_num], bad_place_y[bad_num])←(path_perm_x[_s+i], path_perm_y[_s+i]+_pl, path_perm_z[_s+i])とする。
_d=2のとき:（線要素がY向き）
(bad_place_x[bad_num], bad_place_y[bad_num], bad_place_y[bad_num])←(path_perm_x[_s+i], path_perm_y[_s+i], path_perm_z[_s+i] +_pl)とする。
_d=3のとき:（線要素がZ向き）
(bad_place_x[bad_num], bad_place_y[bad_num], bad_place_y[bad_num])←(path_perm_x[_s+i] +_pl, path_perm_y[_s+i], path_perm_z[_s+i])とする。
(2.3)bad_numをインクリメントする。
(3)ロボットout_0,out_1が位置(bad_place_x[], bad_place_y[], bad_place_y[])に接しているかどうか調べる。ロボットout_0,out_1のうち少なくとも１つが接していなければ終了する。
(4)ロボットout_0,out_1の接する(bad_place_x[], bad_place_y[], bad_place_y[])内の位置を(bad_place_x[o_0], bad_place_y[o_0], bad_place_y[o_0]), (bad_place_x[o_1], bad_place_y[o_1], bad_place_y[o_1])とする。
(5)ロボットout_0,out_1を以下のようにして安全なところに移動させる。
(5.1)as=1, 3のとき：
_d=1, 3のとき：（ロボットout_0,out_1が(bad_place_x[], bad_place_y[],bad_place_y[])に接しない位置にスライド移動する。）
If Yr[out_0]が奇数, Then,
ロボットout_0,out_1をY負方向に1ステップ移動。
Else,
ロボットout_0,out_1をY正方向に1ステップ移動。
_d=2のとき：(図２０の位置(a)〜(f)をすべて避ける。)
If nm > 0, Then, (図２０の位置(a),(c),(d),(f)をすべて避ける。)
If (o_0= 2*nm+2 && o_1= 2*nm+3) || (o_0= 2*nm+3 && o_1= 2*nm+2)
|| (o_0= 0 && o_1= 1) || (o_0= 1 && o_1= 0), Then,
If Yr[out_0]が奇数, Then,
ロボットout_0,out_1をY負方向に1ステップ移動。
Else,
ロボットout_0,out_1をY正方向に1ステップ移動。
Else, (図２０の位置(d),(f)以外をすべて避ける。)
If !( (o_0= 0 && o_1= 1) || (o_0= 1 && o_1= 0) ) Then,
If Yr[out_0]が奇数, Then,
ロボットout_0,out_1をY負方向に1ステップ移動。
Else,
ロボットout_0,out_1をY正方向に1ステップ移動。
(5.2)as=2, 4のとき：
_d=1, 2のとき：（ロボットout_0,out_1が(bad_place_x[], bad_place_y[], bad_place_y[])に接しない位置にスライド移動する。）
If Zr[out_0]が奇数, Then,
ロボットout_0,out_1をZ負方向に1ステップ移動。
Else,
ロボットout_0,out_1をZ正方向に1ステップ移動。
_d=3のとき：(図２０の位置(a)〜(f)をすべて避ける。)
If nm > 0, Then, (図２０の位置(a),(c),(d),(f)をすべて避ける。)
If (o_0= 2*nm+2 && o_1= 2*nm+3) || (o_0= 2*nm+3 && o_1= 2*nm+2)
|| (o_0= 0 && o_1= 1) || (o_0= 1 && o_1= 0), Then,
If Zr[out_0]が奇数, Then,
ロボットout_0,out_1をZ負方向に1ステップ移動。
Else,
ロボットout_0,out_1をZ正方向に1ステップ移動。
Else, (図２０の位置(d),(f)以外をすべて避ける。)
If !( (o_0= 0 && o_1= 1) || (o_0= 1 && o_1= 0) ) Then,
If Zr[out_0]が奇数, Then,
ロボットout_0,out_1をZ負方向に1ステップ移動。
Else,
ロボットout_0,out_1をZ正方向に1ステップ移動。
(5.3)as=5, 6のとき：
_d=2, 3のとき：（ロボットout_0,out_1が(bad_place_x[], bad_place_y[], bad_place_y[])に接しない位置にスライド移動する。）
If Xr[out_0]が奇数, Then,
ロボットout_0,out_1をX負方向に1ステップ移動。
Else,
ロボットout_0,out_1をX正方向に1ステップ移動。
_d=1のとき：(図２０の位置(a)〜(f)をすべて避ける。)
If nm > 0, Then, (図２０の位置(a),(c),(d),(f)をすべて避ける。)
If (o_0= 2*nm+2 && o_1= 2*nm+3) || (o_0= 2*nm+3 && o_1= 2*nm+2)
|| (o_0= 0 && o_1= 1) || (o_0= 1 && o_1= 0), Then,
If Xr[out_0]が奇数, Then,
ロボットout_0,out_1をX負方向に1ステップ移動。
Else,
ロボットout_0,out_1をX正方向に1ステップ移動。
Else, (図２０の位置(d),(f)以外をすべて避ける。)
If !( (o_0= 0 && o_1= 1) || (o_0= 1 && o_1= 0) ) Then,
If Xr[out_0]が奇数, Then,
ロボットout_0,out_1をX負方向に1ステップ移動。
Else,
ロボットout_0,out_1をX正方向に1ステップ移動。
(6)Record_Historyを実行する。

＜第一実施形態に係る行動制御システム１００＞
第一実施形態に係る行動制御システム１００は、以上に説明した各処理によって構成される。全体動作（ホモジニアス隊列制御とロボット位置入れ替え制御）について以下にまとめる。

（ホモジニアス隊列制御）
(1)移動先隊列決定用動作計画のために使用するマンハッタン距離δ（各位置単位から入口位置単位PeUまでのマンハッタン距離）を計算する。
(2)以下(3),(4)を全GU内にロボットが充填されるまで繰り返す。
(3)移動先隊列決定用動作計画処理Next_Formation_Decisionを実行する。
(4)各ロボット動作決定用動作計画処理(ここではVoid_Control)を実行する。

（ロボット位置入れ替え制御）
(5)ロボット位置入れ替え制御処理手順（Robot_Position_Permutation）を実行する。つまり、すべてのロボットについて、各個ロボットの位置が目標位置に到達している状態になるまで、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）を実行する。

以下、これらの処理を実行する構成について説明する。

図２１は第一実施形態に係る行動制御システム１００の機能ブロック図を、図２２、図２３はそれぞれ行動制御システム１００の処理フローの一部であるホモジニアス隊列制御、ロボット位置入れ替え制御の例を示す。行動制御システム１００は、図２１に示すように、動作計画部１１０と、行動選択部１２０と、隣接状態判定部１２４と、位置更新部１２３と、位置判定部１２６と、各個制御対象物位置入れ替え制御部１３０と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。行動選択部１２０は、移動先隊列決定用動作計画部１２１と各制御対象物動作用動作計画部１２２とを含む。各個制御対象物位置入れ替え制御部１３０は、入れ替え経路生成部１３１と、ボイド生成部１３２と、入れ替え制御部１３３と、ボイド消去部１３４とを含む。

本実施形態では、行動制御システム１００は、p(pは、16以上の整数の何れかであって、8の倍数)台のロボットの行動を制御し、p台のロボットの内の１つのロボット上に実装される。

＜動作計画部１１０＞
動作計画部１１０は、上述の[マンハッタン距離δの計算]及び[パスPの決定]で説明した方法により、開始位置の集合SU、目標位置の集合GU及びパスPの何れかの各位置から入口位置Peまでのマンハッタン距離δを、ロボットの任務行動開始前に事前に計算し（Ｓ１１０）、記憶部１４０に格納する。ただし、目標位置の集合の各位置から入口位置Peまでのマンハッタン距離δは、その符号を負とする。また、開始位置の集合SU、目標位置の集合GU及びパスPの何れにも含まれないすべての位置のマンハッタン距離δをs_max（状態空間内の格子数より大きな値）として記憶部１４０に格納する。なお、別装置でマンハッタン距離δを計算しておき、ロボットの任務行動開始前に事前に記憶部１４０に格納しておけば、行動制御システム１００は、動作計画部１１０を備えなくともよい。

＜入力部１６０＞
入力部１６０には、p個の開始位置の集合S={(Xr0[0],Yrs[0]),(Xr0[1],Yrs[1]),…,(Xr0[p-1],Yrs[p-1])}及びp個の目標位置の集合G={(Xre[0],Yre[0]),(Xre[1],Yre[1]),…,(Xre[p-1],Yre[p-1])}が入力され、記憶部１４０に記憶される。

なお、目標位置は、所定の入口位置Peを含むとする。この入口位置Peについての情報も、入力部１６０から入力され、記憶部１４０に記憶されるとする。

＜記憶部１４０＞
記憶部１４０には、各位置sから入口位置Peまでのマンハッタン距離δが記憶されているとする。sの取りうる範囲は、対象となる三次元空間上の領域内のロボット単位が存在しうるすべての座標である。

＜通信部１５０＞
行動制御システム１００が実装されているロボットも含め、すべてのロボットは、通信部１５０を介して、三次元平面上の上下左右前後方向(以下「６方向」ともいう)において隣接する他のロボットと通信することができる。

＜行動選択部１２０＞
行動選択部１２０は、記憶部１４０からマンハッタン距離δを取り出す。

行動選択部１２０は、上述の方法で説明した方法で、p台のロボットを制御する（Ｓ１２０）。

行動選択部１２０の移動先隊列決定用動作計画部１２１は、マンハッタン距離δを用いて、移動先隊列決定用動作計画処理Next_Formation_Decisionを行い（Ｓ１２１）、開始位置の集合に配置されたp台のロボットを目標位置の集合に移動させる際に、頭部ロボット単位が移動の先頭を務めるように、各位置単位におけるマンハッタン距離δを用いてある時刻の隊列Gaに対して隊列Gbを決定し、頭部ロボット単位及び尾部ロボット単位の位置を出力する。

さらに、行動選択部１２０の各制御対象物動作用動作計画部１２２は、頭部ロボット単位及び尾部ロボット単位の位置を受け取り、Void_Controlを実行し各ロボット動作用動作計画を行い（Ｓ１２２）、頭部ロボット単位の位置に８台のロボットが位置するように、各ロボットに対して制御信号を送り、各ロボットの動作を制御する。各ロボットは制御信号に従って行動する。

各ロボットは制御信号に従って行動する。

なお、一回の行動制御により、１回の移動先隊列決定用動作計画処理Next_Formation_Decisionで決定された頭部ロボット単位を充填する処理が行われる。

移動先隊列決定用動作計画処理Next_Formation_Decisionにおいて行われる頭部決定処理Head_Robot_Decisionの中で、各ロボット単位の位置や障害物位置を必要とするが、各ロボット単位の位置ついては前述の通り、取得可能である。また、障害物の位置に関しては、マンハッタン距離δを計算する際に利用した障害物の位置を利用してもよいし、隣接状態判定部１２４で判定した判定結果を用いて得られるものを利用してもよい。

本実施形態により制御され、図２Ａの開始位置の集合から図２Ｂの目標位置の集合に変形する際のロボット群の遷移状態の例を図２４に示す。

＜位置更新部１２３＞
位置更新部１２３は、各i=0,1,…,p-1について、i番目のロボットの現在の位置（Xr[i],Yr[i]）において、行動選択部１２０で決定した行動を実行した場合のロボットの移動後（行動後）の位置（Xr'[i],Yr'[i]）を計算し、計算された（Xr'[i],Yr'[i]）で記憶部１４０に格納されたi番目のロボットの位置を更新する（Ｓ１２３）。言い換えれば、位置更新部１２３は、行動選択部１２０で決定した行動に基づいて、ロボットが行動した場合に想定される位置（以下、「想定位置」ともいう）を計算し、ロボットの位置を更新し記憶部１４０に格納する。具体的には、図２２のＳ１２３（位置の更新）を行う。

＜隣接状態判定部１２４＞
隣接状態判定部１２４は、ロボットの三次元空間上の上下左右前後の隣接する位置に、障害物または他のロボットが存在するか否かを判定し、（Ｓ１２４），判定結果を記憶部１４０に格納する。

なお、上述の通り、行動制御システム１００が実装されていないp-１台のロボットについても、通信部１５０と、隣接状態判定部１２４とを含む。そのため、各ロボットiは隣接状態判定部１２４において、自身の上下左右前後方向に障害物または他のロボットがいるかどうかを判定し、通信部１５０を介して判定結果を行動制御システム１００に出力することができる。行動制御システム１００は、通信部１５０を介して各ロボットiから判定結果を受け取り、行動制御システム１００に含まれる隣接状態判定部１２４の判定結果と一緒に記憶部１４０に格納する。なお、p台のロボットは、各ロボットの隣り合う位置（６方向）に必ず、他のロボットが存在し、隣り合うロボット同士がなす群れは、一塊なので、各ロボットiは通信部１５０を介してp-1個の判定結果を直接、または、他のロボットを介して、行動制御システム１００に送信することができる。また、行動制御システム１００は、通信部１５０を介して、直接、または、他のロボットを介して、各ロボットiに選択した行動を実行するように制御信号を送信することができる。また、他の情報もp台のロボット間で送受信可能となる。

なお、ロボットが移動するように制御したとしても、何らかのトラブル（感知できなかった障害物の存在や、機器の故障等）により、行動選択部１２０の制御通りに移動できるとは限らない。一方、動かなかったロボットの位置は、制御する前と変わらない。よって、少なくとも１台のロボットが動かないように制御することで、動かなかったロボットの位置を基準として、隣接状態判定部１２４による判定結果を用いて、移動するように制御されたロボットの、実際に行動した後の位置（以下「行動後位置」ともいう）（Xr"[i],Yr"[i]）を求めることができる。また、GPS等の少なくとも一台のロボットがGPSを備えれば、そのロボットを基準として、同様に移動後位置を求めることができる。

＜位置判定部１２６＞
位置判定部１２６は、隣接状態判定部１２４の判定結果を用いて、行動後位置を求め、行動後位置（Xr"[i],Yr"[i]）と想定位置（Xr'[i],Yr'[i]）とが一致するか否かを判定する（Ｓ１２６）。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。この場合、行動後位置（Xr"[i],Yr"[i]）と想定位置（Xr'[i],Yr'[i]）との少なくとも一方を補正すればよい。補正方法としては様々な手法が考えられる。例えば、移動したすべてのロボットに対して、制御前の位置に戻るように指示し、行動後位置（Xr"[i],Yr"[i]）を補正してもよいし、想定位置（Xr'[i],Yr'[i]）を行動後位置（Xr"[i],Yr"[i]）に合わせて補正してもよい。

各時刻ステップごとに、すべてのロボットがG内にあるかどうかを判定し（Ｓ１２７）、すべてのロボットがG内にあるときは、任務を終了する。そうでないときは、任務を継続する。

例えば、図示しない目標位置到達判定部において、各i=0,1,…,p-1について、位置判定部１２６から出力された行動後位置（Xr"[i],Yr"[i]）∈Gであるか否かを判定し、すべてのiについて（Xr"[i],Yr"[i]）∈Gである場合には、任務を終了する。少なくとも１つ以上のiについて（Xr"[i],Yr"[i]）∈Gを満たさない場合には、行動選択部１２０を再度実行するよう制御する。

以上に述べた処理Ｓ１２０〜Ｓ１２７を毎時刻ステップごとに行う。

＜各個制御対象物位置入れ替え制御部１３０＞
各個制御対象物位置入れ替え制御部１３０は、ロボットiの位置が移動先ロボット位置d(i)に一致しない（i=d(i)でない）最小のiについて、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）を行う（Ｓ１３０）。

入れ替え経路生成部１３１は、入れ替え経路生成処理手順Calculate_Path_From_Origin_To_Destinationを実行して、入れ替え元のロボットiと入れ替え先のロボットd(i)と間の入れ替え経路を生成する（Ｓ１３１）。入れ替え元のロボットiと入れ替え先のロボットd(i)と間の入れ替え経路は、図１５に示すように、複数の線要素と角要素、１つの最終線要素に分割される。

入れ替え元のロボットiと入れ替え先のロボットd(i)の入れ替えに先立って、ボイド生成部１３２は、ボイド生成処理手順Void_Generationを実行して、目標位置の集合G内に２つのボイドを生成する（Ｓ１３２）。その結果として、G外に２つのロボットout_0, out_1が待避することになる。なお、待避ロボットout_0, out_1は最終的には、G内に戻す必要があるため、当該２つのロボットの位置は(escape_cube_buffer_x[0], escape_cube_buffer_y[0], escape_cube_buffer_z[0])、(escape_cube_buffer_x[1], escape_cube_buffer_y[1], escape_cube_buffer_z[1])に記録しておく（Void_Generationの(6)参照）。

入れ替え制御部１３３は、入れ替え制御処理手順Robot_Exchangeを実行して、入れ替え元のロボットiと入れ替え先のロボットd(i)を入れ替える（Ｓ１３３）。まず、第１の入れ替え制御処理手順Robot_Exchange_1を実行して、ロボットiをロボットd(i)の位置まで移動させる。その際、入れ替え経路を分割することで生成された線要素、角要素ごとに入れ替え制御（Line_Perm, Corner_Perm）を行っていく。各要素での入れ替え処理（Line_Perm, Corner_Perm）の中で、まずNavigate_Voidを実行し、先ほど生成した２つのボイドを適切な位置に移動させる。また、線要素（Line_Perm）の場合は、Before_Careを実行して、先ほど同様、２つの待避ロボットout_0、out_1が他のロボットからのディスコネクションを引き起こさないような位置に移動させる。その後、Move_ModuleとRecord_Historyを実行することによりロボットiを移動させていくが、ロボットiを移動させる前には必ずAdd_MovableとEvacuating_Cube_Controlを実行し、２つの待避ロボットout_0、out_1が他のロボットからのディスコネクションを引き起こさないようにする。このようにして、ロボットiをロボットd(i)の位置に徐々に近づけていく。つまり、Add_MovableとEvacuating_Cube_Control、Move_ModuleとRecord_Historyの実行を１セットとして、ロボットiをロボットd(i)の位置に近づけていく。

ロボットiがロボットd(i)の位置に到達したら、次は、第２の入れ替え制御処理手順Robot_Exchange_2を実行して、ロボットd(i)をロボットiが元いた位置まで移動させる。Robot_Exchange_1との違いは、ロボットd(i)が移動していく線要素と角要素の順が先ほどと逆になる点、Line_Perm, Corner_Permの中でNavigate_Voidの代わりにReverse_Void_Toを実行する点、Last_Line_Permの実行がない点である。

ロボットd(i)が、ロボットiが元いた位置まで移動した時点でRobot_Exchangeの処理が終了する。このように処理することで、ロボットiとロボットd(i)の入れ替え前後において入れ替え元ロボットiの位置と入れ替え先ロボットd(i)の位置が入れ替わる以外、その他のロボットの位置は変化せず保持することができる。

最後に、ボイド消去部１３４は、ボイド消去処理手順Void_UnGenerationを実行して、G外に待避していた２つのロボットout_0, out_1をボイド生成直前の位置にそれぞれ戻す（Ｓ１３４）。

各時刻ステップごとに、すべてのロボットが目標位置に到達しているかどうかを判定し（Ｓ１３５）、すべてのロボットが目標位置に到達しているときは、処理を終了する。そうでないときは、処理を継続する。

以上に述べた処理Ｓ１３０〜Ｓ１３５を毎時刻ステップごとに行う。

＜効果＞
このような構成により、多数のロボットの存在を考慮しつつも、計画計算に必要な計算時間や計算機の記憶容量を少ないものに低減可能で、かつ、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ障害物のある環境にて各ロボット個々の位置を隊列内で指定する形の変形動作（ヘテロジニアス変形動作）を行うことを可能とし、従来よりも必要なロボット数を小さくすることができる。ロボットが変形を行う空間に含まれる格子数に比例した計算量で、ロボットのホモジニアス変形動作が可能であり、その動作の結果を利用することで、ロボット台数の2乗に比例した実行時間で、任意形状の障害物が存在する環境において、ロボットに任意の形状から任意の形状へ、各ロボット個々の位置を隊列内で指定する形での変形（ヘテロジニアス変形）をさせることが可能である。

＜変形例＞
本実施形態では、各格子（マス）は、立方体であるが、他の形状であってもよい。格子は左右方向、上下方向及び前後方向に連続して配置される。また、各格子は左右方向で他の二つの格子と隣接し、上下方向で他の二つの格子と隣接し、前後方向で他の二つの格子と隣接する。言い換えると、各格子は、ロボットの移動できる方向と同じ方向においてのみ、他の格子と隣接する。この条件を満たせば、各格子はどのような形状であってもよい。また、「直交」とは、厳密に「垂直に交わること」を意味しなくともよく、例えば、各格子は、平行六面体であってもよく、各格子が他の二つの格子と隣接する方向の一方を上下方向とし、他方を左右方向とすればよく、上下方向及び左右方向とからなる平面に対して平行でない方向を前後方向とすればよい。

別の言い方をすると、制御対象物は、三次元空間上の、第一方向（例えば右方向）、第一方向に対して平行でない方向である第二方向（例えば上方向）、第一方向に対して反対方向である第三方向（例えば左方向）、第二方向に対して反対方向である第四方向（例えば下方向）、第一方向及び第二方向の成す平面に対して平行でない方向を第五方向（例えば前方向）、第五方向に対して反対方向である第六方向に移動可能であり、一回の行動制御により、現在いる領域（格子、マス）から、現在いる領域に対して、第一方向、第二方向、第三方向、第四方向、第五方向、第六方向において隣接する領域の何れかに移動するように制御される。また、この場合、ロボットの２次元平面上の、第一方向において隣接する位置を第一位置、第二方向において隣接する位置を第二位置、第三方向において隣接する位置を第三位置、第四方向において隣接する位置を第四位置、第五方向において隣接する位置を第五位置、第六方向において隣接する位置を第六位置と呼んでもよい。

本実施形態では、行動制御システム１００が、p台のロボットの内の１つのロボット上に実装される例を示したが、コンピュータ上の制御対象物に対して行動制御を行ってもよい。その場合、意図的にトラブルを発生させない限り、制御した通りに制御対象物は移動するため、想定位置（Xr'[i],Yr'[i]）と移動後位置（Xr"[i],Yr"[i]）とが一致する。そのような状態で、各制御対象物i毎の理想的な行動計画を行ってもよい。その結果得られた任務を終了するまでの行動計画を実ロボットに実行させる。実ロボットは、通信部１５０と隣接状態判定部１２４を備え、各行動を終えた後に、隣接状態を判定し、判定結果を用いて、行動後位置を求め、行動後位置（Xr"[i],Yr"[i]）と想定位置（Xr'[i],Yr'[i]）とが一致するか否かを判定する。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。その後の処理については第一実施形態と同様でもよいし、他の処理を行ってもよい。このような構成により、少なくとも制御通りに移動できなかったことを検知することができる。

本実施形態では、一回の行動制御に対して、一回の位置判定を行っているが、一回の行動制御において、複数のロボットを移動させる場合には、各ロボットが移動の度に位置判定を行う構成としてもよい。

本実施形態では、ロボット単位は、２×２×２の８台のロボットからなるが、M×N×Q台のロボットからなってもよい。ただし、M、N及びQはそれぞれ２以上の整数の何れかである。なお、ロボットの台数pはロボット単位に含まれるロボットの台数M×N×Qの2以上の倍数とする。つまり、p=M×N×Q×Rであり、Rは2以上の整数の何れかである。このような状態で移動を行った場合にも、２×２×２のロボット単位のときと同様に、ロボットの群れの中のいずれのロボットが一台欠けても、各ロボットはお互いに一つの辺で接しあう位置関係を崩さずに済む。M×N×Qの台数を大きくすると、ロボット単位の個数が減り、行動できるパターンが減るため、マンハッタン距離δの計算量が減る。しかし、M×N×Q個のマスを一つの単位とするマス単位の中に1つでも障害物が含まれる場合には、そのマス単位には移動できないものとして扱うため、M×N×Qの台数を大きくすると、移動できない範囲が多くなり、行動できる範囲が小さくなる。そもそも動作計画において、各位置から入口位置Peまでのマンハッタン距離を調べさえすればよいため、従来技術に比べれば、十分に計算量が減っている。そのため、行動できる範囲を大きく保つために２×２×２台のロボットを一つの単位とするロボット単位とすることが望ましい。なお、ロボット単位をM×N×Q台のロボットからなるものとした場合も、ボイド発生条件は、ahとatに基づき、ロボット単位Hと頭部ロボット単位Dとの間、及び、尾部ロボット単位Tとロボット単位T'との間において隣接状態を維持するためのロボット単位Hにおけるボイドの発生位置の条件であればよい。なお、M×N×Q台のロボット単位は、２×２×２台のロボット単位よりも接続を維持しやすくなるため、ボイド発生条件は緩くなる。

＜第二実施形態＞
待避ロボットが入れ替え対象のロボットではない場合（つまり、入れ替え対象のロボットorigin,destinationが待避ロボットout_0,out_1のいずれとも一致しない場合）には、ボイド発生処理であるVoid_Generationを省略することができる。これにより、処理の効率化を図ることができる。

第一実施形態のロボット位置入れ替え制御処理手順Robot_Position_Permutationと各個ロボット位置入れ替え制御処理手順Each_Robot_Position_Exchangeをそれぞれ以下で述べるロボット位置入れ替え制御処理手順Robot_Position_Permutation_2と各個ロボット位置入れ替え制御処理手順Each_Robot_Position_Exchange_2に変更する。

［Robot_Position_Permutation_2］
(1)すべてのロボットiについて変数p(i)=0とする。（pは、ロボット位置入れ替え済み判定フラグである）。tp=0とし、Record_Historyを実行する（ただし、tpは時刻カウンタとする）。
(2)i=d(i)となるすべてのロボットiについて、p(i)=1とする。
(3)すべてのiについてp(i)=1となるまで、(4)〜(6)を繰り返す。
(4)p(i)=0である最小のiを選択する。
(5)i=d(i)でないうちは、(6)を繰り返す。
(6)入れ替え元のロボットorigin←i, 入れ替え先のロボットdestination←d(i)とし、 Each_Robot_Position_Exchange_2を実行する。
(7)Reverse_Void_To(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0], void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])を実行し、Void_UnGenerationを実行する。

なお、(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0]), (void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])は、Void_Generationで定義した待避ロボットout_0,out_1の位置を格納する変数である。

[Each_Robot_Position_Exchange_2]
(1)Calculate_Path_From_Origin_To_Destinationを実行する。
(2)本処理が最初に実行されるときは、Void_Generationを実行する。２回目以降の実行の場合、入れ替え元ロボットorigin, 入れ替え先ロボットdestinationのいずれかが待避ロボットout_0,out_1と一致するとき、Reverse_Void_To(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0], void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])を実行し、Void_UnGenerationを実行し、Void_Generationを実行する。
(3)Robot_Exchangeを実行する。

＜第二実施形態に係る行動制御システム２００＞
第二実施形態に係る行動制御システム２００は、以上に説明したロボット位置入れ替え制御によって構成される。なお、ホモジニアス隊列制御については第一実施形態に係る行動制御システム１００のそれと同一である。

図２５は第二実施形態に係る行動制御システム２００の機能ブロック図を、図２６は行動制御システム２００の処理フローの一部であるロボット位置入れ替え制御の例を示す。行動制御システム２００は、図２５に示すように、動作計画部１１０と、行動選択部１２０と、隣接状態判定部１２４と、位置更新部１２３と、位置判定部１２６と、各個制御対象物位置入れ替え制御部２３０と、最終ボイド消去部２４０と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。つまり、各個制御対象物位置入れ替え制御部１３０の代わりに、各個制御対象物位置入れ替え制御部２３０と最終ボイド消去部２４０とを含む点で第一実施形態に係る行動制御システム１００と異なる。各個制御対象物位置入れ替え制御部２３０は、入れ替え経路生成部１３１と、ボイド生成部２３２と、入れ替え制御部１３３とを含む。

＜各個制御対象物位置入れ替え制御部２３０＞
各個制御対象物位置入れ替え制御部２３０は、ロボットiの位置が移動先ロボット位置d(i)に一致しない（i=d(i)でない）最小のiについて、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange_2）を行う（Ｓ２３０）。

入れ替え経路生成部１３１は、入れ替え経路生成処理手順Calculate_Path_From_Origin_To_Destinationを実行して、入れ替え元のロボットiと入れ替え先のロボットd(i)と間の入れ替え経路を生成する（Ｓ１３１）。

入れ替え元のロボットiと入れ替え先のロボットd(i)の入れ替えに先立って、ボイド生成部２３２は、ボイド生成部２３２による最初の処理であるかチェックし、最初の実行である場合、ボイド生成処理手順Void_Generationを実行、２回目以降の実行の場合、入れ替え元のロボットiと入れ替え先のロボットd(i)のいずれかが待避ロボットout_0,out_1と一致するときは、Reverse_Void_To(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0], void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])、Void_UnGeneration、Void_Generationを順に実行する（Ｓ２３２）。

入れ替え制御部１３３は、入れ替え制御処理手順Robot_Exchangeを実行して、入れ替え元のロボットiと入れ替え先のロボットd(i)を入れ替える（Ｓ１３３）。

各時刻ステップごとに、すべてのロボットが目標位置に到達しているかどうかを判定し（Ｓ１３５）、すべてのロボットが目標位置に到達しているときは、処理を終了するために最終ボイド消去処理を実行する。そうでないときは、各個ロボット位置入れ替え処理を継続する。

＜最終ボイド消去部２４０＞
最終ボイド消去部２３４は、Reverse_Void_To(void_buffer_x[0], void_buffer_y[0], void_buffer_z[0], void_buffer_x[1], void_buffer_y[1], void_buffer_z[1])、Void_UnGenerationを順に実行し、G外に待避していた２つのロボットout_0, out_1をボイド生成直前の位置にそれぞれ戻す（Ｓ２４０）。

以上に述べた処理Ｓ２３０〜Ｓ２４０を毎時刻ステップごとに行う。

＜その他の変形例＞
本発明は上記の実施形態及び変形例に限定されるものではない。例えば、上述の各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。その他、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。

＜プログラム及び記録媒体＞
また、上記の実施形態及び変形例で説明した各装置における各種の処理機能をコンピュータによって実現してもよい。その場合、各装置が有すべき機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、上記各装置における各種の処理機能がコンピュータ上で実現される。

この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。コンピュータで読み取り可能な記録媒体としては、例えば、磁気記録装置、光ディスク、光磁気記録媒体、半導体メモリ等どのようなものでもよい。

また、このプログラムの流通は、例えば、そのプログラムを記録したＤＶＤ、ＣＤ−ＲＯＭ等の可搬型記録媒体を販売、譲渡、貸与等することによって行う。さらに、このプログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することにより、このプログラムを流通させてもよい。

このようなプログラムを実行するコンピュータは、例えば、まず、可搬型記録媒体に記録されたプログラムもしくはサーバコンピュータから転送されたプログラムを、一旦、自己の記憶部に格納する。そして、処理の実行時、このコンピュータは、自己の記憶部に格納されたプログラムを読み取り、読み取ったプログラムに従った処理を実行する。また、このプログラムの別の実施形態として、コンピュータが可搬型記録媒体から直接プログラムを読み取り、そのプログラムに従った処理を実行することとしてもよい。さらに、このコンピュータにサーバコンピュータからプログラムが転送されるたびに、逐次、受け取ったプログラムに従った処理を実行することとしてもよい。また、サーバコンピュータから、このコンピュータへのプログラムの転送は行わず、その実行指示と結果取得のみによって処理機能を実現する、いわゆるＡＳＰ（Application Service Provider）型のサービスによって、上述の処理を実行する構成としてもよい。なお、プログラムには、電子計算機による処理の用に供する情報であってプログラムに準ずるもの（コンピュータに対する直接の指令ではないがコンピュータの処理を規定する性質を有するデータ等）を含むものとする。

また、コンピュータ上で所定のプログラムを実行させることにより、各装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。

１００ 行動制御システム
１１０ 動作計画部
１２０ 行動選択部
１２１ 移動先隊列決定用動作計画部
１２２ 各個制御対象物動作用動作計画部
１２３ 位置更新部
１２４ 隣接状態判定部
１２６ 位置判定部
１３０ 各個制御対象物位置入れ替え制御部
１３１ 入れ替え経路生成部
１３２ ボイド生成部
１３３ 入れ替え制御部
１３４ ボイド消去部
１４０ 記憶部
１５０ 通信部
１６０ 入力部
２００ 行動制御システム
２３０ 各個制御対象物位置入れ替え制御部
２３２ ボイド生成部
２４０ 最終ボイド消去部

Claims (5)

1. M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、開始位置の集合に配置されたp台の制御対象物を所定の入口位置を含む目標位置の集合に、障害物がある場合には障害物を回避しつつ、前記所定の入口位置から前記目標位置の集合に入るように、移動させるための行動制御を行う行動制御システムであって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置を一つの位置単位とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれR個の位置単位からなる一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   p台の前記制御対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ３つの方向において当該制御対象物単位を構成する他の制御対象物と隣接し、
   制御対象物単位がその制御対象物単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数に基づいて制御され、静止するか、または、３次元空間上の第一〜第六方向の何れかのM×N×Q個の位置からなる位置単位に移動するように制御されるものとし、
   前記価値関数が記憶される記憶部と、
   複数の制御対象物単位が成すある隊列をGaとし、複数の制御対象物単位が成す他の隊列をGbとし、隊列Gbに存在し隊列Gaに存在しない制御対象物単位を頭部制御対象物単位とし、隊列Gaに存在し隊列Gbに存在しない制御対象物単位を尾部制御対象物単位とし、前記開始位置の集合に配置されたp台の前記制御対象物を前記目標位置の集合に移動させる際に、頭部制御対象物単位が移動の先頭を務めるように、前記価値関数を用いてある時刻の隊列Gaに対して隊列Gbを決定する移動先隊列決定用動作計画部と、
   頭部制御対象物単位の位置にM×N×Q台の制御対象物が位置するように各制御対象物の動作を制御する各制御対象物動作用動作計画部と、
   制御対象物の前記目標位置の集合の中での現在の位置を現在位置とし、制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、
   制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、前記目標位置の集合の中に２つのボイドを生成するボイド生成部と、
   前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、
   前記ボイド生成部がボイドを生成する際に前記目標位置の集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、前記待避制御対象物を各々が存在していた前記目標位置の集合の中の元の位置に戻すことにより、前記２つのボイドを消去するボイド消去部とを含み、
   前記価値関数は、各位置において隣接する第一方向〜第六方向に障害物があるか否かを考慮して計算される、ある位置単位から前記所定の入口位置を含む位置単位までのマンハッタン距離を用いて得られ、
   前記移動先隊列決定用動作計画部は、前記価値関数を用いて前記頭部制御対象物単位と前記尾部制御対象物単位を決定し、隊列Gaから前記尾部制御対象物単位を削除し、前記頭部制御対象物単位を追加することにより隊列Gbを決定し、
   前記各制御対象物動作用動作計画部は、隊列Gaの中で最もマンハッタン距離が小さい制御対象物単位を制御対象物単位Hとし、制御対象物単位Hから頭部制御対象物単位へ移動する方向をahとし、尾部制御対象物単位に隣接し、尾部制御対象物単位よりもマンハッタン距離が小さい制御対象物単位を制御対象物単位T'とし、尾部制御対象物単位から制御対象物単位T'へ移動する方向をatとし、制御対象物単位Hから頭部制御対象物単位へ制御対象物を移動させる際に、制御対象物単位H内に発生したボイドの位置と、他の制御対象物単位内に発生したボイドの位置とが同じになるように、ボイドを移動させるものとし、方向ahと方向atに基づき、制御対象物単位Hと頭部制御対象物単位との間、及び、尾部制御対象物単位と制御対象物単位T'との間において隣接状態を維持するように、制御対象物単位Hにおいてボイドを発生させる、
   行動制御システム。
2. M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、開始位置の集合に配置されたp台の制御対象物を所定の入口位置を含む目標位置の集合に、障害物がある場合には障害物を回避しつつ、前記所定の入口位置から前記目標位置の集合に入るように、移動させるための行動制御を行う行動制御システムであって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置を一つの位置単位とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれR個の位置単位からなる一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   p台の前記制御対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ３つの方向において当該制御対象物単位を構成する他の制御対象物と隣接し、
   制御対象物単位がその制御対象物単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数に基づいて制御され、静止するか、または、３次元空間上の第一〜第六方向の何れかのM×N×Q個の位置からなる位置単位に移動するように制御されるものとし、
   前記価値関数が記憶される記憶部と、
   複数の制御対象物単位が成すある隊列をGaとし、複数の制御対象物単位が成す他の隊列をGbとし、隊列Gbに存在し隊列Gaに存在しない制御対象物単位を頭部制御対象物単位とし、隊列Gaに存在し隊列Gbに存在しない制御対象物単位を尾部制御対象物単位とし、前記開始位置の集合に配置されたp台の前記制御対象物を前記目標位置の集合に移動させる際に、頭部制御対象物単位が移動の先頭を務めるように、前記価値関数を用いてある時刻の隊列Gaに対して隊列Gbを決定する移動先隊列決定用動作計画部と、
   頭部制御対象物単位の位置にM×N×Q台の制御対象物が位置するように各制御対象物の動作を制御する各制御対象物動作用動作計画部と、
   制御対象物の前記目標位置の集合の中での現在の位置を現在位置とし、制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、
   制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、所定の場合に前記目標位置の集合の中に２つのボイドを生成するボイド生成部と、
   前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、
   前記ボイド生成部がボイドを生成する際に前記目標位置の集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、すべての制御対象物が各々の目標位置に到達している場合に、前記待避制御対象物を各々が存在していた前記目標位置の集合の中の元の位置に戻すことにより、前記２つのボイドを消去する最終ボイド消去部とを含み、
   前記価値関数は、各位置において隣接する第一方向〜第六方向に障害物があるか否かを考慮して計算される、ある位置単位から前記所定の入口位置を含む位置単位までのマンハッタン距離を用いて得られ、
   前記移動先隊列決定用動作計画部は、前記価値関数を用いて前記頭部制御対象物単位と前記尾部制御対象物単位を決定し、隊列Gaから前記尾部制御対象物単位を削除し、前記頭部制御対象物単位を追加することにより隊列Gbを決定し、
   前記各制御対象物動作用動作計画部は、隊列Gaの中で最もマンハッタン距離が小さい制御対象物単位を制御対象物単位Hとし、制御対象物単位Hから頭部制御対象物単位へ移動する方向をahとし、尾部制御対象物単位に隣接し、尾部制御対象物単位よりもマンハッタン距離が小さい制御対象物単位を制御対象物単位T'とし、尾部制御対象物単位から制御対象物単位T'へ移動する方向をatとし、制御対象物単位Hから頭部制御対象物単位へ制御対象物を移動させる際に、制御対象物単位H内に発生したボイドの位置と、他の制御対象物単位内に発生したボイドの位置とが同じになるように、ボイドを移動させるものとし、方向ahと方向atに基づき、制御対象物単位Hと頭部制御対象物単位との間、及び、尾部制御対象物単位と制御対象物単位T'との間において隣接状態を維持するように、制御対象物単位Hにおいてボイドを発生させ、
   前記ボイド生成部がボイドを生成する所定の場合とは、前記ボイド生成部の実行が１回目であるか、または、２回目以降の場合において前記現在位置にいる制御対象物と前記目標位置にいる制御対象物のいずれかが前記２つの待避制御対象物と一致するときである、
   行動制御システム。
3. 行動制御システムが、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、開始位置の集合に配置されたp台の制御対象物を所定の入口位置を含む目標位置の集合に、障害物がある場合には障害物を回避しつつ、前記所定の入口位置から前記目標位置の集合に入るように、移動させるための行動制御を行う行動制御方法であって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置を一つの位置単位とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれR個の位置単位からなる一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   p台の前記制御対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ３つの方向において当該制御対象物単位を構成する他の制御対象物と隣接し、
   制御対象物単位がその制御対象物単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数に基づいて制御され、静止するか、または、３次元空間上の第一〜第六方向の何れかのM×N×Q個の位置からなる位置単位に移動するように制御されるものとし、
   前記価値関数は、前記行動制御システムの記憶部に記憶されるものとし、
   前記行動制御システムが、複数の制御対象物単位が成すある隊列をGaとし、複数の制御対象物単位が成す他の隊列をGbとし、隊列Gbに存在し隊列Gaに存在しない制御対象物単位を頭部制御対象物単位とし、隊列Gaに存在し隊列Gbに存在しない制御対象物単位を尾部制御対象物単位とし、前記開始位置の集合に配置されたp台の前記制御対象物を前記目標位置の集合に移動させる際に、頭部制御対象物単位が移動の先頭を務めるように、前記価値関数を用いてある時刻の隊列Gaに対して隊列Gbを決定する移動先隊列決定用動作計画ステップと、
   前記行動制御システムが、頭部制御対象物単位の位置にM×N×Q台の制御対象物が位置するように各制御対象物の動作を制御する各制御対象物動作用動作計画ステップと、
   前記行動制御システムが、制御対象物の前記目標位置の集合の中での現在の位置を現在位置とし、制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成ステップと、
   前記行動制御システムが、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、前記目標位置の集合の中に２つのボイドを生成するボイド生成ステップと、
   前記行動制御システムが、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御ステップと、
   前記行動制御システムが、前記ボイド生成ステップにおいてボイドを生成する際に前記目標位置の集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、前記待避制御対象物を各々が存在していた前記目標位置の集合の中の元の位置に戻すことにより、前記２つのボイドを消去するボイド消去ステップとを含み、
   前記価値関数は、各位置において隣接する第一方向〜第六方向に障害物があるか否かを考慮して計算される、ある位置単位から前記所定の入口位置を含む位置単位までのマンハッタン距離を用いて得られ、
   前記移動先隊列決定用動作計画ステップは、前記価値関数を用いて前記頭部制御対象物単位と前記尾部制御対象物単位を決定し、隊列Gaから前記尾部制御対象物単位を削除し、前記頭部制御対象物単位を追加することにより隊列Gbを決定し、
   前記各制御対象物動作用動作計画ステップでは、隊列Gaの中で最もマンハッタン距離が小さい制御対象物単位を制御対象物単位Hとし、制御対象物単位Hから頭部制御対象物単位へ移動する方向をahとし、尾部制御対象物単位に隣接し、尾部制御対象物単位よりもマンハッタン距離が小さい制御対象物単位を制御対象物単位T'とし、尾部制御対象物単位から制御対象物単位T'へ移動する方向をatとし、制御対象物単位Hから頭部制御対象物単位へ制御対象物を移動させる際に、制御対象物単位H内に発生したボイドの位置と、他の制御対象物単位内に発生したボイドの位置とが同じになるように、ボイドを移動させるものとし、方向ahと方向atに基づき、制御対象物単位Hと頭部制御対象物単位との間、及び、尾部制御対象物単位と制御対象物単位T'との間において隣接状態を維持するように、制御対象物単位Hにおいてボイドを発生させる、
   行動制御方法。
4. 行動制御システムが、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、開始位置の集合に配置されたp台の制御対象物を所定の入口位置を含む目標位置の集合に、障害物がある場合には障害物を回避しつつ、前記所定の入口位置から前記目標位置の集合に入るように、移動させるための行動制御を行う行動制御方法であって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置を一つの位置単位とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれR個の位置単位からなる一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   p台の前記制御対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ３つの方向において当該制御対象物単位を構成する他の制御対象物と隣接し、
   制御対象物単位がその制御対象物単位の現在の位置単位において各行動を取ったときの適切さを表す１個の価値関数に基づいて制御され、静止するか、または、３次元空間上の第一〜第六方向の何れかのM×N×Q個の位置からなる位置単位に移動するように制御されるものとし、
   前記価値関数は、前記行動制御システムの記憶部に記憶されるものとし、
   前記行動制御システムが、複数の制御対象物単位が成すある隊列をGaとし、複数の制御対象物単位が成す他の隊列をGbとし、隊列Gbに存在し隊列Gaに存在しない制御対象物単位を頭部制御対象物単位とし、隊列Gaに存在し隊列Gbに存在しない制御対象物単位を尾部制御対象物単位とし、前記開始位置の集合に配置されたp台の前記制御対象物を前記目標位置の集合に移動させる際に、頭部制御対象物単位が移動の先頭を務めるように、前記価値関数を用いてある時刻の隊列Gaに対して隊列Gbを決定する移動先隊列決定用動作計画ステップと、
   前記行動制御システムが、頭部制御対象物単位の位置にM×N×Q台の制御対象物が位置するように各制御対象物の動作を制御する各制御対象物動作用動作計画ステップと、
   前記行動制御システムが、制御対象物の前記目標位置の集合の中での現在の位置を現在位置とし、制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成ステップと、
   前記行動制御システムが、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、所定の場合に前記目標位置の集合の中に２つのボイドを生成するボイド生成ステップと、
   前記行動制御システムが、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御ステップと、
   前記行動制御システムが、前記ボイド生成ステップにおいてボイドを生成する際に前記目標位置の集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、すべての制御対象物が目標位置に到達している場合に、前記待避制御対象物を各々が存在していた前記目標位置の集合の中の元の位置に戻すことにより、前記２つのボイドを消去する最終ボイド消去ステップとを含み、
   前記価値関数は、各位置において隣接する第一方向〜第六方向に障害物があるか否かを考慮して計算される、ある位置単位から前記所定の入口位置を含む位置単位までのマンハッタン距離を用いて得られ、
   前記移動先隊列決定用動作計画ステップは、前記価値関数を用いて前記頭部制御対象物単位と前記尾部制御対象物単位を決定し、隊列Gaから前記尾部制御対象物単位を削除し、前記頭部制御対象物単位を追加することにより隊列Gbを決定し、
   前記各制御対象物動作用動作計画ステップでは、隊列Gaの中で最もマンハッタン距離が小さい制御対象物単位を制御対象物単位Hとし、制御対象物単位Hから頭部制御対象物単位へ移動する方向をahとし、尾部制御対象物単位に隣接し、尾部制御対象物単位よりもマンハッタン距離が小さい制御対象物単位を制御対象物単位T'とし、尾部制御対象物単位から制御対象物単位T'へ移動する方向をatとし、制御対象物単位Hから頭部制御対象物単位へ制御対象物を移動させる際に、制御対象物単位H内に発生したボイドの位置と、他の制御対象物単位内に発生したボイドの位置とが同じになるように、ボイドを移動させるものとし、方向ahと方向atに基づき、制御対象物単位Hと頭部制御対象物単位との間、及び、尾部制御対象物単位と制御対象物単位T'との間において隣接状態を維持するように、制御対象物単位Hにおいてボイドを発生させ、
   前記ボイド生成ステップでボイドを生成する所定の場合とは、前記ボイド生成ステップの実行が１回目であるか、または、２回目以降の場合において前記現在位置にいる制御対象物と前記目標位置にいる制御対象物のいずれかが前記２つの待避制御対象物と一致するときである、
   行動制御方法。
5. 請求項１または２に記載の行動制御システムとしてコンピュータを機能させるためのプログラム。