JP6559582B2 - 行動制御システム、その方法及びプログラム - Google Patents

Description

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

近年、多数の自律移動ロボットを効率的に制御にするための研究が活発に行われている。その任務内容は、人の入れない箇所の監視、物品の搬送などさまざまであるが、多数のロボットの協調動作による隊列形成を効率的に行わせるための技術が求められており盛んに研究が行われている（例えば、非特許文献１参照）。多数のロボットによる効率的な隊列形成を実現するには、それぞれのロボットの配置、動作順序などを事前に計画することが重要である。このような問題を解く際に前提とするロボットの形状や、維持すべき接続状態の条件の違いによってさまざまな解が提案されている（例えば、非特許文献２及び３）。

例えば、非特許文献１に示すものでは立方体形状のロボットについての隊列変形のための動作計画が行われている。非特許文献２では、六角形形状のロボットについての隊列形成制御の研究が報告されている。また、非特許文献３では、円形ロボットの隊列形成制御に関する研究が報告されている。

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を5以上の整数の何れかとし、p台の制御対象物をp個の開始位置の集合からp個の目標位置の集合に移動させるための行動制御を行う。行動制御システムは、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、開始位置の集合及び目標位置の集合に含まれる位置のうち、第一方向における最大の座標値をXthreshとし、第三方向における最大の座標値をYthreshとし、第五方向における最小の座標値をZthreshとし、第一方向における座標値Xthresh+1に固定された第一方向に垂直な第一仮想面と、第三方向無限遠から第一方向に移動してくる第二仮想面とを想定し、第二仮想面に当たった開始位置の集合に含まれる位置、または、含まれていた位置を第二仮想面の移動方向に押して、かつ、すべての位置が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、第二方向における座標値Ythresh+1に固定された第二方向に垂直な第三仮想面と、第四方向無限遠から第二方向に移動してくる第四仮想面とを想定し、第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された位置の集合に含まれる位置を第四仮想面の移動方向に押して、かつ、すべての位置が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、中間位置の集合M1を求める圧縮過程処理部と、第二仮想面に当たった目標位置の集合に含まれる位置、または、含まれていた位置を押して、かつ、すべての位置が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された位置の集合に含まれる位置を第四仮想面の移動方向に押して、かつ、すべての位置が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、中間位置の集合M3を求め、目標位置の集合から中間位置の集合M3を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から中間位置の集合M3を求める際の移動方向とは反対の方向に各位置を移動させる展開過程処理部と、中間位置の集合M1を中間位置の集合M3に変形する過程を求める中間変形過程処理部とを含む。

上記の課題を解決するために、本発明の他の態様によれば、行動制御方法は、pを5以上の整数の何れかとし、p台の制御対象物をp個の開始位置の集合からp個の目標位置の集合に移動させるための行動制御を行う。行動制御方法は、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、開始位置の集合及び目標位置の集合に含まれる位置のうち、第一方向における最大の座標値をXthreshとし、第三方向における最大の座標値をYthreshとし、第五方向における最小の座標値をZthreshとし、第一方向における座標値Xthresh+1に固定された第一方向に垂直な第一仮想面と、第三方向無限遠から第一方向に移動してくる第二仮想面とを想定し、第二仮想面に当たった開始位置の集合に含まれる位置、または、含まれていた位置を第二仮想面の移動方向に押して、かつ、すべての位置が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、第二方向における座標値Ythresh+1に固定された第二方向に垂直な第三仮想面と、第四方向無限遠から第二方向に移動してくる第四仮想面とを想定し、圧縮過程処理部が、第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された位置の集合に含まれる位置を第四仮想面の移動方向に押して、かつ、すべての位置が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、中間位置の集合M1を求める圧縮過程処理ステップと、展開過程処理部が、第二仮想面に当たった目標位置の集合に含まれる位置、または、含まれていた位置を押して、かつ、すべての位置が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された位置の集合に含まれる位置を第四仮想面の移動方向に押して、かつ、すべての位置が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、中間位置の集合M3を求め、目標位置の集合から中間位置の集合M3を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から中間位置の集合M3を求める際の移動方向とは反対の方向に各位置を移動させる展開過程処理ステップと、中間変形過程処理部が、中間位置の集合M1を中間位置の集合M3に変形する過程を求める中間変形過程処理ステップとを含む。

本発明によれば、制御対象物に複雑な機構を備えなくとも、制御対象物同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ変形動作を行うことを可能とすることができるという効果を奏する。

ロボットの移動の例を説明するための図。 開始隊列の例を説明するための図。 ロボットの移動を説明するための図。 中間位置の集合M1,M3の形状を説明するための図。 中間位置の集合M2の形状を説明するための図 圧縮過程を説明するための図。 シードロボットの生成方法を説明するための図。 シードロボットの動作方法を説明するための図。 Vertical_Permutation過程の動作例を説明するための図。 第一実施形態に係る行動制御システムの機能ブロック図。 第一実施形態に係る行動制御システムの処理フローの例を示す図。

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

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

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

本実施形態では、開始位置の集合Sから目標位置の集合Gへの隊列形成の際に３つの中間位置の集合M1,M2,M3を経る形で隊列形成を行う。

また、本実施形態では、開始位置の集合Sと目標位置の集合Gにおいて、各ロボットがとるべき位置は、各ロボット毎に決まっているものとする。そのため、開始位置の集合Sから中間位置の集合M1を経て、中間位置の集合M2に変形したロボット群は、その形態から中間位置の集合M3を経て目標位置の集合Gへの変形において、各ロボットが適切に目標位置の集合G内の各目標位置にたどり着くことが出来るように、中間位置の集合M2の形態を維持しつつ、各ロボット同士の位置の入れ替え動作も行う。

ロボットについては、例えば図１に示すように、ロボットの周囲上下左右前後６マスのうち一つに他のロボットが存在している状態を維持しながら移動をするものとする。この手法では１つのロボット自身が、一台のロボットのサイズ分の距離を移動することで、一回の動作の移動量を正確に測ることができるというメリットがある。また、一つの面を共有する隣り合うロボットとの相対的な位置を計測しあうことで、ロボットの群れ全体の中での各ロボットの位置も容易に知ることができる。このため、ロボットの移動量の誤差によって、隊列が崩れるといった問題を起こしにくい。また、図３のように、複数のロボットを連結したように、同時に複数のロボットを移動させていくことが可能である。図３では、隊列Aから隊列Bに変形する際に、及び、隊列Dから隊列Eに変形する際に二台のロボットが連結したように同時に移動する。なお、ロボットは、隣の位置に他のロボットが存在しているか否か、そして、自身が目標位置上にいるかどうかを知ることができるものとする。例えば、何れのロボットもロボットの周囲のマスに存在する他のロボットと通信できるものとし、少なくとも一台のロボットがGPSを備えるか、または、少なくとも１台のロボットが一回の行動制御において移動しない場合、GPSを備えるロボットや移動しなかったロボットを基準として、隣接するロボットと通信することで、各ロボットは自身の絶対的な位置を容易に知ることができる。もしくは、少なくとも一台のロボットの絶対位置が開始位置にて既知であるならば、それを使用してGPSなしでも、各時刻の各ロボットの絶対位置を知ることが出来る。

本手法では、ロボットの動作開始前に、行動制御システムにおいて、各ロボットの動作の順序をあらかじめ計算し、各ロボットはそれにしたがって動作するものとする。

任務を行うロボットは、p台（pは５以上の整数の何れか）であり、各ロボットは、隣接するロボットと一面以上を共有しつつ、三次元空間におけるX-Y-Z軸方向（この例では、図面の紙面上下左右前後の六方向）に移動可能とする。ただし、p台のロボットで１つの群れを成すものとする。各格子にはロボットは一台しか存在することができない。それぞれのロボットは、移動しようとする方向に他のロボットがある場合には、静止をするものと仮定する。図２において、格子はロボットが存在する位置を示す。また、カブトムシの図形は開始位置の集合Sを示し、チョウの図形は目標位置の集合G（以下、この領域のことを「目標隊列エリアG」ともいう）を示す。

開始位置の集合S、目標位置の集合Gがなす形状には、隣り合う各目標位置が上下左右前後で連なる形でさえあれば特に制限はない。言い換えると、各目標位置は、それぞれ上下左右前後方向の少なくとも何れかの方向において他の目標位置と隣接し、ロボットの目標位置での隊列形状は一塊の任意の形状である。

開始位置の集合S,中間位置の集合M1、M2、M3、目標位置の集合Gにおいて、ロボットは、全体で必ず3次元形状をなすものとする。すなわち、各ロボットの位置のX座標、Y座標、Z座標のうち、いずれの値においても、異なる値をもつロボットが存在しているものとする（たとえば、全ロボットのZ座標が同じ値であるという場合はありえないものとする。つまり、任意のロボットのZ座標の値がaのとき、少なくともZ座標値がa+1またはa-1となるロボットが1台以上存在する。X座標値、Y座標値に関しても同様である）。別の言い方をすると、開始位置の集合S,中間位置の集合M1、M2、M3、目標位置の集合Gにおいて、ロボットは、全体で必ず1次元形状(すなわち、各ロボットの位置のX座標、Y座標、Z座標のうち、いずれか二つの座標値が全てのロボットにおいて同じ)、2次元形状(すなわち、各ロボットの位置のX座標、Y座標、Z座標のうち、いずれか一つの座標値が全てのロボットにおいて同じ)にならないように設定する。

[ロボットの座標設定]
それぞれのロボットi（iはロボット番号を表す、i=0,1,2,3,…,p-1）の位置を（Xr[i],Yr[i],Zr[i]）とし、開始位置の集合Sに含まれる位置を（Xrs[j],Yrs[j],Zrs[j]）とし、目標位置の集合Gに含まれる位置を（Xrg[j],Yrg[j],Zrg[j]）(jは目標位置の番号を表す、j=0,1,2,3,…,p-1)とする。中間位置の集合M1に含まれる位置を（Xm1[j],Ym1[j],Zm1[j]）、中間位置の集合M2に含まれる位置を（Xm2[j],Ym2[j],Zm2[j]）、中間位置の集合M3に含まれる位置を（Xm3[j],Ym3[j],Zm3[j]）とし、開始位置の集合S、目標位置の集合G、中間位置の集合M1,M2,M3に含まれる位置のうち、X、Y座標が最大かつ、Z座標が最小の位置を点(Xthresh,Ythresh,Zthresh)とする。言い換えると、集合S、G、M1,M2,M3に含まれる位置のうち、X座標が最大となる位置のX座標値をXthreshとし、集合S、G、M1,M2,M3に含まれる位置のうち、Y座標が最大となる位置のY座標値をYthreshとし、集合S、G、M1,M2,M3に含まれる位置のうち、Z座標が最小となる位置のZ座標値をZthreshとする。さらに、中間位置の集合M１,M2,M3にあるロボットは、以下の条件［中間位置条件１］を満たす(図４参照)。
(1)全てのロボットのX座標は、ある定数値Xthresh以下の値である。
(2)全てのロボットのY座標は、ある定数値Ythresh以下の値である。
(3)全てのロボットのZ座標は、ある定数値Zthresh以上の値である。
(4)X座標値がXthreshでない全てのロボットは、X軸正方向に接するロボットを持つ。
(5)Y座標値がYthreshでない全てのロボットは、Y軸正方向に接するロボットを持つ。
(6)Z座標値がZthreshでなく、X座標値がXthresh,Y座標値がYthreshの全てのロボットは、Z軸負方向に接するロボットを持つ。

