(57)【特許請求の範囲】
【請求項１】
q個の開始位置の集合に配置されたq台の制御対象物をq個の目標位置の集合に移動させるための行動制御を行う行動制御システムであって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、前記各開始位置及び前記各目標位置は、それぞれ前記第一方向〜第四方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれ一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の２次元平面上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、及び、第四方向において隣接する第四位置とし、静止するか、または、二次元平面上の第一〜第四位置の何れかに移動するように制御されるものとし、
前記目的位置の各々を前記第一方向と平行な方向に平行移動させることで、中間形態隊列を成す制御対象物の位置を求める中間遷移過程処理部と、
前記中間形態隊列を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理部とを含み、
前記中間形態隊列は、前記第一方向におけるある位置を通る第一方向に垂直な軸を基準軸として、前記目的位置の集合に含まれる各位置を、前記第一方向における各目的位置の順序を維持したまま、可能な限り前記基準軸に近づく方向へ平行移動させることで得られる隊列であり、
前記中間遷移過程処理部における前記中間形態隊列を成す制御対象物の位置を求める工程は、
前記第一方向と平行な方向における前記基準軸からの距離が最も大きい前記目的位置と、当該目的位置から前記第一方向と平行な方向に連続する目的位置とを、前記第一方向と平行な方向に平行移動させる工程を、前記中間形態隊列が得られるまで順次繰り返すものである
行動制御システム。
【請求項２】
請求項１の行動制御システムであって、
前記中間形態隊列と第二方向における同じ位置に位置する制御対象物の個数が同じで、かつ、第二方向における同じ位置に位置する制御対象物全てが第一方向において連続して隣接している隊列を押し出し隊列とし、少なくともq台の制御対象物を移動させ、押し出
し隊列を成すように制御する押し出し過程処理部を含み、
前記中間遷移過程処理部は、前記押し出し隊列を、第二方向の位置毎に第一方向に平行移動させることで、前記中間形態隊列に変形する、
行動制御システム。
【請求項３】
請求項２の行動制御システムであって、
前記押し出し隊列は、第二方向における位置i_yに位置する制御対象物の個数n[i_y]と
、第二方向における前記位置i_yに隣接する位置i_y-1に位置する制御対象物の個数n[i_y-1]との大小関係に応じて、前記位置i_yにおいて第一方向において連続して隣接している
制御対象物の一端または他端の制御対象物と、前記位置i_y-1において第一方向において
連続して隣接している制御対象物の一端または他端の制御対象物とを第二方向において合わせた隊列であり、
前記押し出し過程処理部は、前記押し出し隊列の第一方向における同じ位置に位置するの制御対象物の個数分だけ、開始位置の集合から制御対象物を第二方向に移動させ、その後、開始位置の集合の外にある制御対象物を全て第一方向に移動させる処理を繰り返し、その後、第二方向に移動させることで、押し出し隊列を形成する、
行動制御システム。
【請求項４】
請求項２の行動制御システムであって、
前記押し出し過程処理部は、制御対象物が第二方向に平行な列をなすように、開始位置の集合から制御対象物を移動させ、その後、前記第二方向に平行な列に沿って、他の制御対象物を第二方向に、前記中間形態隊列内での第二方向の位置まで移動させ、その後、第一方向に移動させることで、押し出し隊列を形成する、
行動制御システム。
【請求項５】
q個の開始位置の集合に配置されたq台の制御対象物をq個の目標位置の集合に移動させるための行動制御を行う行動制御方法であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、前記各開始位置及び前記各目標位置は、それぞれ前記第一方向〜第四方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、前記目標位置の集合及び前記開始位置の集合はそれぞれ一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の２次元平面上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、及び、第四方向において隣接する第四位置とし、静止するか、または、二次元平面上の第一〜第四位置の何れかに移動するように制御されるものとし、
中間遷移過程処理部が、前記目的位置の各々を前記第一方向と平行な方向に平行移動させることで、中間形態隊列を成す制御対象物の位置を求める中間遷移過程処理ステップと、
最終展開過程処理部が、前記中間形態隊列を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理ステップとを含み、
前記中間形態隊列は、前記第一方向におけるある位置を通る第一方向に垂直な軸を基準軸として、前記目的位置の集合に含まれる各位置を、前記第一方向における各目的位置の順序を維持したまま、可能な限り前記基準軸に近づく方向へ平行移動させることで得られる隊列であり、
前記中間遷移過程処理ステップにおける前記中間形態隊列を成す制御対象物の位置を求める工程は、
前記第一方向と平行な方向における前記基準軸からの距離が最も大きい前記目的位置と、当該目的位置から前記第一方向と平行な方向に連続する目的位置とを、前記第一方向と平行な方向に平行移動させる工程を、前記中間形態隊列が得られるまで順次繰り返すものである
行動制御方法。
【請求項６】
請求項１から４の何れかの行動制御システムとしてコンピュータを機能させるためのプログラム。
【請求項７】
q台の制御対象物をq個の開始位置の集合からq個の目標位置の集合に移動させるための行動制御を行う行動制御システムであって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
前記目的位置の集合に含まれる各位置を前記第一方向と平行な方向、および／または、前記第二方向と平行な方向に平行移動させることで、押し出し終了時形状を成す制御対象物の位置を求める中間遷移過程処理部と、
前記押し出し終了時形状を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理部とを含み、
前記押し出し終了時形状は、前記第一方向におけるある位置を通る第一方向に垂直な軸を第一基準軸とし、前記第二方向におけるある位置を通る第二方向に垂直な軸を第二基準軸として、
前記目的位置の集合に含まれる各位置を、前記第一方向および前記第二方向における各目的位置の順序を維持したまま、可能な限り前記第一基準軸および第二基準軸に近づく方向へ平行移動させることで得られる隊列であり、
前記中間遷移過程処理部における前記押し出し終了時形状を成す制御対象物の位置を求める工程は、
(1) 前記第一方向と平行な方向における前記第一基準軸からの距離が最も大きい前記目的位置と、当該目的位置から前記第一方向と平行な方向に連続する目的位置とを、前記第一方向と平行な方向に平行移動させる工程と、
(2) 前記第二方向と平行な方向における前記第二基準軸からの距離が最も大きい前記目的位置と、当該目的位置から前記第二方向と平行な方向に連続する目的位置とを、前記第二方向と平行な方向に平行移動させる工程と、
を前記押し出し終了時形状が得られるまで繰り返すものである
行動制御システム。
【請求項８】
請求項７の行動制御システムであって、
前記中間遷移過程処理部は、前記座標値O1及び前記座標値O2において制御対象物が平面を成すように制御対象物の位置を求める、
行動制御システム。
【請求項９】
請求項７または請求項８の行動制御システムであって、
p>qとし、p台の制御対象物を、q個の開始位置の集合からp個の中間位置の集合に移動させ、p個の中間位置の集合からq個の目標位置の集合に移動させ、
M,N,Qをそれぞれ2以上の整数の何れかとし、中間位置の集合において、p台の前記制御
対象物は、M×N×Q台毎に１つの制御対象物単位を構成し、１つの制御対象物単位を構成
するM×N×Q台の制御対象物はそれぞれ３つ以上の方向において当該制御対象物単位を構
成する他の制御対象物と隣接し、
Rを3以上の整数の何れかとし、p=M×N×Q×Rとし、中間位置の集合は、第五方向におけるある位置を通る、第一方向と第二方向との成す第1-2平面上にR個の制御対象物単位がL
字または矩形の平板状を成すように配置された形状である、
行動制御システム。
【請求項１０】
請求項９の行動制御システムであって、
制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、p台の制御対象物を、p個の中間位置の集合から押し出し終了時形状に変形させる際に、中間位置の集合の中から外へ制御対象物を移動させるときに中間位置の集合の中に生じたボイドがボイドが生じた位置単位から、中間位置の集合の最も遠い位置に存在する制御対象物に移動するように制御する、
行動制御システム。
【請求項１１】
請求項７または請求項８の行動制御システムであって、
p>qとし、p台の制御対象物を、q個の開始位置の集合からp個の中間位置の集合に移動させ、p個の中間位置の集合からq個の目標位置の集合に移動させ、
M,Nをそれぞれ2以上の整数の何れかとし、中間位置の集合において、M×N台毎に１つの制御対象物単位を構成し、Rを１以上の整数の何れかとし、中間位置の集合は、第五方向
における最大値または最小値となる位置を通る、第一方向と第二方向との成す第1-2平面
上にR個の制御対象物単位が矩形の平板状を成すように配置された形状である、
行動制御システム。
【請求項１２】
q台の制御対象物をq個の開始位置の集合からq個の目標位置の集合に移動させるための行動制御を行う行動制御方法であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
前記目的位置の集合に含まれる各位置を前記第一方向と平行な方向、および／または、前記第二方向と平行な方向に平行移動させることで、押し出し終了時形状を成す制御対象物の位置を求める中間遷移過程処理ステップと、
前記押し出し終了時形状を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理ステップとを含み、
前記押し出し終了時形状は、前記第一方向におけるある位置を通る第一方向に垂直な軸を第一基準軸とし、前記第二方向におけるある位置を通る第二方向に垂直な軸を第二基準軸として、
前記目的位置の集合に含まれる各位置を、前記第一方向および前記第二方向における各目的位置の順序を維持したまま、可能な限り前記第一基準軸および第二基準軸に近づく方向へ平行移動させることで得られる隊列であり、
前記中間遷移過程処理ステップにおける前記押し出し終了時形状を成す制御対象物の位置を求める工程は、
(1) 前記第一方向と平行な方向における前記第一基準軸からの距離が最も大きい前記目的位置と、当該目的位置から前記第一方向と平行な方向に連続する目的位置とを、前記第一方向と平行な方向に平行移動させる工程と、
(2) 前記第二方向と平行な方向における前記第二基準軸からの距離が最も大きい前記目的位置と、当該目的位置から前記第二方向と平行な方向に連続する目的位置とを、前記第二方向と平行な方向に平行移動させる工程と、
を前記押し出し終了時形状が得られるまで繰り返すものである
行動制御方法。
【請求項１３】
請求項７から１１の何れかの行動制御システムとしてコンピュータを機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の制御対象物の行動を制御する技術に関する。例えば、複数のロボットを、開始位置における隊列形成状態から協調して移動させ、目標位置で隊列形成をさせるための各ロボットの行動計画を求めるロボット協調制御技術に関する。
【背景技術】
【０００２】
近年、多数の自律移動ロボットを効率的に制御にするための研究が活発に行われている。その任務内容は、人の入れない箇所の監視、物品の搬送などさまざまであるが、多数のロボットの協調動作による隊列形成を効率的に行わせるための技術が求められており盛んに研究が行われている（例えば、非特許文献１参照）。多数のロボットによる効率的な隊列形成を実現するには、それぞれのロボットの配置、動作順序などを事前に計画することが重要である。このような問題を解く際に前提とするロボットの形状や、維持すべき接続状態の条件の違いによってさまざまな解が提案されている（例えば、非特許文献２及び３）。
【０００３】
例えば、非特許文献１に示すものでは立方体形状のロボットについての隊列変形のための動作計画が行われている。非特許文献２では、六角形形状のロボットについての隊列形成制御の研究が報告されている。また、非特許文献３では、円形ロボットの隊列形成制御に関する研究が報告されている。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】Robert Fitch, Zack Butler, Daniel Rus, “Reconfiguration Planning for Heterogeneous Self-Reconfiguring Robots”, Proceedings of the 2003 IEEE Internatinoal Conference on Intelligent Robts and Systems, Las Vegas Nevada, 2003, pp.2460-2467.
【非特許文献２】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, Karlsruhe, Germany, May 6-10, 2013, pp.537-543.
【非特許文献３】Michael Rubenstein, Alejandro Cornejo, Radhika Nagpal, “Programmable self-assembly in a thousand-robot swarm”, SCIENCE, August, 2014, Vol. 345, Issue 6198, pp.795-799.
【発明の概要】
【発明が解決しようとする課題】
【０００５】
従来の研究では、ロボットが移動を行う際に、他のロボットが形成する壁に沿って動く（回り込む）ことが前提となっている。例えば、一つの立方体形状のロボットの周りを回り込んで動く動作を実現するためには、ロボットの備えるべき機構が複雑化してしまうという問題点がある。
【０００６】
このような現状に鑑みて、本発明では、制御対象物が移動する際に、他の制御対象物の周りを回り込まずに、制御対象物同士が、接続状態を維持したまま、移動を行うという前提での隊列制御を実現し、複雑な機構を必要とせずに、制御対象物同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ変形動作を行うことを可能とする、制御対象物協調隊列形成技術を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記の課題を解決するために、本発明の一態様によれば、行動制御システムは、p>qとし、p個の開始位置の集合に配置されたp台の制御対象物をq個の目標位置の集合に移動させるための行動制御を行う。行動制御システムは、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、各開始位置及び各目標位置は、それぞれ第一方向〜第四方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、制御対象物は、当該制御対象物の２次元平面上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、及び、第四方向において隣接する第四位置とし、静止するか、または、二次元平面上の第一〜第四位置の何れかに移動するように制御されるものとし、第一方向におけるある位置を通る、第二方向に平行な軸に向かって、平行な軸に対する距離が大きい、目標位置の集合に含まれる位置から順に、平行な軸に対する距離が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、平行移動させることで、中間形態隊列を成す制御対象物の位置を求める中間遷移過程処理部と、中間形態隊列を成す制御対象物に対して、目標位置の集合に含まれる位置から中間形態隊列を成す制御対象物の位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から中間形態隊列を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理部とを含む。
【０００８】
上記の課題を解決するために、本発明の他の態様によれば、行動制御方法は、p>qとし、p個の開始位置の集合に配置されたp台の制御対象物をq個の目標位置の集合に移動させるための行動制御を行う。行動制御方法は、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、各開始位置及び各目標位置は、それぞれ第一方向〜第四方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、制御対象物は、当該制御対象物の２次元平面上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、及び、第四方向において隣接する第四位置とし、静止するか、または、二次元平面上の第一〜第四位置の何れかに移動するように制御されるものとし、中間遷移過程処理部が、第一方向におけるある位置を通る、第二方向に平行な軸に向かって、平行な軸に対する距離が大きい、目標位置の集合に含まれる位置から順に、平行な軸に対する距離が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、平行移動させることで、中間形態隊列を成す制御対象物の位置を求める中間遷移過程処理ステップと、最終展開過程処理部が、中間形態隊列を成す制御対象物に対して、目標位置の集合に含まれる位置から中間形態隊列を成す制御対象物の位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から中間形態隊列を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理ステップとを含む。
【０００９】
上記の課題を解決するために、本発明の他の態様によれば、行動制御システムは、p>qとし、p台の制御対象物をq個の開始位置の集合からq個の目標位置の集合に移動させるための行動制御を行う。行動制御システムは、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ、(2)第一方向における座標値が座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ、(4)第二方向における座標値が座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させることで、押し出し終了時形状を成す制御対象物の位置を求める中間遷移過程処理部と、押し出し終了時形状を成す制御対象物に対して、目標位置の集合に含まれる位置から押し出し終了時形状を成す制御対象物の位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から押し出し終了時形状を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理部とを含む。
【００１０】
上記の課題を解決するために、本発明の他の態様によれば、行動制御方法は、p>qとし、p台の制御対象物をq個の開始位置の集合からq個の目標位置の集合に移動させるための行動制御を行う。行動制御方法は、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ、(2)第一方向における座標値が座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ、(4)第二方向における座標値が座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させることで、押し出し終了時形状を成す制御対象物の位置を求める中間遷移過程処理ステップと、押し出し終了時形状を成す制御対象物に対して、目標位置の集合に含まれる位置から押し出し終了時形状を成す制御対象物の位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から押し出し終了時形状を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理ステップとを含む。
【発明の効果】
【００１１】
本発明によれば、制御対象物に複雑な機構を備えなくとも、制御対象物同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ変形動作を行うことを可能とすることができるという効果を奏する。
【図面の簡単な説明】
【００１２】
【図１】ロボットの移動の例を説明するための図。
【図２】開始隊列の例を説明するための図。
【図３】ロボットの移動を説明するための図。
【図４】開始位置の集合を説明するための図。
【図５】図５Ａは開始位置の集合Sにて位置Psの属するマス単位に対してどの方向に4つのロボットが充填されたマス単位が接しているかに応じたaの値の違いを説明するための図、図５Ｂは開始位置の集合Sにて位置Psの属するマス単位に対してどの方向に4つのロボットが充填されたマス単位が接しているかに応じたaの値の違いを説明するための図、図５Ｃは開始位置の集合Sにて位置Psの属するマス単位に対してどの方向に4つのロボットが充填されたマス単位が接しているかに応じたaの値の違いを説明するための図、図５Ｄは開始位置の集合Sにて位置Psの属するマス単位に対してどの方向に4つのロボットが充填されたマス単位が接しているかに応じたaの値の違いを説明するための図。
【図６】開始隊列から押し出し隊列に変形する例を説明するための図。
【図７】押し出し隊列の例を説明するための図。
【図８】中間形態隊列の例を説明するための図。
【図９】中間形態隊列から目標隊列に変形する例を説明するための図。
【図１０】目標隊列の例を説明するための図。
【図１１】横押し出し位置対応付け処理の例を説明するための図。
【図１２】目標位置に対応付けられる押し出し隊列内位置を示す例を説明するための図。
【図１３】ボイド制御の例を説明するための図。
【図１４】ボイド制御の例を説明するための図。
【図１５】ボイド制御の例を説明するための図。
【図１６】開始隊列から押し出し隊列に変形する例を説明するための図。
【図１７】押し出し隊列の例を説明するための図。
【図１８】縦押し出し位置対応付け処理の例を説明するための図。
【図１９】中間形態隊列生成処理の例を説明するための図。
【図２０】中間形態隊列生成処理の例を説明するための図。
【図２１】中間形態隊列生成処理の例を説明するための図。
【図２２】第一実施形態に係る行動制御システムの機能ブロック図。
【図２３】第一実施形態に係る行動制御システムの処理フローの例を示す図。
【図２４】各格子が菱形での場合の例を示す図。
【図２５】ロボットの移動の例を説明するための図。
【図２６】ロボットの移動を説明するための図。
【図２７】中間位置の集合を説明するための図。
【図２８】上方向押し出し過程を説明するための図。
【図２９】旗押し出し過程を説明するための図。
【図３０】最終押し出し過程を説明するための図。
【図３１】マンハッタン距離δを説明するための図。
【図３２】ボイドの発生位置を説明するための図。
【図３３】ボイド制御の例を説明するための図。
【図３４】最終展開過程を説明するための図。
【図３５】ロボットの移動を説明するための図。
【図３６】Extend_Process_Sを説明するための図。
【図３７】中間位置の集合を説明するための図。
【図３８】押し出し終了時形状１から中間位置の集合M2への変形過程を説明するための図。
【図３９】押し出し終了時形状１から中間位置の集合M2への変形過程を説明するための図。
【図４０】押し出し終了時形状１から中間位置の集合M2への変形過程を説明するための図。
【図４１】Extend_Process_Gを説明するための図。
【図４２】Translate_from_M2_to_Mobile_Gを説明するための図。
【図４３】Make_Seed_M2を説明するための図。
【図４４】中間位置の集合M2におけるロボット入れ替え過程を説明するための図。
【発明を実施するための形態】
【００１３】
以下、本発明の実施形態について説明する。なお、以下の説明に用いる図面では、同じ機能を持つ構成部や同じ処理を行うステップには同一の符号を記し、重複説明を省略する。
【００１４】
＜第一実施形態＞
まず、行動制御システム及び方法の理論的背景について説明する。以下、行動制御の対象である制御対象物が、ロボットである場合を例に挙げて説明するが、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。
【００１５】
[問題設定]
多数のロボットが協調して開始位置における隊列形成状態から、各ロボットが接した状態を維持しつつ移動を行い、目標位置での隊列形成を行う任務は、例えば図１に例示するような、互いに接する辺同志をスライドさせて移動していくことが可能な正方形型のロボットの使用を想定し、図２に示すように、開始位置の集合Sから目標位置の集合Gまで複数のロボットの移動によって実現するものである。
【００１６】
ロボットについては、例えば図１に示すように、ロボットの周囲上下左右４マスのうち一つに他のロボットが存在している状態を維持しながら移動をするものとする。この手法では１つのロボット自身が、一台のロボットのサイズ分の距離を移動することで、一回の動作の移動量を正確に測ることができるというメリットがある。また、一つの辺を共有する隣り合うロボットとの相対的な位置を計測しあうことで、ロボットの群れ全体の中での各ロボットの位置も容易に知ることができる。このため、ロボットの移動量の誤差によって、隊列が崩れるといった問題を起こしにくい。また、図３のように、複数のロボットを連結したように、同時に複数のロボットを移動させていくことが可能である。図３では、隊列Aから隊列Bに変形する際に、及び、隊列Dから隊列Eに変形する際に二台のロボットが連結したように同時に移動する。なお、ロボットは、隣の位置に他のロボットが存在しているか否か、そして、自身が目標位置上にいるかどうかを知ることができるものとする。例えば、何れのロボットもロボットの周囲のマスに存在する他のロボットと通信できるものとし、少なくとも一台のロボットがGPSを備えるか、または、少なくとも１台のロボットが一回の行動制御において移動しない場合、GPSを備えるロボットや移動しなかったロボットを基準として、隣接するロボットと通信することで、各ロボットは自身の絶対的な位置を容易に知ることができる。もしくは、少なくとも一台のロボットの絶対位置が開始位置にて既知であるならば、それを使用してGPSなしでも、各時刻の各ロボットの絶対位置を知ることが出来る。
【００１７】
本手法では、ロボットの動作開始前に、行動制御システムにおいて、各ロボットの動作の順序をあらかじめ計算し、各ロボットはそれにしたがって動作するものとする。
【００１８】
任務を行うロボットは、p台（pは2以上の整数の何れかであり、例えばp≧50）であり、各ロボットは、隣接するロボットと一辺以上を共有しつつ（言い換えると、一辺以上を接した状態で）、二次元平面におけるX-Y軸方向（この例では、図面の紙面上下左右の四方向）に移動可能とする。ただし、p台のロボットで１つの群れを成すものとする。各格子にはロボットは一台しか存在することができない。それぞれのロボットは、移動しようとする方向に他のロボットがある場合には、静止をするものと仮定する。図２において、Rが記載された格子はロボットが存在する位置を示す。また、太線の破線で囲まれた領域は開始位置の集合Sを示し、太線の実線で囲まれた領域は目標位置の集合G（以下、この領域のことを「目標隊列エリアG」ともいう）を示す。
【００１９】
田の字状に隣接した4つのロボットを一つの単位とする（以下、「ロボット単位」ともいう）。言い換えると、四台毎に１つのロボット単位を構成し、１つのロボット単位を構成する四台のロボットはそれぞれ２つの方向において１つのロボット単位を構成する他のロボットと隣接する。このロボット単位の集団は、開始位置の集合において、互いにロボット単位ごとに一辺を共有する。なお、4台のロボットがなすロボット単位に対応する四つのマスを一つのマスの単位（以下「マス単位」ともいう）とする。
【００２０】
目標位置の集合Gに含まれる目標位置の総数をqとし、ロボットの台数pは、目標位置の総数qよりもa個多いものとする。p=q+aである。なお、なお、pは開始位置の集合Sに含まれる開始位置の総数とも言える。aの値は、開始位置の集合Sの形状により異なる。開始位置の集合Sの形状は、田の字状に隣接した4つのロボットがなすロボット単位が連なった形で構成される芯の部分と、それに接する形のロボット単位をなさない端数（1〜３個）のロボットで構成されるものとする。目標位置の集合Gがなす形状には、隣り合う各目標位置が上下左右で連なる形でさえあれば特に制限はない。言い換えると、各目標位置は、それぞれ上下左右方向の少なくとも何れかの方向において他の目標位置と隣接し、ロボットの目標位置での隊列形状は一塊の任意の形状である。
【００２１】
なお、開始位置の集合Sは以下の３つの条件を満たすことを前提とする。(1)開始位置の集合Sが芯の部分から飛び出している部分は芯の部分に隣接するマス単位に含まれるものとする（図４参照）。(2)開始位置の集合Sが芯の部分から飛び出している部分は、かぎ型のように飛び出したL字型とはならないものとする。（3）芯となるロボット単位一つに対し、飛び出している部分が接するロボット単位の辺は一つ以内に収めるものとする。（図４参照）。
【００２２】
[ロボットの座標設定]
それぞれのロボットi（iはロボット番号を表す、i=0,1,2,3,…,p-1）の初期位置(開始位置)を（Xr0[i],Yr0[i]）とし、目標位置を（Xre[j],Yre[j]）(jは目標位置の番号を表す、j=0,1,2,3,…,q-1、p=q+a)とするとき、本問題は、開始位置に配置されたロボットが、目標位置まで移動するための行動計画を求めることと定義できる。
【００２３】
開始位置（Xr0[i],Yr0[i]）の集合Sと目標位置（Xre[j],Yre[j]）の集合Gとは接している位置が一点以上あり、その開始位置の集合S側の位置の内、X座標が最も小さい位置をPs（Xs,Ys）とし、原点(0,0)とする(図２参照)。目標位置の集合Gに属するすべての位置は、位置Psよりもy座標値は大きい値であり、開始位置の集合Sに属するすべてのロボットのy座標値は、位置Psのy座標値以下の値であるとする。また、位置Psの属するマス単位には４つのロボットが充填されているものとする。
【００２４】
開始位置の集合Sに属するロボット数pと、目標位置の集合Gに属するロボット数qの差をaとしたとき、aの値は、開始位置の集合Sにおいて、位置Psの属するマス単位に隣接する4つのロボットが充填されているマス単位の個数や、その相対位置関係に応じて異なる。図５に、開始位置の集合Sにて、位置Psの属するマス単位に対してどの方向に4つのロボットが充填されたマス単位が接しているかに応じたaの値の違いを図示する。図５において、破線で示したのは、q個のロボットの目標位置の集合G内への移動が済んだ後の残りのa個のロボットの隊列の様子である。例えば、図５Ａの場合、位置Psの属するマス単位に上方向にのみ接するマス単位に4つのロボットが充填されていて、a=8である。q個のロボットの目標位置の集合G内への移動が済んだ後にこのようになるのは、開始位置の集合Sにおいて、これと同様に、位置Psの属するマス単位に上方向にのみ４つのロボットが充填されたマス単位が存在していて、その他の方向には、ロボットが接してはいない状態であった場合である。図５Ｂの場合は、開始位置の集合Sにおいて、位置Psの属するマス単位に上と左方向に接するマス単位に4つのロボットが充填されていた場合の、q個のロボットの目標位置の集合G内への移動が済んだ後の残りのa個のロボットの隊列の様子を示しており、a=16である。図５Ｃの場合は、開始位置の集合Sにおいて、位置Psの属するマス単位に上と右方向に接するマス単位に4つのロボットが充填されていた場合の、q個のロボットの目標位置の集合G内への移動が済んだ後の残りのa個のロボットの隊列の様子を示しており、a=12である。図５Ｄの場合は、開始位置の集合Sにおいて、位置Psの属するマス単位に上と左右方向に接するマス単位に4つのロボットが充填されていた場合の、q個のロボットの目標位置の集合G内への移動が済んだ後の残りのa個のロボットの隊列の様子を示しており、a=20である。ここで、Psの位置は位置Psの属するマス単位内において、左下位置にあるものとしている。なお、位置Psの属するマス単位に隣接するマス単位にロボットが存在する場合には、隣接するマス単位に必ず4つのロボットが充填されているように開始位置の集合Sを設定する。また、位置Psの属するマス単位に隣接するマス単位のうち、少なくとも1つのマス単位は目標位置の集合Gの一部を含むため、4つのロボットが充填されていない。
【００２５】
[ロボット動作の定義]
行動主体は部屋に配置されている各ロボットとなる。ロボットiの行動aは、静止、上下左右方向への１格子分の移動の計５種類のうちのいずれかを取る。例えば、a[i]∈{0,1,2,3,4}として、
0:静止
1:二次元平面上でX座標値が増える方向（図2で右方向）に１格子だけ移動する
2:二次元平面上でY座標値が増える方向（図2で下方向）に１格子だけ移動する
3:二次元平面上でX座標値が減る方向（図2で左方向）に１格子だけ移動する
4:二次元平面上でY座標値が減る方向（図2で上方向）に１格子だけ移動する
とする。
【００２６】
[探索計算上の問題点]
このような任務環境における状態空間は、ロボット数×2の次元数の状態を持ち、かつ選択可能な行動数は、ロボットの行動（＝５通り）のロボット数乗だけ存在する。例えば、ロボット数が５０で、部屋の縦横方向の格子数がそれぞれ２０であるとすれば状態数は２０の100乗個にもなり、探索計算に要する資源の量は膨大なものとなる。さらにロボット数が１台増えるごとに、その状態数は400倍増加していくことになる。
【００２７】
[問題設定]の項で説明したように、ロボット同士が接しているという拘束条件を取り入れても、根本的な計算量の削減は難しく、複数のロボットを使用する場合の大きな問題となっている。
【００２８】
具体的には、例えば、多数のロボットを複数の目標位置に誘導する問題においては、一台のロボットがある位置へ至るための経路を探索計算により求めることができるとしても、各ロボットにどの目標位置を割り振るかが大きな問題となる。この目標位置の割り振りがうまくいかなければ、早めに目標位置内に到達したロボットが、後から来るロボットの移動を邪魔するような位置に居座ってしまい、全体でのロボットの隊列形成を困難にしてしまう。そのようなことを避けるための各ロボットへの目標位置への割り振り方を探索により計算することを考えた場合、ロボットの台数の階乗分だけの目標位置への割り振り方が存在するため、計算にかかる負荷はロボットの台数とともに劇的に増加する。
【００２９】
また、ロボットが互いに一辺を共有しつつ移動を行うための探索計算について考えた場合の探索計算も、各ロボットのお互いの移動を考慮したうえで行うために、必要な計算量はロボットの台数に対して指数関数的に増えてしまう。
【００３０】
[三つの動作過程の導入]
本実施形態では、これらの計算負荷の問題を解決するための方策の一つとして、ロボットの動作の過程を３つの段階にわけて考える。一つ目は、目標位置の集合Gの隊列を形成するロボットを開始位置の集合Sから位置Psを経て開始位置の集合Sの外に押し出すロボット押し出し過程であり、二つ目は、押し出されたロボット群を、目標位置の集合Gの隊列形状に遷移させるための中間形態に変形する中間遷移過程であり、三つ目はその中間形態から、目標位置の集合Gの隊列形態に展開動作を行う最終展開過程である。これらのうち、一つ目の過程でのみ動的計画法による探索計算を行う。
【００３１】
[ボイド制御]
これらの一つ目のロボット押し出し過程にて、本実施形態では、ボイド制御の考え方を導入する。まず、ここでいうボイドとは、あるロボットが別の位置に移動した後に、元いた位置にできる空隙のことである。別の言い方をすると、ボイドとは、ロボットの移動する方向と反対の方向に移動する仮想的な存在である。こうした群ロボットの隊列形成問題においては、複数のロボットの動作に着目するがゆえに、その探索計算量が爆発してしまうが、視点を変えて、ボイドの動きに着目すれば、多数のロボットの動作計画の問題を単一のボイドの動作計画として考えることができ、探索計算負荷の軽減に適している。
【００３２】
ここで、開始位置の集合Sに属するロボット群のうち、位置Psにあるロボットを頭部ロボットとし、その動作について考えてみる。本実施形態では、各ロボットは、開始位置の集合Sから位置Psの属するマス単位を経由して開始位置の集合Sの外に出るものとする。まず、頭部ロボットが位置Psより、開始位置の集合Sの外側に出たとすると、位置Psの位置に一つのボイドが発生する。続いて、そのボイドは頭部ロボット以外の、ボイド位置に隣接しているロボットに順次埋められて行く。そうすることでボイドは、ロボット群の中を移動していく。それを繰り返して、最終的に、位置Psからもっとも遠い、ロボットが存在する位置（以下「尾部ロボット位置」ともいう）にボイドが到達したら、また、位置Psにあるロボットを次の新しい頭部ロボットに設定すればよい。
【００３３】
このようなボイドの動作は、頭部ロボットの位置Psをスタート位置とし、尾部ロボット位置をゴール位置とした、一つのボイドの動作計画によって計算可能であり、計算負荷も小さい。例えば、位置Psから最もマンハッタン距離が大きい、ロボットが存在する位置（尾部ロボット位置）を計算し、何れかの経路に沿って位置Psから尾部ロボット位置までボイドを移動させればよい。この計算において考慮すべきボイドの移動の拘束条件としては、ボイドが移動する際に、ロボットが存在する位置をたどっていくという制限を設けるだけでよい。
【００３４】
[ロボット押し出し過程]
ロボットの押し出し過程として、本実施形態では、2通りの過程を用意し、そのいずれかを使用することにする。一つ目は、横方向押し出し過程であり、二つ目は縦方向押し出し過程である。
【００３５】
[横方向押し出し過程]
ロボット押し出し過程として、横方向押し出し過程を使用した場合のロボットの動作の様子を図２及び図６〜１０に示す。横方向押し出し過程は図２、６及び７である。横方向押し出し過程では、位置Psからロボットをある数だけ押し出す動作と、押し出されたロボットを右方向（Xの正方向）に1マスずらす動作の繰り返しでロボットを開始位置の集合Sの外に押し出す。このように押し出されたロボットの隊形（以下「押し出し隊列」ともいう）は、目標位置の集合Gに位置するロボットの隊形（以下「目標隊列」ともいう）へ容易に変形することができる。
【００３６】
まず、目標位置の集合Gに属する各目標位置(Xre[j],Yre[j])を、全ロボットの開始位置の集合Sからの押し出しが完了した時点での各ロボットがとる隊列（押し出し隊列）内の位置に対応付けるための計算（押し出し位置対応付け処理）を行う。jは目標位置のインデックスであり、j=0,1,2,…,q-1である。押し出し位置対応付け処理の計算過程は以下のとおりである。ここで、目標位置の集合Gに属し、y座標の値がi_yである目標位置の集合をG_line(i_y)とする。また、目標位置jの押し出し隊列内番号をjp[j]と定義する。押し出し隊列内位置を(Xp[jp],Yp[jp])とする。jp=0,1,2,…,q-1である。
【００３７】
＜横押し出し位置対応付け処理＞
(1)  各目標位置(Xre[j],Yre[j])をG_line(i_y)に振り分ける。例えば、図２の場合、G_line(1)={(0,1)}、G_line(2)={(0,2)}、G_line(3)={(0,3)}、G_line(4)={(-1,4),(0,4),(1,4),(8,4),(9,4),(10,4),(11,4)}、…、G_line(13)={(-4,13),(-3,13),(3,13),(4,13),}である。
(2)  i_y←1とする。カウンタ変数cnt←0とする。
(3)  G_line(i_y)に属する要素があれば、そのもっともX座標の小さな目標位置jのjp[j]の値をcntとし（jp[j]←cnt）、cntをインクリメントする(cnt←cnt+1)。さらに、変数x_min[i_y]の値を目標位置jのx座標値とし（x_min[i_y]←Xre[j]）、(4)へ移行する。要素がなければ(5)へ移行する。例えば、図２の場合、x_min[1]←0,x_min[2]←0,x_min[3]←0,x_min[4]←-1,…,x_min[13]←-4とする。
【００３８】
(4)  G_line(i_y)内に、目標位置jの次にX座標が小さな位置があれば、それを目標位置jとする。目標位置jのjp[j]の値をcntとし、cntをインクリメントして、(4)を繰りかえす。なければ、変数x_max[i_y]の値を目標位置jのx座標値とし（x_max[i_y]←Xre[j]）、i_yをインクリメントして(i_y←i_y+1)、(3)に戻る。例えば、図２の場合、x_max[1]←0,x_max[2]←0,x_max[3]←0,x_max[4]←11,…,x_max[13]←4とする。(3)と(4)を繰り返すことで、目標位置の集合Gにおけるjp[j]の値は図２のようになる。
(5)  現、i_y値を変数y_maxに記録し(6)へ移行する。例えば、図２の場合、y_max←14とする。
(6)  j=0〜q-1の全ての値について、Yp[jp[j]]←Yre[j]とする。この処理により、押し出し隊列内位置のy座標値Yp[jp[j]]を目標位置jのy座標値Yre[j]とする。
【００３９】
(7)  カウンタ変数cnt←１とする。
(8)  Xp[0]←0とする。G_line(1)内に、一つしか要素がないなら(10)へ移行する。そうでないなら、(9)へ移行する。なお、Xp[0]は位置Ps(0,0)にy方向において隣接する、目標位置の集合G側のマスであり、位置Psの定義上、Xp[0]←0となる。なお、位置Psは、開始位置の集合と目標位置の集合とが接している位置であって、開始位置の集合側の位置であって、X座標が最も小さい位置であり、原点(0,0)である。
(9)  G_line(1)の要素のうち、cnt+1番目にX座標が小さい目標位置があるとき、それを目標位置jとして、Xp[jp[j]]←cntとする。cntをインクリメントし(9)を繰りかえす。ないとき(10)へ移行する。
【００４０】
(10)  i_y←2とする。i_y=y_maxならば、終了し、そうでない場合(11)へ移行する。
(11)  G_line(i_y)の要素数n[i_y]が、G_line(i_y-1)の要素数n[i_y-1]より大きいとき、G_line(i_y)の要素のうち、1番目にX座標が小さい目標位置を目標位置jとして、Xp[jp[j]]←Xp[jp[j]-n[i_y-1]]とする。左端を揃える処理に相当する。G_line(i_y)の要素数n[i_y]が、G_line(i_y-1)の要素数n[i_y-1]以下の時、G_line(i_y)の要素のうち、1番目にX座標が小さい目標位置を目標位置jとして、Xp[jp[j]]←Xp[jp[j]-1]-(n[i_y]-1)とする。右端を揃える処理に相当する。
(12)  G_line(i_y)の要素のうち、目標位置jの次にX座標が小さい目標位置があれば、それを次の目標位置jとして、Xp[jp[j]]←Xp[jp[j]-1]+1とし、(12)を繰りかえす。なければ、(13)へ移行する。
(13)  i_yをインクリメントし、i_y=y_maxならば終了する。そうでなければ、(11)へ移行する。(6)〜(13)の処理を行うことで、図７の押し出し隊列におけるjp[j]の値は図１１のようになる。
【００４１】
jp[j]の値を介して、図２の目標隊列内位置と図７の押し出し隊列内位置とが対応付けられる（図２と図１１参照）。言い換えると、この処理の結果計算された押し出し隊列内位置(Xp[jp],Yp[jp])と目標位置jの押し出し隊列内番号jp[j]により、目標位置(Xre[j],Yre[j])と、押し出し隊列内位置(Xp[jp[j]],Yp[jp[j]])との対応付けが計算できる（図２及び図１１参照）。押し出し隊列は、まず、目標隊列位置を同じy座標を持つもの同士でX方向に隙間がないように圧縮したとして、そのあと、y座標値i_yを持つ目標位置がなす行内の目標位置数n[i_y]が、ｙ座標値i_y-1を持つ目標位置でなされる行内の目標位置数より少ない場合はその右端位置を、y座標i_y-1を持つ目標位置でなされる行の右端に合わせ、そうでない場合は、その左端位置を合わせていくことにより(上述の手順(11)に対応)、図７のような形に形成される。なお、図７に示す押し出し隊列は、位置PsからYの正方向に一つずつロボットを押し出していくには好都合な形状である。
【００４２】
続いてロボットの押し出し順序を計算する。（押し出し順序計算処理）。押し出し順序がk番目の目標位置番号をjk[k]とする。
＜押し出し順序計算処理＞
(1)  Xp_max←Xp[q-1],Yp_max←Yp[q-1]とする(図１１参照)。cnt←1とし、jp[j]=q-1となるjをjk[0]の値に格納する（図１２参照）。
(2)  Xp_max=Xp[jp_next]かつYp_max-1=Yp[jp_next]となるjp_next値を探す。つまり、押し出し隊列において位置(Xp_max,Yp_max)に位置するロボットのYの負方向にロボットjp_nextが存在するか否かを判定する。
見つかれば、jp[j]=jp_nextとなるjをjk[cnt]に格納する(jk[cnt]←j)。Xp_max←Xp[jp_next]、Yp_max←Yp[jp_next]とし、cntをインクリメントする。
なければ、Xp_max-1=Xp[jp_next]を満たすjp_nextの中で、最もYp[jp_next]値の大きいものを選び、jp[j]=jp_nextとなるjをjk[cnt]に格納する(jk[cnt]←j)。Xp_max←Xp[jp_next]、Yp_max←Yp[jp_next]とし、cntをインクリメントする。
(3)  cnt=qでなければ、(2)を繰り返す。であれば終了。
【００４３】
以上の処理は、押し出し隊列内のもっとも右下にある位置に対応する目標位置に向かうロボットから順に押し出されることを意味する。すなわち、最初に位置Psから開始位置の集合Sの外へ押し出されるロボットは目標位置jk[0]へ向かうロボットであり、以降、ｋ番目に押し出されるロボットは、目標位置jk[k]へ向かうロボットということになる。図１２にjk[k]の値が指す目標位置に対応付けられる押し出し隊列内位置を示す。
【００４４】
以下、実際にロボットを位置Psから押し出していく処理を述べる（押し出し制御処理）。変数robot_target_index[i]を定義し、ロボットiが向かうべき目標位置の番号を格納する。
＜押し出し制御処理＞
(1)  k←0とする。
(2)  位置Psにあるロボットをロボットiとし、robot_target_index[i]←jk[k]とする。
(3)  k=0のときは、なにもせず(4)へ移行する。
k≠0のときは、Xp[jp[jk[k]]]=Xp[jp[jk[k-1]]]であるかどうかを判定する。
Xp[jp[jk[k]]]=Xp[jp[jk[k-1]]]のとき、(4)へ移行する。
Xp[jp[jk[k]]]≠Xp[jp[jk[k-1]]]のとき、開始位置の集合S内にない全てのロボットをa=1方向に1マス移動させて、(4)へ移行する。
【００４５】
(4)  ロボットiをロボットi_pushed_1とし（i_pushed_1←i）、ロボットiとX座標が同じで、Y座標が一つ小さな値のロボットをロボットi_pushed_2とする。ロボットi_pushed_1とロボットi_pushed_2、さらに、ロボットi_pushed_1とX座標が同じで、ロボットi_pushed1よりY座標が大きな値のロボット全て（ロボットi_pushed_1とX座標が同じで、S内にないロボット全て）を同時に行動a=2の方向に1マス移動させる。kをインクリメントする。なお、ロボットi_pushed_1とロボットi_pushed_2とを一緒に同じ方向に移動させることで、開始位置の集合の内側の群れと、開始位置の集合の外側の群れとの接続状態を維持することができる。
(5)  (4)で生じたボイドの位置（ロボットi_pushed_2の元あった位置）を位置void_endとする。
(6)  位置void_endを、ロボットi_pushed_1とロボットi_pushed_2以外のS内のロボットで埋めていくことで、もっとも位置void_endから離れた位置のロボットを現在位置より、1マス分位置void_endに近づける。
【００４６】
(7)  k=qのときは(8)へ移行する。k≠qのときは(2)へ移行する。この(1)〜(7)の処理により、図２の開始位置の集合に位置する隊列（以下「開始隊列」ともいう）は、図６の隊列に変形する。
(8)  xk←0とする。Xp_max←Xp[q-1],Yp_max←Yp[q-1]とする。
(9)  xk=Xr[i]となるS内にないロボットiをひとつ選択する。
(10)  (9)で選択されたロボットiのy座標Yr[i]とYp[jp[robot_target_index[i]]]を比較し、等しくない場合は、Xr[i]≧xkとなる全てのS外のロボットをa=2方向に1マス移動させ(9)へ移行する。等しい場合は、xkをインクリメントし(11)へ移行する。
(11)  xk>Xp_maxでなければ(9)へ移行し、そうであれば終了する。この(8)〜(11)の処理により、図６の隊列は、図７の押し出し隊列に変形する。
【００４７】
以上の処理のうち、手順(6)(ボイド制御処理Robot_Void_Control)について、以下の通り述べる。
＜ボイド制御処理Robot_Void_Control＞
(1)前回、本処理Robot_Void_Controlが実行された際のi_tail値を変数pre_i_tailに格納する(pre_i_tail←i_tail)。本処理が実行されるのがこれが最初ならば、pre_i_tail←-1とする。
(2)各ロボットが位置void_endからどれだけの距離離れているかを計算する。以下、この処理をロボット序列計算処理Robot_Joretu_Decisionともいう。ロボット序列計算処理Robot_Joretu_Decisionでは、各位置の序列変数値Joretu[Xr][Yr]を計算する。ロボット序列計算処理Robot_Joretu_Decisionについては後述する。
【００４８】
(3)(a)pre_i_tailが-1以外の値のとき（本処理を行うのが2回目以降のとき）で、ロボットpre_i_tailが一時刻ステップ前に所属していたマス単位の中に位置するロボットがある場合は、その中で最もvoid_endから離れているロボット（joretu値のもっとも大きい位置にあるロボット）を選択し、ロボットi_tailとする（選択したロボットの番号をi_tailに代入する）。(b)pre_i_tailが-1以外の値のときで、ロボットpre_i_tailが一時刻ステップ前に所属していたマス単位の中に位置するロボットが一つもなく、他に４つのロボットが充填されていないマス単位があるときは、そのようなマス単位内にありかつ、その中で最もvoid_endから離れているロボット（joretu値のもっとも大きい位置にあるロボット）を選択し、ロボットi_tailとする。(b)のような選択を行うことで、4つのロボットが充填されているロボット単位を構成するようにし、ロボット単位を構成しない端数のロボット数が４未満となるようにしている。(c)pre_i_tailが-1以外の値のときで、ロボットpre_i_tailが一時刻ステップ前に所属していたマス単位の中に位置するロボットが一つもなく、他に４つのロボットが充填されていないロボット位置単位がない場合は、尾部ロボット群S_tailに所属するロボットのうち、最もvoid_endから離れているロボット（joretu値のもっとも大きい位置にあるロボット）を選択し、ロボットi_tailとする。なお、位置Psを含むロボット単位をj_headとし、開始位置の集合S内に位置し、ロボット単位j_headに含まれないロボットの群を尾部ロボット群S_tailという。(d)pre_i_tailが-1のとき（本処理を行うのが1回目のとき）、尾部ロボット群S_tailに所属するロボットのうち、最もvoid_endから離れているロボット（joretu値のもっとも大きい位置にあるロボット）を選択し、ロボットi_tailとする。例えば、図１３の状態において、ロボットi_pushed_1とi_pushed_2とが下方向に移動すると、図１４に示すようにボイドが位置void_endに発生し、ロボットi_tailが設定される。なお、i_pushed_1とi_pushed_2とが開始位置の集合Sから出る行動をa_head（=2である）ともいう。
【００４９】
(4)ロボットi_tailを用いて以下の位置void_startを求める。ロボットi_tailの位置にボイドが存在すると仮定し、そのボイドがロボットi_tailの位置から、隣接したロボットをたどって同じ方向に2マス以上移動可能であり、2マス以上移動した先にあるロボットの所属するロボット単位が、ロボット単位j_headではなく、かつ、内に４つのロボットが所属しているという条件を満たす方向を探す。その方向と同じ方向に移動する行動をa_taiｌとする(図１４参照)。続いて、行動a_tailの方向に2マス以上移動した先にあるロボットの所属するロボット単位の番号j_startを特定する。言い換えると、ロボットi_tailの位置から、同じ方向dに2台以上ロボットが連続して隣接しており、その方向dにおいて、ロボットi_tailから2マス以上離れたマス（２つ以上離れた位置）にあり、連続して隣接するロボットの何れかが所属するロボット単位が、ロボット単位j_headではなく、かつ、内に４つのロボットが所属しているという条件を満たすとき、方向dに移動する行動をa_taiｌとする。上述の条件（ロボットi_tailから2つ以上離れた位置にあり、連続して隣接するロボットの何れかが所属し、ロボット単位j_headではなく、かつ、内に４つのロボットが所属しているという条件）を満たすロボット単位の番号j_startを特定する。なお、上述の条件を満たすロボット単位は、複数個存在しうるので、その中から任意のロボット単位を、ロボット単位j_startとして特定しうる。
【００５０】
行動a_tailとロボット単位j_startの位置(Xr_unit[j_start],Yr_unit[j_start])とから、以下のようにから位置void_startを設定する(図１４参照)。
a_tail=1のとき：void_startのX座標を2×Xr_unit[j_start]+1、Y座標をYr[i_tail]とする。
a_tail=2のとき：void_startのX座標をXr[i_tail]、Y座標を2×Yr_unit[j_start]+1、とする。
a_tail=3のとき：void_startのX座標を2×Xr_unit[j_start]、Y座標をYr[i_tail]とする。
a_tail=4のとき：void_startのX座標をXr[i_tail]、Y座標を2×Yr_unit[j_start]、とする。
【００５１】
ただし、(Xr_unit[j],Yr_unit[j])（j=0,1,…,j_max-1）はロボット単位の位置を表し、そのロボット単位に所属するロボットをi1,i2,i3,i4とすれば、その位置は、
Xr[i1]=2×Xr_unit[j]
Yr[i1]=2×Yr_unit[j]
Xr[i2]=2×Xr_unit[j]+1
Yr[i2]=2×Yr_unit[j]
Xr[i3]=2×Xr_unit[j]
Yr[i3]=2×Yr_unit[j]+1
Xr[i4]=2×Xr_unit[j]+1
Yr[i4]=2×Yr_unit[j]+1
である。
(5)位置void_startから、位置void_endにいたるボイドの移動経路を計算する。以下、この処理をボイド経路計算処理Void_Trajectory_Decisionともいう。
(6)  (5)で計算されたボイドの移動経路を、void_endから_void_startに逆にたどり、ボイドを埋めていく。以下、この処理をボイド移動処理Void_Motionともいう。
【００５２】
なお、処理(4)は、最後尾ロボットを動かす際に、ロボットの接続状態を維持するためには、「ボイドの位置が、最後尾ロボットが属するマス単位に隣接するロボット単位に含まれるロボットのうち、最後尾ロボットが属するマス単位に接するロボットの位置であってはならない」ことを考慮したものである。よって、制御システムは、最後尾ロボットが属するマス単位に隣接するロボット単位に含まれるロボットのうち、最後尾ロボットが属するマス単位に接するロボットの位置にボイドが存在しないように、ｐ台のロボットの行動を制御する。図１４において、ボイドが移動経路NGを用いた場合、ロボットi_tailが上下左右方向において隣接するロボットがなくなってしまい、p台のロボット全てが一つの群れを成すことができず、ロボット同士の接続を維持することができない。そこで、位置void_endにあるボイドを一旦、位置void_startに移動させ、その後、一気にロボットi_tailの位置に移動させるように制御する。このような行動制御を行うことで、p台のロボット全てが一つの群れを成し、ロボット同士の接続を維持することができる。
【００５３】
なお、上述のボイド制御を適切に行うために、位置Psの属するマス単位に隣接するマス単位にロボットが存在する場合には、隣接するマス単位に必ず4つのロボットが充填されているように開始位置の集合Sを設定しなければならず、開始位置の集合の個数pを目標位置の集合の個数qよりもa個多く設定なければならない。
【００５４】
以下、このような制御を実現するためのロボット序列計算処理Robot_Joretu_Decision、ボイド経路計算処理Void_Trajectory_Decision、ボイド移動処理Void_Motionについて説明する。
(ロボット序列計算処理Robot_Joretu_Decision)
ロボット序列計算処理Robot_Joretu_Decisionは、ダイクストラ法などを用いることができる。例えば以下のとおりである。
(1)各ロボットにて、上下左右４方向に隣接するロボットの番号を調べる。ロボットiの行動aの移動方向にロボットi_nextが隣接して存在する場合、next[a][i]変数の値をi_nextに設定する。ロボットが存在しないときは、next[a][i]変数の値をp(ロボットの台数を表す)に設定する。
(2)状態空間内（Xr,Yr）各位置の序列変数値Joretu[Xr][Yr]の値をpに初期化する(Joretu[Xr][Yr]←p)。
【００５５】
(3)void_endの位置の序列変数値Joretuの値を0とする(Joretu[Xr][Yr]←0)。さらに、本処理を実行する直前に行動a_headによって移動した全てのロボットの位置の序列変数値Joretuの値を-1に初期化する。さらに、それが所属するロボット単位が、4台のロボットで充填されていないロボットの位置の序列変数値Joretuの値を-1に初期化する。さらに、void_startの位置から行動a_tailの逆向きに隣接するロボット位置のJoretu値を-1にする。さらにS外のロボットの位置のJoretu値を-１に設定する。
(4)void_endの位置と、void_endから、行動a_headの移動方向に隣接してたどれるロボットの位置以外の、各ロボットiの、全行動aについて、next[a][j]の値がpではない場合の、Joretu[Xr[next[a][i]]][Yr[next[a][i]]]の値を調べ、Joretu[Xr[next[a][i]]][Yr[next[a][i]]]の値が-1でない場合で、かつ、その最小値に１を加えた値が、現在のJoretu[Xr[i]][Yr[i]]よりも小さい場合は、その値をJoretu[Xr[i]][Yr[i]]に代入する（Joretu[Xr[i]][Yr[i]]←Joretu[Xr[next[a][i]]][Yr[next[a][i]]]+1）。
(5)  (4)の処理にて、Joretu値の更新がなくなるまで、(4)を繰り返す。
【００５６】
この処理により、(a)行動a_headによって移動した全てのロボットの位置、(b)そのロボットが所属するロボット単位が4台のロボットで充填されていない場合には、そのロボット単位に所属する他のロボットの位置、(c)void_startの位置から行動a_tailの逆向きに隣接するロボットの位置を除き、位置void_endから離れた位置の序列変数値Joretu[Xr[i]][Yr[i]]ほど大きな値となるように更新される。図１５の角括弧内の数値は、図１４において、更新がなくなったときの序列変数値Joretuの値の例を示す。
【００５７】
(ボイド経路計算処理Void_Trajectory_Decision)
ボイド経路計算処理Void_Trajectory_Decisionは、以下のとおりである。
(1)位置void_startの位置をボイド軌道(X,Y)=(void_trj_x[h],void_trj_y[h])(h=0,1,2,3…)の最初の位置とする。
(2)軌道番号h_trj←0とする。
(3)現ボイド軌道位置（void_trj_x[h_trj],void_trj_y[h_trj]）から、全方向にて隣接する位置にあるロボットiのうち、そのロボットiの位置における序列値Joretu[Xr[i]][Yr[i]]が、-1以外であって、かつ、Joretu[void_trj_x[h_trj]][void_trj_y[h_trj]]よりも小さいロボットiの位置を次のボイド軌道位置（void_trj_x[h_trj+1],void_trj_y[h_trj+1]）とする。その際の、ボイドの移動の方向と同じ行動の番号をボイド行動履歴a_void[h_trj]に格納する。h_trjをインクリメントする。
(4)現ボイド軌道位置が、位置void_endに等しくなるまで(3)を繰り返す。
【００５８】
この処理により、位置void_endから位置void_startまでのボイドの経路を計算する。
(ボイド移動処理Void_Motion)
ボイド移動処理Void_Motionは、例えば、以下のとおりである。計算されたボイド軌道に沿って、同じ方向に動くロボットを一度に移動させている。もちろん、一度に移動させず一台ずつ動かしてもよい。
(1)h_trj_buffer←h_trj-1とする。
(2)h_trjをデクリメントし、a_void[h_trj]の値を、a_void_bufferに格納する。
(3)h_trj=0でなければ、h_trjをデクリメントし、a_void[h_trj]=a_void_bufferであるか否かを判定し、a_void[h_trj]=a_void_bufferのとき、(3)を繰りかえす。そうでなければ(a_void[h_trj]≠a_void_bufferのとき)、ボイド軌道(void_trj_x[h_trj_buffer],void_trj_y[h_trj_buffer])から、(void_trj_x[h_trj+1],void_trj_y[h_trj+1])までの位置にあるロボットを皆、a_void_bufferに格納された行動値で移動させ(4)にいく。この処理により、同じ方向に動くロボットを一度に移動させている。h_trj=0のとき、ボイド軌道(void_trj_x[h_trj_buffer],void_trj_y[h_trj_buffer])から、(void_trj_x[0],void_trj_y[0])までの位置にあるロボットをa_void[0]に格納された行動値で移動させ(5)にいく。
(4)h_trjをインクリメントし、(1)に戻る。
(5)ロボットi_tailから行動a_tailの向きに隣接し、位置void_startに至るまでの位置にあるロボットを皆、行動a_tailで移動させる。なお、この移動は、一台ずつではなく、全て一緒に移動させる必要がある。この処理により位置void_startにあるボイドをロボットi_tailの位置に一気に移動させ、ロボットi_tailに隣接するロボットが無くなることを防ぐ。
【００５９】
以上の処理により、ロボット押し出し過程の1つである横方向押し出し過程を実現することができる。次に縦方向押し出し過程について説明する。
[縦方向押し出し過程]
ロボット押し出し過程として、縦方向押し出し過程を使用した場合のロボットの動作の様子を図２、図１６、図１７及び図８〜１０に示す。縦方向押し出し過程は図２、図１６及び図１７である。本過程では、まず、ロボットがY軸に平行な列をなすように押し出し、そのあと押し出される各ロボットが、Y軸に平行な列をなすロボットをガイドにして、それぞれの押し出し隊列内でのY座標の高さまで移動していき、その後X軸正方向に枝を伸ばしていくように押し出し隊列を形成する。以下、処理の詳細について説明する。
【００６０】
(1)  開始位置の集合Sと目標位置の集合Gが接するS内側の位置PsよりX座標が１大きい位置をPaxisとする（図１６参照）。位置Psと位置Paxisは、同じロボット単位内の位置である。目標位置の集合Gのy座標の最大値をy_maxとする。図２の例では、y_max←13とする。
(2)  yｋ←1とする。
(3)  位置Psにあるロボットを選択し、ロボットi_pushed_1とする。
(4)  ロボットi_pushed_1とX座標が同じで、Y座標が一つ小さな値のロボットをロボットi_pushed_2とする。ロボットi_pushed_1とロボットi_pushed_2を行動a=2の方向に1マス移動させる。同時に、すでに開始位置の位置の集合Sの外のロボットを全て、行動a=2方向に1マス移動させる。ykをインクリメントする。
(5)  (4)で生じたボイド（ロボットi_pushed_2の元あった位置）を位置void_endとする。
【００６１】
(6)  位置void_endを、ロボットi_pushed_1とロボットi_pushed_2以外のロボットで埋めていくことで、もっともを位置void_endから離れた位置のロボットを現在位置より、1マス分位置void_endに近づける。例えば、前述のボイド制御処理Robot_Void_Controlを行う。
(7)  yk>y_maxのときは、S外の全てのロボットをa=1の方向へX座標一つ分移動し、(8)へ移行する。そうでないならば、(3)へ移行する。なお、yk>y_maxのとき、Paxisと接し、Y軸に平行な列が完成する（図１６参照）。
(8)  yk←1とする。
(9)  目標位置jのうち、Y座標の値がykであるものの個数をカウントし、cnt_yk[yk]とする。例えば、図１６の場合、yk=1のときcnt_yk[1]=1、yk=2のときcnt_yk[2]=1、yk=3のときcnt_yk[3]=1、yk=4のときcnt_yk[4]=7、…、yk=13のときcnt_yk[13]=4である。
【００６２】
(10)  Y座標がykに等しく、X座標が1以上の値のロボットの個数cnt_endをカウントする。cnt_yk[yk]=cnt_endであればykをインクリメントし(17)へ移行し、cnt_yk≠cnt_endであれば(11)へ移行する。
(11)  X座標が0で、Y座標がykに等しいロボットがあるかどうか判定する。あるなら、Y座標がykに等しいロボットをすべてa=1方向に1マス移動する。
(12)  開始位置の集合Sの外で、X座標が0で、Y座標がykより小さいロボットをa=2方向に1マス移動させる。
(13)  現yk値にて、位置Psから開始位置の集合Sの外へ押し出したロボット数が、cnt_yk[yk]より少ないならば、(14)へ移行する。そうでないなら(10)へ移行する。
【００６３】
(14)  位置Psにあるロボットをi_pushed_1とし、ロボットi_pushed_1とX座標が同じで、Y座標が一つ小さな値のロボットをロボットi_pushed_2とする。ロボットi_pushed_1とロボットi_pushed_2を行動a=2の方向に1マス移動させる。
(15)  (14)で生じたボイド（ロボットi_pushed_2の元あった位置）を位置void_endとする。
(16)  位置void_endを、ロボットi_pushed_1とロボットi_pushed_2以外のS内のロボットで埋めていくことで、もっともを位置void_endから離れた位置のロボットを現在位置より、1マス分位置void_endに近づける。例えば、前述のボイド制御処理Robot_Void_Controlを行う。(10)へ移行する。
(17)  yk>y_maxのときは終了。そうでないならば、(9)へ移行する
以上の処理のうち、(1)〜(7)は、図２の開始隊列を図１６の隊列に変形するためのものであり、(8)以降は、図１６の隊列を図１７の押し出し隊列に変形するためのものである。続いて、縦方向押し出し過程の場合の押し出し位置対応付け処理について述べる。
【００６４】
＜縦押し出し位置対応付け処理＞
以下の手順の内、(1)〜(6)は横方向押し出し過程における押し出し位置対応付け処理と同様である。
(1)  各目標位置(Xre[j],Yre[j])をG_line(i_y)に振り分ける。
(2)  i_y←1とする。カウンタ変数cnt←0とする。
(3)  G_line(i_y)に属する要素があれば、そのもっともX座標の小さな目標位置jのjp[j]の値をcntとし（jp[j]←cnt）、cntをインクリメントする(cnt←cnt+1)。さらに、変数x_min[i_y]の値を目標位置ｊのx座標値とし（x_min[i_y]←Xre[j]）、(4)へ移行する。要素がなければ(5)へ移行する。
(4)  G_line(i_y)内に、目標位置jの次にX座標が小さな位置があれば、それを目標位置jとする。目標位置jのjp[j]の値をcntとし（jp[j]←cnt）、cntをインクリメントして(cnt←cnt+1)、(4)を繰りかえす。なければ、変数x_max[i_y]の値を目標位置jのx座標値とし（x_max[i_y]←Xre[j]）、i_yをインクリメントして(i_y←i_y+1)、(3)に戻る。
【００６５】
(5)  現、i_y値を変数y_maxに記録し(6)へ移行する。
(6)  j=0〜q-1の全ての値について、Yp[jp[j]]←Yre[j]とする。
(7)  カウンタ変数cnt←１とする。
(8)  Xp[0]←1とする。G_line(1)内に、一つしか要素がないなら(10)へ移行する。そうでないなら、(9)へ移行する。
(9)  G_line(1)の要素のうち、cnt+1番目にX座標が小さい目標位置があるとき、それを目標位置jとして、Xp[jp[j]]←cnt+1とする。cntをインクリメントし(9)を繰りかえす。ないとき(10)へ移行する。
【００６６】
(10)  i_y←2とする。i_y=y_maxならば、終了し、そうでない場合(11)へ移行する。
(11)  G_line(i_y)の要素のうち、目標位位置のX座標が最小のものをjとし、Xp[jp[j]]←１とする。各行i_yの左端をx=1に揃える。
(12)  G_line(i_y)の要素のうち、目標位置jの次にX座標が小さい目標位置があれば、それを次の目標位置jとして、Xp[jp[j]]←Xp[jp[j]-1]+1とし(12)を繰りかえす。なければ、(13)へ移行する。
(13)  i_yをインクリメントし、i_y=y_maxならば(14)へ移行する。そうでなければ、(11)へ移行する。
(14)  押し出しの済んだ各ロボットiについて(Xr[i],Yr[i])=（Xp[jp[j]],Yp[jp[j]]）となるjを探し、robot_target_index[i]←jとする。
【００６７】
jp[j]の値を介して、図２の目標隊列内位置と図１７の押し出し隊列内位置とが対応付けられる（図２と図１８参照）。言い換えると、この処理の結果計算された押し出し隊列内位置(Xp[jp],Yp[jp])と目標位置jの押し出し隊列内番号jp[j]により、目標位置(Xre[j],Yre[j])と、押し出し隊列内位置(Xp[jp[j]],Yp[jp[j]])との対応付けが計算できる（図２及び図１８参照）。なお、図１７に示す押し出し隊列は、位置PsからYの正方向に一つずつロボットを押し出していくには好都合な形状である。
【００６８】
[中間遷移過程]
この過程では、まず、目標位置の集合G（図１９参照）を中間形態隊列位置（図２１参照）に変形させるための処理を計算する。ここでいう中間形態隊列は、ちょうど目標隊列（図１０参照）の左右から見えない壁が目標隊列等を圧縮するように迫ってきて、その壁に押されて目標隊列がX座標0の点を通りy方向に平行な軸に向かって、集まった隊列(図８参照)であり、中間形態隊列位置は中間形態隊列を成すロボットの位置である（図１９〜２１参照）。そのような中間形態隊列は、押し出し隊列（図７及び図１７参照）からの変形が容易であり、かつ、中間形態隊列から目標隊列への変形は、この圧縮処理の逆再生であるから、ロボット同士の接続もその変形過程で違反が生じないというメリットがある。
【００６９】
以下、中間形態隊列生成処理について述べる。変形過程の目標位置の履歴を記録する変数を(Xr_hist[j][t],Yr_hist[j][t])とする。
＜中間形態隊列生成処理＞
(1)  目標位置jのうち、X座標が最大のもののX座標の値を壁のX座標値X_wallに格納する。例えば、図１９の場合、X_wall←11とする。
(2)  すべての行動a値、目標位置j値についてdest_next[a][j]←−１に初期化する。(Xr_hist[j][0],Yr_hist[j][0])←(Xre[j],Yre[j])とし、t←1とする。
(3)  各目標位置jの行動a=3方向(左方向)に隣接する目標位置の番号を変数dest_next[3][j]に格納する。ある目標位置jについて、左隣の位置が目標位置であれば、その目標位置の番号をdest_next[3][j]に格納する。
(4)  Xre[j]=X_wallであるロボットjすべてについて以下を行う。
a.  まずj_0←jとする。
b.  dest_next[3][j_0]=-1となるまで、j_0の値をdest_next[3][j_0]の値に置き換える処理（j_0←dest_next[3][j_0]）を繰り返す。要は、左端に存在する目標位置を見つけ、その目標位置の番号をj_0に格納する。
c.  b.の結果得られるj_0について、Xre[j_0]>0であるならば、目標位置j、j_0とその二つの目標位置に挟まれる全ての目標位置を左方向に1マス移動させる。
【００７０】
(5)  (Xr_hist[j][t],Yr_hist[j][t])←(Xre[j],Yre[j])とする。ｔをインクリメントする。X_wallをデクリメントする。X_wall=0ならば(6)へ移行する、そうでなければし(3)へ移行する。(1)〜(5)の処理により、X座標x=0より右側に存在する目標位置を、X座標x=0を通るy方向に平行な軸に集める。図１９の目標位置を図２０の中間形態隊列位置の生成途中の状態に変形する。
(6)  目標位置jのうち、X座標が最小のもののX座標値を壁のX座標値X_wallに格納する。例えば、図１９の場合、X_wall←−５とする。
(7)  すべての行動a値、目標位置j値にてdest_next[a][j]←−１に初期化する。
(8)  各目標位置jの行動a=1方向(右方向)に隣接する目標位置の番号をdest_next[1][j]変数に格納する。ある目標位置jについて、右隣の位置が目標位置であれば、その目標位置の番号をdest_next[1][j]に格納する。
【００７１】
(9)  Xre[j]=X_wallであるロボットjすべてについて以下を行う。
a.  まずj_0←jとする。
b.  dest_next[1][j_0]=-1となるまで、j_0の値をdest_next[1][j_0]の値に置き換える処理（j_0←dest_next[1][j_0]）を繰り返す。要は、右端に存在する目標位置を見つけ、その目標位置の番号をj_0に格納する。
c.  b.の結果得られるj_0について、Xre[j_0]<0であるならば、目標位置j、j_0とその二つの目標位置jに挟まれる全ての目標位置を右方向に1マス移動させる。
(10)  (Xr_hist[j][t],Yr_hist[j][t])←(Xre[j],Yre[j])とする。ｔをインクリメントする。X_wallをデクリメントする。X_wall=0ならばt_all=t-1として終了し、そうでなければ(8)へ移行する。(6)〜(10)以上の処理により、X座標x=0よりも左側に存在する目標位置を、X座標0を通るy方向に平行な軸に集める。図２０の中間形態隊列の生成途中の状態を図２１の中間形態隊列に変形する。
【００７２】
以上の処理により、目標位置の集合から中間形態隊列位置への変形履歴が変数(Xr_hist[j][t],Yr_hist[j][t])（t=0,…,t_all）に記録される。
【００７３】
押し出し隊列から中間形態隊列へのロボットの移動処理は以下のとおりである。単純に各y値のロボット行にて各ロボットのX座標が中間形態隊列内でのそれに合うまで、移動すればよい。
＜中間遷移処理＞
(1)  yk←1とする。
(2)  Y座標の値がykに等しいロボットiを一台選択する。
(3)  Xr[i]>Xr_hist[robot_target_index[i]][t_all]ならば、Y座標がyk以上のロボットすべてを行動a=3方向(左方向)に1マス移動させる。Xr[i]<Xr_hist[robot_target_index[i]][t_all]ならば、Y座標がyk以上のロボットすべてを行動a=1方向(右方向)に1マス移動させる。Xr[i]=Xr_hist[robot_target_index[i]][t_all]ならば、ykをインクリメントし、(4)へ移行する。そうでないなら(2)に戻る。
(4)  Y座標の値がykに等しいロボットがあるなら(2)へ移行する、ないなら終了する。
以上の処理により、図７または図１７の押し出し隊列から図８の中間形態隊列に変形する。
【００７４】
[最終展開過程]
最終展開処理は、変数(Xr_hist[j][t],Yr_hist[j][t])に格納された目標位置jの移動の履歴をt=t_allからt=0まで逆にたどらせればよい。
＜最終展開処理＞
(1)  ｔ←t_all-1とする。
(2)  各ロボットiについて、（Xr_hist[robot_target_index[i]][t],Yr_hist[robot_target_index[i]][t]）と(Xr[i],Yr[i])の値を比較し、移動先が、（Xr_hist[robot_target_index[i]][t],Yr_hist[robot_target_index[i]][t]）になるような行動で1マス移動する。tをデクリメントする。
(3)  ｔ<０ならば終了、そうでないなら(2)へ移行する。
以上の処理により、図８の中間形態隊列から図９を経て図１０の目標位置の集合に変形する。
【００７５】
以下、これらの処理を実行する構成について説明する。
＜第一実施形態に係る行動制御システム１００＞
図２２は第一実施形態に係る行動制御システム１００の機能ブロック図を、図２３はその処理フローの例を示す。行動制御システム１００は、図２２に示すように、行動選択部１２０と、隣接状態判定部１２４と、位置更新部１２３と、位置判定部１２６と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。行動選択部１２０は押し出し過程処理部１２０Ａと中間遷移過程処理部１２０Ｂと最終展開過程処理部１２０Ｃとを含む。
【００７６】
以下では、制御の対象となる制御対象物が、ロボットである場合を例に挙げて説明する。もちろん、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。
【００７７】
本実施形態では、行動制御システム１００は、p台のロボットの行動を制御し、p台のロボットの内の１つのロボット上に実装される。なお、行動制御システム１００が実装されていないp-１台のロボットについても、通信部１５０と、隣接状態判定部１２４とを含む。
【００７８】
＜入力部１６０＞
入力部１６０には、p台のロボットiのそれぞれの初期位置(Xr0[i],Yr0[i])(言い換えると、p個の開始位置の集合S={(Xr0[0],Yr0[0]),  (Xr0[1],Yr0[1]),  …,  (Xr0[p-1],Yr0[p-1])})及びq個の目標位置の集合G={(Xre[0],Yre[0]),  (Xre[1],Yre[1]),…,(Xre[q-1],Yre[q-1])}が入力され、記憶部１４０に記憶される。
【００７９】
＜通信部１５０＞
行動制御システム１００が実装されているロボットも含め、全てのロボットは、通信部１５０を介して、二次元平面上の上下左右方向(以下「４方向」ともいう)において隣接する他のロボットと通信することができる。
【００８０】
＜行動選択部１２０＞
行動選択部１２０は、上述の方法で、p台のロボットを制御する（ｓ１２０）。なお、押し出し過程処理部１２０Ａ、中間遷移過程処理部１２０Ｂ、最終展開過程処理部１２０Ｃでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部１４０に記憶しておく。行動選択部１２０は、記憶部１４０から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。
【００８１】
（押し出し過程処理部１２０Ａ）
行動選択部１２０の押し出し過程処理部１２０Ａは、記憶部１４０からp個の開始位置の集合S及びq個の目標位置の集合を取り出し、p個の開始位置の集合Sに位置する少なくともq台のロボットを移動させ、押し出し隊列を成すように制御し、押し出し隊列における各ロボットの位置を中間遷移過程処理部１２０Ｂに出力する。
【００８２】
なお、押し出し隊列は、中間形態隊列と同じy座標値に位置するロボットの個数が同じで、かつ、同じy座標値に位置するロボット全てがx軸方向において連続して隣接している隊列であり、横方向押し出し過程、または、縦方向押し出し過程により生成される隊列である。
【００８３】
［横方向押し出し過程］
横方向押し出し過程の場合、押し出し隊列は、y座標値i_yに位置するロボットの個数n[i_y]と、y座標値i_y-1に位置するロボットの個数n[i_y-1]との大小関係に応じて、y座標値i_yにおいてx軸方向において連続して隣接しているロボットの一端または他端のロボットと、y座標値i_y-1においてx軸方向において連続して隣接しているロボットの一端または他端のロボットとを第二方向において合わせた隊列である。本実施形態では、個数n[i_y]が個数n[i_y-1]よりも大きいとき、y座標値i_yにおいてx軸方向において連続して隣接しているロボットの左端とy座標値i_y-1においてx軸方向において連続して隣接しているロボットの左端とを合わせ、個数n[i_y]が個数n[i_y-1]以下のとき、y座標値i_yにおいてx軸方向において連続して隣接しているロボットの右端とy座標値i_y-1においてx軸方向において連続して隣接しているロボットの右端とを合わせた隊列を押し出し隊列としている。
【００８４】
押し出し過程処理部１２０Ａは、記憶部１４０からp個の開始位置の集合S及びq個の目標位置の集合を取り出し、＜横押し出し位置対応付け処理＞を行い、押し出し隊列におけるロボットの位置と、目標位置におけるロボットの位置を対応付ける。次に、押し出し過程処理部１２０Ａは、＜押し出し順序計算処理＞により、開始位置の集合から押し出されたロボットが押し出し隊列のどの位置に移動するかを決定する。押し出し過程処理部１２０Ａは、＜押し出し制御処理＞により、押し出し隊列の同じx座標値に位置するのロボットの個数分だけ、開始位置の集合からロボットをy軸の正方向に移動させ、その後、開始位置の集合の外にあるロボットを全てx軸の正方向に移動させる処理を繰り返し、図２の開始隊列を図６の隊列に変形する。その後、＜押し出し制御処理＞により、x座標値の小さい順にロボットを選択し、押し出し隊列におけるロボットの位置と一致するか否かを判定し、一致しない場合には、そのx座標値以上のx座標値をもつロボットを全てy軸の正方向に移動させることで、図６の隊列を図７の押し出し隊列に変形する。
【００８５】
［縦方向押し出し過程処理］
押し出し過程処理部１２０Ａは、記憶部１４０からp個の開始位置の集合S及びq個の目標位置の集合を取り出し、＜縦方向押し出し過程＞により、ロボットがy軸に平行な列をなすように、開始位置の集合からロボットを移動させ、その後、y軸に平行な列に沿って、他のロボットをy軸の正方向に、中間形態隊列内でのy座標値の位置まで移動させ、その後、x軸の正方向に移動させることで、押し出し隊列を形成する。その後、＜縦押し出し位置対応付け処理＞により、押し出し隊列におけるロボットの位置と目標位置におけるロボットの位置とを対応付ける。
【００８６】
（中間遷移過程処理部１２０Ｂ）
行動選択部１２０の中間遷移過程処理部１２０Ｂは、q個の目標位置の集合を取り出し、＜中間形態隊列生成処理＞により、あるx座標値を通る、y軸に平行な軸(本実施形態ではx=0)に向かって、目標位置の集合に含まれる位置を平行移動させる(圧縮する)ことで、中間形態隊列を成すロボットの位置を求める。このとき、平行な軸に対する距離が大きい、目標位置の集合に含まれる位置から順に、平行移動させる。平行移動させる際に、目標位置の集合に含まれる他の位置と隣接する場合には、その隣接する位置と一緒に平行移動させる。目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部１２０Ｃに出力する。
【００８７】
中間遷移過程処理部１２０Ｂは、押し出し隊列におけるロボットの位置を押し出し過程処理部１２０Ａから受け取り、＜中間遷移処理＞により、中間形態隊列におけるロボットの位置と押し出し隊列におけるロボットの位置とを比較し、押し出し隊列を、同じy座標値毎にx方向に平行移動させることで、中間形態隊列に変形する。
【００８８】
（最終展開過程処理部１２０Ｃ）
行動選択部１２０の最終展開過程処理部１２０Ｃは、中間遷移過程処理部１２０Ｂから受け取った、目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを使用して、[最終展開過程]により、中間形態隊列を成すロボットに対して、目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の平行移動方向とは反対の方向にロボットを移動させることで、中間形態隊列を成すロボットを目標位置の集合に移動させる。
【００８９】
＜位置更新部１２３＞
位置更新部１２３は、各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がロボットで充填されているか否かを判定し（Ｓ１２７）、充填されているときは、任務を終了する。そうでないときは、任務を継続する。
【００９５】
例えば、図示しない目標位置到達判定部において、各i=0,1,…,p-1について、位置判定部１２６から出力された行動後位置（Xr"[i],Yr"[i]）により、全ての目標位置の集合Gがロボットで充填されているか否かを判定し、充填されている場合には、任務を終了する。少なくとも１つ以上の目標位置にロボットが充填されていない場合には、行動選択部１２０を再度実行するよう制御する。
【００９６】
以上に述べた処理ｓ１２０〜ｓ１２７を毎時刻ステップごとに行う。
＜効果＞
このような構成により、ロボットに複雑な機構を備えなくとも、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ変形動作を行うことを可能とすることができる。また、計算負荷がロボット数の階乗に比例せず、実時間で実行可能な低計画計算負荷での自己位置座標定義型隊列形成アルゴリズム獲得ができる。また、静止しているロボットに対する相対的な位置を判定することで、絶対的な位置を取得することができるため、付加的な位置計測用の装備を必要としない。または少なくとも一台のGPSを備えるロボットか、開始状態にて位置の定義されたロボットに対する相対的な位置を判定することで、絶対的な位置を取得することができるため、全てのロボットがGPS等を備えなくともよい。
【００９７】
＜変形例＞
開始位置の集合Sは図４の条件（(1)開始位置の集合Sが芯の部分から飛び出している部分は芯の部分に隣接するマス単位に含まれるものとする。(2)開始位置の集合Sが芯の部分から飛び出している部分は、かぎ型のように飛び出したL字型とはならないものとする。（3）芯となるロボット単位一つに対し、飛び出している部分が接するロボット単位の辺は一つ以内に収めるものとする。）を満たさずともよく、一塊の任意の形状を成せばよい。前述の通り、目標位置の集合Gは一塊の任意の形状であればよいため、図４の条件を満たす一塊の任意の形状を目標位置の集合Gに変形する際の行動制御を、逆の手順で、逆の方向に移動させることで、接続状態を維持したまま、目標位置の集合Gから図４の条件を満たす一塊の任意の形状に変更することができる。よって、図４の条件を満たさない、一塊の任意の形状を成す開始位置の集合S'を、一旦、図４の条件を満たす一塊の任意の形状に変形し（図４の条件を満たす一塊の任意の形状から一塊の任意の形状を成す開始位置の集合S'に変形する手順を考え、逆の手順で、逆の方向に移動させることで変形する）、変形後の形状（図４の条件を満たす一塊の任意の形状）を、一塊の任意の形状を成す目標位置の集合Gに変形すればよい（本実施形態における開始位置の集合が変形後の形状に対応するものとして計算すればよい）。ただし、開始位置の集合Sは少なくとも図５Ａの隊列を内部に含むか、その一部が図５Ａの隊列に変形できる形状であることを必要とする。
【００９８】
また、開始位置の集合S'と目標位置の集合Gとは、接していなくともよい。開始位置の集合S'を図４の条件を満たす一塊の任意の形状に変形し、その変形後の形状Sが、[ロボットの座標設定]で説明した条件を満たすように、移動させればよい。つまり、変形後の形状Sと目標位置の集合Gとは接している位置が一点以上あるように、形状Sを維持したまま移動し、その接している位置の形状S側の位置の内、X座標が最も小さい位置をPs（Xs,Ys）とし、原点(0,0)とする。このとき、目標位置の集合Gに属するすべての位置は、位置Psよりもy座標値は大きい値であり、形状Sに属するすべてのロボットのy座標値は、位置Psのy座標値以下の値となるように形状Sを維持したまま移動させればよい。また、位置Psの属するマス単位には４つのロボットが充填されるように移動させればよい。なお、形状Sを接続状態を維持したまま移動させる方法としては、どのような既存の技術を用いてもよい。
【００９９】
本実施形態では、各格子（マス）は、正方形であるが、他の形状であってもよい。格子は左右方向及び上下方向に連続して配置される。また、各格子は左右方向で他の二つの格子と隣接し、上下方向で他の二つの格子と隣接する。言い換えると、各格子は、ロボットの移動できる方向と同じ方向においてのみ、他の格子と隣接する。この条件を満たせば、各格子はどのような形状であってもよい。また、「直交」とは、厳密に「垂直に交わること」を意味しなくともよく、例えば、図２４のように、各格子は、菱形であってもよく、各格子が他の二つの格子と隣接する方向の一方を上下方向とし、他方を左右方向とすればよく、このとき、上下方向と左右方向とは直交するものとする。
【０１００】
別の言い方をすると、制御対象物は、二次元平面上の、第一方向（例えば右方向）、第一方向に対して平行でない方向である第二方向（例えば下方向）、第一方向に対して反対方向である第三方向（例えば左方向）、第二方向に対して反対方向である第四方向（例えば上方向）に移動可能であり、一回の行動制御により、現在いる領域（格子、マス）から、現在いる領域に対して、第一方向、第二方向、第三方向、第四方向において隣接する領域の何れかに移動するように制御される。また、この場合、ロボットの２次元平面上の、第一方向において隣接する位置を第一位置、第二方向において隣接する位置を第二位置、第三方向において隣接する位置を第三位置、第四方向において隣接する位置を第四位置と呼んでもよい。
【０１０１】
本実施形態では、行動制御システム１００が、p台のロボットの内の１つのロボット上に実装される例を示したが、コンピュータ上の制御対象物に対して行動制御を行ってもよい。その場合、意図的にトラブルを発生させない限り、制御した通りに制御対象物は移動するため、想定位置（Xr'[i],Yr'[i]）と移動後位置（Xr"[i],Yr"[i]）とが一致する。そのような状態で、各制御対象物i毎の理想的な行動計画を行ってもよい。その結果得られた任務を終了するまでの行動計画を実ロボットに実行させる。行動制御システム１００は少なくとも行動選択部１２０を含み、少なくとも行動計画を行えばよい。実ロボットは、通信部１５０と隣接状態判定部１２４を備え、各行動を終えた後に、隣接状態を判定し、判定結果を用いて、行動後位置を求め、行動後位置（Xr"[i],Yr"[i]）と想定位置（Xr'[i],Yr'[i]）とが一致するか否かを判定する。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。その後の処理については第一実施形態と同様でもよいし、他の処理を行ってもよい。このような構成により、少なくとも制御通りに移動できなかったことを検知することができる。
【０１０２】
＜第二実施形態＞
第一実施形態と異なる部分を中心に説明する。
まず、本実施形態における行動制御システム及び方法の理論的背景について説明する。
[問題設定]
多数のロボットが協調して開始位置における隊列形成状態から、各ロボットが接した状態を維持しつつ移動を行い、目標位置での隊列形成を行う任務は、例えば図２５に例示するような、互いに接する面同志をスライドさせて移動していくことが可能な立方体型のロボットの使用を想定し、図２６に示すように、開始位置の集合Sから目標位置の集合Gまで複数のロボットの移動によって実現するものである。
【０１０３】
本実施形態では、開始位置の集合Sから目標位置の集合Gへの隊列形成の際に中間位置の集合Mを経る形で隊列形成を行う。以下、中間位置の集合Mから目標位置の集合Gへの変形について説明する。なお、開始位置の集合Sから中間位置の集合Mへの変形については、中間位置の集合Mから目標位置の集合Gへの変形と同じ手法で、変形動作を計算し、その動作の時系列の履歴を逆再生するようにロボットの動作を制御することで実現する。
【０１０４】
また、本実施形態では、開始位置の集合Sと目標位置の集合Gにおいて、各ロボットがとるべき位置は、各ロボット毎に決まっているものとする。そのため、開始位置の集合Sから変形して中間位置の集合Mにあるロボット群は、その形態から目標位置の集合Gへの変形において、各ロボットが適切に目標位置の集合G内の各目標位置にたどり着くことが出来るように、中間位置の集合Mの形態を維持しつつ、各ロボット同士の位置の入れ替え動作も行う。
【０１０５】
ロボットについては、例えば図２５に示すように、ロボットの周囲上下左右前後６マスのうち一つに他のロボットが存在している状態を維持しながら移動をするものとする。この手法では１つのロボット自身が、一台のロボットのサイズ分の距離を移動することで、一回の動作の移動量を正確に測ることができるというメリットがある。また、一つの面を共有する隣り合うロボットとの相対的な位置を計測しあうことで、ロボットの群れ全体の中での各ロボットの位置も容易に知ることができる。このため、ロボットの移動量の誤差によって、隊列が崩れるといった問題を起こしにくい。また、図３のように、複数のロボットを連結したように、同時に複数のロボットを移動させていくことが可能である。図３では、隊列Aから隊列Bに変形する際に、及び、隊列Dから隊列Eに変形する際に二台のロボットが連結したように同時に移動する。なお、図３の例では、移動方向において、二台のロボットが連結したように同時に移動しているが、移動方向に対して直交する方向において二台のロボットが連結したように同時に移動してもよい。例えば、X軸正方向に移動する場合に、Y軸方向またはZ軸方向において二台のロボットが連結したように同時に移動してもよい。ここで、ロボットは、隣の位置に他のロボットが存在しているか否か、そして、自身が目標位置上にいるかどうかを知ることができるものとする。例えば、何れのロボットもロボットの周囲のマスに存在する他のロボットと通信できるものとし、少なくとも一台のロボットがGPSを備えるか、または、少なくとも１台のロボットが一回の行動制御において移動しない場合、GPSを備えるロボットや移動しなかったロボットを基準として、隣接するロボットと通信することで、各ロボットは自身の絶対的な位置を容易に知ることができる。もしくは、少なくとも一台のロボットの絶対位置が開始位置にて既知であるならば、それを使用してGPSなしでも、各時刻の各ロボットの絶対位置を知ることが出来る。
【０１０６】
本手法では、ロボットの動作開始前に、行動制御システムにおいて、各ロボットの動作の順序をあらかじめ計算し、各ロボットはそれにしたがって動作するものとする。
【０１０７】
任務を行うロボットは、p台（pは９以上の整数の何れか）であり、各ロボットは、隣接するロボットと一面以上を共有しつつ（言い換えると、一面以上を接した状態で）、三次元空間におけるX-Y-Z軸方向（この例では、図面の紙面上下左右前後の六方向）に移動可能とする。ただし、p台のロボットで１つの群れを成すものとする。各格子にはロボットは一台しか存在することができない。それぞれのロボットは、移動しようとする方向に他のロボットがある場合には、静止をするものと仮定する。なお、ロボットが存在しうる立方体状の空間をマス、または、格子ともいう。図２６において、各格子（立方体）はそれぞれロボットが存在する位置を示す。また、ピラミッドを上下逆にした図形（略四角錐形状であって、底面がなく、頭頂点が下に位置する図形）は開始位置の集合Sを示し、ピラミッドの上に旗が立った形状の図形（略四角錐形状であって、底面がなく、頭頂点が上に位置し、頭頂点の上に旗が立った形状の図形）は目標位置の集合G（以下、この領域のことを「目標隊列エリアG」ともいう）を示す。
【０１０８】
中間位置の集合Mにあるロボットは、田の字状に隣接した８つのロボットを一つの単位とし（以下、「ロボット単位」ともいう）、ロボット単位で構成された長方形形状の平板上の隊列を形成する。言い換えると、８台毎に１つのロボット単位を構成し、１つのロボット単位を構成する８台のロボットはそれぞれ３つの方向において１つのロボット単位を構成する他のロボットと隣接する。このロボット単位の集団は、中間位置の集合Mにおいて、互いにロボット単位ごとに一面を共有する。なお、８台のロボットがなすロボット単位に対応する８つのマスを一つのマスの単位（以下「マス単位」ともいう）とする。
【０１０９】
目標位置の集合Gに含まれる目標位置の総数をqとし、中間位置の集合Mに含まれるロボットの台数pは、目標位置の総数qよりもa個多いものとする。p=q+aである。aは、8以上15以下の整数であり、pの値が、8の倍数になるように選ばれる。これは中間位置の集合Mがマス単位で構成されるが故の条件である。なお、pは中間位置の集合Mに含まれる位置の総数とも言える。中間位置の集合Mの形状は、田の字状に隣接した８つのロボットがなすロボット単位が連なった形で構成される長方形平板、直線形、もしくはL字形平板の形状である。言い換えると、ロボット単位はL字または矩形の平板状を成すように配置される。中間位置の集合Mに含まれる位置のZ座標は、０か１の値のみをとる形とする。目標位置の集合Gがなす形状には、隣り合う各目標位置が上下左右前後で連なる形でさえあれば特に制限はない。言い換えると、各目標位置は、それぞれ上下左右前後方向の少なくとも何れかの方向において他の目標位置と隣接し、ロボットの目標位置での隊列形状は一塊の任意の形状である。
【０１１０】
[ロボットの座標設定]
それぞれのロボットi（iはロボット番号を表す、i=0,1,2,3,…,p-1）の初期位置(ここでは中間位置の集合Mから目標位置の集合Gへの変形を行うため、中間位置を初期位置とも呼ぶ)を（Xr0[i],Yr0[i],Zr0[i]）とし、目標位置を（Xre[j],Yre[j],Zre[j]）(jは目標位置の番号を表す、j=0,1,2,3,…,q-1、p=q+a)、開始位置を（Xrs[j],Yrs[j],Zrs[j]）(jは開始位置の番号を表す、j=0,1,2,3,…,q-1、p=q+a)とするとき、本問題は、中間位置に配置されたロボットが、目標位置まで移動するための行動計画を求めることと定義できる。
【０１１１】
中間位置（Xr0[i],Yr0[i],Zr0[i]）の集合Mに含まれる位置のうち、X、Y、Z座標がともに最も小さい位置を原点(0,0,0)とし、原点とX,Y座標が同じで、Z座標が１の位置をPs＝（0,0,1）、PsにY軸の正の向きで接している位置をPｒ=(0,1,1)とする（図２６参照、ただし、原点の位置は他のロボットにより見えない）。目標位置の集合Gに属するすべての位置は、位置PsよりもZ座標値は大きい値であるとする。
【０１１２】
中間位置の集合Mをなすマス単位の位置のうち、位置Psを含むマス単位をシード位置単位Useedと呼ぶことにする。シード位置単位Useedに対して、X軸方向の正方向には、必ず2つ以上のマス単位が連続して接していなければならない。すなわち、図２７に示すように、シード位置単位Useedを含む3つ以上のマス単位がX軸方向に直線をなす形で存在しているものとする。よって、pは24以上の8の倍数の何れかである。前述の通り、目標位置の集合Gに属するすべての位置は、シード位置単位Useedに含まれる位置PsよりもZ座標値は大きい値であり、シード位置単位Useedは目標位置の集合Gに属さない。p=q+aであり、シード位置単位Useedはaの中に含まれる。
【０１１３】
ロボット単位の位置を（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とすれば、
Xr[i1]=2xXr_unit[j]
Yr[i1]=2xYr_unit[j]
Zr[i1]=2xZr_unit[j]
Xr[i2]=2xXr_unit[j]+1
Yr[i2]=2xYr_unit[j]
Zr[i2]=2xZr_unit[j]
Xr[i3]=2xXr_unit[j]
Yr[i3]=2xYr_unit[j]+1
Zr[i3]=2xZr_unit[j]
Xr[i4]=2xXr_unit[j]+1
Yr[i4]=2xYr_unit[j]+1
Zr[i4]=2xZr_unit[j]
Xr[i5]=2xXr_unit[j]
Yr[i5]=2xYr_unit[j]
Zr[i5]=2xZr_unit[j]+1
Xr[i6]=2xXr_unit[j]+1
Yr[i6]=2xYr_unit[j]
Zr[i6]=2xZr_unit[j]+1
Xr[i7]=2xXr_unit[j]
Yr[i7]=2xYr_unit[j]+1
Zr[i7]=2xZr_unit[j]+1
Xr[i8]=2xXr_unit[j]+1
Yr[i8]=2xYr_unit[j]+1
Zr[i8]=2xZr_unit[j]+1
である。
【０１１４】
[ロボット動作の定義]
行動主体は部屋に配置されている各ロボットとなる。ロボットiの行動aは、静止、上下左右前後方向への１格子分の移動の計７種類のうちのいずれかを取る。例えば、a[i]∈{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倍増加していくことになる。
【０１１６】
[問題設定]の項で説明したように、ロボット同士が接しているという拘束条件を取り入れても、根本的な計算量の削減は難しく、複数のロボットを使用する場合の大きな問題となっている。
【０１１７】
具体的には、例えば、多数のロボットを複数の目標位置に誘導する問題においては、一台のロボットがある位置へ至るための経路を探索計算により求めることができるとしても、各ロボットにどの目標位置を割り振るかが大きな問題となる。この目標位置の割り振りがうまくいかなければ、早めに目標位置内に到達したロボットが、後から来るロボットの移動を邪魔するような位置に居座ってしまい、全体でのロボットの隊列形成を困難にしてしまう。そのようなことを避けるための各ロボットへの目標位置への割り振り方を探索により計算することを考えた場合、ロボットの台数の階乗分だけの目標位置への割り振り方が存在するため、計算にかかる負荷はロボットの台数とともに劇的に増加する。
【０１１８】
また、ロボットが互いに一面を共有しつつ移動を行うための探索計算について考えた場合の探索計算も、各ロボットのお互いの移動を考慮したうえで行うために、必要な計算量はロボットの台数に対して指数関数的に増えてしまう。
【０１１９】
[二つの動作過程の導入]
本実施形態では、これらの計算負荷の問題を解決するための方策の一つとして、ロボットの動作の過程を２つの段階にわけて考える。一つ目は、目標位置の集合Gの隊列を形成するロボットを中間位置の集合Mから位置Psか、位置Prを経て中間位置の集合Mの外に押し出すロボット押し出し過程であり、二つ目はその押し出された形態から、目標位置の集合Gの隊列形態に展開動作を行う最終展開過程である。一つ目の過程においては、以下に示すボイド制御を行う。
【０１２０】
[ボイド制御]
これらの一つ目のロボット押し出し過程にて、本実施形態では、ボイド制御の考え方を導入する。まず、ここでいうボイドとは、あるロボットが別の位置に移動した後に、元いた位置にできる空隙のことである。別の言い方をすると、ボイドとは、ロボットの移動する方向と反対の方向に移動する仮想的な存在である。こうした群ロボットの隊列形成問題においては、複数のロボットの動作に着目するがゆえに、その探索計算量が爆発してしまうが、視点を変えて、ボイドの動きに着目すれば、多数のロボットの動作計画の問題を単一のボイドの動作計画として考えることができ、探索計算負荷の軽減に適している。
【０１２１】
ここで、中間位置の集合Mに属するロボット群のうち、たとえば、位置Psにあるロボットを頭部ロボットとし、その動作について考えてみる。本実施形態では、各ロボットは、中間位置の集合Mから位置Psの属するマス単位を経由して開始位置の集合Sの外に出るものとする。まず、頭部ロボットが位置Psより、中間位置の集合Mの外側に出たとすると、位置Psの位置に一つのボイドが発生する。続いて、そのボイドは頭部ロボット以外の、ボイド位置に隣接しているロボットに順次埋められて行く。そうすることでボイドは、ロボット群の中を移動していく。それを繰り返して、最終的に、位置Psからもっとも遠い、ロボットが存在する位置（以下「尾部ロボット位置」ともいう）にボイドが到達したら、また、位置Psにあるロボットを次の新しい頭部ロボットに設定すればよい。
【０１２２】
このようなボイドの動作は、頭部ロボットの位置Psをスタート位置とし、尾部ロボット位置をゴール位置とした、一つのボイドの動作計画によって計算可能であり、計算負荷も小さい。例えば、位置Psから最もマンハッタン距離が大きい、ロボットが存在する位置（尾部ロボット位置）を計算し、何れかの経路に沿って位置Psから尾部ロボット位置までボイドを移動させればよい。この計算において考慮すべきボイドの移動の拘束条件としては、ボイドが移動する際に、ロボットが存在する位置をたどっていくという制限を設けるだけでよい。
【０１２３】
[ロボット押し出し過程]
ロボットの押し出し過程は、3段階の過程からなるものとする。一つ目は、位置Psから、Z軸正方向にロボットを押し出して、押し出されたロボットが位置Psから立つ柱の形状を形成する上方向押し出し過程である（図２８参照）。二つ目は、位置PrからZ軸正方向に押し出されたロボットが、その柱の形状にそって上昇した後、しかるべきZ座標値において、その柱にあるロボットを利用して、Y軸の負方向に一ステップだけ移動することで、旗上の形態を形成する旗押し出し過程である（図２９参照）。三つ目は、位置PrからZ軸正方向に押し出されたロボットが、シード位置単位Useed上を移動することで、旗押し出し過程で形成された旗面（押し出し済みのロボットのX軸正方向(X=0)にある面で構成された平面）に接する位置(本実施形態では位置(1,0,2))まで移動し、旗面上をしかるべき、Z座標値、Y座標値まで移動した後に、旗面上にあるロボットを利用して、X軸負方向へ１ステップ移動することで、最終展開過程の開始状態である圧縮形態(押し出し終了時形状)を形成するための最終押し出し過程である（図３０参照）。
【０１２４】
[各押し出し過程共通の押し出しボイド制御]
各押し出し過程で共通の中間位置の集合Mからのロボットの押し出しの制御は、ロボットの隊列制御開始前にあらかじめ実行される尾部マス単位決定用動作計画、隊列形成動作中に実行される各ロボット押し出し動作決定用動作計画からなる。本制御を押し出しボイド制御と呼ぶことにする。本制御における、ロボット中間位置の集合Mに含まれるマス単位の集合(中間位置単位の集合ともいう)をMUとする。
【０１２５】
[尾部マス単位決定用動作計画]
ロボットが動作を開始する前にあらかじめ、２つの動作計画のために使用する任務空間内の各位置単位のシード位置単位Useedからのマンハッタン距離(１つの「マス」におけるマンハッタン距離ではなく、８つのマスからなる「マス単位」におけるマンハッタン距離)の計算を行う。ただし、UseedにX方向に隣接して存在する２つのマス単位Unext1=（1,0,0）、Unext2=（2,0,0）のシード位置単位Useedからのマンハッタン距離はそれぞれ1,2に固定し、Useed、Unext1、Unext2以外のマス単位は、形式上、Unext2以外とは、接していないものとして扱う。言い換えると、Useed、Unext1、Unext2以外のマス単位は、Unext2を経由してUseed、Unext1と間接的に接する。そのために、まず、任務空間内の各位置単位（X,Y,Z）にて、シード位置単位Useedからの各位置単位へのマンハッタン距離δ[X][Y][Z]を以下の計算手続きで求める。なお、以下において、説明を簡単にするため「位置単位」を単に「位置」ともいい、「シード位置単位Useed」を単に「シード位置Useed」ともいう。
【０１２６】
[マンハッタン距離δの計算]
(1)中間位置単位の集合MU内の各位置（X,Y,Z）において、next[a][X][Y][Z]を用意し、隣接する行動aの方向に中間位置単位の集合MU内に含まれてUnext1,Useed以外である位置（X,Y,Z）が存在しない場合は、next[a][X][Y][Z]←0とし、それ以外の場合はnext[a][X][Y][Z]←１とする。なお、中間位置単位の集合MUは、ロボット単位で構成された長方形形状の平板上の隊列を形成するため、Z軸方向には隣接するロボット単位が存在しないことは明らかである。そのため、行動aは1〜4についてのみ計算すればよい。
(2)中間位置単位の集合MU内の各位置（X,Y,Z）のマンハッタン距離δ[X][Y][Z]を中間位置単位の集合MU内の位置単位数より大きな値s_maxに初期化する。
(3)シード位置Useedのマンハッタン距離δを0に、Unext1のマンハッタン距離δを1に、Unext2のマンハッタン距離δを2に初期化する。
(4)位置Unext2,Unext1,Useed以外の、中間位置単位の集合MU内の各位置（X,Y,Z）の全行動aについて、next[a][X][Y][Z]の値が0ではない場合の、行動aによって位置(X,Y,Z)から移動した先の位置（X',Y',Z'）で位置Unext1,Useed以外のもののうち、マンハッタン距離δ[X'][Y'][Z']を調べ、その最小値に１を加えた値が、現在のδ[X][Y][Z]よりも小さい場合は、その値をδ[X][Y][Z]に代入する。
(5)上述の(4)の処理にて、δ[X][Y][Z]値の更新がなくなるまで、(4)を繰り返す。
【０１２７】
以上の処理で計算されたマンハッタン距離δは、中間位置単位の集合MUの中のみでのロボットの移動を考慮したマンハッタン距離の値となる。図３１は、図２７における各位置単位のマンハッタン距離δを示す。位置Unext2,Unext1,Useed以外の位置のマンハッタン距離δは、3以上となる。本実施形態において、押し出し位置は、位置Psか位置Prであり、押し出し位置をPとする。Pとなる位置は、いずれもシード位置単位Useedに含まれる。この例では、押し出し方向a_headはa=5の方向(Z軸正方向への移動)である。マンハッタン距離δを使用して、尾部ロボット単位を決定するための手法Tail_Robot_Decisionを述べる。
【０１２８】
[Tail_Robot_Decision]
中間位置単位の集合MUに含まれる各ロボット位置単位（Xr_unit[j],Yr_unit[j],Zr_unit[j]）において、マンハッタン距離δ値が最大のものを選択して、尾部ロボット単位Tとする。また、常にシード位置Useedを以下の処理におけるロボット単位Hとする。
【０１２９】
本実施形態では、８つのロボットを組にしたロボット単位を維持しつつロボット群が移動を行うようにしているので、ボイド制御を行う際の、各ロボットの接続維持を考える際には、尾部ロボット単位Tとロボット単位H付近でのボイドの動きにさえ気を配れば、それ以外は、ボイドが各ロボット単位内に１つ以内であるようにすればよい。
【０１３０】
[各ロボット動作決定用動作計画]
以下、各ロボット動作決定用動作計画の動作について述べる。この動作計画では、８つのロボットからなる「ロボット単位」ではなく、１つの「ロボット」における動作計画を行う。各ロボット動作決定用動作計画においては、ロボット単位Hから一つのマス単位分のロボットを押し出すたびに、計８つのボイドが発生するので、まずは、それら発生した各々のボイドについてそのボイドがロボット単位Hから尾部ロボット単位Tまでたどるべき経路を計算する。
【０１３１】
ここで、目標位置の集合G内の位置のZ座標値の最大値をZmaxとすると、上方向押し出し過程で押し出されるロボットの台数は合計でZmax-1となる。ある時刻までに押し出し過程で押し出されたロボットの台数をi_poleとすると、0<Zmax-1-i_pole≦8のとき、次にロボット単位Hから押し出される８つのロボットのうち、上方向押し出し過程にて押し出し位置となるPsから押し出されるロボットの数はnp←Zmax-1-i_poleであり、旗押し出し過程以降の押し出し過程で押し出し位置となるPrから押し出されるロボットの数は、8-npである。なお、Zmax-1-i_pole>8のとき、次にロボット単位Hから押し出される８つのロボットは、全て位置Psから押し出され、Zmax-1-i_pole≦0のとき、次にロボット単位Hから押し出される８つのロボットは全て位置Prから押し出される。
【０１３２】
まず、ロボット単位Hに所属するロボットのうち、位置Psに行動a_headの移動方向の逆方向に接しているロボットをロボットi_move_ps、位置Prに行動a_headの移動方向の逆方向に接しているロボットをロボットi_move_prとする。
【０１３３】
ロボット単位Hからのロボット押し出しにおいては、ロボット単位H内の位置Psに位置するロボットとロボットi_move_ps,位置Prに位置するロボットとロボットi_move_prがペアとなって、a_head方向にスライドすることで、ロボットが移動する。そのため、ロボット単位H内にて発生するボイドの位置は、かならずロボットi_move_ps,i_move_prの移動前の位置のいずれかとなる。図３２は、a_head方向をZ軸正方向としたときの、スライド移動、及び、発生するボイドの位置の例を示す。ロボットが中間位置単位の集合MUから押し出し過程を開始してからの発生ボイドの通し番号をiv(=0,1,2,3…q-1)で管理し、フル充填されたロボット単位H（ロボット単位Hに8台のロボットが存在する状態）から押し出しが開始されたときに、それまでに発生しているボイドの数をiv_prev+1とすれば、ロボット単位Hが、中間位置単位の集合MUから押し出されるとき、iv_prev+1は8の倍数であり、iv_prevは、(8の倍数-1)である。ロボット単位Hからの押し出しで発生するボイドをvoid[iv_prev+k](k=1,2,3,4,5,6,7,8)とする。各ボイドの軌道を(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])(t=0,1,2,3…)で表現する。なお、(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])は時刻tにおけるボイドvoid[iv_prev+k]の位置を表し、tが変化することでボイドの軌道を表す。
【０１３４】
p台の制御対象物を、p個の中間位置の集合Mから押し出し終了時形状に変形させる際に、中間位置の集合の中から外へ制御対象物を移動させるときに中間位置の集合の中に生じたボイドがボイドが生じた位置単位（つまりロボット単位H）から、中間位置の集合の最も遠い位置に存在するロボットに移動するように制御する、
【０１３５】
各ボイドの軌道計算処理（Void_Trajectory_Decision）は、ロボット単位Tのδ値が3以上の場合に、以下の手順で行われる。なお、ロボット単位Tのδ値が2以下の場合には、後述のVoid_Trajectory_Decision_Seedが行われる。
[Void_Trajectory_Decision]
(1)各ボイドvoid[iv_prev+k]の発生位置を以下のようにする。
k≦npのとき、上方向押し出し過程(図２８参照)が終わっていないため、ロボットｉは位置Psから押し出される。このとき、void[iv_prev+k]の発生位置は、ロボットi_move_psの移動前の位置である。
k>npのとき、上方向押し出し過程(図２８参照)が終わっているため、ロボットｉは位置Prから押し出される(図２９、図３０)。このとき、void[iv_prev+k]の発生位置は、ロボットi_move_prの移動前の位置である。
(2)各ボイドの発生位置(ロボットi_move_psまたはロボットi_move_prの移動前の位置)を各ボイドの軌道の終点void_end[iv_prev+k]とする。
【０１３６】
(3）上述の(1)で発生位置を設定した８つのボイドがロボット単位Hの位置から尾部ロボット単位Tの位置までたどるべき位置単位の道筋(1つの「マス」ではなく8つのマスからなる「マス単位」での道筋)を(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])を決定する。tuの値は、尾部ロボット単位Tからロボット単位Hまでの道筋上の点を表し、尾部ロボット単位Tの位置をtu=0としてロボット単位Hに近づくにつれて大きくなるように８つのボイドの動きと逆向きに設定する。
(4)(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])をT'、(void_unit_trj_x[2],void_unit_trj_y[2],void_unit_trj_z[2])をT"とする。T'は尾部ロボット単位Tに隣接するロボット単位であり、T"はロボット単位T'に隣接するロボット単位である。
【０１３７】
(5)ロボット単位T'内にあり、尾部ロボット単位T内の位置に接しない位置にある４つのロボットをロボットi_tail_0,i_tail_1,i_tail_2,i_tail_3とし、各ボイドの目標位置を以下のように設定する。
void[iv_prev+1]、void[iv_prev+2]の目標位置→ロボットi_tail_0の位置
void[iv_prev+3]、void[iv_prev+4]の目標位置→ロボットi_tail_1の位置
void[iv_prev+5]、void[iv_prev+6]の目標位置→ロボットi_tail_2の位置
void[iv_prev+7]、void[iv_prev+8]の目標位置→ロボットi_tail_3の位置
(6)T"内における各ボイドの発生位置と同じ位置（ロボット単位HをT"の位置に平行移動させた場合のロボット単位H内の各ボイドの発生位置）から、T'内の各ボイド目標位置までの軌道(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])を後述するVoid_Local_Trajectory_Decisionで決定する。tの値は、T'内の目標位置で0としてT"内の各ボイド発生位置に近づくにつれて大きくなるようにボイドの動きと逆向きに設定する。
【０１３８】
(7)上述の(6)で決定された軌道(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])の終点(t=tsとする。)から、ロボット単位Hの位置内のボイド発生位置に至る軌道を(8)以下の処理で設定する。
(8)it←2とする。
(9)(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][ts+it-2],void_trj_y[iv_prev+k][ts+it-2],void_trj_z[iv_prev+k][ts+it-2])から１ステップ移動した位置を(void_trj_x[iv_prev+k][ts+it-1],void_trj_y[iv_prev+k][t+it-1],void_trj_z[iv_prev+k][t+it-1])とし、２ステップ移動した位置を(void_trj_x[iv_prev+k][ts+it],void_trj_y[iv_prev+k][ts+it],void_trj_z[iv_prev+k][ts+it])とし、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の位置に一致するまで(9)を繰り返す。(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]←ts+it-2として終了する。
【０１３９】
[Void_Local_Trajectory_Decision]
Void_Local_Trajectory_Decisionの処理は、以下のとおりである。まず、ロボットが動作を開始する前に、あらかじめ、以下の6つの位置関係にある尾部ロボット単位T及びロボット単位T'において、ロボット単位T'に5通りのロボット単位T"[kt](kt=1,2,3,4,5)が接しているとして、ロボット単位T'内の尾部ロボット単位Tに接しない位置とロボット単位T"の全ての位置について、ロボット単位T'内の尾部ロボット単位Tに接しない4つの位置からのマンハッタン距離δlocal[vs][al](X,Y,Z)(vs=0,1,2,3。al=1,2,3,4,5,6)を全て計算する。ここでは、８台のロボットからなるロボット単位ではなく、1台のロボットのマンハッタン距離を求める。なお、alはロボット単位Tの位置からロボット単位T'の位置に向かう６つの方向の行動であり、vsはロボット単位T'内の尾部ロボット単位Tに接しない4つのロボット(その位置)のインデックスである。
【０１４０】
(1)al=1,T=(0,0,0),T'=(1,0,0),T"[1]=(1,1,0),T"[2]=(1,-1,0),T"[3]=(2,0,0),T"[4]=(1,0,1),T"[5]=(1,0,-1)
(2)al=2,T=(0,0,0),T'=(0,1,0),T"[1]=(1,1,0),T"[2]=(-1,1,0),T"[3]=(0,2,0),T"[4]=(0,1,1),T"[5]=(0,1,-1)
(3)al=3,T=(0,0,0),T'=(-1,0,0),T"[1]=(-1,1,0),T"[2]=(-1,-1,0),T"[3]=(-2,0,0),T"[4]=(-1,0,1),T"[5]=(-1,0,-1)
(4)al=4,T=(0,0,0),T'=(0,-1,0),T"[1]=(1,-1,0),T"[2]=(-1,-1,0),T"[3]=(0,-2,0),T"[4]=(0,-1,1),T"[5]=(0,-1,-1)
(5)al=5,T=(0,0,0),T'=(0,0,1),T"[1]=(0,1,1),T"[2]=(0,-1,1),T"[3]=(0,0,2),T"[4]=(1,0,1),T"[5]=(-1,0,1)
(6)al=6,T=(0,0,0),T'=(0,0,-1),T"[1]=(0,1,-1),T"[2]=(0,-1,-1),T"[3]=(0,0,-2),T"[4]=(1,0,-1),T"[5]=(-1,0,-1)
【０１４１】
なお、中間位置の集合Mの形状は、長方形平板、直線形、もしくはL字形平板の形状なので、Z軸方向で、尾部ロボット単位T、ロボット単位T'、T"が接することはない。そのため、尾部ロボット単位T及びロボット単位T'の位置関係を4つに限定してもよい。また、ロボット単位T'に3通りのロボット単位T"[kt](kt=1,2,3)が接していると限定してもよい。また、ロボット単位T'内の尾部ロボット単位Tに接しない4つの位置からのマンハッタン距離δlocal[vs][al](X,Y,Z)について、(vs=0,1,2,3。al=1,2,3,4)の16通りのみを計算してもよい。
【０１４２】
以下、δlocal[vs][al](X,Y,Z)の計算方法(Calculate_delta_local)を示す。
[Calculate_delta_local]
(1)ロボット単位T'内の尾部ロボット単位Tに接しない位置とロボット単位T"の全ての位置（X,Y,Z）において、next_local[al][a][X][Y][Z]を用意し、位置（X,Y,Z）に隣接するロボット単位T'内の尾部ロボット単位Tに接しない位置か、ロボット単位T"内の位置があるならば、next_local[al][a][X][Y][Z]←１とし、それ以外の場合はnext_local[al][a][X][Y][Z]←0とする。
(2)ロボット単位T'内の尾部ロボット単位Tに接しない位置とロボット単位T"の全ての位置(X,Y,Z）について、各位置(X,Y,Z）のδlocal[vs][al](X,Y,Z)値を44(T'内4つ、T"内8x5つの位置の数)より大きな値s_maxに初期化する。
【０１４３】
(3)各vs,alについて、以下の位置を目標位置target_local[vs][al]とし、目標位置でのマンハッタン距離δlocal[vs][al](24通り)を0に初期化する。
target_local[0][1]＝（3,0,0）
target_local[1][1]＝（3,0,1）
target_local[2][1]＝（3,1,0）
target_local[3][1]＝（3,1,1）
target_local[0][2]＝（0,3,0）
target_local[1][2]＝（0,3,1）
target_local[2][2]＝（1,3,0）
target_local[3][2]＝（1,3,1）
target_local[0][3]＝（-2,0,0）
target_local[1][3]＝（-2,0,1）
target_local[2][3]＝（-2,1,0）
target_local[3][3]＝（-2,1,1）
target_local[0][4]＝（0,-2,0）
target_local[1][4]＝（0,-2,1）
target_local[2][4]＝（1,-2,0）
target_local[3][4]＝（1,-2,1）
target_local[0][5]＝（0,0,3）
target_local[1][5]＝（0,1,3）
target_local[2][5]＝（1,0,3）
target_local[3][5]＝（1,1,3）
target_local[0][6]＝（0,0,-2）
target_local[1][6]＝（0,1,-2）
target_local[2][6]＝（1,0,-2）
target_local[3][6]＝（1,1,-2）
(4)目標位置target_local[vs][al]以外の、各位置（X,Y,Z）の全行動aについて、next_local[al][a][X][Y][Z]の値が0ではない場合の、δlocal[vs][al](X,Y,Z)の値を調べ、その最小値に１を加えた値が、現在のδlocal[vs][al](X,Y,Z)よりも小さい場合は、その値をδlocal[vs][al](X,Y,Z)に代入する。δlocal[vs][al](X,Y,Z)値の更新がなくなるまで、(4)を繰り返す。
【０１４４】
Void_Local_Trajectory_Decisionの処理内容に戻る。
[Void_Local_Trajectory_Decision]
(1)ロボット単位T'の位置を（XT',YT',ZT'）、ロボット単位T"の位置を（XT",YT",ZT"）として、ロボット単位Tの位置からロボット単位T'の位置に向かう方向の行動をal値とする。i_tail_0の位置をtarget_local[0][al](vs←0とする),i_tail_1の位置をtarget_local[1][al](vs←1とする),i_tail_2の位置をtarget_local[2][al](vs←2とする),i_tail_3の位置をtarget_local[3][al](vs←3とする)とする。
【０１４５】
(2)各ボイドvoid[iv_prev+k]の発生位置を(xv[iv_prev+k],yv[iv_prev+ｋ],zv[iv_prev+k])として、ロボット単位T"内の位置vt"を以下のように計算する。Hのマス単位の位置を（xrh,yrh,zrh）、T'からT"への移動量ベクトルの値を(Xdt,Ydt,Zdt)←（XT",YT",ZT"）-（XT',YT',ZT'）として、
もしal=1ならば
vt"←(xv[iv_prev+k]-xrh*2+2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=2ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2+2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=3ならば
vt"←(xv[iv_prev+k]-xrh*2-2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=4ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2-2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=5ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2+2+Zdt*2)
とし、もしal=6ならば
vt"←(xv[iv_prev+ｋ]-xrh*2+Xdt*2,yv[iv_prev+ｋ]-yrh*2+Ydt*2,zv[iv_prev+ｋ]-zrh*2-2+Zdt*2)
とする。
【０１４６】
(3)tl←0とし、vt"の位置を(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])とする。
(4)位置(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])が、target_local[vs][al]に一致しないとき、
位置(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])に隣接し、δlocal[vs][al]の値が(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])の位置のδlocal[vs][al]の値より小さい位置を(void_local_x[iv_prev+k][tl+1],void_local_y[iv_prev+k][tl+1],void_local_z[iv_prev+k][tl+1])とし、tlをインクリメントする。(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])が、target_local[vs][al]に一致するまで(4)を繰り返す。位置(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])が、target_local[vs][al]に一致するとき、(5)へ移行する。
(5)(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_local_x[iv_prev+k][tl-t],void_local_y[iv_prev+k][tl-t],void_local_z[iv_prev+k][tl-t])とし、ts←tlとする。
【０１４７】
(6) もしal=1ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]-2+XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=2ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]XT’,void_trj_y[iv_prev+k][t]-2 +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=3ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]+2+XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=4ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]XT’,void_trj_y[iv_prev+k][t]+2 +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=5ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]+XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] -2+ZT’)
とする。
もしal=6ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] +2+ZT’)
とする。
【０１４８】
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])とする。
(2)もし、(void_unit_trj_x[tu],void_unit_trj_y[tu],void_unit_trj_z[tu])がロボット単位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])がロボット単位Hの位置になるまで(2)を繰り返す。
【０１４９】
ロボット単位Tのδ値が2以下の場合に、各ボイドの軌道計算処理（Void_Trajectory_Decision_Seed）は、以下の手順で行われる。
[Void_Trajectory_Decision_Seed]
(1)各ボイドvoid[iv_prev+k]の発生位置を以下のようにする。
k≦npのとき、void[iv_prev+k]の発生位置は、ロボットi_move_psの移動前の位置である。
k>npのとき、void[iv_prev+k]の発生位置は、ロボットi_move_prの移動前の位置である。
(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…)とし、以下のように設定する。
T=Unext2のとき
(void_unit_trj_x[0],void_unit_trj_y[0],void_unit_trj_z[0])←(2,0,0)
(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])←(1,0,0)
(void_unit_trj_x[2],void_unit_trj_y[2],void_unit_trj_z[2])←(0,0,0)
とし、T=Unext1のとき
(void_unit_trj_x[0],void_unit_trj_y[0],void_unit_trj_z[0])←(1,0,0)
(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])←(0,0,0)
とする。なお、tuの値は、尾部ロボット単位Tからロボット単位Hまでの道筋上の点を表し、尾部ロボット単位Tの位置をtu=0としてHに近づくにつれて大きくなるようにボイドの動きと逆向きに設定する。
【０１５１】
(4)(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])をT'とする。
(5)T'内にあり、尾部ロボット単位T内の位置に接しない位置にあり、Y座標が１、Z座標が0の点を各ボイドの目標位置とする。例えば、
T=Unext2のとき→(2,1,0)
T=Unext1のとき→(1,1,0)
とする。
(6)各ボイドの目標位置を（void_trj_x[0],void_trj_y[0],void_trj_z[0]）とする。t←0とする。
(7)tをインクリメントして、void_trj_x[t-1]をデクリメントした値が0以上ならば、（void_trj_x[t],void_trj_y[t],void_trj_z[t]）←（void_trj_x[t-1]-1,void_trj_y[t-1],void_trj_z[t-1]）とする。void_trj_x[t-1]をデクリメントした値が0より小さいならば、（8）へ移行する。
(8)k≦npのとき、tをインクリメントして、（void_trj_x[t],void_trj_y[t],void_trj_z[t]）＝（0,0,0）とする。
(9)tをインクリメントして、trj_num[iv_prev+k]←tとする。al←3とする。
【０１５２】
[ボイド移動制御]
以下、上記の各ロボット動作決定用動作計画を使用して、ボイドをロボット単位Hの位置から尾部ロボット単位Tの位置に移動させる制御方法を示す。図３３に示すように、ロボット単位T'内の尾部ロボット単位Tに接する位置にあるロボット位置に空白が生じないように、ロボット単位Hでのボイド発生と尾部ロボット単位Tでのボイド追い出しをしていることで、ロボット群全体の接続は保たれている。ロボット単位Hとロボット単位T'が直接、接しないように、尾部ロボット単位Tの選択アルゴリズムにおいて、尾部ロボット単位Tの位置のδ(ロボット単位Hと尾部ロボット単位Tとのマンハッタン距離)が3以上という条件を設定している。本制御方法で、各ロボット単位内において、ボイドが一つ以内に収まるように制御されている。
【０１５３】
以下を、Unext1,Unext2,Useed以外が尾部ロボット単位Tの場合の押し出し過程が終了し、その過程で生成されたボイドがすべて追い出されるまでの押し出し制御にて、繰り返し呼び出し実行する。呼び出しのアルゴリズムは後述する。
[Void_Control]
(1)本処理が最初に実行されたとき、もしくは内部変数i_motion_flg=0のとき、i_motion_flg←1とし、iv_prev<(p-24)-1のときにVoid_Trajectory_Decisionを、q-1>iv_prev≧(p-24)-1のときにVoid_Trajectory_Decision_Seedを実行し、trj_cnt[iv_prev+1],…,trj_cnt[iv_prev+8],を-1にする。q-1-iv_prevが８以上のときは、iv_prevの値を8回インクリメントする。そうでないときは、iv_prevの値をq-1-iv_prev回インクリメントする。
【０１５４】
(2)i_motion_flg=1のとき、以下をvoid[iv_prev]がロボット単位Hの外に出るまで実行する。void[iv_prev]が追い出されたら、i_motion_flg←0とする。
発生番号の大きいもの（iv=iv_prev）から以下の処理をする。
(2-i)trj_cnt[iv]=-10のとき→void[iv]について何もしない。
(2-ii)trj_cnt[iv]<trj_num[iv]かつtrj_cnt[iv]≧0のとき→
i=trj_cnt[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]の位置から、alの方向の逆方向に隣接していて、たどれるロボットを全て、al方向に1ステップ移動させて、trj_cnt[iv]←‐10とする。iv≧p-24のときで、ivが奇数のとき、i_motion_flgを3にする。
(2-iv)trj_cnt[iv]=‐1のとき→
ロボット単位Hが８つのロボットで満たされている場合、void[iv]の発生位置とX,Y座標が同じ位置にあるZ座標が2以上のロボットを全てa_headの方向に1ステップ移動させる。また、void[iv]の発生位置にあるロボットと、そのロボットに行動a_headの方向で隣接しているロボット（位置PsかPr）をa_headの方向に1ステップ移動させる。trj_cnt[iv]←0とする。満たされてない場合なにもしない。
【０１５５】
(3)i_motion_flg=5のとき、尾部ロボット単位T内のロボットについて以下の動作を行う。
Y座標が0、Z座標が0の全てのロボットのZ座標をインクリメントし、iv<q-1ならば、i_motion_flgを1にして終了する。
(4)i_motion_flg=4のとき、尾部ロボット単位T内のロボットについて以下の動作を行う。
Y座標が0、Z座標が１の全てのロボットのY座標をインクリメントし、i_motion_flgを5にして終了する。
(5)i_motion_flg=3のとき、尾部ロボット単位T内のロボットについて以下の動作を行う。
Y座標が１、Z座標が１の全てのロボットのZ座標をデクリメントし、i_motion_flgを4にして終了する。
【０１５６】
[上方向押し出し過程]
以下の押し出し過程における各時刻ステップtmでの各ロボットiの位置を格納する変数を(motion_x[tm][i],motion_y[tm][i],motion_z[tm][i])とする。
【０１５７】
以下、説明する各押し出し過程にて、各ロボットの動作計画を行い、その結果を(motion_x[tm][i],motion_y[tm][i],motion_z[tm][i])に格納していく。
[Motion_Data_Update]
(motion_x[tm][i],motion_y[tm][i],motion_z[tm][i])に各ロボットiの現時点の位置を格納し、tmをインクリメントする。
【０１５８】
目標位置の集合G内の位置のZ座標値の最大値をZmaxとして、Zmax-1個のロボットからなるZ軸に平行な柱を形成する。位置Psにあるロボットを先頭として、Z軸正の向き（上方）にロボットを押し出す（図２８参照）。本プロセス(Upward_Pushing)の処理は以下のとおりである。
【０１５９】
[Upward_Pushing]
(1)押し出し済みの柱の高さを表す変数として、i_pole←0とする。tm←0とする。[Motion_Data_Update]を実行する。
(2)Zmax-1-i_pole>8のとき、np←8として、上述のVoid_Controlを実行する。Zmax-1-i_pole≦8のとき、np←Zmax-1-i_poleとして、Void_Controlを実行する。Z座標が2以上のロボットの総数をi_poleに代入する。Motion_Data_Updateを実行する。
(3)Zmax-1＝i_poleとなるまで、(2)を繰り返す。
【０１６０】
[旗押し出し過程]
旗押し出し過程（図２９参照）で、形成される旗形状は、前過程で生成済みの柱における各Z座標値（2からZmaxまで）の高さの位置からY軸負の向きに連続して接しているロボットの数_y_num[Z]で表現される。Z＝2から、Zmaxまでの_y_num[Z]の総数を_y_allとする。_y_num[Z]は、後述の最終展開過程で計算される。なお、Zmax及び_y_num[Z]の値は、予め与えられる目標位置の集合から求めることができる。旗押し出し過程は、位置Prから押し出されたロボットが、押し出し済みの柱面をたどりながら、自身のたどり着くべきZ座標値まで、柱に沿って上昇して、たどり着くべきZ座標値にたどり着いたら、同じZ座標値のロボット全てと同時に、Y軸負方向に位置を1ステップ移動させる動作を、旗形状が完成するまで繰り返す処理である。以下の処理で実行される。
【０１６１】
[Flag_Pushing]
(1)変数_z←0とする。押し出し済みロボット数_i_pushed←0とする。
(2)_z<Zmax-1の間、（3）〜（7）以下を繰り返す。
(3)_y←0とし、_y<_y_num[_z+2]の間、（4）〜（6）を繰り返す。
(4)後述のUpdate_Y_Branchを実行する。Z座標が_ｚ+2の位置にあるロボットの総数をカウントし、変数_yに、その数-１を格納する。
(5)_i_pushed<_y_allのとき、np←0としてVoid_Controlを実行する（位置Prから押し出し）。VoidControlが新たなロボットを押し出したならばi_pushedをインクリメントする。_i_pushed=_y_allのとき、X座標が0、Y座標が１、Z座標が2以上のロボットをZ軸正方向に１ステップ移動させる。
(6)[Motion_Data_Update]を実行する。
(7)_zをインクリメントする。
【０１６２】
[Update_Y_Branch]
Update_Y_Branchでは、たどり着くべきZ座標値にたどり着いたロボットを、同じZ座標値のロボット全てと同時に、Y軸負方向に位置を1ステップ移動させる。
(1)位置（0,1,_z+2）にロボットがあるとき、Z座標値が_z+2のロボットを全てY方向負の向きに1ステップ移動させる。
（2）[Motion_Data_Update]を実行する。
【０１６３】
[最終押し出し過程]
最終押し出し過程で形成される形状は、各Z値におけるXY平面内のフロア形状を表す変数_x_num[Y][Z](Y=0〜_y_num[Z],  Z=2〜Zmax)によって表現される。_x_numの値は後述の最終展開過程で計算される。なお、_x_numの値は、予め与えられる目標位置の集合から求めることができる。_x_numの値には、各Y、Z値をとるロボットがX軸に平行な向きに前過程で形成された旗形状に連続して接している個数が格納されている。最終押し出し過程が終了した後の形状を、押し出し終了時形状と呼ぶことにする。最終押し出し過程は、位置Prから押し出されたロボットが、前過程で形成された旗形状を辿りながら、自身がたどり着くべきY座標値、Z座標値まで、旗形状に沿って移動し（本実施形態では、まず、自身がたどり着くべきZ座標値にたどり着いた後で、自身がたどり着くべきY座標値に移動する）、たどり着くべきY座標値、Z座標値にたどり着いたら、同じY座標値、Z座標値のロボット全てと同時に、X軸負方向に位置を1ステップ移動させる動作を押し出し終了時形状が完成するまで繰り返す処理である。以下の処理で実行される。
【０１６４】
[Last_Pushing]
(1)i_pushed←0とする。_z←0とする。
(2)_z<Zmax-1の間、（3）〜（9）以下を繰り返す。
(3)_y←0とし、_y≦_y_num[_z+2]の間、（4）〜（8）を繰り返す。
(4)_x←0、押し出し済みロボット数を格納する変数_x_all←0とし、_x<_x_num[_y][_z+2]の間、（5）〜（7）を繰り返す。
(5)後述のUpdate_X_Floorを実行する。実行後Y=_y、Z=_z+2の位置にあるロボットの個数-1を_xに格納する。
(6)_x_all<_x_num[_y][_z+2]のとき、np←0として、Void_Controlを実行する。実行後、位置Prからロボットが押し出された場合、_x_allをインクリメントし、押し出されたロボットの番号iを変数pushed_floor[i_pushed]に格納して、後述のSet_Pushed_Cube_Trajectoryを実行し、i_pushedをインクリメントする。
(7)[Motion_Data_Update]を実行する。
(8)_yをインクリメントする。
(9)_zをインクリメントする。
(10)生成済みボイドを全て追い出すまでVoid_Controlを繰り返す。
【０１６５】
[Set_Pushed_Cube_Trajectory]
(1)処理Set_Pushed_Cube_Trajectoryでは、ロボットpushed_floor[i_pushed]の時刻tpでの目標動作action_floor[i_pushed][tp]を設定する。例えば、以下のように設定する。
action_floor[i_pushed][0]←１、
action_floor[i_pushed][1]←４、
(1-1)2≦tp<_z+2で、
action_floor[i_pushed][tp]←５、
(1-2)_z+2≦tp<_z+2+_yで、
action_floor[i_pushed][tp]←4、
(1-3)tp=_z+2+_yで、
action_floor[i_pushed][tp]←3、
(1-4)tp>_z+2+_yで、
action_floor[i_pushed][tp]←0、
とする。
(2)time_pushed_robot[i_pushed]←0とする。
【０１６６】
[Update_X_Floor]
処理Update_X_Floorでは、処理Set_Pushed_Cube_Trajectoryで設定された目標動作action_floor[i_pushed][tp]を実行する。例えば、以下のように実行する。
(1)_p=0〜i_pushed-1の全ての_ｐについて、（2）を実行する。
(2)action_floor[_p][time_pushed_robot[_p]]の値が
0のとき：何もしない
1のとき：ロボットpushed_floor[_p]のX座標をインクリメントする。time_pushed_robot[_p]をインクリメントする。
【０１６７】
3のとき：Y座標が_y,Z座標が_z+2の全てのロボットをX軸負方向に1ステップ移動させる。time_pushed_robot[_p]をインクリメントする。
【０１６８】
4のとき：ロボットpushed_floor[_p]のY座標をデクリメントする。time_pushed_robot[_p]をインクリメントする。
【０１６９】
5のとき：ロボットpushed_floor[_p]のZ座標をインクリメントする。time_pushed_robot[_p]をインクリメントする。
(3)[Motion_Data_Update]を実行する。
【０１７０】
[最終展開過程]
図３４を用いて、最終展開過程について説明する。なお、図３４Ｅは押し出し終了時形状の例を、図３４Ａは目標位置の集合に位置する仮想的なロボット（以下、仮想ロボットともいう）の例を示す。最終展開過程においては、まず、X軸に垂直な仮想面が、目標位置(Xre[j],Yre[j],Zre[j])にある仮想的なロボットをX軸正方向無限遠からX軸負方向に移動して、仮想面に当たった仮想ロボットを押して、すべてのロボットがX座標値が０以下になるように圧縮し(図３４Ｂ、処理Extend_Process_X_Minusに相当)、続いて、X軸に垂直な仮想面が、目標位置にある仮想的なロボットをX軸負方向無限遠からX軸正方向に移動して、仮想面に当たった仮想ロボットを押して、X座標が0ではないすべてのロボットについて、ロボットのX軸正方向に接するロボットが存在している状態になるように圧縮し(図３４Ｃ、処理Extend_Process_X_Plusに相当)、続いて、Y軸に垂直な仮想面が、目標位置にある仮想的なロボットをY軸正方向無限遠からY軸負方向に移動して、仮想面に当たった仮想ロボットを押して、すべてのロボットがY座標値が０以下になるように圧縮し(図３４Ｄ、処理Extend_Process_Y_Minusに相当)、続いて、Y軸に垂直な仮想面が、目標位置にある仮想的なロボットをY軸負方向無限遠からY軸正方向に移動して、仮想面に当たった仮想ロボットを押して、Y座標が0ではないすべてのロボットについて、ロボットのY軸正方向に接するロボットが存在している状態になるように圧縮する(図３４Ｅ、処理Extend_Process_Y_Plusに相当)ときの各ロボットの動作を計算する過程である。本過程によって、目標位置の集合Gの形状が、最終押し出し過程を終えた後のロボットの形状（押し出し終了時形状）と同一になるように変形される。そのための処理は以下のとおりである。
【０１７１】
[Extend_Process]
(1)tm←0とする。仮想ロボットiの位置を目標位置(Xre[i],Yre[i],Zre[i])に設定する。[Motion_Data_Update]を実行する。後述の[Extend_Process_X_Minus]を実行する。
(2)後述の[Extend_Process_X_Plus]を実行する。
(3)後述の[Extend_Process_Y_Minus]を実行する。
(4)後述の[Extend_Process_Y_Plus]を実行する。
(5)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、各Z座標値_zでの、X座標が0である仮想ロボットの数-１を_y_num[_z]に、各Z座標値_zでの、Y座標値が_yである仮想ロボットの数-１を_x_num[_y][_z]に格納する。
【０１７２】
[Extend_Process_X_Minus]
(1)全仮想ロボットでのX座標の最大値をXmaxとする。
(2)_xw＝Xmaxとして、_xw>0の間、(3)〜(5)を繰り返す。
(3)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(4)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(4-1）X座標値が_xwである全ての仮想ロボットiについて、destination_to_be_compressed[i]←1とする。
(4-2)仮想ロボットiからX軸負方向に途切れず隣接している全ての仮想ロボットinについて、destination_to_be_compressed[in]←1とする。
(4-3)_flg_c←1とする。
(5)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をデクリメントし、[Motion_Data_Update]を実行する。_xwをデクリメントする。
【０１７３】
[Extend_Process_X_Plus]
(1)現時刻ｔmでの仮想ロボット位置のX座標の最小値をXminとし、_xw＝Xminとして、_xw<0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(3-1）仮想ロボットiからX軸正方向に途切れず隣接している全ての仮想ロボットinの中にX座標が0のものがない場合、それら隣接の仮想ロボットi〜inのdestination_to_be_compressedの値を１とし、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をインクリメントし、[Motion_Data_Update]を実行する。_xwをインクリメントする。
【０１７４】
[Extend_Process_Y_Minus]
(1)現時刻ｔmでの仮想ロボット位置のY座標の最大値をYmaxとし、_yw＝Ymaxとして、_yw>0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1）Y座標値が_ywである全ての仮想ロボットiについて、destination_to_be_compressed[i]←1とする。
(3-2)仮想ロボットiからY軸負方向に途切れず隣接している全ての仮想ロボットinについて、destination_to_be_compressed[in]←1とする。
(3-3)_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をデクリメントし、[Motion_Data_Update]を実行する。_ywをデクリメントする。
【０１７５】
[Extend_Process_Y_Plus]
(1)現時刻ｔmでの仮想ロボット位置のY座標の最小値をYminとし、_yw＝Yminとして、_yw<0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1）仮想ロボットiからY軸正方向に途切れず隣接している全ての仮想ロボットinの中にY座標が0のものがない場合、それら隣接の仮想ロボットi〜inのdestination_to_be_compressedの値を1とし、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をインクリメントし、[Motion_Data_Update]を実行する。_ywをインクリメントする。
【０１７６】
[中間位置の集合Mから目標位置の集合Gへの隊列変形]
以上、中間位置の集合Mにあるロボットが、隊列制御によって、目標位置の集合Gに移動するまでの動作を計算する処理Transform_from_M_to_Gは以下のようになる。
[Transform_from_M_to_G]
(1)Extend_Processを実行する。t=0〜tm-1において、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を変数（compressed_x[t][i],compressed_y[t][i],compressed_z[t][i]）に格納する。tmの値を変数tcに格納する。
(2)Upward_Pushingを実行する。
(3)Flag_Pushingを実行する。
(4)Last_Pushingを実行する。
【０１７７】
(5)全ての仮想ロボットjに対し、(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])＝（compress_x[j][tc-1],compress_y[j][tc-1],compress_z[j][tc-1]）となるロボットiを特定し、destination_robot_index[j]の値に格納する。
(6)時刻t=tm〜tm+tc-1について、以下の処理を行う。
(6-1)全てのロボットiについて(motion_x[t][i],motion_y[t][i],motion_z[t][i])←(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])とする。
(6-2)全ての仮想ロボットjについて(motion_x[t][destination_robot_index[j]],motion_y[t][destination_robot_index[j]],motion_z[t][destination_robot_index[j]])←（compress_x[j][tc-1-(t-tm)],compress_y[j][tc-1-(t-tm)],compress_z[j][tc-1-(t-tm)]）とする。
(7)t=0〜tm+tc-1において、変数(transform_M_to_G_x[t][i],transform_M_to_G_y[t][i],transform_M_to_G_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。t_m_to_g←tm+tc-1とする。
【０１７８】
以上の処理により、中間位置の集合Mにあるロボットが、目標位置の集合Gに移動するまでの過程が(transform_M_to_G_x[t][i],transform_M_to_G_y[t][i],transform_M_to_G_z[t][i])に格納される。
【０１７９】
[開始位置の集合Sから中間位置の集合Mへの隊列変形]
開始位置の集合Sから中間位置の集合Mへの隊列変形は、上述の中間位置の集合Mから目標位置の集合Gへの隊列変形過程[Transform_from_M_to_G]における(1)から(4)を目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。その処理にて計算された変形の時系列結果を逆再生すればよい。
【０１８０】
以上、開始位置の集合Sにあるロボットが、隊列制御によって、中間位置の集合Mに移動するまでの動作を計算する処理は以下のようになる。
[Transformation_from_S_to_M]
(1)Extend_Processを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。t=0〜tm-1において、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を変数（compressed_x[t][i],compressed_y[t][i],compressed_z[t][i]）に格納する。tmの値を変数tcに格納する。
(2)Upward_Pushingを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。
(3)Flag_Pushingを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。
(4)Last_Pushingを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。
(5)全ての仮想ロボットjに対し、(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])＝（compress_x[j][tc-1],compress_y[j][tc-1],compress_z[j][tc-1]）となるロボットiを特定し、destination_robot_index_2[j]の値に格納する。
【０１８１】
(6)時刻t=tm〜tc-1について、以下の処理を行う。
(6-1)全てのロボットiについて(motion_x[t][i],motion_y[t][i],motion_z[t][i])←(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])とする。
(6-2)全ての仮想ロボットjについて(motion_x[t][destination_robot_index_2[j]],motion_y[t][destination_robot_index_2[j]],motion_z[t][destination_robot_index_2[j]])←（compress_x[j][tc-1-(t-tm)],compress_y[j][tc-1-(t-tm)],compress_z[j][tc-1-(t-tm)]）とする。
(7)t=0〜tm+tc-1において、変数(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])に、(motion_x[tm+tc-1-t][i],motion_y[tm+tc-1-t][i],motion_z[tm+tc-1-t][i])の値を格納する。t_ｓ_to_m←tm+tc-1とする。
【０１８２】
以上の処理により、開始位置の集合Sにあるロボットが、中間位置の集合Mに移動するまでの過程が(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])に格納される。
【０１８３】
[開始位置の集合Sから目標位置の集合Gまでのホモ隊列変形]
以上の処理を結合して、開始位置の集合Sから目標位置の集合Gまでの変形過程が求められる。
[Transformation_from_S_to_G_Homo]
(1)[Transformation_from_M_to_G]を実行する。
(2)[Transformation_from_S_to_M]を実行する。
(3)t=0〜t_s_to_mにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])の値を格納する。
(4)t=t_s_to_m+1〜t_s_to_m+1+t_m_to_gにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M_to_G_x[t-(t_ｓ_to_m+1)][i],transform_M_to_G_y[t-(t_ｓ_to_m+1)][i],transform_M_to_G_z[t-(t_ｓ_to_m+1)][i])の値を格納する。
【０１８４】
以上の処理により、開始位置の集合Sにあるロボットが、目標位置の集合Gに移動するまでの過程が(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に格納される。
【０１８５】
[開始位置の集合Sから目標位置の集合Gまでのヘテロ隊列変形]
開始位置の集合S内の位置jと目標位置の集合G内の位置jは、同じロボットによって占有されるものとする。そのような場合、開始位置の集合S内の特定の位置jにあったロボットiが、目標位置の集合G内の決まった特定位置jに移動するためには、先述のTrandformation_from_S_to_G_Homoの処理に加えて、中間位置の集合Mにあるロボットの位置の入れ替えを行う必要がある。そのための入れ替え処理を[Transformation_M_to_M]として、開始位置の集合Sから目標位置の集合Gまでのヘテロ隊列変形処理は以下のとおりである。
【０１８６】
[Transformation_from_S_to_G_Hetero]
(1)[Transformation_from_M_to_G]を実行する。
(2)[Transformation_from_S_to_M]を実行する。
(3)[Transformation_from_M_to_M]を実行する。
(4)t=0〜t_ｓ_to_mにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])の値を格納する。
(5)t=t_ｓ_to_m+1〜t_ｓ_to_m+1+t_m_to_mにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M_to_M_x[t-(t_s_to_m+1)][i],transform_M_to_M_y[t-(t_s_to_m+1)][i],transform_M_to_M_z[t-(t_s_to_m+1)][i])の値を格納する。
(6)t=t_ｓ_to_m+1+t_m_to_m+1〜t_ｓ_to_m+1+t_m_to_m+1+t_m_to_gにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i]t,ransform_S_to_G_z[t][i])に、(transform_M_to_G_x[t-(t_ｓ_to_m+1+t_m_to_m+1)][cube_after[i]],transform_M_to_G_y[t-(t_ｓ_to_m+1+t_m_to_m+1)][cube_after[i]],transform_M_to_G_z[t-(t_ｓ_to_m+1+t_m_to_m+1)][cube_after[i]])の値を格納する。
【０１８７】
以上の処理により、開始位置の集合S内の特定の位置jにあったロボットiが、目標位置の集合G内の決まった特定位置jに移動しながら、開始位置の集合Sにあるロボットが、目標位置の集合Gに移動するまでの過程が(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に格納される。以下、Transformation_M_to_Mの処理内容を説明する。
【０１８８】
[Transformation_M_to_M]
処理Transformation_M_to_Mでは、開始位置の集合S内の特定の位置jにあったロボットiが目標位置の集合G内の決まった特定位置jに移動するために、中間位置の集合Mにあるロボットの位置を入れ替える。
(1)中間位置の集合M内にあるすべてのロボットiに対して、cube_id[i],cube_id_inv[i]の値を、qより大きな値に設定する。本処理実行前の中間位置の集合M内のロボットiの位置をバッファ、(cube_buffer_x[i],cube_buffer_y[i],cube_buffer_z[i])に格納する。tm←0とする。Motion_Data_Updateを実行する。
(2)すべての開始位置の集合S内の位置jと目標位置の集合G内の位置jに対して、cube_id[destination_robot_index_2[j]]の値をdestination_robot_index[j]に設定する(cube_id[destination_robot_index_2[j]]←destination_robot_index[j])。さらに、すべての開始位置の集合S内の位置jと目標位置の集合G内の位置jに対して、cube_id_inv[destination_robot_index[j]]の値をdestination_robot_index_2[j]に設定する(cube_id_inv[destination_robot_index[j]]←destination_robot_index_2[j])。これにより、開始位置の集合S内の位置jから移動して、中間位置の集合M内の位置destination_robot_index_2[j]に移動したロボットが、目標位置の集合G内の位置jへの移動の前に中間位置の集合M内の位置にて、destination_robot_index[j]に移動しなけれならないという関連づけが行われたことになる。
【０１８９】
(3)すべてのロボット番号iについて、iの小さい順に、もし、cube_id[i]＞ｑの場合に、そのcube_id[i]の値を、cube_id_inv[iv]＞ｑを満たしてかつ最小のivの値に設定していく。この処理により、中間位置の集合M内において、開始位置の集合S内のどの位置にもいかない位置にあるロボットと、中間位置の集合M内において、目標位置の集合Gのどの位置にもいかないロボットを関連づけている。
(4)全てのロボット番号iについて、変数already[i]を0に設定する。
(5)全てのロボット番号iについてiの小さい順に（6）〜（11）を実行する。
(6)_i←cube_id[i]、_flg_id←0として、i=_iでない限り(7)〜(10)を繰り返す。
(7)_flg_id=0のとき、_flg_id←1として、_iにiの値を代入する。
(8)変数cube_after[_i]←cube_id[_i]と設定する。
(9)後述のChange_Heteroを実行する。
(10)_already[_i]←1とする。_i←cube_id[_i]とする。
(11)_already[i]←1とする。
【０１９０】
上述の(3)以降の処理では、中間位置の集合M内i=0の位置のロボットから、cube_id[i]の示す中間位置の集合M内位置への移動をし、cube_id[i]に元あったロボットは、cube_id[cube_id[i]]の示す中間位置の集合M内位置に移動していくという形にリレー形式で移動をしていき、最終的にcube_id[……cube_id[cube_id[cube_id[cube_id[cube_id[i]]]]]]の示す行先がiに戻るまでをひとまとめの移動処理にしている。
【０１９１】
[Change_Hetero]
処理Change_Heteroでは、中間位置の集合M内の位置destination_robot_index_2[j]にあるロボットが、中間位置の集合M内の位置destination_robot_index[j]に移動するように制御する。例えば以下のように制御する。
(1)i=_iのとき、(2)〜(12)を実行する。そうでないとき、(13)〜(23)を実行する。
(2)ロボット_iのZ座標値＞０のとき、（3）〜（7）を実行する。そうでないとき（8）〜（12）を実行する。
(3)Xr[_i]=cube_buffer_x[cube_id[_i]]とYr[_i]=cube_buffer_y[cube_id[_i]]との何れかを満たさないとき、（4）〜（5）を実行する。そうでないとき（何れも満たすとき）、(6)〜(7)を実行する。
【０１９２】
(4)ロボット_iと、ロボット_iにZ軸負方向で接するロボットをともに1ステップZ軸正方向に移動させる。
(5)ロボットcube_id[_i]のZ座標が１のとき、ロボットcube_id[_i]にZ軸負方向で接するロボットとともに1ステップZ軸正方向に移動させる。そうでないときは、ロボットcube_id[_i]にZ軸正方向で接するロボットとともに1ステップZ軸正方向に移動させる。Motion_Data_Updateを実行する。後述のHorizontal_ChangeとVertical_Changeを実行する。
(6)ロボット_iとロボットcube_id[_i]をともにZ軸正方向に１移動させる。Motion_Data_Updateを実行する。
(7)ロボット_iのX座標が偶数ならば、ロボット_iをX軸正方向に１ステップ移動させる。そうでなければ、ロボット_iをX軸負方向に１ステップ移動させる。Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(8)Xr[_i]=cube_buffer_x[cube_id[_i]]とYr[_i]=cube_buffer_y[cube_id[_i]]との何れかを満たさないとき、（9）〜（10）を実行する。そうでないとき（何れも満たすとき）、(11)〜(12)を実行する。
【０１９３】
(9)ロボット_iと、ロボット_iにZ軸正方向で接するロボットをともに1ステップZ軸負方向に移動させる。
(10)ロボットcube_id[_i]のZ座標が1のとき、ロボットcube_id[_i]にZ軸負方向で接するロボットとともに1ステップZ軸負方向に移動させる。そうでないときは、ロボットcube_id[_i]にZ軸正方向で接するロボットとともに1ステップZ軸負方向に移動させる。Motion_Data_Updateを実行する。後述のHorizontal_ChangeとVertical_Changeを実行する。
(11)ロボット_iとロボットcube_id[_i]をともにZ軸負方向に１移動させる。Motion_Data_Updateを実行する。
(12)ロボット_iのX座標が偶数ならば、ロボット_iをX軸正方向に１ステップ移動させる。そうでなければ、ロボット_iをX軸負方向に１ステップ移動させる。Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(13)ロボット_iのZ座標値＞0のとき、（14）〜（18）を実行する。そうでないとき（19）〜（23）を実行する。
【０１９４】
(14)X座標がcube_buffer_x[i]に等しく、かつY座標がcube_buffer_y[i]に等しいロボットをロボットinit_cubeとし、かつそのZ座標が0の場合、ロボットinit_cubeをZ軸方正向に１ステップ移動させる。Motion_Data_Updateを実行する。Horizontal_Changeを実行する。
(15)cube_id[_i]の値が、iでもinit_cubeでもないとき、(16)〜(17)を実行する。そうでないとき（18）を実行する。
(16)cube_buffer_z[cube_id[_i]]>0のとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸負方向で接しているロボットをともにZ軸正方向に１ステップ移動させる。そうでないとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸正方向で接しているロボットをともにZ軸正方向に１ステップ移動させる。
(17)Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(18)cube_id[_i]=iのとき、Vertical_Single_Changeを実行し、そうでないとき(cube_id[_i]=init_cube)、Vertical_Single_Change_Invを実行する。
【０１９５】
(19)X座標がcube_buffer_x[i]に等しく、かつY座標がcube_buffer_y[i]に等しいロボットをロボットinit_cubeとし、かつそのZ座標が１の場合、ロボットinit_cubeをZ軸負方向に１ステップ移動させる。Motion_Data_Updateを実行する。Horizontal_Changeを実行する。
(20)cube_id[_i]の値が、iでもinit_cubeでもないとき、(21)〜(22)を実行する。そうでないとき（23）を実行する。
(21)cube_buffer_z[cube_id[_i]]>0のとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸負方向で接しているロボットをともにZ軸負方向に１ステップ移動させる。そうでないとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸正方向で接しているロボットをともにZ軸負方向に１ステップ移動させる。
(22)Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(23)cube_id[_i]=iのとき、Vertical_Single_Changeを実行し、そうでないとき、Vertical_Single_Change_Invを実行する。
【０１９６】
[Horizontal_Change]
処理Horizontal_Changeでは、ロボット_iを移動先のロボットcube_id[_i]の位置のX座標、Y座標の何れかにおいて隣接する位置に移動させる。ここでは、移動先の位置のX座標値がcube_buffer_x[cube_id[_i]]であり、Y座標値がcube_buffer_y[cube_id[_i]]であり、ロボット_iと移動先の位置との位置関係に応じて、ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]±1となるように、または、Y座標値がcube_buffer_y[cube_id[_i]]±1となるように、ロボット_iを移動させる。例えば以下のように制御する。
(1)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]に等しくかつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に等しいとき、以下を実行して終了する。そうでないなら(2)へ移行する。
ロボット_iのX座標値が偶数ならば、ロボット_iをX軸正方向に１ステップ移動させ、奇数ならばX軸負方向に１移動させる。Motion_Data_Updateを実行する。
【０１９７】
(2)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]より小さく、かつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に等しくないとき、以下を実行して終了する。そうでないなら(3)へ移行する。
ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]に等しくなるまで、ロボット_iをX軸正方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。続いて、ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]との差の絶対値が１に等しくなるまで、ロボット_iをロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に近づく方向に、Y軸方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。
【０１９８】
(3)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]より小さく、かつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に等しいとき、以下を実行して終了する。そうでないなら（4）へ移行する。
ロボット_iのX座標値とcube_buffer_x[cube_id[_i]]との差の絶対値が１に等しくなるまで、ロボット_iをX軸正方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。
【０１９９】
(4)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]より大きく、かつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に等しくないとき、以下を実行して終了する。そうでないなら(5)へ移行する。
ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]が等しくなるまで、ロボット_iをロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に近づく方向に、Y軸方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。続いて、ロボット_iのX座標値とcube_buffer_x[cube_id[_i]]との差の絶対値が１に等しくなるまで、ロボット_iをX軸負方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。
【０２００】
(5)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]より大きく、かつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に等しいとき、以下を実行して終了する。そうでないなら（6）へ移行する。
ロボット_iのX座標値とcube_buffer_x[cube_id[_i]]との差の絶対値が１に等しくなるまで、ロボット_iをX軸負方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。
【０２０１】
(6)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]に等しく、かつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]よりも大きいとき、以下を実行して終了する。そうでないなら（7）へ移行する。
ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]との差の絶対値が１に等しくなるまで、ロボット_iをY軸負方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。
【０２０２】
(7)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]に等しく、かつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]よりも小さいとき、以下を実行して終了する。
ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]との差の絶対値が１に等しくなるまで、ロボット_iをY軸正方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。
【０２０３】
[Vertical_Change]
処理Vertical_Changeでは、ロボット_iを移動先の位置のX座標、Y座標の何れかにおいて隣接する位置から、移動先の位置に移動させる。
(1)Zr[_i]=2かつZr[cube_id[_i]]=2のとき、以下を実行し終了する。そうでないなら（2）へ移行する。
(1-1)もしXr[_i]=Xr[cube_id[_i]]ならば(1-1-1)に移行し、そうでないなら(1-1-2)へ移行する。
(1-1-1)もしXr[_i]が偶数ならば(1-1-1-1)に移行し、そうでないなら(1-1-1-2)へ移行する。
(1-1-1-1)ロボット_iとロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[cube_id[_i]]ならば、(1-1-1-1-1)に移行し、それ以外ならば、(1-1-1-1-2)に移行する。
(1-1-1-1-1)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-1-1-2)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-1-2)ロボット_iとロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[cube_id[_i]]ならば(1-1-1-2-1)に移行し、それ以外ならば(1-1-1-2-2)に移行する。
(1-1-1-2-1)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-1-2-2)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2)(Xr[_i]=Xr[cube_id[_i]]以外の場合)もしYr[_i]が偶数ならば、(1-1-2-1)に移行し、それが以外の場合(1-1-2-2)に移行する。
(1-1-2-1)ロボット_iとロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-1-1)に移行し、それ以外の場合(1-1-2-1-2)に移行する。
(1-1-2-1-1)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2-1-2)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2-2)ロボット_iとロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-2-1)に移行し、それ以外の場合(1-1-2-2-2)に移行する。
(1-1-2-2-1)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2-2-2)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２０４】
(2)Zr[_i]=2かつZr[cube_id[_i]]=1のとき、(2-1)に移行し、それ以外の場合(3)に移行する。
(2-1)ロボットcube_id[_i]にZ軸正方向に接するロボットをロボット_pairとする。
もしXr[_i]=Xr[_pair]ならば(2-1-1)に移行し、それ以外の場合(2-1-2)に移行する。
(2-1-1)もしXr[_i]が偶数ならば(2-1-1-1)に移行し、それ以外の場合(2-1-1-2)に移行する。
(2-1-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[_pair]ならば(2-1-1-1-1)に移行し、それ以外の場合(2-1-1-1-2)に移行する。
(2-1-1-1-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-1-1-2)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-1-2)(Xr[_i]が偶数ではない場合)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[_pair]ならば(2-1-1-2-1)に移行し、それ以外の場合(2-1-1-2-2)に移行する。
(2-1-1-2-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-1-2-2)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2)(Xr[_i]=Xr[_pair]ではない場合)もしYr[_i]が偶数ならば(2-1-2-1)に移行し、それ以外の場合(2-1-2-2)に移行する。
(2-1-2-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[_pair]ならば(2-1-2-1-1)に移行し、それ以外の場合(2-1-2-1-2)に移行する。
(2-1-2-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2-1-2)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2-2)（Yr[_i]が偶数ではない場合）ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[_pair]ならば(2-1-2-2-1)に移行し、それ以外の場合(2-1-2-2-2)に移行する。
(2-1-2-2-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2-2-2)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２０５】
(3)Zr[_i]<0かつZr[cube_id[_i]]<0のとき、(3-1)に移行し、それ以外の場合(4)へ移行する。
(3-1)もしXr[_i]=Xr[cube_id[_i]]ならば(3-1-1)に移行し、それ以外の場合(3-1-2)に移行する。
(3-1-1)もしXr[_i]が偶数ならば(3-1-1-1)に移行し、それ以外の場合(3-1-1-2)に移行する。
(3-1-1-1)ロボット_iとロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[cube_id[_i]]ならば(3-1-1-1-1)に移行し、それ以外の場合(3-1-1-1-2)に移行する。
(3-1-1-1-1)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-1-1-2)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-1-2)(Xr[_i]が偶数ではない場合)ロボット_iとロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[cube_id[_i]]ならば(3-1-1-2-1)に移行し、それ以外の場合(3-1-1-2-2)に移行する。
(3-1-1-2-1)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-1-2-2)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2)(Xr[_i]=Xr[cube_id[_i]]ではない場合)もしYr[_i]が偶数ならば(3-1-2-1)に移行し、それ以外の場合(3-1-2-2)に移行する。
(3-1-2-1)ロボット_iとロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(3-1-2-1-1)に移行し、それ以外の場合(3-1-2-1-2)に移行する。
(3-1-2-1-1)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2-1-2)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2-2)ロボット_iとロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(3-1-2-2-1)に移行し、それ以外の場合(3-1-2-2-2)に移行する。
(3-1-2-2-1)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2-2-2)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２０６】
(4)Zr[_i]<0かつZr[cube_id[_i]]=0のとき、(4-1)に移行し、それ以外の場合、処理を終了する。
(4-1)ロボットcube_id[_i]にZ軸負方向に接するロボットをロボット_pairとする。もしXr[_i]=Xr[_pair]ならば(4-1-1)に移行し、それ以外の場合(4-1-2)に移行する。
(4-1-1)もしXr[_i]が偶数ならば(4-1-1-1)に移行し、それ以外の場合(4-1-1-2)に移行する。
(4-1-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[_pair]ならば(4-1-1-1-1)に移行し、それ以外の場合(4-1-1-1-2)に移行する。
【０２０７】
(4-1-1-1-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２０８】
(4-1-1-1-2)それ以外の場合、ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２０９】
(4-1-1-2)(Xr[_i]が偶数ではない場合)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[_pair]ならば(4-1-1-2-1)に移行し、それ以外の場合(4-1-1-2-2)に移行する。
【０２１０】
(4-1-1-2-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２１１】
(4-1-1-2-2)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２１２】
(4-1-2)(Xr[_i]=Xr[_pair]ではない場合)もしYr[_i]が偶数ならば(4-1-2-1)に移行し、それ以外の場合(4-1-2-2)に移行する。
【０２１３】
(4-1-2-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[_pair]ならば(4-1-2-1-1)に移行し、それ以外の場合(4-1-2-1-2)に移行する。
【０２１４】
(4-1-2-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２１５】
(4-1-2-1-2)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２１６】
(4-1-2-2)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[_pair]ならば(4-1-2-2-1)に移行し、それ以外の場合(4-1-2-2-2)に移行する。
【０２１７】
(4-1-2-2-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２１８】
(4-1-2-2-2)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２１９】
[Vertical_Single_Change]
(1)Xr[_i]-cube_buffer_x[cube_id[_i]]の値が、0のとき(1-1)に移行し、1のとき(1-2)に移行し、-1のとき(1-3)に移行する。
(1-1)Yr[_i]-cube_buffer_y[cube_id[_i]]の値が1のときに、ロボット_iをY軸負方向に１ステップ移動し、-1のとき、Y軸正方向に１ステップ移動する。
(1-2)ロボット_iをX軸負方向に１ステップ移動する。
(1-3)ロボット_iをX軸正方向に１ステップ移動する。
(2)Motion_Data_Updateを実行する。
(3)Zr[_i]>0のとき、ロボット_iとロボット_iにZ軸負方向に接するロボットをともに、Z軸負の向きに１ステップ移動させる。そうでないときは、ロボット_iとロボット_iにZ軸正方向に接するロボットをともに、Z軸正の向きに１ステップ移動させる。
(4)Motion_Data_Updateを実行する。
【０２２０】
[Vertical_Single_Change_Inv]
(1)Zr[_i]=2かつZr[cube_id[_i]]=1のとき、(1-1)へ移行し、それ以外の場合(2)に移行する。
(1-1)もしXr[_i]=Xr[cube_id[id]]ならば(1-1-1)に移行し、それ以外の場合(1-1-2)に移行する。
(1-1-1)もしXr[_i]が偶数ならば(1-1-1-1)に移行し、それ以外の場合(1-1-1-2)に移行する。
【０２２１】
(1-1-1-1)ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[cube_id[_i]]ならば(1-1-1-1-1)に移行し、それ以外の場合、(1-1-1-1-2)に移行する。
【０２２２】
(1-1-1-1-1)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２２３】
(1-1-1-1-2)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２２４】
(1-1-1-2)(Xr[_i]が偶数ではない場合)ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[cube_id[_i]]ならば(1-1-1-2-1)に移行し、それ以外の場合(1-1-1-2-2)に移行する。
【０２２５】
(1-1-1-2-1)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２２６】
(1-1-1-2-2)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２２７】
(1-1-2)(Xr[_i]=Xr[cube_id[id]]ではない場合)もしYr[_i]が偶数ならば(1-1-2-1)に移行し、それ以外の場合(1-1-2-2)に移行する。
【０２２８】
(1-1-2-1)ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-1-1)に移行し、それ以外の場合(1-1-2-1-2)に移行する。
【０２２９】
(1-1-2-1-1)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２３０】
(1-1-2-1-2)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。}    }
(1-1-2-2)(Yr[_i]が偶数ではない場合)ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-2-1)に移行し、それ以外の場合(1-1-2-2-2)に移行する。
【０２３１】
(1-1-2-2-1)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２３２】
(1-1-2-2-2)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2)Zr[_i]=-1かつZr[cube_id[_i]]=0のとき、(2-1)に移行し、処理を終了する。
【０２３３】
(2-1)もしXr[_i]=Xr[cube_id[id]]ならば(2-1-1)に移行し、それ以外の場合(2-1-2)に移行する。
【０２３４】
(2-1-1)もしXr[_i]が偶数ならば(2-1-1-1)に移行し、それ以外の場合(2-1-1-2)に移行する。
【０２３５】
(2-1-1-1)ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[cube_id[_i]]ならば(2-1-1-1-1)に移行し、それ以外の場合(2-1-1-1-2)に移行する。
【０２３６】
(2-1-1-1-1)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２３７】
(2-1-1-1-2)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２３８】
(2-1-1-2)(Xr[_i]が偶数ではない場合)ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[cube_id[_i]]ならば(2-1-1-2-1)に移行し、それ以外の場合(2-1-1-2-2)に移行する。
【０２３９】
(2-1-1-2-1)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２４０】
(2-1-1-2-2)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２４１】
(2-1-2)(Xr[_i]=Xr[cube_id[id]]ではない場合)もしYr[_i]が偶数ならば(2-1-2-1)に移行し、それ以外の場合(2-1-2-2)に移行する。
【０２４２】
(2-1-2-1)ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[cube_id[_i]]ならば(2-1-2-1-1)に移行し、それ以外の場合(2-1-2-1-2)に移行する。
【０２４３】
(2-1-2-1-1)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２４４】
(2-1-2-1-2)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２４５】
(2-1-2-2)(Yr[_i]が偶数ではない場合)ロボットcube_id[_i]をZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[cube_id[_i]]ならば(2-1-2-2-1)に移行し、それ以外の場合(2-1-2-2-2)に移行する。
【０２４６】
(2-1-2-2-1)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２４７】
(2-1-2-2-2)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
【０２４８】
＜第二実施形態に係る行動制御システム１００＞
図２２は第二実施形態に係る行動制御システム１００の機能ブロック図を、図２３はその処理フローの例を示す。行動制御システム１００は、図２２に示すように、行動選択部１２０と、隣接状態判定部１２４と、位置更新部１２３と、位置判定部１２６と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。行動選択部１２０は押し出し過程処理部１２０Ａと中間遷移過程処理部１２０Ｂと最終展開過程処理部１２０Ｃと位置入れ替え部１２０Ｄとを含む。
【０２４９】
以下では、制御の対象となる制御対象物が、ロボットである場合を例に挙げて説明する。もちろん、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。
【０２５０】
本実施形態では、行動制御システム１００は、p台のロボットの行動を制御し、p台のロボットの内の１つのロボット上に実装される。なお、行動制御システム１００が実装されていないp-１台のロボットについても、通信部１５０と、隣接状態判定部１２４とを含む。
【０２５１】
＜入力部１６０＞
入力部１６０には、p台のロボットiのそれぞれの中間位置(Xr0[i],Yr0[i],Zr0[i])(言い換えると、p個の中間位置の集合M={(Xr0[0],Yr0[0],Zr0[0]),(Xr0[1],Yr0[1],Zr0[1]),…,(Xr0[p-1],Yr0[p-1],Zr0[p-1])})及び、q個の開始位置の集合S={(Xrs[0],Yrs[0]),(Xrs[1],Yrs[1]),…,(Xrs[q-1],Yrs[q-1])}、q個の目標位置の集合G={(Xre[0],Yre[0]),(Xre[1],Yre[1]),…,(Xre[q-1],Yre[q-1])}が入力され、記憶部１４０に記憶される。
【０２５２】
＜通信部１５０＞
行動制御システム１００が実装されているロボットも含め、全てのロボットは、通信部１５０を介して、二次元平面上の上下左右前後方向(以下「６方向」ともいう)において隣接する他のロボットと通信することができる。
【０２５３】
＜行動選択部１２０＞
行動選択部１２０は、上述の方法で、p台のロボットを制御する（ｓ１２０）。なお、押し出し過程処理部１２０Ａ、中間遷移過程処理部１２０Ｂ、最終展開過程処理部１２０Ｃ、位置入れ替え部１２０Ｄでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部１４０に記憶しておく。行動選択部１２０は、記憶部１４０から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。
【０２５４】
（押し出し過程処理部１２０Ａ）
行動選択部１２０の押し出し過程処理部１２０Ａは、記憶部１４０からp個の中間位置の集合M及びq個の目標位置の集合G、q個の開始位置の集合Sを取り出し、p個の中間位置の集合Mに位置する少なくともq台のロボットを移動させ、押し出し終了時形状を成すように制御し、押し出し終了時形状における各ロボットの位置を最終展開過程処理部１２０Ｃに出力する。
【０２５５】
なお、押し出し終了時形状は、座標値O1及び前記座標値O2においてロボットが平面を成す形状である。押し出し終了時形状は、座標値O1をx=0とし、座標値O2をy=0としたときに、上述の[最終展開過程]により得られる仮想ロボットの成す隊列に相当する。押し出し終了時形状をこのような形状とすることで、中間位置の集合Mから押し出し終了時形状へ変形する際に、ロボットの移動経路を確保することができる。
【０２５６】
押し出し過程処理部１２０Ａは、p個の中間位置の集合M及びq個の目標位置の集合Gを用いて、上述の[上方向押し出し過程]、[旗押し出し過程]、[最終押し出し過程]を実行し、p個の中間位置の集合Mから押し出し終了時形状に変形させるための各ロボットの動作の順序、方向を取得する。
【０２５７】
（中間遷移過程処理部１２０Ｂ）
行動選択部１２０の中間遷移過程処理部１２０Ｂは、q個の目標位置の集合Gを取り出し、(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ(圧縮する)、(2)第一方向における座標値が座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ(圧縮する)、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ(圧縮する)、(4)第二方向における座標値が座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させることで、押し出し終了時形状を成すロボットの位置を求める。例えば、第一方向、第二方向、第三方向、第四方向をそれぞれx軸正方向、y軸正方向、x軸負方向、y軸負方向とし、座標値O1をx=0とし、座標値O2をy=0としたときに、上述の[最終展開過程]を実行することで、押し出し終了時形状を成すロボットの位置を求めることができる。目標位置の集合Gに含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部１２０Ｃに出力する。
【０２５８】
（最終展開過程処理部１２０Ｃ）
行動選択部１２０の最終展開過程処理部１２０Ｃは、押し出し過程処理部１２０Ａから押し出し終了時形状における各ロボットの位置を取得し、中間遷移過程処理部１２０Ｂから目標位置の集合Gに含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを取得する。そして、最終展開過程処理部１２０Ｃは、押し出し終了時形状を成すロボットに対して、目標位置の集合に含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の平行移動方向とは反対の方向にロボットを移動させることで、押し出し終了時形状を成すロボットを目標位置の集合に移動させる。
【０２５９】
なお、押し出し過程処理部１２０Ａ、中間遷移過程処理部１２０Ｂ及び最終展開過程処理部１２０Ｃにおける処理において、q個の目標位置の集合Gに代えてq個の開始位置の集合Sを用いることで、中間位置の集合Mから開始位置の集合Sへ変形するための各ロボットの移動の順番と方向とを取得することができる。開始位置の集合Sに位置するロボットに対して、取得した順番と方向とは逆の順番で、逆の方向にロボットを移動させることで、開始位置の集合Sに位置するロボットを中間位置の集合に移動させることができる。
【０２６０】
（位置入れ替え部１２０Ｄ）
位置入れ替え部１２０Ｄは、目標位置の集合Gから中間位置の集合Mに変形した際の、ロボットiの中間位置の集合Mにおける位置を最終展開過程処理部１２０Ｃから取得する。
【０２６１】
位置入れ替え部１２０Ｄは、開始位置の集合Sから中間位置の集合Mに変形した際の、ロボットiの中間位置の集合Mにおける位置を押し出し過程処理部１２０Ａから取得する。
【０２６２】
位置入れ替え部１２０Ｄは、処理Transformation_M_to_Mを実行し、開始位置の集合Sから中間位置の集合Mに変形した際の、ロボットiの中間位置の集合Mにおける位置から、ロボットiを、目標位置の集合Gから中間位置の集合Mに変形した際の、ロボットiの中間位置の集合Mにおける位置に移動させるための、各ロボットの移動の順番と方向とを取得する。
【０２６３】
＜位置更新部１２３＞
位置更新部１２３は、各i=0,1,…,p-1について、i番目のロボットの現在の位置（Xr[i],Yr[i],Zr[i]）において、行動選択部１２０で決定した行動を実行した場合のロボットの移動後（行動後）の位置（Xr'[i],Yr'[i],Zr'[i]）を計算し、計算された（Xr'[i],Yr'[i],Zr'[i]）で記憶部１４０に格納されたi番目のロボットの位置を更新する（Ｓ１２３）。言い換えれば、位置更新部１２３は、行動選択部１２０で決定した行動に基づいて、ロボットが行動した場合に想定される位置（以下、「想定位置」ともいう）を計算し、ロボットの位置を更新し記憶部１４０に格納する。
【０２６４】
＜隣接状態判定部１２４＞
隣接状態判定部１２４は、ロボットの２次元平面上の上下左右の隣接する位置に、他のロボットが存在するか否かを判定し、（Ｓ１２４），判定結果を記憶部１４０に格納する。
【０２６５】
なお、上述の通り、行動制御システム１００が実装されていないp-１台のロボットについても、通信部１５０と、隣接状態判定部１２４とを含む。そのため、各ロボットiは隣接状態判定部１２４において、自身の上下左右前後方向に他のロボットがいるかどうかを判定し、通信部１５０を介して判定結果を行動制御システム１００に出力することができる。行動制御システム１００は、通信部１５０を介して各ロボットiから判定結果を受け取り、行動制御システム１００に含まれる隣接状態判定部１２４の判定結果と一緒に記憶部１４０に格納する。なお、p台のロボットは、各ロボットの隣り合う位置（６方向）に必ず、他のロボットが存在し、隣り合うロボット同士がなす群れは、一塊なので、各ロボットiは通信部１５０を介してp-1個の判定結果を直接、または、他のロボットを介して、行動制御システム１００に送信することができる。また、行動制御システム１００は、通信部１５０を介して、直接、または、他のロボットを介して、各ロボットiに選択した行動を実行するように制御信号を送信することができる。また、他の情報もp台のロボット間で送受信可能となる。
【０２６６】
なお、ロボットが移動するように制御したとしても、何らかのトラブル（機器の故障等）により、行動選択部１２０の制御通りに移動できるとは限らない。一方、動かなかったロボットの位置は、制御する前と変わらない。よって、少なくとも１台のロボットが動かないように制御することで、動かなかったロボットの位置を基準として、隣接状態判定部１２４による判定結果を用いて、移動するように制御されたロボットの、実際に行動した後の位置（以下「行動後位置」ともいう）（Xr"[i],Yr"[i],Zr"[i]）を求めることができる。また、GPS等の少なくとも一台のロボットがGPSを備えれば、そのロボットを基準として、同様に移動後位置を求めることができる。
【０２６７】
＜位置判定部１２６＞
位置判定部１２６は、隣接状態判定部１２４の判定結果を用いて、行動後位置を求め、行動後位置（Xr"[i],Yr"[i],Zr"[i]）と想定位置（Xr'[i],Yr'[i],Zr'[i]）とが一致するか否かを判定する（Ｓ１２６）。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。この場合、行動後位置（Xr"[i],Yr"[i],Zr"[i]）と想定位置（Xr'[i],Yr'[i],Zr'[i]）との少なくとも一方を補正すればよい。補正方法としては様々な手法が考えられる。例えば、移動した全てのロボットに対して、制御前の位置に戻るように指示し、行動後位置（Xr"[i],Yr"[i],Zr"[i]）を補正してもよいし、想定位置（Xr'[i],Yr'[i],Zr'[i]）を行動後位置（Xr"[i],Yr"[i],Zr"[i]）に合わせて補正してもよい。
【０２６８】
各時刻ステップごとに、すべての目標位置の集合Gがロボットで充填されているか否かを判定し（Ｓ１２７）、充填されているときは、任務を終了する。そうでないときは、任務を継続する。
【０２６９】
例えば、図示しない目標位置到達判定部において、各i=0,1,…,p-1について、位置判定部１２６から出力された行動後位置（Xr"[i],Yr"[i],Zr"[i]）により、全ての目標位置の集合Gがロボットで充填されているか否かを判定し、充填されている場合には、任務を終了する。少なくとも１つ以上の目標位置にロボットが充填されていない場合には、行動選択部１２０を再度実行するよう制御する。
【０２７０】
以上に述べた処理ｓ１２０〜ｓ１２７を毎時刻ステップごとに行う。
【０２７１】
＜効果＞
このような構成により、三次元空間において、第一実施形態と同様の効果を得ることができる。また、中間位置の集合Mの形態を8個のロボットからなるマス単位で構成されたL字もしくは、長方形の平板とすることで処理Transformation_M_to_Mを簡略化している。
【０２７２】
＜変形例＞
本実施形態では、各格子（マス）は、立方体であるが、他の形状であってもよい。格子は左右方向、上下方向及び前後方向に連続して配置される。また、各格子は左右方向で他の二つの格子と隣接し、上下方向で他の二つの格子と隣接し、前後方向で他の二つの格子と隣接する。言い換えると、各格子は、ロボットの移動できる方向と同じ方向においてのみ、他の格子と隣接する。この条件を満たせば、各格子はどのような形状であってもよい。また、「直交」とは、厳密に「垂直に交わること」を意味しなくともよく、例えば、各格子は、平行六面体であってもよく、各格子が他の二つの格子と隣接する方向の一方を上下方向とし、他方を左右方向とすればよく、上下方向及び左右方向とからなる平面に対して平行でない方向を前後方向とすればよい。
【０２７３】
別の言い方をすると、制御対象物は、三次元空間上の、第一方向（例えば右方向）、第一方向に対して平行でない方向である第二方向（例えば上方向）、第一方向に対して反対方向である第三方向（例えば左方向）、第二方向に対して反対方向である第四方向（例えば下方向）、第一方向及び第二方向の成す平面に対して平行でない方向を第五方向（例えば前方向）、第五方向に対して反対方向である第六方向に移動可能であり、一回の行動制御により、現在いる領域（格子、マス）から、現在いる領域に対して、第一方向、第二方向、第三方向、第四方向、第五方向、第六方向において隣接する領域の何れかに移動するように制御される。また、この場合、ロボットの３次元平面上の、第一方向において隣接する位置を第一位置、第二方向において隣接する位置を第二位置、第三方向において隣接する位置を第三位置、第四方向において隣接する位置を第四位置、第五方向において隣接する位置を第五位置、第六方向において隣接する位置を第六位置と呼んでもよい。
【０２７４】
本実施形態では、行動制御システム１００が、p台のロボットの内の１つのロボット上に実装される例を示したが、コンピュータ上の制御対象物に対して行動制御を行ってもよい。その場合、意図的にトラブルを発生させない限り、制御した通りに制御対象物は移動するため、想定位置（Xr'[i],Yr'[i],Zr'[i]）と移動後位置（Xr"[i],Yr"[i],Zr"[i]）とが一致する。そのような状態で、各制御対象物i毎の理想的な行動計画を行ってもよい。その結果得られた任務を終了するまでの行動計画を実ロボットに実行させる。実ロボットは、通信部１５０と隣接状態判定部１２４を備え、各行動を終えた後に、隣接状態を判定し、判定結果を用いて、行動後位置を求め、行動後位置（Xr"[i],Yr"[i],Zr"[i]）と想定位置（Xr'[i],Yr'[i],Zr'[i]）とが一致するか否かを判定する。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。その後の処理については第一実施形態と同様でもよいし、他の処理を行ってもよい。このような構成により、少なくとも制御通りに移動できなかったことを検知することができる。
【０２７５】
本実施形態では、一回の行動制御に対して、一回の位置判定を行っているが、一回の行動制御において、複数のロボットを移動させる場合には、各ロボットが移動の度に位置判定を行う構成としてもよい。
【０２７６】
本実施形態では、ロボット単位は、２×２×２の８台のロボットからなるが、M×N×Q台のロボットからなってもよい。ただし、M、N及びQはそれぞれ２以上の整数の何れかである。このような状態で移動を行った場合にも、２×２×２のロボット単位のときと同様に、ロボットの群れの中のいずれのロボットが一台欠けても、各ロボットはお互いに一つの面で接しあう位置関係を崩さずに済む。M×N×Qの台数を大きくすると、ロボット単位の個数が減り、行動できるパターンが減るため、マンハッタン距離δの計算量が減る。しかし、M×N×Q個のマスを一つの単位とするマス単位の中に1つでも障害物が含まれる場合には、そのマス単位には移動できないものとして扱うため、M×N×Qの台数を大きくすると、移動できない範囲が多くなり、行動できる範囲が小さくなる。そもそも動作計画において、各位置単位からロボット単位Hの位置単位までのマンハッタン距離を調べさえすればよいため、従来技術に比べれば、十分に計算量が減っている。そのため、行動できる範囲を大きく保つために２×２×２台のロボットを一つの単位とするロボット単位とすることが望ましい。
【０２７７】
また、本実施形態では、開始位置の集合Sと目標位置の集合Gにおいて、各ロボットがとるべき位置は、各ロボット毎に決まっているものとしているが、各ロボットがとるべき位置を決めなくともよい。その場合、処理Transformation_from_S_to_G_Homoを実行し、その結果得られる(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に基づきロボットを制御すればよい。
【０２７８】
＜第三実施形態＞
第二実施形態と異なる部分を中心に説明する。
[開始、目標および中間位置の集合]
第三実施形態においては、第二実施形態とは中間位置の集合の形状がことなる。第三実施形態における中間位置の集合をM2とする。中間位置の集合M2の形状の条件は、天井が、2×2の4マスによるロボット単位によって構成されており、天井の厚みは１つのロボット分でよい代わりに、かならず天井の下に接するロボットが1つ以上存在していることである（図３７参照）。なお、天井にあるロボットi([Xri],[Yri],[Zri])の下に、そのロボットと同じX,Y座標値を持つロボットが存在している場合、それらのロボットは皆、天井からぶら下がって、天井のロボットiに接していなければならない。第三実施形態における開始位置の集合Sと目標位置の集合Gには、４つのロボットが田の字状に連結した形状からなるシードロボットが含まれる。開始位置の集合SにおいてはそのZ軸座標値が最小の位置に、目標位置の集合GにおいてはそのZ軸座標値が最大の位置にX座標、Y座標がともに0の位置にシードロボットが位置する（図３５参照）。なお、開始位置Sと目標位置Gにおけるシードロボットの座標位置は同一である。
【０２７９】
第三実施形態での開始位置の集合Sから目標位置の集合Gまでの隊列移動は以下のプロセスに分けられる。
(1)開始位置の集合Sから押し出し終了時形状１への圧縮過程：[Extend_Process_S]
(2)押し出し終了時形状１から中間位置の集合M2への変形過程：［Make_Ceiling］
(3)押し出し終了時形状２から目標位置の集合Gへの展開過程:[Extend_Process_G]
(4)中間位置の集合M2から押し出し終了時形状２への変形過程：[Translate_from_M2_to_Mobile_G]
(5)中間位置の集合M2におけるロボット入れ替え過程：[Permutation_in_M2]
【０２８０】
以下、各プロセスについて説明する。
[開始位置の集合Sから押し出し終了時形状１への圧縮過程]
開始位置の集合Sの形状から押し出し終了時形状１への圧縮過程[Extend_Process_S]は、第二実施形態における最終展開過程にZ軸上方向への以下の圧縮過程を追加したものである。
【０２８１】
[Extend_Process_Z_Plus]
(1)現時刻tmでの仮想ロボット位置のZ座標の最小値をZmin、Z座標の最大値をZmaxとし、_zw←Zminとして、_zw<Zmaxの間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Z座標値が_zwである仮想ロボットiについて、以下を実行する。
(3-1）仮想ロボットiからZ軸正方向に途切れず隣接している全ての仮想ロボットinの中にZ座標がZmaxのものがない場合、仮想ロボットiから、仮想ロボットinの間の全ての仮想ロボットのdestination_to_be_compressed値を1とし、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのZ座標値をインクリメントし、[Motion_Data_Update]を実行する。_ｚwをインクリメントする。
【０２８２】
以上の処理により、Z軸に垂直な仮想面が、現時刻tmにおいてp台のロボットが成す群れ形状をZ軸負方向無限遠からZ軸正方向に移動して、仮想面に当たったロボットを押して、Z座標がZmaxではないすべてのロボットについて、ロボットのZ軸正方向に接するロボットが存在している状態になるように圧縮する。
[Extend_Process_S]
(1)tm←0とし、各仮想ロボットiの位置を開始位置の集合Sとし[Motion_Data_Update]を実行する。[Extend_Process_X_Minus]を実行する（図３６Ａから図３６Ｂへの変形に相当）。
(2)[Extend_Process_X_Plus]を実行する（図３６Ｂから図３６Ｃへの変形に相当）。
(3)[Extend_Process_Y_Minus]を実行する（図３６Ｃから図３６Ｄへの変形に相当）。
(4)[Extend_Process_Y_Plus]を実行する（図３６Ｄから図３６Ｅへの変形に相当）。
(5)[Extend_Process_Z_Plus]を実行する（図３６Ｅから図３６Ｆへの変形に相当）。
(6)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、最小値をZmin、各Z座標値_zでの、X座標が0であるロボットの数-１を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-１を_x_num[-_y][ _z-Zmin]に格納する。なお、[Extend_Process_Z_Plus]を実行後の、ロボット群の成す形状を押し出し終了時形状１という。
【０２８３】
[押し出し終了時形状１から中間位置の集合M2への変形過程]
押し出し終了時形状１から中間位置の集合M2への変形過程（図３６Ｆから図３７への変形に相当）は、以下のプロセスである。
(1)押し出し終了時形状１の天井（Z座標が最大値（Zmax）をとるロボットがなす面）に存在するロボット数が４の倍数になるように天井にないロボットのいくつかを天井へ移動させる(図３８参照)。
(2)X,Y,Z軸正方向に圧縮を行う。
(3)天井形状が、４つのロボットで構成されるマス単位で作られた長方形になるように変形する（図３９、図４０参照）。
(4)X軸、Y軸、Z軸正方向で圧縮を行い、ロボットによりX=0、Y=0、Z＝最大値の平面が構成されるようにする。
【０２８４】
上記(1)のプロセスは、後述の[Move_Odd_Robots]により実行される。[Move_Odd_Robots]では、Z座標がZmax-1以下で、X座標値が0であるロボットのうち、それらのロボットにX軸負方向に連なって接するロボットの数が１つ以上あるものを順次、X=0の面に沿って天井に移動させていくことを、天井に含まれるロボット数が4の倍数になるまで繰り返す。X=0の面で移動させることが可能なロボットがなくなってしまったら、Z座標がZmax-1以下で、Y座標が0、X座標値が0であるロボットのうち、それらのロボットにY軸負方向に連なって接するロボットの数が１つ以上あるものを順次、X=0、Y=0の柱に沿って天井に移動させていくことを、天井に含まれるロボット数が4の倍数になるまで繰り返す。すなわち本実施形態では、天井下に含まれるロボット数が、４(整数を４で割った余りの最大数＋１)つ以上存在していることが本実施形態適用のための条件となる。
【０２８５】
上記(3)のプロセスは、後述の[Make_2x2_Rectangle_Ceiling]により実行される。図３９、図４０に示すように、目標の長方形からはみ出ているロボットの位置を長方形内に移動させることで、天井の形を長方形に整えていく。
[Make_Ceiling]
(1)後述の[Move_Odd_Robots]を実行する。
(2)[Extend_Process_X_Plus]を実行する。
(3)[Extend_Process_Y_Plus]を実行する。
(4)[Extend_Process_Z_Plus]を実行する。
(5)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、各Z座標値_zでの、X座標が0であるロボットの数-１を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-１を_x_num[-_y][ _z-Zmin]に格納する。
(6)後述の[Make_2x2_Rectangle_Ceiling]を実行する。
(7)[Extend_Process_X_Plus]を実行する。
(8)[Extend_Process_Y_Plus]を実行する。
(9)[Extend_Process_Z_Plus]を実行する。
(10)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、各Z座標値_zでの、X座標が0であるロボットの数-１を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-１を_x_num[-_y][ _z-Zmin]に格納する。tm-1を変数push_time_startに格納する。
(11)[Extend_Process_S]開始時に開始位置の集合S内の各位置にあった仮想ロボットiが[Extend_Process_S][Make_Ceiling]により移動した先の、押し出し終了時形状２内の位置として、各仮想ロボットiの位置を変数（Xsp[i],Ysp[i],Zsp[i]）に格納する。
(12)t=0,push_time_startの各時刻において、変数(transform_S_to_M2_x[t][i],transform_S_to_M2_y[t][i],transform_S_to_M2_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。
【０２８６】
[Move_Odd_Robots]
(1)Z座標がZmaxであるロボット数をカウントし、変数ceiling_cntに格納する。ceiling_cntの値を4で割った余りの値を、変数add_numに格納する。
(2)_z=(Zmax-Zmin+1)−１から_z=0まで、(3)〜（4）を繰り返す。
(3)_y=_y_num[_z]から_y=0まで、(4)を繰り返す。
(4)もし、_x_num[_z][_y]が、1以上かつadd_numが0でないならば、以下を実行する。（動作例を図３８に示す。）
(4-1)変数add_x←0とする。add_xが_x_num[_z][_y]より小さく、add_numが0でない間、(4-2)〜(4-5)を繰り返す。
(4-2)Y座標が_y,Z座標が_zのロボットを全て、X軸正方向に1ステップ移動させる（図３８Ａから図３８Ｂへの変形に相当）。[Motion_Data_Update]を実行する。このとき、X座標が1、Y座標が_y,Z座標が_zのロボットをロボットipとする。
(4-3)add_xをインクリメントし、add_numをデクリメントする。
(4-4)ロボットipを、Z軸正方向に1ステップ移動させ[Motion_Data_Update]を実行することを、ロボットipのZ座標がZmaxになるまで繰り返す（図３８Ｂから図３８Ｃへの変形に相当）。
(4-5)Y座標がロボットipのY座標,Z座標がZ_maxのロボットを全て、X軸負方向に1ステップ移動させる（図３８Ｃから図３８Ｄへの変形に相当）。[Motion_Data_Update]を実行する。
(5)_z=Zmax−１から_z=0まで、（6）を繰り返す。
(6)もし、_y_num[_z]が、1以上かつadd_numが0でないならば、以下を実行する。
(6-1)変数add_x←0とする。add_xが_y_num[_z]より小さく、add_numが0でない間、(6-2)〜(6-5)を繰り返す。
(6-2)X座標が0,Z座標が_zのロボットを全て、Y軸正方向に1ステップ移動させる。[Motion_Data_Update]を実行する。このとき、X座標が0、Y座標が1,Z座標が_zのロボットをロボットipとする。
(6-3)add_xをインクリメントし、add_numをデクリメントする。
(6-4)ロボットipを、Z軸正方向に1ステップ移動させ[Motion_Data_Update]を実行することを、ロボットipのZ座標がZmaxになるまで繰り返す。
(6-5)X座標がロボットipのX座標,Z座標がZ_maxのロボットを全て、Y軸負方向に1ステップ移動させる。[Motion_Data_Update]を実行する。
【０２８７】
[Make_2x2_Rectangle_Ceiling]
(1)天井のX方向、Y方向の長さを決定する。
(1-1)天井に含まれる（Z座標値がZmaxの）ロボットの数をカウントし、ceiling_cntに格納する。ceiling_cntの値を4で割った値に入れ替える(ceiling_cnt←ceiling_cnt/4)。
(1-2)変数ic=1からic=ceiling_cnt-1までの中で、ceiling_cntを割った余りが0となるicを抽出し、ceiling_cntを抽出したicで割った値をjc(=ceiling_cnt/ic)とし、icとjcの組合せの中で(言い換えると、ceiling_cntを被除数とし、icを除数とし、剰余が0のときの除数icと商jcとの組合せの中で)、2×jcがy_num[Zmax-Zmin]より小さく、かつ、2×icがx_num[Zmax-Zmin][0]より小さくなる組合せを抽出し、抽出した組合せの中で、icとjcの差の絶対値が最小となるものを長方形の横縦長さとして選ぶ。選んだicを変数_latに、jcを変数_lonに格納する。
(2)Y軸方向の入れ替えを行う（図３９参照）。
(2-1)天井に含まれるロボットのうち、X座標が-_xのロボットの数をカウントし変数y_ceiling_num[_x]に格納する。
(2-2)変数_x=0から_x=-x_num[Zmax-Zmin][0]まで、(2-3)〜(2-8)を繰り返す。
(2-3)y_ceiling_num[_x]が2×_lonより大きい間(2-4)〜(2-8)を繰り返す。
(2-4)X座標が-_xのロボットを全てY軸正方向に１ステップ移動させる(図３９Ａから図３９Ｂへ変形に相当)。[Motion_Data_Update]を実行する。
(2-5)天井に含まれるロボットのうち、X座標が-_xのロボットの数をカウントし変数y_ceiling_num[_x]に格納する。
(2-6)変数_xx=_x+1から_xx=-x_num[Zmax-Zmin][0]までで、y_ceiling_num[_xx]が2×_lonより小さい最小の_xxについて、(2-7)〜(2-8)を実行する。
(2-7)Y座標が１のロボットを全て、X軸負方向に１ステップ移動させ、[Motion_Data_Update]を実行することを、_xx-_x回実行する(図３９Ｂから図３９Ｃへ変形に相当)。
(2-8)X座標が-_xxのロボットを全て、Y軸負方向に１ステップ移動させ(図３９Ｃから図３９Ｄへ変形に相当)、[Motion_Data_Update]を実行する。
(3)X軸方向の入れ替えを行う（図４０参照）。
(3-1)天井に含まれるロボットのうち、Y座標が-_yのロボットの数をカウントし変数x_ceiling_num[_y]に格納する。
(3-2)変数_y=0から_y=-y_num[Zmax-Zmin]まで、(3-3)〜(3-8)を繰り返す。
(3-3)x_ceiling_num[_y]が2×_latより大きい間(3-4)〜(3-8)を繰り返す。
(3-4)Y座標が-_yのロボットを全てX軸正方向に１ステップ移動させる(図４０Ａから図４０Ｂへの変形に相当)。[Motion_Data_Update]を実行する。
(3-5)天井に含まれるロボットのうち、Y座標が-_yのロボットの数をカウントし変数x_ceiling_num[_y]に格納する。
(3-6)変数_yy=_y+1から_yy=-y_num[Zmax-Zmin]までで、x_ceiling_num[_yy]が2×_latより小さい最小の_yyについて、(2-7)〜(2-8)を実行する。
(3-7)X座標が１のロボットを全て、Y軸負方向に１ステップ移動させ、[Motion_Data_Update]を実行することを、_yy-_y回実行する(図４０Ｂから図４０Ｃへの変形に相当)。
(3-8)Y座標が-_yyのロボットを全て、X軸負方向に１ステップ移動させ(図４０Ｃから図４０Ｄへの変形に相当)、[Motion_Data_Update]を実行する。
【０２８８】
[押し出し終了時形状２から目標位置の集合Gへの展開過程]
押し出し終了時形状２から目標位置の集合Gへ変形する最終展開過程[Extend_Process_G]は、第二実施形態における最終展開過程にZ軸下方向へ圧縮過程を追加した過程となる。よって、押し出し終了時形状２は、Z軸に垂直な仮想面が、第二実施形態における押し出し終了時形状をZ軸正方向無限遠からZ軸負方向に移動して、仮想面に当たったロボットを押して、Z座標がZminではないすべてのロボットについて、ロボットのZ軸負方向に接するロボットが存在している状態になるように圧縮した形状である。ただし、圧縮後の隊形がX座標、Y座標が正の空間に押し込まれるため、X軸、Y軸方向の圧縮の方向が第二実施形態と異なる。
【０２８９】
[Extend_Process_G]
(1)tm←0とし、各仮想ロボットiの位置を目標位置の集合Gとし、[Extend_Process_X_Minus_Inv]を実行する（図４１Ａから図４１Ｂへの変形に相当）。
(2)後述の[Extend_Process_X_Plus_Inv]を実行する（図４１Ｂから図４１Ｃへの変形に相当）。
(3)後述の[Extend_Process_Y_Minus_Inv]を実行する（図４１Ｃから図４１Ｄへの変形に相当）。
(4)後述の[Extend_Process_Y_Plus_Inv]を実行する（図４１Ｄから図４１Ｅへの変形に相当）。
(5)後述の[Extend_Process_Z_Minus]を実行する（図４１Ｅから図４１Ｆへの変形に相当）。
(6)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax_g、最小値をZmin_g、各Z座標値_zでの、X座標が0である仮想ロボットの数-１を_y_num_g[_z-_Zmin_g]に、各Z座標値_zでの、Y座標値が_yである仮想ロボットの数-１を_x_num_g[_y][ _z-_Zmin_g]に格納する。tm-1の値を変数push_time_goalに格納する。
(7)目標位置の集合G内の各位置iが圧縮により移動した先の、押し出し終了時形状２内の位置iを変数（Xgp[i],Ygp[i],Zgp[i]）に格納する。
(8)t=0〜push_time_goalの各時刻において、変数(transform_P2_to_G_x[t][j],transform_P2_to_G_y[t][j],transform_P2_to_G_z[t][j])に、(motion_x[push_time_goal-t][j],motion_y[push_time_goal-t][j],motion_z[push_time_goal-t][j])の値を格納する。
【０２９０】
[Extend_Process_Z_Minus]
(1)現時刻ｔmでの仮想ロボット位置のZ座標の最小値をZmin、Z座標の最大値をZmaxとし、_zw＝Zmax-1として、_zw>Zminの間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Z座標値が_zwである仮想ロボットiについて、以下を実行する。
(3-1）仮想ロボットiからZ軸負方向に途切れず隣接している全ての仮想ロボットinの中にZ座標がZminのものがない場合、仮想ロボットiから、仮想ロボットinの間の全ての仮想ロボットのdestination_to_be_compressed値を1として、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのZ座標値をデクリメントし、[Motion_Data_Update]を実行する。_zwをデクリメントする。
【０２９１】
[Extend_Process_X_Minus_Inv]
(1)全仮想ロボットでのX座標の最小値をXminとする。
(2)_xw＝Xminとして、_xw<0の間、(3)〜(5)を繰り返す。
(3)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(4)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(4-1）destination_to_be_compressed[i]←1とする。
(4-2)仮想ロボットiからX軸正方向に途切れず隣接している全ての仮想ロボットinのdestination_to_be_compressed[in]←1とする。
(4-3)_flg_c←1とする。
(5)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をインクリメントし、[Motion_Data_Update]を実行する。_xwをインクリメントする。
【０２９２】
[Extend_Process_X_Plus_Inv]
(1)現時刻ｔmでの仮想ロボット位置のX座標の最大値をXmaxとし、_xw＝Xmaxとして、_xw>0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(3-1）仮想ロボットｊからX軸負方向に途切れず隣接している全ての仮想ロボットinの中にX座標が0のものがない場合、隣接する仮想ロボットi〜inのdestination_to_be_compressed[in]←1として、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をデクリメントし、[Motion_Data_Update]を実行する。_xwをデクリメントする。
【０２９３】
[Extend_Process_Y_Minus_Inv]
(1)現時刻ｔmでの仮想ロボット位置のY座標の最小値をYminとし、_yw＝Yminとして、_yw<0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1）destination_to_be_compressed[i]←1とする。
(3-2)仮想ロボットｊからY軸正方向に途切れず隣接している全ての仮想ロボットinのdestination_to_be_compressed[in]←1とする。
(3-3)_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をインクリメントし、[Motion_Data_Update]を実行する。_ywをインクリメントする。
【０２９４】
[Extend_Process_Y_Plus_Inv]
(1)現時刻ｔmでの仮想ロボット位置のY座標の最大値をYmaxとし、_yw＝Ymaxとして、_yw>0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1）仮想ロボットiからY軸正方向に途切れず隣接している全ての仮想ロボットinの中にY座標が0のものがない場合、それら隣接の仮想ロボットi〜inのdestination_to_be_compressed[in]←1として、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をデクリメントし、[Motion_Data_Update]を実行する。_ywをデクリメントする。
【０２９５】
[中間位置の集合M2から押し出し終了時形状２への変形過程]
本過程では、まず中間位置の集合M2からシードロボットをZ座標が最低となる位置に移動させる(後述するMake_seed_M2に相当)。続いて、中間位置の集合M2にあるロボットを第二実施形態における押し出し過程の逆再生の形で、押し出し終了時形状２へ押し出していく。すなわち、中間位置の集合M2にあるロボットは、まず、X=0の面からX軸正方向に押し出し可能なロボットから、1ずつ押し出されていき、X=0の面にそってシードロボットに接する位置まで移動していく。X=0の面からX軸正方向に押し出し可能なロボットがなくなったら、続いて、X=0、Y=0の柱からY軸正方向に押し出し可能なロボットから、1ずつ押し出されていき、柱にそってシードロボットに接する位置まで移動していく。シードロボットに接する位置に達したロボットは、接しているシードロボットをZ軸下方に押しのけて自らがシードロボットの位置に収まる。押しのけられた元シードであったロボットは、押し出し終了時形状２内の位置に移動していく。その移動は、第二実施形態で示した押し出し過程と同じ移動形態（柱形成→旗形成→最終押し出し形態形成）である。
【０２９６】
続いて、X=0、Y=0の柱からY軸正方向に押し出し可能なロボットがなくなったときには、もはや、X=0、Y=0の柱をなすロボット以外は、シードロボットの上方には残っていないので、柱を一つずつ下方にスライドさせていく形で、ロボットを一ずつシードロボットの下方に押し出していく。
[Translate_from_M2_to_Mobile_G]
(1)tm←0とし、各ロボットiの位置を（Xsp[i],Ysp[i],Zsp[i]）とし、Motion_Data_Updateを実行する。後述の[Make_Seed_M2]を実行する(図４２Ａから図４２Ｂへの変形に相当)。
(2)後述の[Robot_Translation]を実行する(図４２Ｂから図４２Ｅへの変形に相当)。
(3)シードロボット以外の全ロボットを、X軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(4)シードロボット以外の全ロボットを、Y軸負方向に1ステップ移動させる((3)(4)の処理を併せて図４２Ｅから図４２Ｆへの変形に相当)。Motion_Data_Updateを実行する。tm-1をtranslate_timeに格納する。
(5)t=0〜translate_timeの各時刻において、変数(transform_M2_to_P2_x[t][i],transform_M2_to_P2_y[t][i],transform_M2_to_P2_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。
(6)各ロボットiの位置を（Xtr[i],Ytr[i],Ztr[i]）に格納する。
【０２９７】
[Make_Seed_M2]
(1)Z座標値がZmaxのロボットを全て、X軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(2)Z座標値がZmaxのロボットを全て、Y軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する((1)(2)の処理を併せて図４３Ａから図４３Ｂへの変形に相当)。
(3)Z座標値がZmaxで、Y座標値が-1以下のロボットを全て、X軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(4)Z座標値がZmaxで、X座標値が-1以下のロボットを全て、Y軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する((3)(4)の処理を併せて図４３Ｂから図４３Ｃへの変形に相当)。
(5)[Extend_Process_X_Plus]を実行する。
(6)位置（1,0,Zmax）,(1,1,Zmax),(0,1,Zmax)にあるロボットをそれぞれのZ座標値がZ座標の最小値Zminに等しくなるまで、1ステップZ軸負方向に移動させ、Motion_Data_Updateを実行することを繰り返す((5)(6)の処理を併せて図４３Ｃから図４３Ｄへの変形に相当)。
(7)現時刻tmでのロボットiの位置で、Z座標の最大値をZmax、Z座標の最小値をZmin、各Z座標値_zでの、X座標が0であるロボットの数-１を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-１を_x_num[-_y][_z-Zmin]に格納する。
【０２９８】
[Robot_Translation]
(1)_z=Zmax- Zminから、_z=1までの全ての値で、(2)〜(8)を繰り返す。
(2)_y=y_num[_z]から、_y=0までの全ての値で、(3)〜(8)を繰り返す。
(3)_x=x_num[_z][_y]から、_x=0までの全ての値で、(4)〜(8)を繰り返す。
(4)位置（0,-_y,_z+Zmin）にあるロボットをipとする。
(5)ロボットipとY,Z座標が同じロボットを全て、X軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(6)ロボットipのY座標が0になるまで、ロボットipをY軸正方向に移動させ、Motion_Data_Updateを実行することを繰り返す。
(7)ロボットipがシードロボットの一つに接するまで、ロボットipをZ軸負方向に移動させ、Motion_Data_Updateを実行することを繰り返す。
(8)ロボットipについて、後述のTravel_Goalを実行する。
(9) _z=Zmax- Zminから、_z=1までの全ての値で、(10)〜(14)を繰り返す。
(10)_y=y_num[_z]から、_y=1までの全ての値で、(11)〜(14)を繰り返す。
(11)位置（0,0,_z+Zmin）にあるロボットをipとする。
(12)ロボットipとZ座標が同じロボットを全て、Y軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(13)ロボットipがシードロボットの一つに接するまで、ロボットipをZ軸負方向に移動させ、Motion_Data_Updateを実行することを繰り返す。
(14)ロボットipについて、後述のTravel_Goalを実行する。
(15)_z=Zmaxから、_z=Zmin+1までの全ての値で、(16)〜(17)を繰り返す。
(16)位置（0,0,Zmin+1）にあるロボットをipとする。
(17)ロボットipについて、後述のTravel_Goalを実行する。
【０２９９】
[Travel_Goal]
(1)最初に本処理が実行されたときに、変数_pole_cnt,_flag_cnt,_flag_y,_flag_z,_flag_x,_flag_zz,_flag_yy,_floor_cntを0に初期化する。最初に本処理が実行されたときに、変数_flag_allに、押し出し終了時形状２内の、X座標が0で、Y座標が負のロボットの総和を、変数_floor_allにX座標が負のロボットの総和を格納する。
【０３００】
(2)_pole_cnt<(Zmax_g-Zmin_g+1)であるならば、以下を実行して終了する(図４２Ｂから図４２Ｃへの変形に相当)。そうでないなら(3)へ移行する。
(2-1)ロボットipのX座標が１、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。
(2-2)ロボットipのX座標が0、Y座標が１ならば、ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。
(2-3)ロボットipのX座標が0、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。
(2-4)ロボットipとX,Y座標値が同じで、ロボットipと連なっている全てのロボットをZ軸負方向に移動させる。Motion_Data_Updateを実行する。_pole_cntをインクリメントする。
【０３０１】
(3)_flag_cnt<_flag_allであるならば、以下を実行して終了する(図４２Ｃから図４２Ｄへの変形に相当)。そうでないなら(4)へ移行する。
(3-1)_flag_y<_y_num_g[_flag_z+Zmin_g]ならば、(3-2)〜(3-8)を実行する。そうでないならば、(3-8)のみ実行する。
(3-2)ロボットipのX座標が0、Y座標が1ならば、ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。ロボットipの上方に隣接する全てのロボットをY軸負方向に移動させる。Motion_Data_Updateを実行する。
(3-3)ロボットipのX座標が1、Y座標が１ならば、ロボットipの上方に隣接する全てのロボットをY軸負方向に移動させる。Motion_Data_Updateを実行する。
(3-4)ロボットipのX座標が0、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。
(3-5)ロボットipとX,Y座標値が同じで、ロボットipと連なっている全てのロボットをZ軸負方向に移動させる。Motion_Data_Updateを実行する。
(3-6)ロボットipがZ軸負方向で接しているロボットをigとし、ロボットigをZ軸負方向に移動させ、Motion_Data_Updateを実行することを、ロボットigのZ座標が_flag_z+Zmin_gになるまで繰り返す。
(3-7)ロボットigとZ座標が等しいロボットを全て、Y軸正方向に１ステップ移動させる。_flag_cntをインクリメントする。
(3-8)_flag_yをインクリメントし、_flag_y>y_num_g[_flag_z+Z_min_g]ならば、_flag_zをインクリメントし、_flag_yを0にする。
【０３０２】
(4)_flag_x<_x_num_g[_flag_zz+Zmin_g][_flag_yy]であるならば、(4-1)〜(4-8)を実行する(図４２Ｄから図４２Ｅへの変形に相当)。そうでないなら(4-8)のみを実行して終了。
(4-1)ロボットipのX座標が1、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。ロボットipの上方に隣接する全てのロボットをX軸負方向に移動させる。Motion_Data_Updateを実行する。
(4-2)ロボットipのX座標が1、Y座標が１ならば、ロボットipの上方に隣接する全てのロボットをX軸負方向に移動させる。Motion_Data_Updateを実行する。
(4-3)ロボットipのX座標が0、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。
(4-4)ロボットipとX,Y座標値が同じで、ロボットipに連なる全てのロボットをZ軸負方向に移動させる。Motion_Data_Updateを実行する。
(4-5)ロボットipがZ軸負方向で接しているロボットをigとし、ロボットigをZ軸負方向に移動させ、Motion_Data_Updateを実行することを、ロボットigのZ座標が_flag_zz+Zmin_gになるまで繰り返す。
(4-6)ロボットigをY軸正方向に移動させ、Motion_Data_Updateを実行することを、ロボットigのZ座標が_flag_yy+1になるまで繰り返す。
(4-7)ロボットigとY,Z座標が等しいロボットを全て、X軸正方向に１ステップ移動させる。_flag_cntをインクリメントする。
(4-8)_flag_xをインクリメントし、_flag_x>x_num_g[_flag_zz+Z_min_g][_flag_yy]ならば、_flag_yyをインクリメントし、さらに_flag_yy>y_num_g[_flag_zz+Z_min_g]ならば、_flag_yy←0とし、_flag_zzをインクリメントする。_flag_xを0にする。
【０３０３】
[中間位置の集合M2におけるロボット入れ替え過程]
中間位置の集合M2の形状に関する条件により、中間位置の集合M2の天井を構成する各ロボットのうち、Z軸負方向に隣接するロボットを持つかどうかについては、天井を構成するロボットのうち、少なくとも一つがそれに当てはまるという保証しかない。図４４に示すように、本過程でのロボット入れ替え動作においては、入れ替え元のロボットi_originが天井の上部に押し出されたのちに、天井の上を入れ替え元ロボットi_originが入れ替え先ロボットi_targetの近傍まで移動する動作が含まれるため、入れ替え元ロボットi_originを選ぶ際に、必ず、Z軸負方向に隣接するロボットを持つロボットを入れ替え元のロボットi_originとして選択しなければならないという制限がある。そのため、本処理では、最初に入れ替え元のロボットi_originとしてそのような、Z軸負方向に隣接するロボットを持つロボットを選択する。そして、その後の入れ替え動作の後に、入れ替え元のロボットi_originが元あった位置（X_origin,Y_origin,Z_origin）を入れ替え先とするロボットi_origin_2が、位置（X_origin,Y_origin,Z_origin）にたどりついたのちは、ロボットi_origin_2がその後の入れ替え動作における仮想的な、入れ替え元ロボットとしてふるまうようにする。すなわち、以下のような流れになる。
【０３０４】
(1)入れ替え元ロボットi_originを、Z軸負方向に隣接するロボットを持つ天井内のロボットから選択する。
(2)ロボットi_originを入れ替え先ロボットi_target[0]の位置に移動させ、ロボットi_target[0]をロボットi_target[0]が元あった場所から追い出す。i_perm←0とする。
(3)以下の繰り返し処理(4)を実行する。
(4)ロボットi_target[i_perm]をロボットi_target[i_perm]の入れ替え先ロボットi_target[i_perm+1]の位置に移動させる。i_target[i_perm+1]＝i_originでない場合は、ロボットi_target[i_perm+1]をロボットi_target[i_perm+1]が元あった場所から追い出す。i_permをインクリメントし、（４）を繰り返す。i_target[i_perm+1]＝i_originであるなら終了し（５）へ。
(5) i_origin_2←i_target[i_perm]とし、入れ替えの済んでいない任意のロボットをi_target[i_perm]とする。入れ替えの済んでいないロボットがなくなるまで(7)〜(10)を繰り返す。
(7)ロボットi_origin_2を入れ替え先ロボットi_target[i_perm]の位置に移動させ、ロボットi_target[i_perm]をロボットi_target[i_perm]が元あった場所から追い出す。
(8)以下の繰り返し処理(9)を実行する。
(9)ロボットi_target[i_perm]をロボットi_target[i_perm]の入れ替え先ロボットi_target[i_perm+1]の位置に移動させ、ロボットi_target[i_perm+1]をロボットi_target[i_perm+1]が元あった場所から追い出す。追い出されたロボットがロボットi_origin_2であるならば終了して（10）へ、そうでないなら（９）を繰り返す。
(10)ロボットi_origin_2を(5)の処理が行われた時点の位置に戻す。
【０３０５】
実際の処理を以下に示す。
[Permutation_in_M2]
(1)後述のPermutation_Initializationを実行する。
(2)後述のPermutaiton_Originを実行する。
(3)後述のPermutaiton_Othersを実行する。
(4)tm-1の値を、permutation_timeに格納し、t=0〜permutation_timeの各時刻において、変数(transform_M2_to_M2_x[t][i],transform_M2_to_M2_y[t][i],transform_M2_to_M2_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。
【０３０６】
[Permutation_Initialization]
(1)全てのロボットi(i=0,…,p-1)で、変数already[i]を0に、cube_id[i]をpに、cube_after[i]をpに初期化する。
(2)全てのi(i=0,…,p-1)について、位置（Xgp[i],Ygp[i],Zgp[i]）と等しくなる位置(Xtr[j],Ytr[j],Ztr[j])を特定し、ロボットiの入れ替え先を示す変数cube_id[i]の値をjとする。
(3)(Xsp[i],Ysp[i],Zsp[i])の値を(cube_buffer_x[i],cube_buffer_y[i],cube_buffer_z[i])に格納する。
(4)tm←0とし、各ロボットの位置を（Xsp[i],Ysp[i],Zsp[i]）とする。Motion_Data_Updateを実行する。
【０３０７】
[Permutation_Origin]
(1)天井に存在し、Z軸負方向に接するロボットを持つロボットciを一つ選択する。
(2)flg_ci←0とし、_i＝cube_id[ci]とする。
(3)_iがciに等しくない間、(4)〜(7)を繰り返す。
(4)flg_ci=0の場合、_i←ci、flg_ci←1とする。
(5)cube_after[_i]←cube_id[_i]とする。
【０３０８】
(6)ciがcube_id[_i]に等しくない場合、変数_dest←cube_id[_i]として、後述の[Change_Hetero_Ceiling(ci,_i,cube_id[_i])]を実行する。等しい場合、以下を実行する。
(6-1)X座標がcube_buffer_x[ci]に、Y座標がcube_buffer_y[ci]に等しいロボットiiを一台特定し、_dest←iiとして、[Horizontal_Change_Odd]を実行する。
(6-2)ロボット_iのX座標と、cube_buffer_x[ci]の差が１の場合は、ロボット_iをX軸負方向に１ステップ移動させる。―１の場合は、ロボット_iをX軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(6-3)ロボット_iのY座標と、cube_buffer_y[ci]の差が１の場合は、ロボット_iをY軸負方向に１ステップ移動させる。―１の場合は、ロボット_iをY軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(6-4)ロボット_iのZ座標値がZmaxになるまで、ロボット_iとX,Y座標が同じ全てのロボットをZ軸負方向に１ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(6-5)変数seed_perm←_i,zero_perm=ciとする。
【０３０９】
(7)already[_i]←1とし、_i＝cube_id[_i]とする。
(8)already[ci]←1とする。
【０３１０】
[Permutation_Others]
(1)全てのロボットciに対して、already[i]=0のとき(2)以下を実行する。
(2)flg_ci←0、flg_oo=0とし、_i＝cube_id[ci]とする。
(3)ci=_iでない場合、
flg_oo←1とし、変数x0にロボットseed_permのX座標を、変数y0にロボットseed_permのY座標を、バッファリングする。[Change_Hetero_Ceiling(seed_perm,seed_perm,ci)]を実行する。
(4)_iがciに等しくない間、(5)〜(8)を繰り返す。
(5)flg_ci=0の場合、_i←ci、flg_ci←1とする。
(6)cube_after[_i]←cube_id[_i]とする。
(7)ciがcube_id[_i]に等しくない場合、後述の[Change_Hetero_Ceiling(seed_perm,_i,cube_id[_i])]を実行する。等しい場合、[Change_Hetero_Ceiling(seed_perm,_i,seed_perm)]を実行する。
(8)already[_i]←1とし、_i＝cube_id[_i]とする。
(9)already[ci]←1とする。
【０３１１】
(10)flg_oo＝１のとき、以下を実行する。
(10-1)X座標がx0に、Y座標がy0に、等しいロボットiiを1つ選んで、_dest←iiとし、[Horizontal_Change_Odd]における_iをseed_permに置き換えて、[Horizontal_Change_Odd]を実行する。
(10-2)ロボットseed_permのX座標と、ロボットiiのX座標との差が１の場合は、ロボットseed_permをX軸負方向に１ステップ移動させる。―１の場合は、ロボットseed_permをX軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(10-3)ロボットseed_permのY座標と、ロボットiiのY座標との差が１の場合は、ロボットseed_permをY軸負方向に１ステップ移動させる。―１の場合は、ロボットseed_permをY軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(10-4)ロボットseed_permと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットseed_permと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３１２】
[Horizontal_Change_Odd]
[Horizontal_Change_Odd]は、第二実施形態の[Horizontal_Change]にて、cube_id[_i]を_destに置き換えて、ステップ(1)を以下のものに置き換えた処理である。
(1)ロボット_iのX座標値がcube_buffer_x[_dest]に等しくかつロボット_iのY座標値がcube_buffer_y[_dest]に等しいとき、以下を実行して終了する。そうでないなら(2)へ移行する。
ロボット_iのX座標値が偶数ならば、ロボット_iをX軸負方向に１ステップ移動させ、奇数ならばX軸正方向に１移動させる。Motion_Data_Updateを実行する。
【０３１３】
[Change_Hetero_Ceiling(_io,_i,_id)]
(1)_io=_iの場合、_iと同じX,Y座標値のロボットを全て、Z軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(2)_dest←_idとし[Horizontal_Change_Odd]を実行する。
(3)ロボット_iのZ座標とロボット_idのZ座標が等しくなるまで、_idと同じX,Y座標値のロボットを全て、Z軸正方向に１ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(4)後述の[Vertical_Change_Ceiling]を実行する。
(5) ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxより小さいならば、ロボット_iと同じX,Y座標値のロボットを全て、Z軸正方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxより大きいならば、ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３１４】
[Vertical_Change_Ceiling]
(1)ロボット_iとロボット_idのX座標が等しい時、(1-1)に移行し、それ以外の場合(2)に移行する。
【０３１５】
(1-1)もしロボット_iのX座標が奇数ならば(1-1-1)に移行し、それ以外の場合(1-1-2)に移行する。
【０３１６】
(1-1-1)ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボット_iのY座標がロボット_idのY座標より大きいならば(1-1-1-1)に移行し、それ以外の場合は(1-1-1-2)に移行する。
【０３１７】
(1-1-1-1)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３１８】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３１９】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２０】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２１】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３２２】
(1-1-1-2)
ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２３】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２４】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２５】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２６】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３２７】
(1-1-2)(ロボット_iのX座標が奇数ではない場合)ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボット_iのY座標がロボット_idのY座標より大きいならば(1-1-2-1)に移行し、それ以外の場合、(1-1-2-2)に移行する。
【０３２８】
(1-1-2-1)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３２９】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３０】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３１】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３２】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３３３】
(1-1-2-2)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３４】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３５】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３６】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３３７】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
(2)もしロボット_iのY座標が奇数ならば(2-1)に移行し、それ以外の場合(2-2)に移行する。
【０３３８】
(2-1)ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボット_iのX座標がロボット_idのX座標より大きいならば(2-1-1)に移行し、それ以外の場合(2-1-2)に移行する。
【０３３９】
(2-1-1)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４０】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４１】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４２】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４３】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３４４】
(2-1-2)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４５】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４６】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４７】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３４８】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３４９】
(2-2)ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボット_iのX座標がロボット_idのX座標より大きいならば(2-2-1)に移行し、それ以外の場合(2-2-2)に移行する。
【０３５０】
(2-2-1)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５１】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５２】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５３】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５４】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３５５】
(2-2-2)ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５６】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５７】
ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５８】
ロボット_iおよび、ロボット_iとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動し、ロボット_idおよび、ロボット_idとX、Y座標が等しくZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。
【０３５９】
ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
【０３６０】
以上により、第三実施形態の全過程は以下のようになる。
[Transformation_from_S_to_G_Hetero_2]
(1)[Extend_Process_S]を実行する。
(2)［Make_Ceiling］を実行する。
(3)[Extend_Process_G]を実行する。
(4)[Translate_from_M2_to_Mobile_G]を実行する。
(5)[Permutation_in_M2]を実行する。
(6)t=0〜push_time_startにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_S_to_M2_x[t][i],transform_S_to_M2_y[t][i],transform_M_to_M2_z[t][i])の値を格納する。
(7)t=push_time_start+1〜push_time_start+1+permutation_timeにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M2_to_M2_x[t-(push_time_start+1)][i],transform_M2_to_M2_y[t-(push_time_start+1)][i],transform_M2_to_M2_z[t-(push_time_start+1)][i])の値を格納する。
(8)t=push_time_start+1+permutation_time+1
〜push_time_start+1+permutation_time+1+translate_timeにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M2_to_P2_x[t-(push_time_start+1+permutation_time+1)][cube_after[i]],transform_M2_to_P2_y[t-(push_time_start+1+permutation_time+1)][cube_after[i]],transform_M2_to_P2_z[t-(push_time_start+1+permutation_time+1)][cube_after[i]])の値を格納する。
(9)t=push_time_start+1+permutation_time+1+translate_time+1〜push_time_start+1+permutation_time+1+translate_time+1+push_time_goalにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_P2_to_G_x[t-(push_time_start+1+permutation_time+1+translate_time+1)][i],transform_P2_to_G_y[t-(push_time_start+1+permutation_time+1+translate_time+1)][i],transform_P2_to_G_z[t-(push_time_start+1+permutation_time+1+translate_time+1)][i])の値を格納する。
【０３６１】
＜第三実施形態に係る行動制御システム１００＞
図２２は第三実施形態に係る行動制御システム１００の機能ブロック図を、図２３はその処理フローの例を示す。行動制御システム１００は、図２２に示すように、行動選択部１２０と、隣接状態判定部１２４と、位置更新部１２３と、位置判定部１２６と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。行動選択部１２０は押し出し過程処理部１２０Ａと中間遷移過程処理部１２０Ｂと最終展開過程処理部１２０Ｃと位置入れ替え部１２０Ｄと中間位置変形部１２０Ｅを含む。
行動選択部１２０の処理内容以外は第二実施形態と同様である。
【０３６２】
＜行動選択部１２０＞
行動選択部１２０は、上述の方法で、p台のロボットを制御する（ｓ１２０）。なお、押し出し過程処理部１２０Ａ、中間遷移過程処理部１２０Ｂ、最終展開過程処理部１２０Ｃ、位置入れ替え部１２０Ｄ、中間位置変形部１２０Ｅでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部１４０に記憶しておく。行動選択部１２０は、記憶部１４０から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。
【０３６３】
（中間遷移過程処理部１２０Ｂ）
行動選択部１２０の中間遷移過程処理部１２０Ｂは、q個の目標位置の集合Gを取り出し、(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ(圧縮する)、(2)第一方向における座標値が座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ(圧縮する)、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ(圧縮する)、(4)第二方向における座標値が座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させ、(5)第五方向における座標値が座標値O3ではない目標位置の集合に含まれる位置を、第五方向において隣接する目標位置の集合に含まれる位置が存在するように、第六方向に平行移動させることで、押し出し終了時形状２を成すロボットの位置を求める。例えば、第一方向、第二方向、第三方向、第四方向、第五方向、第六方向をそれぞれx軸正方向、y軸正方向、x軸負方向、y軸負方向、z軸正方向、z軸負方向とし、座標値O1をx=0とし、座標値O2をy=0とし座標値O3をz=Zminとしたときに、上述の[Extend_Process_G]を実行することで、押し出し終了時形状２を成すロボットの位置を求めることができる。目標位置の集合Gに含まれる位置から押し出し終了時形状２を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部１２０Ｃに出力する。
【０３６４】
行動選択部１２０の中間遷移過程処理部１２０Ｂは、q個の開始位置の集合Sを取り出し、上述の[Extend_Process_S]を実行することで、押し出し終了時形状１を成すロボットの位置を求めることができる。目標位置の集合Sに含まれる位置から押し出し終了時形状１を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部１２０Ｃに出力する。
【０３６５】
（中間位置変形部１２０Ｅ）
中間位置変形部１２０Ｅは、押し出し終了時形状１を成すロボットの位置を受け取り、上述の[Make_Ceiling]を実行することで、中間位置の集合M2を成すロボットの位置を求め、押し出し終了時形状１を中間位置の集合M2に変形する際の各ロボットの移動の順番と方向とを求める。
【０３６６】
（押し出し過程処理部１２０Ａ）
行動選択部１２０の押し出し過程処理部１２０Ａは、中間位置の集合M2を成すロボットの位置を受け取り、上述の[Translate_from_M2_to_Mobile_G]を実行することで、押し出し終了時形状２を成すロボットの位置を求め、中間位置の集合M2を押し出し終了時形状２にに変形する際の各ロボットの移動の順番と方向とを求める。
【０３６７】
なお、押し出し終了時形状２は、座標値O1及び座標値O2においてロボットが平面を成す形状である。押し出し終了時形状２は、座標値O1をx=0とし、座標値O2をy=0としたときに、上述の[Translate_from_M2_to_Mobile_G]により得られるロボットの成す隊列に相当する。押し出し終了時形状をこのような形状とすることで、中間位置の集合M2から押し出し終了時形状２へ変形する際に、ロボットの移動経路を確保することができる。
【０３６８】
（最終展開過程処理部１２０Ｃ）
行動選択部１２０の最終展開過程処理部１２０Ｃは、押し出し過程処理部１２０Ａから押し出し終了時形状２における各ロボットの位置を取得し、中間遷移過程処理部１２０Ｂから目標位置の集合Gに含まれる位置から押し出し終了時形状２を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを取得する。そして、最終展開過程処理部１２０Ｃは、押し出し終了時形状２を成すロボットに対して、目標位置の集合Gに含まれる位置から押し出し終了時形状２を成すロボットの位置を求める際の順番とは逆の順番で、目標位置の集合Gに含まれる位置から押し出し終了時形状２を成すロボットの位置を求める際の平行移動方向とは反対の方向にロボットを移動させることで、押し出し終了時形状２を成すロボットを目標位置の集合Gに移動させる。
【０３６９】
（位置入れ替え部１２０Ｄ）
位置入れ替え部１２０Ｄは、目標位置の集合Gから押し出し終了時形状２に変形した際の、ロボットiの押し出し終了時形状２における位置を押し出し過程処理部１２０Ａから取得する。
【０３７０】
また、位置入れ替え部１２０Ｄは、開始位置の集合Sから押し出し終了時形状１を経て中間位置の集合M2に変形し、M2から押し出し終了時形状２に変形した際の、ロボットiの押し出し終了時形状２における位置を押し出し中間位置変形部１２０Ｅから取得する。位置入れ替え部は、取得したそれらの位置の値を使用して、開始位置の集合Sから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置から、ロボットiを、目標位置の集合Gから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置に移動させるための各ロボットの位置の対応関係を計算する。
【０３７１】
位置入れ替え部１２０Ｄは、処理[Permutation_in_M2]を実行し、開始位置の集合Sから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置から、ロボットiを、目標位置の集合Gから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置に移動させるための、各ロボットの移動の順番と方向とを取得する。
【０３７２】
＜効果＞
このような構成により、第二実施形態と同様の効果を得ることができる。なお、第一実施形態では、中間位置の集合Mの形は２×２×２の8マスのロボット単位で構成される矩形またはL字の平板形状であるが、第三実施形態では、中間位置の集合M2の形の条件は２×２の4マスのロボット単位で構成される長方形からなる天井または床を持っていさえすればよく（要はZ軸方向における最大値または最小値となる位置を通る平面上にロボット単位で構成される矩形の平板状を成すようにロボットが配置されていればよい）、それに伴い、変形過程が単純化することができる。なお、第二実施形態の変形例と同様の変形が可能である。
【０３７３】
＜その他の変形例＞
本発明は上記の実施形態及び変形例に限定されるものではない。例えば、上述の各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。その他、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
【０３７４】
＜プログラム及び記録媒体＞
また、上記の実施形態及び変形例で説明した各装置における各種の処理機能をコンピュータによって実現してもよい。その場合、各装置が有すべき機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、上記各装置における各種の処理機能がコンピュータ上で実現される。
【０３７５】
この処理内容を記述したプログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。コンピュータで読み取り可能な記録媒体としては、例えば、磁気記録装置、光ディスク、光磁気記録媒体、半導体メモリ等どのようなものでもよい。
【０３７６】
また、このプログラムの流通は、例えば、そのプログラムを記録したＤＶＤ、ＣＤ−ＲＯＭ等の可搬型記録媒体を販売、譲渡、貸与等することによって行う。さらに、このプログラムをサーバコンピュータの記憶装置に格納しておき、ネットワークを介して、サーバコンピュータから他のコンピュータにそのプログラムを転送することにより、このプログラムを流通させてもよい。
【０３７７】
このようなプログラムを実行するコンピュータは、例えば、まず、可搬型記録媒体に記録されたプログラムもしくはサーバコンピュータから転送されたプログラムを、一旦、自己の記憶部に格納する。そして、処理の実行時、このコンピュータは、自己の記憶部に格納されたプログラムを読み取り、読み取ったプログラムに従った処理を実行する。また、このプログラムの別の実施形態として、コンピュータが可搬型記録媒体から直接プログラムを読み取り、そのプログラムに従った処理を実行することとしてもよい。さらに、このコンピュータにサーバコンピュータからプログラムが転送されるたびに、逐次、受け取ったプログラムに従った処理を実行することとしてもよい。また、サーバコンピュータから、このコンピュータへのプログラムの転送は行わず、その実行指示と結果取得のみによって処理機能を実現する、いわゆるＡＳＰ（Application Service Provider）型のサービスによって、上述の処理を実行する構成としてもよい。なお、プログラムには、電子計算機による処理の用に供する情報であってプログラムに準ずるもの（コンピュータに対する直接の指令ではないがコンピュータの処理を規定する性質を有するデータ等）を含むものとする。
【０３７８】
また、コンピュータ上で所定のプログラムを実行させることにより、各装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。
【図１】
0006392187.tif 000002
【図２】
0006392187.tif 000003
【図３】
0006392187.tif 000004
【図４】
0006392187.tif 000005
【図５】
0006392187.tif 000006
【図６】
0006392187.tif 000007
【図７】
0006392187.tif 000008
【図８】
0006392187.tif 000009
【図９】
0006392187.tif 000010
【図１０】
0006392187.tif 000011
【図１１】
0006392187.tif 000012
【図１２】
0006392187.tif 000013
【図１３】
0006392187.tif 000014
【図１４】
0006392187.tif 000015
【図１５】
0006392187.tif 000016
【図１６】
0006392187.tif 000017
【図１７】
0006392187.tif 000018
【図１８】
0006392187.tif 000019
【図１９】
0006392187.tif 000020
【図２０】
0006392187.tif 000021
【図２１】
0006392187.tif 000022
【図２２】
0006392187.tif 000023
【図２３】
0006392187.tif 000024
【図２４】
0006392187.tif 000025
【図２５】
0006392187.tif 000026
【図２６】
0006392187.tif 000027
【図２７】
0006392187.tif 000028
【図２８】
0006392187.tif 000029
【図２９】
0006392187.tif 000030
【図３０】
0006392187.tif 000031
【図３１】
0006392187.tif 000032
【図３２】
0006392187.tif 000033
【図３３】
0006392187.tif 000034
【図３４】
0006392187.tif 000035
【図３５】
0006392187.tif 000036
【図３６】
0006392187.tif 000037
【図３７】
0006392187.tif 000038
【図３８】
0006392187.tif 000039
【図３９】
0006392187.tif 000040
【図４０】
0006392187.tif 000041
【図４１】
0006392187.tif 000042
【図４２】
0006392187.tif 000043
【図４３】
0006392187.tif 000044
【図４４】
0006392187.tif 000045