(57)【要約】【課題】制御対象物の位置の入れ替え開始前後の制御対象物隊列が同一という条件下で、各個制御対象物を制御対象物同士の面せん断動作によって制御対象物隊列内の適切な位置に配置させることができる制御対象物位置入れ替え技術を提供する。【解決手段】入れ替え経路生成部１３１とボイド生成部１３２と入れ替え制御部１３３とボイド消去部１３４を用いて、各個制御対象物が各々の目標位置に到達するように制御対象物の位置を入れ替えていく。その際、入れ替え対象となる制御対象物以外の制御対象物の位置は不変となるようにする。【選択図】図１４
2018010440.tif 000002
【特許請求の範囲】
【請求項１】
M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、入れ替え開始前と入れ替え終了後の位置の集合が同一になるという条件のもと、p台の制御対象物の各々が前記集合の中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御装置であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合を構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合はR個の位置単位からなる一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
制御対象物が到達すべき前記集合の中での位置を目標位置、前記制御対象物の前記集合の中での現在の位置を現在位置とし、前記制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、
制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、前記集合の中に２つのボイドを生成するボイド生成部と、
前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、
前記ボイド生成部がボイドを生成する際に前記集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、前記待避制御対象物を各々が存在していた前記集合の中の元の位置に戻すことにより、前記２つのボイドを消去するボイド消去部とを含む、
制御対象物入れ替え制御装置。
【請求項２】
M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、入れ替え開始前と入れ替え終了後の位置の集合が同一になるという条件のもと、p台の制御対象物の各々が前記集合の中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御装置であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合を構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合はR個の位置単位からなる一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
制御対象物が到達すべき前記集合の中での位置を目標位置、前記制御対象物の前記集合の中での現在の位置を現在位置とし、前記制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、
制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、所定の場合に前記集合の中に２つのボイドを生成するボイド生成部と、
前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、
前記ボイド生成部がボイドを生成する際に前記集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、すべての制御対象物が各々の目標位置に到達している場合に、前記待避制御対象物を各々が存在していた前記集合の中の元の位置に戻すことにより、前記２つのボイドを消去する最終ボイド消去部とを含み、
前記ボイド生成部がボイドを生成する所定の場合とは、当該ボイド生成部の実行が１回目であるか、または、２回目以降の場合において前記現在位置にいる制御対象物と前記目標位置にいる制御対象物のいずれかが前記２つの待避制御対象物と一致するときである、
制御対象物入れ替え制御装置。
【請求項３】
M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、制御対象物入れ替え制御装置が、入れ替え開始前と入れ替え終了後の位置の集合が同一になるという条件のもと、p台の制御対象物の各々が前記集合の中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御方法であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合を構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合はR個の位置単位からなる一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
前記制御対象物入れ替え制御装置が、制御対象物が到達すべき前記集合の中での位置を目標位置、前記制御対象物の前記集合の中での現在の位置を現在位置とし、前記制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成ステップと、
前記制御対象物入れ替え制御装置が、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、前記集合の中に２つのボイドを生成するボイド生成ステップと、
前記制御対象物入れ替え制御装置が、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御ステップと、
前記制御対象物入れ替え制御装置が、前記ボイド生成ステップにおいてボイドを生成する際に前記集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、前記待避制御対象物を各々が存在していた前記集合の中の元の位置に戻すことにより、前記２つのボイドを消去するボイド消去ステップとを含む、
制御対象物入れ替え制御方法。
【請求項４】
M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、制御対象物入れ替え制御装置が、入れ替え開始前と入れ替え終了後の位置の集合が同一になるという条件のもと、p台の制御対象物の各々が前記集合の中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御方法であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合を構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合はR個の位置単位からなる一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
前記制御対象物入れ替え制御装置が、制御対象物が到達すべき前記集合の中での位置を目標位置、前記制御対象物の前記集合の中での現在の位置を現在位置とし、前記制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成ステップと、
前記制御対象物入れ替え制御装置が、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、所定の場合に前記集合の中に２つのボイドを生成するボイド生成ステップと、
前記制御対象物入れ替え制御装置が、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御ステップと、
前記制御対象物入れ替え制御装置が、前記ボイド生成ステップにおいてボイドを生成する際に前記集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、すべての制御対象物が各々の目標位置に到達している場合に、前記待避制御対象物を各々が存在していた前記集合の中の元の位置に戻すことにより、前記２つのボイドを消去する最終ボイド消去ステップとを含み、
前記ボイド生成ステップでボイドを生成する所定の場合とは、当該ボイド生成ステップの実行が１回目であるか、または、２回目以降の場合において前記現在位置にいる制御対象物と前記目標位置にいる制御対象物のいずれかが前記２つの待避制御対象物と一致するときである、
制御対象物入れ替え制御方法。
【請求項５】
請求項１または２に記載の制御対象物入れ替え制御装置としてコンピュータを機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の制御対象物の行動を制御する技術に関する。例えば、複数のロボットを、開始位置における隊列形成状態から協調して移動させ、障害物を回避させ、目標位置で隊列形成をさせるための各ロボットの行動計画を求めるロボット協調制御技術に関する。
【背景技術】
【０００２】
近年、多数の自律移動ロボットを効率的に制御にするための研究が活発に行われている。その任務内容は、人の入れない箇所の監視、物品の搬送などさまざまであるが、多数のロボットの協調動作による隊列形成を効率的に行わせるための技術が求められており盛んに研究が行われている（例えば、非特許文献１参照）。多数のロボットによる効率的な隊列形成を実現するには、それぞれのロボットの配置、動作順序などを事前に計画することが重要である。このような計画においては、当然ながら、複数のロボットが動作する実環境における障害物の存在や経路の形状なども十分に考慮しなければならない。
【０００３】
このような計画計算を行うための効果的な手法の一つとして、マルコフ決定過程における動的計画法や強化学習の手法があり、さまざまな研究が行われている（例えば、非特許文献２参照）。
【０００４】
また、ロボットの隊列制御の中でも、ロボット同士が互いに接したままの状態で、アメーバのように全体で移動を行うという仮定の下でのロボット隊列制御においては、ロボット同士の相対的な位置関係から、各ロボットの絶対位置の決定が可能であるという利点と、付加的な位置計測用の装備を必要としないという利点があり、そのようなロボットの研究もおこなわれている。例えば、非特許文献３に示すものでは任意の矩形形状隊列から他の矩形形状隊列までの隊列制御が示されている。
【０００５】
また、非特許文献４に示す研究に至る一連の研究や非特許文献５では、ある隊列から他の隊列に変化する隊列制御が示されている。
【０００６】
非特許文献６に示す研究に示されている手法では、複数の立方体形状のロボット同士での面せん断動作(あるロボットが、他のロボットと接した状態で、接する面上をスライド移動する動作)によるロボットの隊列変形が扱われている。
【先行技術文献】
【非特許文献】
【０００７】
【非特許文献１】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.
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、非特許文献１の手法では、流体力学的な特性をロボット動作に組み込む手法を用いて群ロボットの動作を制御しており、低い計算負荷での制御を可能にしている利点があるが、任意の形状の隊列形成をすることができるとは限らない。
【０００９】
また、非特許文献２の手法のように、マルコフ決定過程における動的計画法や強化学習を使用してこのような計画を行おうとすると、単体のロボットを使用する場合に比べて複数のロボットを使用する場合には、その計算に要する時間や計算機の記憶容量がロボットの数に対して指数関数的に増大してしまう。その主たる原因となるのが、探索計算のためのマルコフ状態空間内の状態数の莫大な増加である。非特許文献２では、検証された強化学習の手法では、ロボット数の増加に伴い、指数関数的に計算負荷が増加するという、マルコフ状態空間内の爆発問題への解決策は示されていない。
【００１０】
また、非特許文献１，２の手法ともに、付加的な位置計測用の装備を必要とする。
【００１１】
また、非特許文献３では、各時刻でロボットに与えられる動作命令が皆同じ方向であるという条件を考慮しており、付加的な位置計測用の装備を必要としないが、その実現には障害物の存在を必要としている。また、一度の動作における各ロボットの移動誤差から発生する隊列の崩れの問題も解決できていない。
【００１２】
非特許文献４の手法においては、一度、線形隊列への変換をしなければならず、可能な隊列形成動作そのものへの制約が大きい。
【００１３】
非特許文献５の手法においては、開始隊列と目標隊列が共有する点がなければならないことや、障害物を考慮していないなどの問題点がある。
【００１４】
非特許文献６の手法においては、障害物の存在を考慮した、各ロボット個々の位置を隊列内で指定しない方式のホモジニアス（Homogeneous）隊列制御は実現できているが、各ロボット個々の位置を隊列内で指定する方式のヘテロジニアス（Heterogeneous）隊列制御は実現できていない。このため、例えば、カメラを搭載したロボット、車輪を搭載したロボット等各ロボット個々に異なる役割があって、各個のロボットをロボット隊列内の適切な位置に配置させつつ、隊列の形状を制御するといった運用はできない。したがって、すべてのロボットに必要なすべての機能を実装しなければならないという問題がある。
【００１５】
このような現状に鑑みて、本発明では、多数のロボットの存在を考慮しつつ、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ障害物のある環境にてホモジニアス隊列変形動作を行った後に各ロボットを個々の目標位置に移動させることによりヘテロジニアス隊列制御を実現する場合のように、ロボット位置の入れ替え開始前後のロボット隊列が同一という条件下で、各個のロボットをロボット隊列内の適切な位置に配置させることができる、ロボット位置入れ替え技術を提供することを目的とする。
【課題を解決するための手段】
【００１６】
本発明の一態様は、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、入れ替え開始前と入れ替え終了後の位置の集合が同一になるという条件のもと、p台の制御対象物の各々が前記集合の中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御装置であって、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合を構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合はR個の位置単位からなる一塊の任意の形状を成し、前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、制御対象物が到達すべき前記集合の中での位置を目標位置、前記制御対象物の前記集合の中での現在の位置を現在位置とし、前記制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、前記集合の中に２つのボイドを生成するボイド生成部と、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、前記ボイド生成部がボイドを生成する際に前記集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、前記待避制御対象物を各々が存在していた前記集合の中の元の位置に戻すことにより、前記２つのボイドを消去するボイド消去部とを含む。
【００１７】
本発明の一態様は、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、入れ替え開始前と入れ替え終了後の位置の集合が同一になるという条件のもと、p台の制御対象物の各々が前記集合の中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御装置であって、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合を構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合はR個の位置単位からなる一塊の任意の形状を成し、前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、制御対象物が到達すべき前記集合の中での位置を目標位置、前記制御対象物の前記集合の中での現在の位置を現在位置とし、前記制御対象物の現在位置が前記制御対象物の目標位置と一致しない場合、前記現在位置から前記目標位置に至る入れ替え経路を生成する入れ替え経路生成部と、制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、所定の場合に前記集合の中に２つのボイドを生成するボイド生成部と、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記現在位置にいる制御対象物と前記目標位置にいる制御対象物とを入れ替える入れ替え制御部と、前記ボイド生成部がボイドを生成する際に前記集合と隣接する位置に移動させた制御対象物を待避制御対象物とし、すべての制御対象物が各々の目標位置に到達している場合に、前記待避制御対象物を各々が存在していた前記集合の中の元の位置に戻すことにより、前記２つのボイドを消去する最終ボイド消去部とを含み、前記ボイド生成部がボイドを生成する所定の場合とは、当該ボイド生成部の実行が１回目であるか、または、２回目以降の場合において前記現在位置にいる制御対象物と前記目標位置にいる制御対象物のいずれかが前記２つの待避制御対象物と一致するときである。
【発明の効果】
【００１８】
本発明に拠れば、ロボット位置の入れ替え開始前後のロボット隊列が同一という条件下で、各個のロボットをロボット同士の面せん断動作によってロボット隊列内の適切な位置に配置させることができる。
【図面の簡単な説明】
【００１９】
【図１】ロボットの移動を説明するための図。
【図２】図２Ａは開始位置の集合を説明するための図、図２Ｂは目標位置の集合を説明するための図。
【図３】ある位置にいた一つのボイドが、別の位置に移動していく様子を示す図。
【図４】ロボット単位を説明するための図。
【図５】多数のロボットが協調して開始位置における隊列形成状態から移動を行い、目標位置での隊列形成を行う任務を説明するための図。
【図６】ロボット位置入れ替え制御の制約を説明するための図。
【図７】各個ロボット位置入れ替え制御の様子を示す図。
【図８】分割されたパスの様子を示す図。
【図９】線要素でのロボット位置入れ替えの様子を示す図。
【図１０】角要素でのロボット位置入れ替えの様子を示す図。
【図１１】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倍増加していくことになる。本実施形態の[問題設定]の項で説明したように、ロボット同士が接しているという拘束条件を取り入れる場合、ロボットのお互いの移動を考慮したうえで探索計算行わなければならないために、根本的な計算量の削減は難しく、複数ロボットを使用する場合の大きな問題となっている。
【００３０】
[非特許文献６におけるホモジニアス隊列制御における特徴]
非特許文献６におけるホモジニアス隊列制御では、上述の計算負荷の問題を解決するための方策の一つとして、ボイド制御の考え方を導入している。また、［問題設定］で述べたような隊列変形の問題を克服するために8マスロボット単位の考え方も導入している。
【００３１】
まず、ボイド制御について説明する。ここでいうボイドとは、あるロボットが別の位置に移動した後に、元いた位置にできる空隙のことである。別の言い方をすると、ボイドとは、ロボットの移動する方向と反対の方向に移動する仮想的な存在である。こうした群ロボットの隊列形成問題においては、複数のロボットの動作に着目するがゆえに、その探索計算量が爆発してしまうが、視点を変えて、ボイドの動きに着目すれば、多数のロボットの動作計画の問題を単一のボイドの動作計画として考えることができ、探索計算負荷の軽減に適している。ロボットの移動に伴ってボイドが移動していく様子を示す図を図３に示す。
【００３２】
[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
である。
【００３５】
以下、ロボットの全体数pを8の倍数とする。また、ロボットの開始位置の集合Sからロボットの目標位置の集合Gへの隊列変形は非特許文献６の方法にて完了しているとすると、各ロボット個々を目標位置の集合G内の指定された位置に移動させていく問題（ヘテロジニアスロボット位置入れ替え制御）が残っている。以下、ヘテロジニアスロボット位置入れ替え制御について説明する。
【００３６】
参考までに、非特許文献６の手法により制御され、図２Ａの開始位置の集合から図２Ｂの目標位置の集合に変形する際のロボット群の遷移状態の例を図５に示す。なお、開始位置の集合及び目標位置の集合はそれぞれR個(R=p/8)の位置単位からなる一塊の任意の形状を成している。
【００３７】
［ヘテロジニアスロボット位置入れ替え制御］
各ロボットiが目標位置の集合G内にて、任意の位置に存在する状態から、各ロボットiをそれぞれ目標位置（Xre[i],Yre[i],Zre[i]）に配置させるヘテロジニアスロボット位置入れ替え制御の方法について以下説明していく。
【００３８】
［ロボット位置入れ替え制御］
ロボット位置入れ替え制御実行前の、各ロボット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)の位置に移動させていく制御に他ならない。
【００３９】
このロボット位置入れ替え制御において重要なことは、非特許文献６のホモジニアス隊列制御においてロボットが占有可能であった領域以外の領域にロボットを存在させることなく、各ロボット位置入れ替えを行わなければならないことである。すなわち、本ロボット位置入れ替え制御において、ホモジニアス隊列制御前のロボットの開始位置の集合S、パスP、目標位置の集合Gを合わせた領域以外にロボットを移動させてはならない。
【００４０】
なお、パスPとは、非特許文献６の方法にて開始位置の集合Sから目標位置の集合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)非特許文献６の手法を用いて開始位置の集合Sから目標位置の集合Gまでホモジニアス隊列変形を行う。
【０１１４】
（ロボット位置入れ替え制御）
(2)ロボット位置入れ替え制御処理手順（Robot_Position_Permutation）を実行する。つまり、すべてのロボットについて、各個ロボットの位置が目標位置に到達している状態になるまで、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）を実行する。
【０１１５】
以下、これらの処理を実行する構成について説明する。
【０１１６】
図１４は第一実施形態に係る行動制御システム１００の機能ブロック図を、図１５は行動制御システム１００の処理フローの一部であるロボット位置入れ替え制御の例を示す。行動制御システム１００は、図１４に示すように、ホモジニアス隊列制御装置９００と、制御対象物位置入れ替え制御装置１２０と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。制御対象物位置入れ替え制御装置１２０は、各個制御対象物位置入れ替え制御部１３０を含み、各個制御対象物位置入れ替え制御部１３０は、入れ替え経路生成部１３１と、ボイド生成部１３２と、入れ替え制御部１３３と、ボイド消去部１３４とを含む。
【０１１７】
本実施形態では、行動制御システム１００は、p(pは、16以上の整数の何れかであって、8の倍数)台のロボットの行動を制御し、p台のロボットの内の１つのロボット上に実装される。
【０１１８】
＜入力部１６０＞
入力部１６０には、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])}が入力され、記憶部１４０に記憶される。
【０１１９】
＜記憶部１４０＞
記憶部１４０には、入力部１６０から入力されたp個の開始位置の集合Sとp個の目標位置の集合Gの各位置が記憶されている。その他処理の過程で生成される情報のうち、記憶する必要がある情報については適宜記憶される。
【０１２０】
＜通信部１５０＞
行動制御システム１００が実装されているロボットも含め、すべてのロボットは、通信部１５０を介して、三次元平面上の上下左右前後方向(以下「６方向」ともいう)において隣接する他のロボットと通信することができる。
【０１２１】
＜ホモジニアス隊列制御装置９００＞
開始位置の集合Sから目標位置の集合Gまで非特許文献６の手法を用いてホモジニアス隊列変形を行う。
【０１２２】
＜制御対象物位置入れ替え制御装置１２０＞
制御対象物位置入れ替え制御装置１２０は、ロボット位置入れ替え制御処理手順Robot_Position_Permutationを実行して、すべてのロボットについて目標位置に到達するように制御する（Ｓ１２０）。
【０１２３】
＜各個制御対象物位置入れ替え制御部１３０＞
各個制御対象物位置入れ替え制御部１３０は、ロボット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台のロボットの内の１つのロボット上に実装される例を示したが、コンピュータ上の制御対象物に対して行動制御を行ってもよい。
【０１３６】
本実施形態では、ロボット単位は、２×２×２の８台のロボットからなるが、M×N×Q台のロボットからなってもよい。ただし、M、N及びQはそれぞれ２以上の整数の何れかである。なお、ロボットの台数pはロボット単位に含まれるロボットの台数M×N×Qの2以上の倍数とする。つまり、p=M×N×Q×Rであり、Rは2以上の整数の何れかである。このような状態で移動を行った場合にも、２×２×２のロボット単位のときと同様に、ロボットの群れの中のいずれのロボットが一台欠けても、各ロボットはお互いに一つの辺で接しあう位置関係を崩さずに済む。
【０１３７】
もちろん、行動制御システム１００を構成する制御対象物位置入れ替え制御装置１２０単体で実行することも可能である。この場合、入れ替え開始前の位置の集合と入れ替え開始後の位置の集合は同一という条件下で入れ替えていくことになる。
＜第二実施形態＞
待避ロボットが入れ替え対象のロボットではない場合（つまり、入れ替え対象のロボット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を実行する。
【０１４１】
＜第二実施形態に係る行動制御システム２００＞
第二実施形態に係る行動制御システム２００は、以上に説明したロボット位置入れ替え制御によって構成される。なお、ホモジニアス隊列制御については第一実施形態に係る行動制御システム１００のそれと同一である。
【０１４２】
図１６は第二実施形態に係る行動制御システム２００の機能ブロック図を、図１７は行動制御システム２００の処理フローの一部であるロボット位置入れ替え制御の例を示す。行動制御システム２００は、図１６に示すように、ホモジニアス隊列制御装置９００と、制御対象物位置入れ替え制御装置２２０と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。つまり、制御対象物位置入れ替え制御装置１２０の代わりに、制御対象物位置入れ替え制御装置２２０を含む点で第一実施形態に係る行動制御システム１００と異なる。制御対象物位置入れ替え制御装置２２０は、各個制御対象物位置入れ替え制御部２３０と、最終ボイド消去部２４０とを含む。また、各個制御対象物位置入れ替え制御部２３０は、入れ替え経路生成部１３１と、ボイド生成部２３２と、入れ替え制御部１３３とを含む。
【０１４３】
＜制御対象物位置入れ替え制御装置２２０＞
制御対象物位置入れ替え制御装置２２０は、ロボット位置入れ替え制御処理手順Robot_Position_Permutation_2を実行して、すべてのロボットについて目標位置に到達するように制御する（Ｓ２２０）。
【０１４４】
＜各個制御対象物位置入れ替え制御部２３０＞
各個制御対象物位置入れ替え制御部２３０は、ロボット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）型のサービスによって、上述の処理を実行する構成としてもよい。なお、プログラムには、電子計算機による処理の用に供する情報であってプログラムに準ずるもの（コンピュータに対する直接の指令ではないがコンピュータの処理を規定する性質を有するデータ等）を含むものとする。
【０１５５】
また、コンピュータ上で所定のプログラムを実行させることにより、各装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
【符号の説明】
【０１５６】
１００  行動制御システム
１２０  制御対象物位置入れ替え制御装置
１３０  各個制御対象物位置入れ替え制御部
１３１  入れ替え経路生成部
１３２  ボイド生成部
１３３  入れ替え制御部
１３４  ボイド消去部
１４０  記憶部
１５０  通信部
１６０  入力部
９００  ホモジニアス隊列制御装置
２００  行動制御システム
２２０  制御対象物位置入れ替え制御装置
２３０  各個制御対象物位置入れ替え制御部
２３２  ボイド生成部
２４０  最終ボイド消去部
【図１】
2018010440.tif 000003
【図２】
2018010440.tif 000004
【図３】
2018010440.tif 000005
【図４】
2018010440.tif 000006
【図５】
2018010440.tif 000007
【図６】
2018010440.tif 000008
【図７】
2018010440.tif 000009
【図８】
2018010440.tif 000010
【図９】
2018010440.tif 000011
【図１０】
2018010440.tif 000012
【図１１】
2018010440.tif 000013
【図１２】
2018010440.tif 000014
【図１３】
2018010440.tif 000015
【図１４】
2018010440.tif 000016
【図１５】
2018010440.tif 000017
【図１６】
2018010440.tif 000018
【図１７】
2018010440.tif 000019