以上の条件に加え、中間位置の集合M2にあるロボットは、以下の条件［中間位置条件２］を満たす（図５参照）。
(1)Z座標値が最大であるロボットは、全体で一辺の長さが偶数のX、Y平面内の長方形をなす。以下、この長方形状のことを天井ともいう。全体で一辺の長さが偶数となるため、天井は２×２のマスの塊で構成されているとも言える。
(2)Z座標が最大値ではないロボットは、必ずZ軸正方向に接するロボットを持つ。
(3)Z座標が最大値ではないロボットが必ず一つ存在する。

[ロボット動作の定義]
行動主体は部屋に配置されている各ロボットとなる。ロボット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倍増加していくことになる。

[問題設定]の項で説明したように、ロボット同士が接しているという拘束条件を取り入れても、根本的な計算量の削減は難しく、複数のロボットを使用する場合の大きな問題となっている。

具体的には、例えば、多数のロボットを複数の目標位置に誘導する問題においては、一台のロボットがある位置へ至るための経路を探索計算により求めることができるとしても、各ロボットにどの目標位置を割り振るかが大きな問題となる。この目標位置の割り振りがうまくいかなければ、早めに目標位置内に到達したロボットが、後から来るロボットの移動を邪魔するような位置に居座ってしまい、全体でのロボットの隊列形成を困難にしてしまう。そのようなことを避けるための各ロボットへの目標位置への割り振り方を探索により計算することを考えた場合、ロボットの台数の階乗分だけの目標位置への割り振り方が存在するため、計算にかかる負荷はロボットの台数とともに劇的に増加する。

また、ロボットが互いに一面を共有しつつ移動を行うための探索計算について考えた場合の探索計算も、各ロボットのお互いの移動を考慮したうえで行うために、必要な計算量はロボットの台数に対して指数関数的に増えてしまう。

[三つの動作過程の導入]
本実施形態では、これらの計算負荷の問題を解決するための方策の一つとして、ロボットの動作の過程を３つの段階にわけて考える。一つ目は、開始位置の集合Sから中間位置の集合M1、中間位置の集合M3から目標位置の集合Gへの圧縮および展開過程、二つ目は、中間位置の集合M1から中間位置の集合M2、中間位置の集合M2から中間位置の集合M3への中間変形過程、三つ目は、中間位置の集合M2における入れ替え過程である。

[圧縮および展開過程]
圧縮および展開過程のうち、まず圧縮過程について述べる。展開過程については、圧縮過程を時間的に逆再生させることで計算する。圧縮過程は２つの過程を含む。一つ目の過程においては、X=Xthresh+1の位置に固定されたX軸に垂直な第一仮想面と、開始位置(Xrs[j],Yrs[j],Zrs[j])にある仮想的なロボット(以下、「仮想ロボット」、または、単に「ロボット」ともいう)をX負方向無限遠からX軸正の方向に移動してくる第二仮想面を想定し、第二仮想面に当たったロボットを押して、かつ、すべてのロボットが第一仮想面を超えないようにし、X座標値がXthresh以下になるように圧縮する(後述するCompression_Process_Xにより実現する)。ここで、第二仮想面が押そうとするロボットがすでに第一仮想面の支えにより動くことが出来ない場合は、そのロボットについては、第一仮想面の支えを優先する。二つ目の過程においては、Y=Ythresh+1の位置に固定されたY軸に垂直な第三仮想面と、上記一つ目の圧縮過程後の各位置（Xr[i],Yr[i],Zr[i]）にある仮想的なロボットをY負方向無限遠からY軸正の方向に移動してくる第四仮想面を想定し、第四仮想面に当たったロボットを押して、かつ、すべてのロボットが第三仮想面を超えないようにし、Y座標値がYthresh以下になるように圧縮する(後述するCompression_Process_Yにより実現する)。ここで、第三仮想面が押そうとするロボットがすでに第四仮想面の支えにより動くことが出来ない場合は、そのロボットについては、第三仮想面の支えを優先する。

[Motion_Data_Update]
(1)ロボットiの現時刻tmでの位置を変数(motion_x[tm][i],motion_y[tm][i],motion_z[tm][i])に格納する(i=0,1,…,p-1)。
(2)tmをインクリメントする。

[Compression_Process]
(1) tm=0とする。ロボットiの位置を開始位置の集合Sに含まれる位置(Xrs[i],Yrs[i],Zrs[i])に設定する。[Motion_Data_Update]を実行する。
(2)後述の[Compression_Process_X_]を実行する。
(3)後述の[Compression_Process_Y]を実行する。
(4)現時刻tmでのロボットiの位置で、Z座標の最大値をZmax[1]に、各Z座標値が_zでの、X座標が_xであるロボットの数(つまり、__x,_zにおいて、Y軸方向に並んだロボットの数)を_y_num[1][_x][_z]に、各Z座標値_zでの、Y座標値が_yであるロボットの数(つまり、__y,_zにおいて、X軸方向に並んだロボットの数)を_x_num[1][_y][_z]に格納する。なお、_x_num[a][y][z]は、中間位置の集合Ma(aは1,2,3の何れか)における、Y座標値がyかつZ座標値がzであるロボットの個数を示す変数であり、_y_num[a][x][z]は、中間位置の集合Maにおける、X座標値がxかつZ座標値がzであるロボットの個数を示す変数である。Zmax[a]は、中間位置の集合MaにおけるZ座標の最大値を示す変数である。
(5)以上のロボット動作における、各時刻tのロボットiの位置(motion_x[t][i],motion_y[t][i],motion_z[t][i])を変数(motion_x_s_m1[t][i],motion_y_s_m1[t][i],motion_z_s_m1[t][i])に格納する。tm-1の値をt_s_m1に格納する。
(motion_x_s_m1[t][i],motion_y_s_m1[t][i],motion_z_s_m1[t][i])←(motion_x[t][i],motion_y[t][i],motion_z[t][i]).
t_s_m1←tm-1.
なお、a←bは変数aに値bを代入することを示す。この処理により、開始位置の集合Sを中間位置の集合M1に変形（圧縮）する。なお、motion_x_a_b[t][i],motion_y_a_b[t][i],motion_z_a_b[t][i]は、それぞれ、位置の集合a(aはS,M1,M2,M3の何れか)から位置の集合b(M1,M2,M3,Gの何れか)にロボットが移動、変形する際の時刻tにおけるロボットiのX座標値,Y座標値,Z座標値を示す変数である。

[Extension_Process]
(1)tm=0とする。ロボットiの位置を目標位置の集合Gに含まれる位置(Xrg[i],Yrg[i],Zrg[i])に設定する。[Motion_Data_Update]を実行する。
(2)後述の[Compression_Process_X_]を実行する。
(3)後述の[Compression_Process_Y]を実行する。
(4)現時刻tmでのロボットiの位置で、Z座標の最大値をZmax[3]に、各Z座標値_zでの、(X座標が_xであるロボットの数)を_y_num[3][_x][_z]に、各Z座標値_zでの、(Y座標値が_yであるロボットの数)を_x_num[3][_y][_z]に格納する。
(5)以上のロボット動作における、各時刻tのロボットiの位置(motion_x[t][i],motion_y[t][i],motion_z[t][i])を変数(motion_x_m3_g[(tm-1)-t][i],motion_y_m3_g[(tm-1)-t][i],motion_z_m3_g[(tm-1)-t][i])に格納する。tm-1の値をt_m3_gに格納する。
(motion_x_m3_g[(tm-1)-t][i],motion_y_m3_g[(tm-1)-t][i],motion_z_m3_g[(tm-1)-t][i])
←(motion_x[t][i],motion_y[t][i],motion_z[t][i]).
t_m3_g←tm-1.
この処理により、中間位置の集合M3を目標位置の集合Gに変形（展開）する。以下、Compression_Process_X、Compression_Process_Yの例を説明する。

[Compression_Process_X]
(1)現時刻ｔmでの仮想ロボット位置のX座標の最小値をXminとし、_xw=Xminとして、_xw< Xthreshの間、(2)〜(4)を繰り返す。
(2)全てのロボットiについて変数destination_to_be_compressed[i]=0とする。_flg_c=0とする。destination_to_be_compressed[i]はロボットiが移動(圧縮)するか否かを示す変数(1のとき移動し、0のとき移動しない)であり、_flg_cは移動(圧縮)するロボットが存在するか否かを示す変数(1のとき存在し、0のとき存在しない)を示す。
(3)X座標値が_xwである仮想ロボットiについて、以下を実行する。

(3-1）ロボットiからX軸正方向に途切れず隣接している全てのロボットinの中にX座標がXthreshのものがない場合、それら隣接のロボットi〜inのdestination_to_be_compressedの値を1とする。

(3-2)_flg_c=1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全てのロボットのX座標値をインクリメントし、[Motion_Data_Update]を実行する。_xwをインクリメントする。

この処理により、上述のように、X座標値がXthresh以下になるように圧縮する。（図６Ａ〜図６Ｃ参照）

[Compression_Process_Y]
(1)現時刻ｔmでの仮想ロボット位置のY座標の最小値をYminとし、_yw=Yminとして、_yw<Ythreshの間、(2)〜(5)を繰り返す。
(2)全てのロボットiについて変数destination_to_be_compressed[i]=0とする。_flg_c=0とする。
(4)Y座標値が_ywである仮想ロボットiについて、以下を実行する。

(4-1）ロボットiからY軸正方向に途切れず隣接している全てのロボットinの中にY座標がYthreshのものがない場合、それら隣接のロボットi〜inのdestination_to_be_compressedの値を1とする。

(4-2)_flg_c=1とする。
(5)_flg_c=1のとき、destination_to_be_compressed[i]=1である全てのロボットのY座標値をインクリメントし、[Motion_Data_Update]を実行する。_ywをインクリメントする。

この処理により、上述のように、Y座標値がYmin以下になるように圧縮する（図６Ｄ〜図６Ｅ参照）。

次に、[中間変形過程]以下、中間位置の集合M１にあるロボットが、隊列制御によって、中間位置の集合M2に移動するまでの動作を計算する中間変形過程の処理について述べる。

まず、中間位置の集合M2の成す形状を決める。前述の通り、中間位置の集合M2は以下の［中間位置条件１］
(1)全てのロボットのX座標は、ある定数値Xthresh以下の値である。
(2)全てのロボットのY座標は、ある定数値Xthresh以下の値である。
(3)全てのロボットのZ座標は、ある定数値Zthresh以上の値である。
(4)X座標値がXthreshでない全てのロボットは、X軸正方向に接するロボットを持つ。
(5)Y座標値がYthreshでない全てのロボットは、Y軸正方向に接するロボットを持つ。
(6)Z座標値がZthreshでなく、X座標値がXthresh,Y座標値がYthreshの全てのロボットは、Z軸負方向に接するロボットを持つ。
及び、［中間位置条件２］
(1)Z座標値が最大であるロボットは、全体で一辺の長さが偶数のX、Y平面内の長方形をなす。
(2)Z座標が最大値ではないロボットは、必ずZ軸正方向に接するロボットを持つ。
(3)Z座標が最大値ではないロボットが必ず一つ存在する。
を満たす。上記条件を満たすのであれば、中間位置の集合M2の成す形状はどのような形状であってもよい。例えば、天井を一辺が2×a個のロボットの位置からなる正方形状とする（天井に含まれるロボットの数は(2×a)²であり、aは1以上の偶数である）。中間位置の個数pを(2×a)²で割ったときの商bとし、剰余をcとしたとき、(2×a)²×b個のロボットの位置からなる直方体(幅:2×a,奥行き2×a,高さb)と、その直方体の底面に接するようにc個のロボットの位置を配置した形状を中間位置の集合M2の成す形状とする。このような天井により［中間位置条件２］の(1)を満たす。なお、［中間位置条件２］の(2),(3)を満たすために、b≧2とc≧1との少なくとも一方を満たす。c≧1のとき、底面に接するように配置されたc個のロボットの位置の内の1つは、中間位置条件１の(4),(5),(6)を満たすために、X座標値をXthreshとし、Y座標値をYthreshとする位置に配置される。c個のロボットの位置の内の他の位置は、中間位置条件１の(4),(5)を満たすために、X軸正方向及びY軸正方向において、他のロボットの位置と接する［中間位置条件１］の(1)〜(3)を満たす位置にその直方体の底面に接するように配置すればよい。例えば、底面に接するように配置されたc個のロボットの位置の内の1つが原点(Xthresh,Ythresh,Zthresh)に位置するように配置する。

上述のように、中間位置条件１］及び［中間位置条件２］を満たすように、行動制御システムまたはその利用者により、中間位置の集合M2を特定する。なお、ここで計算する中間位置の集合M2を記述する変数は、各ロボットiの位置ではなく、Zmax[2], _y_num[2][_x][_z],_x_num[2][_y][_z]の形で格納される。各ロボットの位置は、あくまで中間位置M1からの変形の結果として得られるものであり、変形前にあらかじめ決められるものではない。

[Transform_from_M1_to_M2]
(1)tm=0とする。[Motion_Data_Update]を実行する。
(2)Seed_Genedationを実行する。
(3)Middle_Transformを実行する。
(4)Seed_UnGenerationを実行する。
(5)各時刻tのロボットiの位置の値(motion_x[t][i], motion_y[t][i], motion_z[t][i])を変数(motion_x_m1_m2[t][i], motion_y_m1_m2[t][i], motion_z_m1_m2[t][i])に格納する。tm-1の値を、t_m1_m2に格納する。
(motion_x_m1_m2[t][i],motion_y_m1_m2[t][i],motion_z_m1_m2[t][i])
← (motion_x[t][i],motion_y[t][i],motion_z[t][i]).
t_m1_m2 ← tm-1.

Seed_Generationでは、中間位置の集合M1にあるロボットの中から、シードロボットとなる２つのロボットを選択し、シードロボットの待機位置に移動させる(図７参照)。シードロボットは、後述するMiddle_Transformにおける柱部、旗部、床部の分解、建造に利するものであり、他のロボットの移動時に隣接状態を維持するために利用する。また、中間位置の集合M1から、シードロボットの位置を除外したあとのZmax[1],_y_num[1][_x][_z],_x_num[1][_y][_z]を計算する。また、変形処理[Middle_Transform]の終了後に、中間位置の集合M2中に、シードロボットが帰るべき位置を計算し、中間位置の集合M2から、それらの位置を除外したあとのZmax[2],_y_num[2][_x][_z],_x_num[2][_y][_z]を計算する。

[Seed_Generation]
（1）Xr[i]=Xthresh-(_x_num[1][Yr[i]][Zr[i]]-1)かつ、Xr[i]!=Xtrheshであり、Yr[i]が_x_num[1][Yr[i]][Zr[i]]>1を満たす最小値となるロボットiを選択し、_x_num[1][Yr[i]][Zr[i]],_y_num[1][Xr[i]][Zr[i]]をデクリメントする。
（2）ロボットiと同じY、Z座標値のロボットを全て、X軸正方向に1ステップ移動させ、X座標がXthresh+1のロボットをロボットi_seed1とする(図７Ａ、図７Ｂ参照)。[Motion_Data_Update]を実行する。
(3)ロボットi_seed1をY座標がYthreshになるまで、Y軸正方向に1ステップ移動させて、[Motion_Data_Update]を実行することを繰り返す。
(4)ロボットi_seed1をZ座標がZthreshになるまで、Z軸負方向に1ステップ移動させて、[Motion_Data_Update]を実行することを繰り返す(図７Ｂ、図７Ｃ参照)。
(5)Yr[i]=Ythresh-(_y_num[1][Xr[i]][Zr[i]]-1)かつ、Yr[i]!=Ytrheshであり、Xr[i]が_y_num[1][Xr[i]][Zr[i]]＞１を満たす最小値となるロボットiを選択し、_y_num[1][Xr[i]][Zr[i]],_x_num[1][Yr[i]][Zr[i]]をデクリメントする。
(6)ロボットiと同じX、Z座標値のロボットを全て、Y軸正方向に1ステップ移動させ、Y座標がYthresh+1のロボットをロボットi_seed2とする。[Motion_Data_Update]を実行する。(図７Ｄ参照)
（7）ロボットi_seed2をX座標がXthreshになるまで、X軸正方向に1ステップ移動させて、[Motion_Data_Update]を実行することを繰り返す。
（8）ロボットi_seed2をZ座標がZthreshになるまで、Z軸負方向に1ステップ移動させて、[Motion_Data_Update]を実行することを繰り返す(図７Ｅ参照)。
(9)中間位置の集合M2内でX座標値Xm2[i]がXthresh-(_x_num[2][Ym2[i]][Zm2[i]]-1)であり、かつ、Xm2[i]!=Xtrheshであり、Ym2[i]が_x_num[2][Ym2[i]][Zm2[i]]＞１を満たす最小値となる位置iを選択し、_x_num[2][Ym2[i]][Zm2[i]],_y_num[2][Xm2[i]][Zm2[i]]をデクリメントする。Xseed[1] ← Xm2[i],Yseed[1] ← Ym2[i],Zseed[1] ← Zm2[i]。なお、Xma[i],Yma[i],Zma[i]はそれぞれ中間位置の集合Ma(aは1,2,3の何れか)におけるロボットiのX座標値、Y座標値、Z座標値を示す変数である。Xseed[a]、Yseed[a]、Zseed[a](aは1,または2)はそれぞれ、2つのシードロボットi_seedaのX座標値、Y座標値、Z座標値を示す変数である。
(10)中間位置の集合M2内でX座標値Xm2[i]がYthresh-( _y_num[2][Xm2[i]][Zm2[i]] - 1 )かつ、Ym2[i] != Ytrheshであり、Xm2[i]が_y_num[2][Xm2[i]][Zm2[i]]>1を満たす最小値となる位置iを選択し、_x_num[2][Ym2[i]][Zm2[i]],_y_num[2][Xm2[i]][Zm2[i]]をデクリメントする。Xseed[2] ← Xm2[i],Yseed[2] ← Ym2[i],Zseed[2] ←Z m2[i]。

Middle_Transformでは、中間位置の集合M1、M2をそれぞれ以下の３つの部分に分ける（図８参照）。
柱部：Xr[i]=XthreshかつYr[i]=Ythreshの位置にあるロボットで構成される部分。
旗部：Xr[i]=XthreshかつYr[i]！＝Ythreshの位置にあるロボットで構成される部分。
床部：柱部、旗部に属さないその他の部分全て。

Middle_Transformでは、中間位置の集合M1を床部、旗部、柱部の順に分解する。床部の分解は、同じY座標、Z座標値を持つロボットをX軸正方向に１ステップ移動させ、旗部もしくは柱部に属するロボットをX座標がXthresh+1の位置に押し出すことで行われる(後述のRemove_from_Floor参照)。旗部の分解は、同じX座標、Z座標値を持つロボットをY軸正方向に１ステップ移動させ、柱部に属するロボットをY座標がYthresh+1の位置に押し出すことで行われる(後述のRemove_from_Flag参照)。柱部の分解にはシードロボットを用いる(後述のRemove_from_Pole参照)。中間位置の集合M1から分解されたロボットは、中間位置の集合M2の建造に用いられるが、中間位置の集合M2の建造は、柱部、旗部、床部の順に行われる。柱部建造にはシードロボットを用いる。旗部の建造は、追加するロボットをY座標がYthresh+1で、追加される位置と同じX座標、Z座標値の位置に移動させ、それと同じX座標、Z座標を持つロボットをY軸負方向に１ステップ移動させることで行われる。床部の建造は、追加するロボットをX座標がXthresh+1で、追加される位置と同じY座標、Z座標値の位置に移動させ、それと同じY座標、Z座標を持つロボットをX軸負方向に１ステップ移動させることで行われる。

[Middle_Transgform]
(1)Remove_from_Floorを実行する。
(2)Remove_from_Flagを実行する。
(3)Remove_from_Poleを実行する。

[Remove_From_Floor]
(1)z = Zmax[1]からZthreshの全てのｚについて、(2)〜(8)を繰り返す。
(2)y=Ythresh-(_y_num[1][Xthresh][z]-1)からYthreshの全てのyについて(3)〜(8)を繰り返す。
(3)_x_num[1][y][z]>_x_num[2][y][z]の間、(4)〜(8)を繰り返す。
(4)後述のAdjust_Seed_height(z)を実行する。
(5)Yr[i]=yかつZr[i]=zのロボットiを全て、X軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。Xr[i]=Xthresh+1のロボットiをロボットi_workとする。
(6)Yr[i_work] < Ythreshの間、ロボットi_workをY軸正方向に１ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(7)後述のAdd_to_Destinationを実行する。
(8)_x_num[1][y][z]をデクリメントする。

[Remove_From_Flag]
(1)z = Zmax[1]からZthreshの全てのｚについて、(2)〜(9)を繰り返す。
(2)_y_num[1][Xthresh][z] > _y_num[2][Xthresh][z]の間、(3)〜(9)を繰り返す。
(3)後述のAdjust_Seed_Height(z)を実行する。
(4)Xr[i]=XthreshかつZr[i]=zのロボットiを全て、Y軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。Yr[i]=Ythresh+1のロボットiをロボットi_workとする。
(5)ロボットi_workとロボットi_seed2をX軸正方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(6)ロボットi_workをY軸負方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(7)ロボットi_seed2をX軸負方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(8)後述のAdd_to_Destinationを実行する。
(9)_y_num[1][Xthresh][z]をデクリメントする。

[Remove_From_Pole]
(1)Zmax[1]がZthreshより大きい間、(2)〜(4)を繰り返す。
(2)ｚ＝Zmax[1]とし、Adjust_Seed_Height(z)を実行する。
(3)Xr[i]=XthreshかつYr[i]=YthreshかつZr[i]=Zmax[1]のロボットiをX軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(4)後述のAdd_to_Destinationを実行する。
(5)Zmax[1]をデクリメントする。

[Adjust_Seed_Height(z)]
(1) z != Zthreshの場合、z_target ← z-1とし、それ以外の場合、z_traget ← z+1とする。
(2) Zr[i_seed1] > z_targetの間、ロボットi_seed1とi_seed2をZ軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(3) Zr[i_seed1] ＜ z_targetの間、ロボットi_seed1とi_seed2をZ軸正方向に１ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。

この処理により、ロボットi_seed1とi_seed2のZ座標値を対象のロボットのZ座標値zの±1とする。

[Add_to_Destination]
(1)中間位置の集合M2の柱部が完成していなければAdd_to_Poleを実行して終了する。
(2)中間位置の集合M2の旗部が完成していなければAdd_to_Flagを実行して終了する。
(3)中間位置の集合M2の床部が完成していなければAdd_to_Floorを実行して終了する。

[Add_to_Pole]
(1)後述のAdjust_Work_height(Zmax[1]+1)を実行する。
(2)Zr[i_work] < Zmax[1]+1の間、ロボットi_work,i_seed1,i_seed2をZ軸正方向に１ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(3)ロボットi_workをX軸負方向に１ステップ移動させる。Motion_Data_Updateを実行する。
(4)Zmax[1]をインクリメントする。

この処理により、中間位置の集合M2の柱部を建造する。

[Add_to_Flag]
(1)z_targetの値を、_y_num[2][Xthresh][z] > _y_num[1][Xthresh][z]を満たす最大のｚの値に設定する。
(2)後述のAdjust_Work_Height(z_target)を実行する。
(3)ロボットi_workとi_seed1をY軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(4)ロボットi_workをX軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(5)ロボットi_seed1をY軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(6)Zr[i_work] > z_targetの間、ロボットi_work、i_seed1,i_seed2をZ軸負方向に1ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(7)Zr[i_work] < z_targetの間、ロボットi_work、i_seed1,i_seed2をZ軸正方向に1ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(8)Xr[i]=XthreshかつZr[i]=z_targetを満たす全てのロボットをY軸負方向に1ステップ移動させ、Motion_Data_Updateを実行する
(9)_y_num[1][Xthresh][z_target]をインクリメントする。

この処理により、中間位置の集合M2の旗部を建造する。

[Add_to_Floor]
(1)y_target, z_targetの値を、_x_num[2][y][z] > _x_num[1][y][z]を満たす最大のy,ｚの値に設定する。
(2)後述のAdjust_Work_Height（z_target）を実行する。
(3)Zr[i_work]>z_targetの間、ロボットi_work、i_seed1,i_seed2をZ軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(4)Zr[i_work]<z_targetの間、ロボットi_work、i_seed1,i_seed2をZ軸正方向に1ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(5)Yr[i_work]>y_targetの間、ロボットi_workをY軸負方向に1ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(6)Yr[i]=y_targetかつZr[i]=z_targetを満たす全てのロボットをX軸負方向に1ステップ移動させ、Motion_Data_Updateを実行する
(7)_x_num[1][y_target][z_target]をインクリメントする。

この処理により、中間位置の集合M2の床部を建造する。

[Adjust_Work_Height(z)]
(1)z != ZthreshかつZr[i_work] < Zr[i_seed1]の場合、ロボットi_workとi_seed1をZ軸正方向に1ステップ移動し、ロボットi_workをシードモジュール1とし、ロボットi_seed1をロボットi_workとして番号を入れ替える。Motion_Data_Updateを実行する。
(2)z = Zthresh かつ Zr[i_work] > Zr[i_seed1]の場合、以下の処理を行う。

(2-1)Zr[i_seed1]=Zthreshならば、ロボットi_seed2をZ軸正方向に１ステップ移動し、ロボットi_workをシードモジュール１とし、ロボットi_seed1をロボットi_workとして番号を入れ替える。Motion_Data_Updateを実行する。

(2-2)Zr[i_seed1]=Zthreshでないならば、ロボットi_workとi_seed1をZ軸負方向に１ステップ移動し、ロボットi_workをシードモジュール１とし、ロボットi_seed1をロボットi_workとして番号を入れ替える。Motion_Data_Updateを実行する。

[Seed_UnGeneration]
(1) Zr[i_seed2] > Zseed[2] の間、ロボットi_seed2をZ軸負方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(2) Zr[i_seed2] < Zseed[2] の間、ロボットi_seed2をZ軸正方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(3) Xr[i_seed2] > Xseed[2] の間、ロボットi_seed2をX軸負方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(4)Xr[i]=Xseed[2], かつZr[i]=Zseed[2]を満たす全てのロボットをY軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(5) Zr[i_seed1] > Zseed[1] の間、ロボットi_seed1をZ軸負方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(6) Zr[i_seed1] < Zseed[1] の間、ロボットi_seed1をZ軸正方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(7) Yr[i_seed1] > Yseed[1] の間、ロボットi_seed1をY軸負方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(8)Yr[i]=Yseed[1], かつZr[i]=Zseed[1]を満たす全てのロボットをX軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。

この処理により、シードロボットを中間位置の集合M2の中に組み込む。

中間位置の集合M2にあるロボットが、隊列制御によって、中間位置の集合M3に移動するまでの動作については、[Transform_from_M1_to_M2]の記述のM1をM2に、M2をM3に置き換えることによって得ることが出来る。

[Transform_from_M2_to_M3]
上述の[Transform_from_M1_to_M2]における変数を以下のように入れ替える。
Zmax[1] → Zmax[2],Zmax[2] → Zmax[3].
_x_num[1][_y][_z] → _x_num[2][_y][_z],_x_num[2][_y][_z] → _x_num[3][_y][_z].
_y_num[1][_x][_z] → _y_num[2][_x][_z],_y_num[2][_x][_z] → _y_num[3][_x][_z].
Xm1→Xm2, Ym1→Ym2, Zm1→Zm2.
Xm2→Xm3, Ym2→Ym3, Zm2→Zm3.
(motion_x_m1_m2[t][i], motion_y_m1_m2[t][i], motion_z_m1_m2[t][i])→
(motion_x_m2_m3[t][i], motion_y_m2_m3[t][i], motion_z_m2_m3[t][i]).
t_m1_m2→t_m2_m3.

[Transform_from_M1_to_M3]
同様に[Transform_from_M1_to_M2]における変数を以下のように入れ替える。
Zmax[2] → Zmax[3].
_x_num[2][_y][_z] → _x_num[3][_y][_z].
_y_num[2][_x][_z] → _y_num[3][_x][_z].
Xm2 → Xm3, Ym2 → Ym3, Zm2 → Zm3
(motion_x_m1_m2[t][i], motion_y_m1_m2[t][i], motion_z_m1_m2[t][i]) →
(motion_x_m1_m3[t][i], motion_y_m1_m3[t][i], motion_z_m1_m3[t][i]).
t_m1_m2→t_m1_m3.
なお、この処理[Transform_from_M1_to_M3]は、中間位置の集合M2を経ずに、中間位置の集合M1から中間位置の集合M3に変形するときに用いる。

[開始位置の集合Sから目標位置の集合Gまでのホモ隊列変形]
以上の処理を結合して、開始位置の集合Sから目標位置の集合Gまでの変形過程が求められる。

[Transformation_from_S_to_G_Homo]
(1)[Compression_Process]を実行する。
(2)[Extension_Process]を実行する。
(3)[Transform_from_M1_to_M3]を実行する。
(4)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i] transform_S_to_G_z[t][i])のt=0〜t_s_m1に、変数 (motion_x_s_m1[t][i], motion_y_s_m1[t][i], motion_z_s_m1[t][i])の値を格納する。
(5)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i] transform_S_to_G_z[t][i])のt=t_s_m1+1〜t_s_m1+1 + t_m1_m3に、変数(motion_x_m1_m3[t][i], motion_y_m1_m3[t][i], motion_z_m1_m3[t][i])の値を格納する。
(6)全てのロボットiに対し(motion_x_m1_m3[t_m1_m3][i], motion_y_m1_m3[t_m1_m3][i], motion_z_m1_m3[t_m1_m3][i])= (motion_x_m3_g[0][j], motion_y_m3_g[0][j],motion_z_m3_g[0][j])となるｊの値を探し、match[i]に格納する。match[i]には、中間位置の集合M1のロボットiに対応する中間位置の集合M3のロボットのインデックスjが格納される。
(7)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i] transform_S_to_G_z[t][i])のt=t_s_m1 + 1 + t_m1_m3 + 1〜t_s_m1 + 1 + t_m1_m3 + 1 + t_m3_gに、変数 (motion_x_m1_m3[t][match[i]], motion_y_m1_m3[t][match[i]], motion_z_m1_m3[t][ match[i]])の値を格納する。

この処理により、(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i]transform_S_to_G_z[t][i])のt=t_s_m1+1+t_m1_m3+1〜t_s_m1+1+t_m1_m3+1+t_m3_gには、開始位置の集合Sから目標位置の集合Gまでの各ロボットiの位置が格納されるため、この位置に合うように各ロボットの行動を制御すればよい。

[開始位置の集合Sから目標位置の集合Gまでのヘテロ隊列変形]
開始位置の集合S内の位置jと目標位置の集合G内の位置jは、同じロボットによって占有されるものとする。そのような場合、開始位置の集合S内の特定の位置jにあったロボットiが、目標位置の集合G内の決まった特定位置jに移動するためには、先述のTrandformation_from_S_to_G_Homoの処理に加えて、中間位置の集合M2にあるロボットの位置の入れ替えを行う必要がある。そのための入れ替え処理を[Transformation_M2_to_M2]として、開始位置の集合Sから目標位置の集合Gまでのヘテロ隊列変形処理は以下のとおりである。

[Transformation_from_S_to_G_Hetero]
(1)[Compression_Process]を実行する。
(2)[Extension_Process]を実行する。
(3)[Transform_from_M1_to_M2]を実行する。
(4)[Transform_from_M2_to_M3]を実行する。
(5)全てのロボットiに対し(motion_x_m2_m3[t_m2_m3][i],motion_y_m2_m3[t_m2_m3][i],motion_z_m2_m3[t_m2_m3][i])=(motion_x_m3_g[0][j],motion_y_m3_g[0][j],motion_z_m3_g[0][j])となるjの値を探し、match[j]=iとする。
(6)[Transformation_from_M2_to_M2]を実行する。
(7)変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i]transform_S_to_G_z[t][i])のt=0〜t_s_m1に、変数(motion_x_s_m1[t][i],motion_y_s_m1[t][i],motion_z_s_m1[t][i])の値を格納する。
(8)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i]transform_S_to_G_z[t][i])のt=t_s_m1+1〜t_s_m1+1 + t_m1_m2に、変数(motion_x_m1_m2[t][i],motion_y_m1_m2[t][i],motion_z_m1_m2[t][i])の値を格納する。
(9)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i]transform_S_to_G_z[t][i])のt=t_s_m1+1+t_m1_m2+1〜t_s_m1+1+t_m1_m2+1+t_m2_m2に、変数(motion_x_m2_m2[t][i],motion_y_m2_m2[t][i],motion_z_m2_m2[t][i])の値を格納する。
(10)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i]transform_S_to_G_z[t][i])のt= t_s_m1+1 + t_m1_m2+1 + t_m2_m2+1〜t_s_m1+1 + t_m1_m2+1 + t_m2_m2+1 + t_m2_m3に、変数 (motion_x_m2_m3[t][match[i]], motion_y_m2_m3[t][match[i]], motion_z_m2_m3[t][ match[i]])の値を格納する。
(11)変数(transform_S_to_G_x[t][i], transform_S_to_G_y[t][i] transform_S_to_G_z[t][i])のt= t_s_m1+1 + t_m1_m2+1 + t_m2_m2+1 + t_m2_m3+1〜t_s_m1+1 + t_m1_m2+1 + t_m2_m2+1 + t_m2_m3+1 + t_m3_gに、変数(motion_x_m3_g[t][i], motion_y_m3_g[t][i], motion_z_m3_g[t][i])の値を格納する。

[Transformation_M2_to_M2]
中間位置の集合M2は、天井がXY平面内の辺の長さが偶数の長方形をなしている。したがって中間位置の集合M2の天井は２×２の正方形の集合である。もし、一つのロボットが天井の上に乗っていると仮定したとき、その天井上のロボットと、中間位置の集合M2内にある任意のロボットは、図９に示すような動作で入れ替えが可能である。しかし、中間位置の集合M2におけるロボットの入れ替え過程の任意の時点で、この天井の上のロボットの存在を確保することが出来なければ、中間位置の集合M2における入れ替え動作は完了できない。通常、天井の上に移動することが可能なロボットは、天井内にあって、Z軸負方向にロボットが接しているロボットである。しかし、中間位置の集合M2は、天井のZ軸方向の厚みには特に制限はないので、天井の厚み１でかつ、天井の下に一つのロボットが接しているのみという状況がありうる。この場合、中間位置の集合M2での入れ替え動作を開始した時点で、天井の上に出ることが可能なロボットは一つのみである。このロボットをi1(図９のi_originに対応)として入れ替えを開始したとする。ロボットi1がまず、Z軸負方向に接するロボットとともにZ軸正方向に１ステップ移動して天井上に押し出される。その後、ロボットi1が、入れ替え先のロボットi2(図９のi_targetに対応)と位置を入れ替える。ロボットi1に追い出されて天井上に出たロボットi2が、入れ替え先のロボットi3と位置を入れ替える。ロボットi2に追い出されて天井上に出たロボットi3が、入れ替え先のロボットi4と位置を入れ替える…の一連の入れ替え動作を繰り返していく。この一連の入れ替え動作は、天井上に追い出されたロボットinの行先がロボットi1が元あった位置になるまでは繰り返すことが可能であるが、それ以降は不可能である。それ以降は、天井の上に出ることが可能となるロボットは、ロボットi1が元あった位置にありすでに入れ替えの済んだロボットinのみだからである。このロボットi1からinまでの入れ替え動作を処理Permutation_1で行う。当然であるが、この時点で、まだ入れ替えが完了していないロボットが存在しない保証はない。よって、これ以降は、このロボットinを利用して、入れ替え動作を継続する必要がある。このロボットinを利用した入れ替え動作を処理Permutation_2で行う。処理Permutation_2では、まず、ロボットinをロボットi1のときと同様に、天井の上に押し出す。続いて、まだ入れ替えの済んでいないロボットのうち一台を選択してロボットi_n+1とする。ここで、ロボットinは、ロボットi_n+1と位置を入れ替えて、ロボットi_n+1を天井上に追い出す。この後、ロボットinに追い出されて天井上に出たロボットi_n+1が、入れ替え先のロボットi_n+2と位置を入れ替える。ロボットi_n+1に追い出されて天井上に出たロボットi_n+2が、入れ替え先のロボットi_n+3と位置を入れ替える…の一連の入れ替え動作を繰り返していく。そして、この一連の入れ替え動作は、天井上に追い出されたロボットi_n+mの行先がロボットi_n+1が元あった位置になるまでは繰り返すことが出来る。ロボットi_n+mの行先がロボットi_n+1が元あった位置に収まったとき、天井に追い出されるのはロボットinである。以後、同様に、ロボットinを使用した入れ替えを開始していけばよい。

[Transformation_M2_to_M2]
(1)全てのロボットiについて、変数perm[i]=0とする。
(2)全てのロボットiについて、もし、match[i]=iならば、perm[i]=1とする。変数perm[i]は、中間位置の集合M2においてロボットiが本来あるべき位置にあるか否かを示す変数である（この例ではperm[i]=1はあるべき位置にあることを示し、perm[i]=0はあるべき位置にないことを示す）。
(3)[Permutation_1]を実行する。
(4)perm[i]=0なるiが存在する間、繰り返し[Permutation_2]を実行する。

[Permutation_1]
(1)天井内に、perm[i]=0かつ、Z軸負方向に接するロボットを持つロボットiを選択し、ロボットi_originとする。もしそのようなロボットが見つからないなら、Z軸負方向に接するロボットを持つロボットiを選択し、ロボットi_origin2とし、Xhome ← Xr[i_origin2],Yhome ← Yr[i_origin2]として、終了する。なお、Xhome,Yhomeはロボットi_origin2の元の位置(X座標値、Y座標値)を示す変数である。
(2)変数i_destの値をmatch[i_origin]とする(i_dest ← match[i_origin])。
(3)i_origin1 ← i_origin, Xhome ← Xr[i_origin], Yhome ← Yr[i_origin].
(4)Xr[i]=Xr[i_origin]かつYr[i]=Yr[i_origin]を満たす全てのロボットをZ軸正方向に1ステップ移動させる。
(5)i_dest!=i_origin1の間、(6)〜(8)を繰り返す。
(6)Cube_Exchange(i_dest,i_origin)を実行する。
(7)perm[i_origin]←1とする。
(8)i_origin ← i_dest,とした後i_dest ← perm[i_origin]。
(9)Go_Home(Xhome,Yhome,i_origin)を実行する。perm[i_origin]←1とする。
(10)位置（Xhome, Yhome, Zmax[2]）にあるロボットをロボットi_origin2とする。

[Permutation_2]
(1)perm[i]=0であるロボットiを選択し、ロボットi_originとする。
(2)i_dest ← perm[i_origin], i_origin3 ← i_originとする。
(3)Xhome2 ← Xr[i_origin3], Yhome2 ← Yr[i_origin3], Zhome2[i_origin3]とする。
(4) Xr[i]=Xr[i_origin2]かつ Yr[i]=Yr[i_origin2]を満たす全てのロボットをZ軸正方向に1ステップ移動させる。
(5)Cube_Exchange(i_origin3, i_origin2)を実行する。
(6)i_dest != i_origin3の間、(7)〜(9)を繰り返す。
(7)Cube_Exchange(i_dest, i_origin)を実行する。
(8)perm[i_origin]←1とする。
(9)i_origin ← i_dest, とした後、i_dest ← perm[i_origin]。
(10)Cube_Exchange(i_origin2, i_origin)を実行する。
(11)perm[i_origin]←1とする。
(12)Go_Home(Xhome,Yhome, i_origin2)を実行する。

[Cube_Exchange(i_dest,i_origin)]
(1)Horizontal_Exchange(i_dest, i_origin)を実行する。
(2)Vertical_Exchange(i_dest, i_origin)を実行する。

[Horizontal_Exchange(i_dest, i_origin)]
天井上のロボットi_originを、位置(Xr[i_dest],Yr[i_dest],Zmax[2]+1)に、XかY方向に接する位置に移動させる。

Horizontal_Exchangeでは、ロボットi_originを移動先のロボットi_destの位置のX座標、Y座標の何れかにおいて隣接する位置に移動させる。ここでは、移動先の位置のX座標値がXr[i_dest]であり、Y座標値がYr[i_dest]であり、ロボットi_originと移動先の位置との位置関係に応じて、ロボットi_originのX座標値がXr[i_dest]±1かつY座標値がYr[i_dest]となるように、または、Y座標値がYr[i_dest]±1かつX座標値がXr[i_dest]となるように、ロボットi_originを移動させる。例えば以下のように制御する。

(1)ロボットi_originのX座標値がXr[i_dest]に等しくかつロボットi_originのY座標値がYr[i_dest]に等しいとき、以下を実行して終了する。そうでないなら(2)へ移行する。

ロボットi_originのX座標値-Xthreshが奇数ならば、ロボットi_originをX軸正方向に１ステップ移動させ、偶数ならばX軸負方向に１移動させる。Motion_Data_Updateを実行する。

(2)ロボットi_originのX座標値がXr[i_dest]より小さく、かつロボットi_originのY座標値がYr[i_dest]に等しくないとき、以下を実行して終了する。そうでないなら(3)へ移行する。

ロボットi_originのX座標値がXr[i_dest]に等しくなるまで、ロボットi_originをX軸正方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。続いて、ロボットi_originのY座標値とYr[i_dest]との差の絶対値が１に等しくなるまで、ロボットi_originをロボットi_originのY座標値がYr[i_dest]に近づく方向に、Y軸方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。

(3)ロボットi_originのX座標値がXr[i_dest]より小さく、かつロボットi_originのY座標値がYr[i_dest]に等しいとき、以下を実行して終了する。そうでないなら（4）へ移行する。

ロボットi_originのX座標値とXr[i_dest]との差の絶対値が１に等しくなるまで、ロボットi_originをX軸正方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。

(4)ロボットi_originのX座標値がXr[i_dest]より大きく、かつロボットi_originのY座標値がYr[i_dest]に等しくないとき、以下を実行して終了する。そうでないなら(5)へ移行する。

ロボットi_originのY座標値とYr[i_dest]が等しくなるまで、ロボットi_originをロボットi_originのY座標値がYr[i_dest]に近づく方向に、Y軸方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。続いて、ロボットi_originのX座標値とXr[i_dest]との差の絶対値が１に等しくなるまで、ロボットi_originをX軸負方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。

(5)ロボットi_originのX座標値がXr[i_dest]より大きく、かつロボットi_originのY座標値がYr[i_dest]に等しいとき、以下を実行して終了する。そうでないなら（6）へ移行する。

ロボットi_originのX座標値とXr[i_dest]との差の絶対値が１に等しくなるまで、ロボットi_originをX軸負方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。

(6)ロボットi_originのX座標値がXr[i_dest]に等しく、かつロボットi_originのY座標値がYr[i_dest]よりも大きいとき、以下を実行して終了する。そうでないなら（7）へ移行する。

ロボットi_originのY座標値とYr[i_dest]との差の絶対値が１に等しくなるまで、ロボットi_originをY軸負方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。

(7)ロボットi_originのX座標値がXr[i_dest]に等しく、かつロボットi_originのY座標値がYr[i_dest]よりも小さいとき、以下を実行して終了する。

ロボットi_originのY座標値とYr[i_dest]との差の絶対値が１に等しくなるまで、ロボットi_originをY軸正方向に１ステップ移動させMotion_Data_Updateを実行することを繰り返す。

[Vertical_Exchange(i_dest,i_origin)]
(1)ロボットi_originとロボットi_destのX座標が等しい時、(1-1)に移行し、それ以外の場合(2)に移行する。

(1-1)もしロボットi_originのX座標-Xthreshが奇数ならば(1-1-1)に移行し、それ以外の場合(1-1-2)に移行する。

(1-1-1)ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボットi_originのY座標がロボットi_destのY座標より大きいならば(1-1-1-1)に移行し、それ以外の場合は(1-1-1-2)に移行する。

(1-1-1-1)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

(1-1-1-2)
ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

(1-1-2)(ロボットi_originのX座標-Xthreshが奇数ではない場合)ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボットi_originのY座標がロボットi_destのY座標より大きいならば(1-1-2-1)に移行し、それ以外の場合、(1-1-2-2)に移行する。

(1-1-2-1)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

(1-1-2-2)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
(2)もしロボットi_originのY座標-Ythreshが奇数ならば(2-1)に移行し、それ以外の場合(2-2)に移行する。

(2-1)ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボットi_originのX座標がロボットi_destのX座標より大きいならば(2-1-1)に移行し、それ以外の場合(2-1-2)に移行する。

(2-1-1)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

(2-1-2)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

(2-2)ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをY軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。もしロボットi_originのX座標がロボットi_destのX座標より大きいならば(2-2-1)に移行し、それ以外の場合(2-2-2)に移行する。

(2-2-1)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

(2-2-2)ロボットi_destとX、Y座標が等しく、ロボットi_destよりZ座標が大きい全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをY軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸負方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originおよび、ロボットi_originとX、Y座標が等しくロボットi_origin から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動し、ロボットi_destおよび、ロボットi_destとX、Y座標が等しくロボットi_dest から見てZ軸正方向にある全てのロボットをX軸正方向に１ステップ移動する。Motion_Data_Updateを実行する。

ロボットi_originと同じX,Y座標値のロボットを全て、Z軸負方向に１ステップ移動させ、Motion_Data_Updateを実行することを、ロボットi_originと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。

[Go_Home(x,y,i)]
ロボットiを位置(x,y,Zmax[2]+1)まで移動させ、その後、ロボットiとX、Y座標が同じ全てのロボットとともにZ軸負方向に１ステップ移動する。

＜第一実施形態に係る行動制御システム１００＞
図１０は第一実施形態に係る行動制御システム１００の機能ブロック図を、図１１はその処理フローの例を示す。行動制御システム１００は、図１０に示すように、行動選択部１２０と、隣接状態判定部１２４と、位置更新部１２３と、位置判定部１２６と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。行動選択部１２０は、圧縮過程処理部１２０Ａと展開過程処理部１２０Ｂと中間変形過程処理部１２０Ｃと入れ替え過程処理部１２０Ｄとを含む。

以下では、制御の対象となる制御対象物が、ロボットである場合を例に挙げて説明する。もちろん、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。

本実施形態では、行動制御システム１００は、p台のロボットの行動を制御し、p台のロボットの内の１つのロボット上に実装される。なお、行動制御システム１００が実装されていないp-１台のロボットについても、通信部１５０と、隣接状態判定部１２４とを含む。

＜入力部１６０＞
入力部１６０には、中間位置M2の形状を表す変数の値Zmax[2], _y_num[2][_x][_z],_x_num[2][_y][_z])及び、
p台のロボットiのそれぞれの開始位置、言い換えると、p個の開始位置の集合S={(Xrs[0],Yrs[0],Zrs[0]),(Xrs[1],Yrs[1],Zrs[1]),…,(Xrs[p-1],Yrs[p-1],Zrs[p-1])}、p個の目標位置の集合G={(Xrg[0],Yrg[0],Zrg[0]),(Xrg[1],Yrg[1],Zrg[1]),…,(Xrg[p-1],Yrg[p-1],Zrg[p-1])}が入力され、記憶部１４０に記憶される。なお、開始位置の集合及び目標位置の集合については、各ロボットiが各位置の集合の中でどの位置に位置するのか特定されている。一方、中間位置の集合M2については、この時点では、各ロボットiが中間位置の集合M2の中でどの位置に位置するのか特定されていない。中間位置の集合M2がどのような形状、配置であるかが特定されており、入力される。

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

＜行動選択部１２０＞
行動選択部１２０は、上述の方法で、p台のロボットを制御する（ｓ１２０）。なお、圧縮過程処理部１２０Ａ、展開過程処理部１２０Ｂ、中間変形過程処理部１２０Ｃ、入れ替え過程処理部１２０Ｄでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部１４０に記憶しておく。なお、開始位置の集合Sに位置するロボットまたは仮想的なロボットを目標位置の集合Gに位置するように隊列変形する処理は、開始位置の集合Sを目標位置の集合Gに変形する処理に相当し、ロボットを移動する処理と、仮想的なロボットを移動する処理と、（開始、目的もしくは中間）位置を移動する処理とは同じ処理に相当する。また、上述の方法により、各時刻におけるロボットの位置が特定できるため、時刻tにおけるロボットの位置と、時刻t-1におけるロボットの位置の差分から各ロボットの動作、方向を計算することができる。行動選択部１２０は、記憶部１４０から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。

（圧縮過程処理部１２０Ａ）
圧縮過程処理部１２０Ａは、X=Xthresh+1の位置に固定されたX軸に垂直な第一仮想面と、X負方向無限遠からX軸正の方向に移動してくる第二仮想面を想定し、第二仮想面に当たった位置（開始位置の集合Sに含まれる位置、または、含まれていた位置）を押して、かつ、すべての位置が第一仮想面を超えないようにし、X座標値がXthresh以下になるように圧縮する。例えば、第二仮想面に当たった、X座標値が小さい、開始位置の集合Sに含まれる位置から順に、X軸正の方向におけるX座標値が大きい、開始位置の集合Sに含まれる位置と隣接する場合にはその隣接する位置と一緒に、X軸正の方向に平行移動させる(圧縮する)。なお、第二仮想面が押そうとする位置がすでに第一仮想面の支えにより（第一仮想面に到達し）動くことが出来ない場合は、その位置については、第一仮想面の支えを優先する（第一仮想面を超えないことを優先する）。

圧縮過程処理部１２０Ａは、Y=Ythresh+1の位置に固定されたY軸に垂直な第三仮想面と、Y負方向無限遠からY軸正の方向に移動してくる第四仮想面を想定し、上記第一および第二仮想面を使用して圧縮された位置の集合のうち、第四仮想面に当たった位置を押して、かつ、すべての位置が第三仮想面を超えないようにし、Y座標値がYthresh以下になるように圧縮する。例えば、第四仮想面に当たった、Y座標値が小さい、開始位置の集合Sに含まれる位置から順に、Y軸正の方向におけるY座標値が大きい、開始位置の集合Sに含まれる位置と隣接する場合にはその隣接する位置と一緒に、Y軸正の方向に平行移動させる(圧縮する)。なお、第四仮想面が押そうとする位置がすでに第三仮想面の支えにより（第三仮想面に到達し）動くことが出来ない場合は、その位置については、第三仮想面の支えを優先する（第三仮想面を超えないことを優先する）。

行動選択部１２０の圧縮過程処理部１２０Ａは、p個の開始位置の集合Sを取り出し、上述の[Compression_Process]を実行することで、開始位置の集合Sを中間位置の集合M1に圧縮することができる。

（展開過程処理部１２０Ｂ）
展開過程処理部１２０Ｂは、X=Xthresh+1の位置に固定されたX軸に垂直な第一仮想面と、X負方向無限遠からX軸正の方向に移動してくる第二仮想面を想定する。第二仮想面に当たった位置（目標位置の集合Gに含まれる位置、または、含まれていた位置）を押して、かつ、すべての位置が第一仮想面を超えないようにし、X座標値がXthresh以下になるように圧縮する。例えば、第二仮想面に当たった、X座標値が小さい、目標位置の集合Gに含まれる位置から順に、X軸正の方向におけるX座標値が大きい、目標位置の集合Gに含まれる位置と隣接する場合にはその隣接する位置と一緒に、X軸正の方向に平行移動させる(圧縮する)。なお、第二仮想面が押そうとする位置がすでに第一仮想面の支えにより（第一仮想面に到達し）動くことが出来ない場合は、その位置については、第一仮想面の支えを優先する（第一仮想面を超えないことを優先する）。

展開過程処理部１２０Ｂは、Y=Ythresh+1の位置に固定されたY軸に垂直な第三仮想面と、Y負方向無限遠からY軸正の方向に移動してくる第四仮想面を想定し、上記第一および第二仮想面を使用して圧縮された位置の集合のうち、第四仮想面に当たった位置を押して、かつ、すべての位置が第三仮想面を超えないようにし、Y座標値がYthresh以下になるように圧縮する。例えば、第四仮想面に当たった、Y座標値が小さい、目標位置の集合Gに含まれる位置から順に、Y軸正の方向におけるY座標値が大きい、目標位置の集合Gに含まれる位置と隣接する場合にはその隣接する位置と一緒に、Y軸正の方向に平行移動させる(圧縮する)。なお、第四仮想面が押そうとする位置がすでに第三仮想面の支えにより（第三仮想面に到達し）動くことが出来ない場合は、その位置については、第三仮想面の支えを優先する（第三仮想面を超えないことを優先する）。

展開過程処理部１２０Ｂは、上述の処理により、目標位置の集合Gを圧縮し、中間位置の集合M3を求めることができる。そこで、目標位置の集合Gから中間位置の集合M3を求める際の順番とは逆の順番で、目標位置の集合Gから中間位置の集合M3を求める際の移動方向とは反対の方向に各位置を移動させることで、中間位置の集合M3から目標位置の集合Gに展開することができる。

行動選択部１２０の展開過程処理部１２０Ｂは、p個の目標位置の集合Gを取り出し、上述の[Extension_Process]を実行することで、中間位置の集合M3を目標位置の集合Gに展開することができる。

（中間変形過程処理部１２０Ｃ）
中間変形過程処理部１２０Ｃは、中間位置の集合M1と中間位置の集合M3とを受け取り、中間位置の集合M1を中間位置の集合M3に変形する過程を計算する。ここで、ホモ隊列変形の場合には、中間位置の集合M2を経ずに、中間位置の集合M1を中間位置の集合M3に変形する。一方、ヘテロ隊列変形の場合には、中間位置の集合M1を中間位置の集合M2に変形する過程、及び、中間位置の集合M2を中間位置の集合M3に変形する過程を計算する。ここで、中間位置の集合M1,M3は、以下の条件(1)-(6）を、M2は以下の条件(1)-(9)を満たす形状である。
(1)全てのロボットのX座標は、ある定数値Xthresh以下の値である。
(2)全てのロボットのY座標は、ある定数値Ythresh以下の値である。
(3)全てのロボットのZ座標は、ある定数値Zthresh以上の値である。
(4)X座標値がXthreshでない全てのロボットは、X軸正方向に接するロボットを持つ。
(5)Y座標値がYthreshでない全てのロボットは、Y軸正方向に接するロボットを持つ。
(6)Z座標値がZthreshでなく、X座標値がXthresh,Y座標値がYthreshの全てのロボットは、Z軸負方向に接するロボットを持つ。
(7)Z座標値が最大であるロボットは、全体で一辺の長さが偶数のX、Y平面内の長方形をなす。
(8)Z座標が最大値ではないロボットは、必ずZ軸正方向に接するロボットを持つ。
(9)Z座標が最大値ではないロボットが必ず一つ存在する。

このような条件を満たす形状であれば、中間位置の集合M2はどのような形状であってもよく、利用者によりその各位置を入力されてもよいし、開始位置の集合Sと目標位置の集合Gとに基づき行動制御システム１００により計算してもよい。

中間変形過程処理部１２０Ｃは、中間位置の集合M1を受け取り、上述の処理Transform_from_M1_to_M2を実行することで、中間位置の集合M1を中間位置の集合M2に変形することができる。例えば、以下のように処理を行う。

中間変形過程処理部１２０Ｃは、中間位置の集合M1にある位置の中から、シード位置となる２つの位置を選択し、シード位置の待機位置(Xthresh,Ythresh+1,Zthresh)、(Xthresh+1,Ythresh,Zthresh)に移動させる。中間変形過程処理部１２０Ｃは、圧縮過程処理部１２０Ａで計算した中間位置の集合M1に基づき、上述の処理Seed_Generationを実行することで、シード位置を選択し、待機位置に移動させることができる。なお、本実施形態では、シード位置の待機位置を(Xthresh,Ythresh+1,Zthresh)、(Xthresh+1,Ythresh,Zthresh)としたが、両方のシード位置の待機位置がおなじZ座標値であればよく、Zthresh以外のZ座標値を待機位置としてもよい。

中間変形過程処理部１２０Ｃは、中間位置の集合M1（ただし、シードロボット分を含まない）を床部、旗部、柱部の順に分解し、中間位置の集合M2（ただし、シードロボット分を含まない）を柱部、旗部、床部の順に建造する。なお、前述のシードロボットを用いて柱部の分解、建造が行われる。なお、
柱部：Xr[i]=XthreshかつYr[i]=Ythreshの位置にあるロボットで構成される部分。
旗部：Xr[i]=XthreshかつYr[i]！＝Ythreshの位置にあるロボットで構成される部分。
床部：柱部、旗部に属さないその他の部分全て。
である。中間変形過程処理部１２０Ｃは、上述のMiddle_Transformを実行することで、中間位置の集合M1から中間位置の集合M2の柱部、旗部、床部を建造する。

中間変形過程処理部１２０Ｃは、シードロボット分を含まない中間位置の集合M2に対して、シードロボットを組み込む。中間変形過程処理部１２０Ｃは、上述のSeed_UnGenerationを実行することで、シードロボットを中間位置の集合M2に組み込む。

中間変形過程処理部１２０Ｃは、同様にTransform_from_M2_to_M3を実行することで、中間位置の集合M2を中間位置の集合M3に変形する過程を計算することができる。

（入れ替え過程処理部１２０Ｄ）
入れ替え部１２０Ｄは、目標位置の集合Gから中間位置の集合M3を経て中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M3における位置を中間変形過程処理部１２０Ｃから取得する。例えば、上述の[Extension_Process]を実行し、中間位置の集合M3を目標位置の集合Gに展開する過程とは、逆の順番で、反対の方向に移動させることで、目標位置の集合Gから中間位置の集合M3に変形する過程を得ることができる。

また、入れ替え過程処理部１２０Ｄは、開始位置の集合Sから中間位置の集合M1を経て中間位置の集合M2に変形し、さらに中間位置の集合M3に変形した際の、ロボットiの中間位置の集合M3における位置を中間変形過程処理部１２０Ｃから取得する。

入れ替え過程処理部１２０Ｄは、取得したそれらの位置の値を使用して、開始位置の集合Sから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置から、ロボットiを、目標位置の集合Gから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置に移動させるための各ロボットの位置の対応関係を計算する。

入れ替え過程処理部１２０Ｄは、処理Transform_from_M2_to_M2を実行し、開始位置の集合Sから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置から、ロボットiを、目標位置の集合Gから中間位置の集合M2に変形した際の、ロボットiの中間位置の集合M2における位置に移動させるための、各ロボットの位置を取得する。

＜位置更新部１２３＞
位置更新部１２３は、各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がロボットで充填されているか否かを判定し、充填されている場合には、任務を終了する。少なくとも１つ以上の目標位置にロボットが充填されていない場合には、行動選択部１２０を再度実行するよう制御する。

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

＜効果＞
このような構成により、制御対象物が移動する際に、他の制御対象物の周りを回り込まずに、制御対象物同士が、接続状態を維持したまま、移動を行うという前提での隊列制御を実現し、複雑な機構を必要とせずに、制御対象物同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ変形動作を行うことができる。

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

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

本実施形態では、行動制御システム１００が、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]）とが一致するか否かを判定する。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。その後の処理については第一実施形態と同様でもよいし、他の処理を行ってもよい。このような構成により、少なくとも制御通りに移動できなかったことを検知することができる。

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

また、本実施形態では、開始位置の集合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])に基づきロボットを制御すればよい。

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

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

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

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

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

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

Claims (7)

1. pを5以上の整数の何れかとし、p台の制御対象物をp個の開始位置の集合からp個の目標位置の集合に移動させるための行動制御を行う行動制御システムであって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合に配置されたp台の制御対象物及び開始位置の集合に配置されたp台の制御対象物はそれぞれ一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   開始位置の集合及び目標位置の集合に含まれる位置のうち、第一方向における最大の座標値をXthreshとし、第二方向における最大の座標値をYthreshとし、第五方向における最小の座標値をZthreshとし、
   第一方向における座標値Xthresh+1に固定された第一方向に垂直な第一仮想面と、第三方向無限遠から第一方向に移動してくる第二仮想面とを想定し、第二仮想面に当たった開始位置の集合に配置される制御対象物、または、配置されていた制御対象物を第二仮想面の移動方向に押して、かつ、すべての制御対象物が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、第二方向における座標値Ythresh+1に固定された第二方向に垂直な第三仮想面と、第四方向無限遠から第二方向に移動してくる第四仮想面とを想定し、第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された形状に含まれる制御対象物を第四仮想面の移動方向に押して、かつ、すべての制御対象物が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、圧縮された形状に含まれる制御対象物の位置を中間位置の集合M1として求める圧縮過程処理部と、
   前記第二仮想面に当たった目標位置の集合に配置される仮想的な制御対象物、または、配置されていた仮想的な制御対象物を押して、かつ、すべての仮想的な制御対象物が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、前記第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された形状に含まれる仮想的な制御対象物を第四仮想面の移動方向に押して、かつ、すべての仮想的な制御対象物が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、圧縮された形状に含まれる仮想的な制御対象物の位置を中間位置の集合M3として求め、前記目標位置の集合から前記中間位置の集合M3を求める際の順番とは逆の順番で、前記目標位置の集合に配置される仮想的な制御対象物から前記中間位置の集合M3を求める際の移動方向とは反対の方向に各制御対象物を移動させる展開過程処理部と、
   前記中間位置の集合M1に配置される制御対象物が成す形状を前記中間位置の集合M3に配置される制御対象物が成す形状に変形する過程を求める中間変形過程処理部とを含む、
   行動制御システム。
2. 請求項１の行動制御システムであって、
   前記中間変形過程処理部は、
   (A)前記中間位置の集合M1に配置される制御対象物の中から2つのシード制御対象物を選択し、一方のシード制御対象物をX座標がXthresh,Y座標がYthresh+1である待機位置に移動させ、他方のシード制御対象物をX座標がXthresh+1,Y座標がYthreshである待機位置に移動させ、両方のシード制御対象物の待機位置がおなじZ座標値であり、シード制御対象物の位置を含まない中間位置の集合M1を求め、
   (B)前記中間位置の集合M1及びM3に配置される制御対象物において、Xthreshであり、かつ、Ythreshである位置に配置される制御対象物を柱部とし、Xthreshであり、かつ、Ythreshでない位置に配置される制御対象物を旗部とし、柱部、旗部に属さない位置に配置される制御対象物を床部とし、前記シード制御対象物を用いて、前記シード制御対象物を含まない中間位置の集合M1に配置される制御対象物を柱部、旗部、床部に分解し、前記シード制御対象物を含まない中間位置の集合M3に配置される制御対象物の柱部、旗部、床部を建造し、
   (C)待機位置のシード制御対象物の位置を中間位置の集合M3に組み込み、前記シード制御対象物の位置を含む中間位置の集合M3を求める、
   行動制御システム。
3. 請求項１の行動制御システムであって、
   (1)中間位置の集合M2に配置される全ての制御対象物の第一方向における座標値はXthresh以下であり、(2)中間位置の集合M2に配置される全ての制御対象物の第二方向における座標値はYthresh以下であり、(3)中間位置の集合M2に配置される全ての制御対象物の第五方向における座標値はZthresh以上であり、(4)中間位置の集合M2に配置される制御対象物の内、第一方向における座標値がXthreshでない全ての制御対象物は、第一方向において他の制御対象物と接し、(5)中間位置の集合M2に配置される制御対象物の内、第二方向における座標値がYthreshでない全ての制御対象物は、第二方向において他の制御対象物と接し、(6)中間位置の集合M2に配置される制御対象物の内、第五方向における座標値がZthreshでなく、第一方向における座標値がXthresh,第二方向における座標値がYthreshの全ての制御対象物は、第六方向において他の制御対象物と接し、(7)中間位置の集合M2に配置される制御対象物の内、第五方向における座標値が最大である制御対象物は、全体で一辺の長さが偶数の第一方向及び第二方向を含む平面内において長方形をなし、(8)中間位置の集合M2に配置される制御対象物の内、第五方向における座標値が最大値ではない制御対象物は、必ず第五方向において他の制御対象物と接し、(9)中間位置の集合M2に配置される制御対象物には、第五方向における座標値が最大値ではない制御対象物が必ず一つ存在するものとし、
   前記中間変形過程処理部は、
   (A)前記中間位置の集合M1に配置される制御対象物の中から2つのシード制御対象物を選択し、一方のシード制御対象物をX座標がXthresh,Y座標がYthresh+1である待機位置に移動させ、他方のシード制御対象物をX座標がXthresh+1,Y座標がYthreshである待機位置に移動させ、両方のシード制御対象物の待機位置がおなじZ座標値であり、シード制御対象物の位置を含まない中間位置の集合M1を求め、
   (B)前記中間位置の集合M1及びM2に配置される制御対象物において、Xthreshであり、かつ、Ythreshである制御対象物を柱部とし、Xthreshであり、かつ、Ythreshでない制御対象物を旗部とし、柱部、旗部に属さない制御対象物を床部とし、前記シード制御対象物を用いて、前記シード制御対象物の位置を含まない中間位置の集合M1に配置される制御対象物を柱部、旗部、床部に分解し、前記シード制御対象物の位置を含まない中間位置の集合M2に配置される制御対象物の柱部、旗部、床部を建造し、
   (C)待機位置のシード制御対象物の位置を前記シード制御対象物の位置を含まない中間位置の集合M2に組み込み、前記シード制御対象物の位置を含む中間位置の集合M2を求め、
   (D)前記中間変形過程処理部は、前記中間位置の集合M2に配置される制御対象物の中から2つのシード制御対象物を選択し、一方のシード制御対象物をX座標がXthresh,Y座標がYthresh+1である待機位置に移動させ、他方のシード制御対象物をX座標がXthresh+1,Y座標がYthreshである待機位置に移動させ、両方のシード制御対象物の待機位置がおなじZ座標値であり、シード制御対象物の位置を含まない中間位置の集合M2を求め、
   (E)前記シード制御対象物を用いて、前記シード制御対象物の位置を含まない中間位置の集合M2に配置される制御対象物を柱部、旗部、床部に分解し、前記シード制御対象物の位置を含まない中間位置の集合M3に配置される制御対象物の柱部、旗部、床部を建造し、
   (F)待機位置のシード制御対象物の位置を前記シード制御対象物の位置を含まない中間位置の集合M3に組み込み、前記シード制御対象物の位置を含む中間位置の集合M3を求め、
   (G)開始位置の集合に配置されたp台の制御対象物を中間位置の集合M2に移動させた際の、中間位置の集合M2における各制御対象物を、目標位置の集合に配置されたp台の仮想的な制御対象物を中間位置の集合M2に移動させた際の、中間位置の集合M2における各制御対象物に移動させるための各制御対象物の対応関係を計算する、
   行動制御システム。
4. pを5以上の整数の何れかとし、p台の制御対象物をp個の開始位置の集合からp個の目標位置の集合に移動させるための行動制御を行う行動制御方法であって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合に配置されたp台の制御対象物及び開始位置の集合に配置されたp台の制御対象物はそれぞれ一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   開始位置の集合及び目標位置の集合に含まれる位置のうち、第一方向における最大の座標値をXthreshとし、第二方向における最大の座標値をYthreshとし、第五方向における最小の座標値をZthreshとし、
   圧縮過程処理部が、第一方向における座標値Xthresh+1に固定された第一方向に垂直な第一仮想面と、第三方向無限遠から第一方向に移動してくる第二仮想面とを想定し、第二仮想面に当たった開始位置の集合に配置される制御対象物、または、配置されていた制御対象物を第二仮想面の移動方向に押して、かつ、すべての制御対象物が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、第二方向における座標値Ythresh+1に固定された第二方向に垂直な第三仮想面と、第四方向無限遠から第二方向に移動してくる第四仮想面とを想定し、第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された形状に含まれる制御対象物を第四仮想面の移動方向に押して、かつ、すべての制御対象物が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、圧縮された形状に含まれる制御対象物の位置を中間位置の集合M1として求める圧縮過程処理ステップと、
   展開過程処理部が、前記第二仮想面に当たった目標位置の集合に配置される仮想的な制御対象物、または、配置されていた仮想的な制御対象物を押して、かつ、すべての仮想的な制御対象物が第一仮想面を超えないようにし、第一方向の座標値がXthresh以下になるように圧縮し、前記第四仮想面に当たった、先の第一仮想面及び第二仮想面を使用して圧縮された形状に含まれる仮想的な制御対象物を第四仮想面の移動方向に押して、かつ、すべての仮想的な制御対象物が第三仮想面を超えないようにし、第二方向の座標値がYthresh以下になるように圧縮し、圧縮された形状に含まれる仮想的な制御対象物の位置を中間位置の集合M3として求め、前記目標位置の集合から前記中間位置の集合M3を求める際の順番とは逆の順番で、前記目標位置の集合に配置される仮想的な制御対象物から前記中間位置の集合M3を求める際の移動方向とは反対の方向に各制御対象物を移動させる展開過程処理ステップと、
   中間変形過程処理部が、前記中間位置の集合M1に配置される制御対象物が成す形状を前記中間位置の集合M3に配置される制御対象物が成す形状に変形する過程を求める中間変形過程処理ステップとを含む、
   行動制御方法。
5. 請求項４の行動制御方法であって、
   前記中間変形過程処理ステップは、
   (A)前記中間位置の集合M1に配置される制御対象物の中から2つのシード制御対象物を選択し、一方のシード制御対象物をX座標がXthresh,Y座標がYthresh+1である待機位置に移動させ、他方のシード制御対象物をX座標がXthresh+1,Y座標がYthreshである待機位置に移動させ、両方のシード制御対象物の待機位置がおなじZ座標値であり、シード制御対象物の位置を含まない中間位置の集合M1を求め、
   (B)前記中間位置の集合M1及びM3に配置される制御対象物において、Xthreshであり、かつ、Ythreshである位置に配置される制御対象物を柱部とし、Xthreshであり、かつ、Ythreshでない位置に配置される制御対象物を旗部とし、柱部、旗部に属さない位置に配置される制御対象物を床部とし、前記シード制御対象物を用いて、前記シード制御対象物を含まない中間位置の集合M1に配置される制御対象物を柱部、旗部、床部に分解し、前記シード制御対象物を含まない中間位置の集合M3に配置される制御対象物の柱部、旗部、床部を建造し、
   (C)待機位置のシード制御対象物の位置を中間位置の集合M3に組み込み、前記シード制御対象物の位置を含む中間位置の集合M3を求める、
   行動制御方法。
6. 請求項４の行動制御方法であって、
   (1)中間位置の集合M2に配置される全ての制御対象物の第一方向における座標値はXthresh以下であり、(2)中間位置の集合M2に配置される全ての制御対象物の第二方向における座標値はYthresh以下であり、(3)中間位置の集合M2に配置される全ての制御対象物の第五方向における座標値はZthresh以上であり、(4)中間位置の集合M2に配置される制御対象物の内、第一方向における座標値がXthreshでない全ての制御対象物は、第一方向において他の制御対象物と接し、(5)中間位置の集合M2に配置される制御対象物の内、第二方向における座標値がYthreshでない全ての制御対象物は、第二方向において他の制御対象物と接し、(6)中間位置の集合M2に配置される制御対象物の内、第五方向における座標値がZthreshでなく、第一方向における座標値がXthresh,第二方向における座標値がYthreshの全ての制御対象物は、第六方向において他の制御対象物と接し、(7)中間位置の集合M2に配置される制御対象物の内、第五方向における座標値が最大である制御対象物は、全体で一辺の長さが偶数の第一方向及び第二方向を含む平面内において長方形をなし、(8)中間位置の集合M2に配置される制御対象物の内、第五方向における座標値が最大値ではない制御対象物は、必ず第五方向において他の制御対象物と接し、(9)中間位置の集合M2に配置される制御対象物には、第五方向における座標値が最大値ではない制御対象物が必ず一つ存在するものとし、
   前記中間変形過程処理ステップは、
   (A)前記中間位置の集合M1に配置される制御対象物の中から2つのシード制御対象物を選択し、一方のシード制御対象物をX座標がXthresh,Y座標がYthresh+1である待機位置に移動させ、他方のシード制御対象物をX座標がXthresh+1,Y座標がYthreshである待機位置に移動させ、両方のシード制御対象物の待機位置がおなじZ座標値であり、シード制御対象物の位置を含まない中間位置の集合M1を求め、
   (B)前記中間位置の集合M1及びM2に配置される制御対象物において、Xthreshであり、かつ、Ythreshである制御対象物を柱部とし、Xthreshであり、かつ、Ythreshでない制御対象物を旗部とし、柱部、旗部に属さない制御対象物を床部とし、前記シード制御対象物を用いて、前記シード制御対象物の位置を含まない中間位置の集合M1に配置される制御対象物を柱部、旗部、床部に分解し、前記シード制御対象物の位置を含まない中間位置の集合M2に配置される制御対象物の柱部、旗部、床部を建造し、
   (C)待機位置のシード制御対象物の位置を前記シード制御対象物の位置を含まない中間位置の集合M2に組み込み、前記シード制御対象物の位置を含む中間位置の集合M2を求め、
   (D)前記中間変形過程処理部は、前記中間位置の集合M2に配置される制御対象物の中から2つのシード制御対象物を選択し、一方のシード制御対象物をX座標がXthresh,Y座標がYthresh+1である待機位置に移動させ、他方のシード制御対象物をX座標がXthresh+1,Y座標がYthreshである待機位置に移動させ、両方のシード制御対象物の待機位置がおなじZ座標値であり、シード制御対象物の位置を含まない中間位置の集合M2を求め、
   (E)前記シード制御対象物を用いて、前記シード制御対象物の位置を含まない中間位置の集合M2に配置される制御対象物を柱部、旗部、床部に分解し、前記シード制御対象物の位置を含まない中間位置の集合M3に配置される制御対象物の柱部、旗部、床部を建造し、
   (F)待機位置のシード制御対象物の位置を前記シード制御対象物の位置を含まない中間位置の集合M3に組み込み、前記シード制御対象物の位置を含む中間位置の集合M3を求め、
   (G)開始位置の集合に配置されたp台の制御対象物を中間位置の集合M2に移動させた際の、中間位置の集合M2における各制御対象物を、目標位置の集合に配置されたp台の仮想的な制御対象物を中間位置の集合M2に移動させた際の、中間位置の集合M2における各制御対象物に移動させるための各制御対象物の対応関係を計算する、
   行動制御方法。
7. 請求項１から請求項３に記載の何れかの行動制御システムとしてコンピュータを機能させるためのプログラム。