JP6685957B2 - 制御対象物位置入れ替え制御装置、制御対象物位置入れ替え制御方法、プログラム - Google Patents

Description

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

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

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

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

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

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

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

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

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

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

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

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

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

非特許文献６の手法においては、障害物の存在を考慮した、各ロボット個々の位置を隊列内で指定しない方式のホモジニアス（Homogeneous）隊列制御は実現できているが、各ロボット個々の位置を隊列内で指定する方式のヘテロジニアス（Heterogeneous）隊列制御は実現できていない。このため、例えば、カメラを搭載したロボット、車輪を搭載したロボット等各ロボット個々に異なる役割があって、各個のロボットをロボット隊列内の適切な位置に配置させつつ、隊列の形状を制御するといった運用はできない。したがって、すべてのロボットに必要なすべての機能を実装しなければならないという問題がある。

このような現状に鑑みて、本発明では、多数のロボットの存在を考慮しつつ、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ障害物のある環境にてホモジニアス隊列変形動作を行った後に各ロボットを個々の目標位置に移動させることによりヘテロジニアス隊列制御を実現する場合のように、ロボット位置の入れ替え開始前後のロボット隊列が同一という条件下で、各個のロボットをロボット隊列内の適切な位置に配置させることができる、ロボット位置入れ替え技術を提供することを目的とする。

本発明の一態様は、M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、入れ替え開始前と入れ替え終了後の位置の集合（以下、集合Gという）が同一になるという条件のもと、p台の制御対象物の各々が前記集合Gの中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御装置であって、第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置からなる一つの位置単位とし、前記集合Gを構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合GはR個の位置単位からなる一塊の任意の形状を成し、前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、面Ssを、前記集合Gを構成するR個の位置単位中のある位置単位が前記集合Gの外部Pと接する面、位置単位Msを、前記面Ssに接する前記集合Gの外部P内の位置単位、位置単位Ms’を、前記面Ssに接する前記集合G内の位置単位、位置単位Mtを、前記位置単位Ms’から前記集合G内にてマンハッタン距離が最も大きい前記集合G内の位置単位、位置単位Mt’を、前記位置単位Mtに接し前記位置単位Mtより前記位置単位Ms’からのマンハッタン距離が１だけ小さい前記集合G内の位置単位、G＋Ms−Mt構造を、前記集合Gと前記位置単位Msから構成される構造であり、前記位置単位Mtには前記位置単位Mt’に接しない位置に２つのボイドが接する形で含まれており、前記位置単位Msには前記位置単位Ms’に接する位置にのみ最大４つの制御対象物が存在する構造、制御対象物構造Rcoreを、前記G＋Ms−Mt 構造内に含まれるp台の制御対象物から位置単位Mt に含まれる6 台の制御対象物を除いたp-6 台の制御対象物で構成される構造とし、制御対象物i(iは1≦i≦pを満たす整数)が到達すべき前記集合Gの中での位置を目標位置i、前記制御対象物iの前記集合Gの中での現在の位置を現在位置iとし、前記G＋Ms-Mt構造を生成する入れ替えモード変換部と、制御対象物構造Rcore内のボイドの初期位置を、前記入れ替えモード変換部により前記G＋Ms-Mt構造が生成された際に前記制御対象物構造Rcoreに含まれる２つのボイドの位置とし、前記現在位置iが前記目標位置iと一致しない場合、前記制御対象物iまたは前記目標位置iにある別の制御対象物j(jは1≦j≦pを満たす整数、j≠i)のいずれかが前記位置単位Mt内にある場合、前記位置単位Mt内にある制御対象物を前記位置単位Mt’内の制御対象物と位置入れ替えを行う事前処理部と、入れ替え対象位置1を、前記事前処理部により前記制御対象物iの位置が入れ替えられなかった場合は前記現在位置i、前記事前処理部により前記制御対象物iの位置が入れ替えられた場合は前記制御対象物iの位置が入れ替え対象となった前記位置単位Mt’内の制御対象物があった位置、入れ替え対象位置2を、前記事前処理部により前記制御対象物jの位置が入れ替えられなかった場合は前記目標位置i、前記事前処理部により前記制御対象物jの位置が入れ替えられた場合は前記制御対象物jの位置が入れ替え対象となった前記位置単位Mt’内の制御対象物があった位置とし、前記入れ替え対象位置1から前記入れ替え対象位置2に至る、前記制御対象物構造Rcore内の入れ替え経路を生成する入れ替え経路生成部と、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記入れ替え対象位置1にいる制御対象物と前記入れ替え対象位置2にいる制御対象物とを入れ替える入れ替え制御部と、前記２つのボイドを前記初期位置に戻すボイド初期位置移動部と、前記事前処理部により、位置を入れ替えられた制御対象物がある場合は、それぞれ元の位置に戻るよう再度入れ替えを行う事後処理部と、前記制御対象物i(iは1≦i≦pを満たす整数)が前記目標位置iにそれぞれ到達した後、前記入れ替えモード変換部により前記G＋Ms-Mt構造を生成される前の構造に戻す回復処理部とを含む。

本発明に拠れば、ロボット位置の入れ替え開始前後のロボット隊列が同一という条件下で、各個のロボットをロボット同士の面せん断動作によってロボット隊列内の適切な位置に配置させることができる。

ロボットの移動を説明するための図。 図２Ａは開始位置の集合を説明するための図、図２Ｂは目標位置の集合を説明するための図。 ある位置にいた一つのボイドが、別の位置に移動していく様子を示す図。 ロボット単位を説明するための図。 多数のロボットが協調して開始位置における隊列形成状態から移動を行い、目標位置での隊列形成を行う任務を説明するための図。 ロボット位置入れ替え制御の制約を説明するための図。 G＋Ms−Mt構造の一例を示す図。 各個ロボット位置入れ替え制御の様子を示す図。 各個ロボット位置入れ替え前後の処理の様子を示す図。 分割されたパスの様子を示す図。 線要素でのロボット位置入れ替えの様子を示す図。 角要素でのロボット位置入れ替えの様子を示す図。 Void_Moveで用いる変数を示す図（その１）。 Void_Moveで用いる変数を示す図（その２）。 第一実施形態に係る行動制御システムの機能ブロック図。 第一実施形態に係る行動制御システムの処理フロー（ロボット位置入れ替え制御）の例を示す図。

以下、本発明の実施形態について説明する。なお、以下の説明に用いる図面では、同じ機能を持つ構成部や同じ処理を行うステップには同一の符号を記し、重複説明を省略する。
＜第一実施形態＞
まず、行動制御システム及び方法の理論的背景について説明する。以下、行動制御の対象である制御対象物が、ロボットである場合を例に挙げて説明するが、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。

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

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

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

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

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

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

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

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

[非特許文献６におけるホモジニアス隊列制御における特徴]
非特許文献６におけるホモジニアス隊列制御では、上述の計算負荷の問題を解決するための方策の一つとして、ボイド制御の考え方を導入している。また、［問題設定］で述べたような隊列変形の問題を克服するために8マスロボット単位の考え方も導入している。

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

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

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

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

以下、ロボットの全体数pを8の倍数とする。また、ロボットの開始位置の集合Sからロボットの目標位置の集合Gへの隊列変形は非特許文献６の方法にて完了しているとすると、各ロボット個々を目標位置の集合G内の指定された位置に移動させていく問題（ヘテロジニアスロボット位置入れ替え制御）が残っている。以下、ヘテロジニアスロボット位置入れ替え制御について説明する。

参考までに、非特許文献６の手法により制御され、図２Ａの開始位置の集合から図２Ｂの目標位置の集合に変形する際のロボット群の遷移状態の例を図５に示す。なお、開始位置の集合及び目標位置の集合はそれぞれR個(R=p/8)の位置単位からなる一塊の任意の形状を成している。

［ヘテロジニアスロボット位置入れ替え制御］
各ロボットiが目標位置の集合G内にて、任意の位置に存在する状態から、各ロボットiをそれぞれ目標位置（Xre[i],Yre[i],Zre[i]）に配置させるヘテロジニアスロボット位置入れ替え制御の方法について以下説明していく。

［ロボット位置入れ替え制御］
ロボット位置入れ替え制御実行前の、各ロボットjの位置を（Xrh[j],Yrh[j],Zrh[j]）とすると、ロボット位置の入れ替え制御とは、（Xrh[j],Yrh[j],Zrh[j]）= （Xre[i],Yre[i],Zre[i]）となるiとjの組を特定し、ロボットiを現状のロボットjの位置に移動させることである。すなわち、ロボットiの移動先ロボット位置をd(i)としたとき、d(i)=jであり、すべてのロボットiを現状のロボットd(i)の位置に移動させていく制御に他ならない。

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

なお、パスPとは、非特許文献６の方法にて開始位置の集合Sから目標位置の集合Gまで各ロボットが移動していった経路上のマスの集合のことである。

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

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

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

ここで、複数の位置単位で構成された構造内に２か所のボイドが存在する場合を考えてみる。このとき、２つのボイドの位置がどこであったとしても、ロボット同士の非接続箇所が発生することはない。さらにロボットiとロボットd(i)の入れ替えを行うには、最低でもボイドが構造内に２つなければならない。以上により、ボイド２つの状態で行われるロボット同士の位置入れ替えにおいて、ロボット同士の非接続が発生することはないといえる。すなわち、ロボット同士の入れ替えの繰り返しを始める前に、適切な形で入れ替えに用いる構造の内部に２つのボイドを発生させ、その状態でロボット同士の位置入れ替えの繰り返しを行えば、ロボット同士の非接続は発生しない。

本実施形態では、これを実現するために、まず、位置単位Ms内には面Ssに接する位置にのみ最大で４つのロボットがあり、かつ、位置単位Ms’から目標位置の集合G内にてマンハッタン距離が最も大きい位置単位をMtとし、Mtに接しMtよりMs’からのマンハッタン距離が１だけ小さい位置単位をMt’としたときに、Mt内のMt’に接しない位置に２つのボイドを、それらのボイドが互いに接するように配置させた状態で構成される構造（以下、G＋Ms−Mt構造という）を使用する(図７参照)。すなわち、G＋Ms−Mt構造とは、目標位置の集合Gと位置単位Msから構成される構造であり、位置単位Mtには、位置単位Mt’に接しない位置に２つのボイドが接する形で含まれており、位置単位Msには位置単位Ms’に接する位置にのみ最大４つのロボットが存在する構造のことをいう。このとき、G＋Ms−Mt構造内に含まれるp台のロボットから位置単位Mtに含まれる6台のロボットを除いたp-6台のロボットで構成される構造（以下、ロボット構造Rcoreという）は、内部に必ず２つのボイドを内包しつつも、２つのボイド位置がどこであってもロボット同士の非接続が発生しないことになる（正確には位置単位Ms内のロボットは４つしかないので、位置単位Msにロボットが充填されていないが、面Ssに接する形で４つのロボットが存在しているので、２つのボイド位置がどこであったとしてもロボット同士の非接続が位置単位Ms近辺で発生することはない）。

したがって、ロボット構造Rcore内に位置入れ替えを行う２つのロボットがある場合は、ロボットの入れ替えにおいてロボット同士の非接続は発生しない。入れ替えを行う２つのロボットのうち１つもしくは２つともが位置単位Mt内にある場合にのみ、２つのボイドを位置単位Msに移動させた後に、あらかじめ位置単位Mt内にあるロボットを位置単位Mt’内の任意の位置のロボットと位置を入れ替え、その状態でロボット位置入れ替えを行えば、同様にロボット同士の非接続を回避できる。

上記考えに基づく、図８に示すロボット位置入れ替え制御処理手順Robot_Position_Permutationは以下の通りである。

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

Record_History, Transform_to_Permutation_Mode, Maintain_Mt_1, Maintain_Mt_2, Each_Robot_Position_Exchange, Recover_to_Goalの動作の詳細については、後述するが、以下、簡単に説明する。

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

Transform_to_Permutation_Modeは、前述のG＋Ms-Mt構造を生成する処理を行う。

Maintain_Mt_1は、各個ロボット位置入れ替え制御を行う前に入れ替え対象のロボットが位置単位Mt内にあるか否かを判定し、ある場合はその位置単位Mt内の入れ替え対象ロボットを位置単位Mt’内のロボットと位置入れ替えを行う処理をする。また、Maintain_Mt_2は、Maintain_Mt_1で入れ替えを行ったロボットの位置関係を元の位置関係に戻す処理をする。

Each_Robot_Position_Exchangeは、各個ロボット位置入れ替え制御を行う処理である。

Recover_to_Goalは、Transform_to_Permutation_Modeの実行前の構造に戻す処理をする。

[G＋Ms-Mt構造の生成]
ここでは、入れ替えモード変換処理手順Transform_to_Permutation_Modeと回復処理手順Recover_to_Goalについて説明する。Transform_to_Permutation_Modeの処理は、例えば、以下のようにすればよい。(2)内のボイド移動処理には後述のNavigate_Voidを使うことができる。なお、処理手順はここで説明したものに限られるものではなく、G+Ms−Mt構造が生成できる処理手順であればどのようなものを用いてもよい。

[Transform_to_Permutation_Mode]
(1) 位置単位Ms’内の面Ssに接しかつ互いに接する任意の２つのロボットと、それら２つのロボットに接しかつ面Ssに接しない２つのロボット（合計４つ）をすべて位置単位Ms’から位置単位Msに向かう方向に1ステップ移動させる。
(2) 位置単位Ms’に発生した２つのボイドを、位置単位Mt内の位置単位Mt’に接しないでかつ互いに接する２つの位置まで移動させる。
(3) 位置単位Ms内に２つあるロボットに接し、かつ面Ssに接する位置にある２つのボイドの位置を、ロボット構造Rcore内のボイドの初期位置とする。２つのボイドの現在位置を初期位置として(void_ini_x[0], void_ini_y[0], void_ini_z[0]）、（void_ini_x[1], void_ini_y[1], void_ini_z[1]）に格納する。

入れ替えモード変換処理手順Transform_to_Permutation_Modeを実行することにより、図７のような位置単位Mt内に6台のロボットが含まれる構造が生成される。

[Recover_to_Goal]
(1) Transform_to_Permutation_Modeにて行われたロボットの動作の時系列的に逆の動作を行う。すなわち、Transform_to_Permutation_Modeにてある時刻で位置（X,Y,Z）にあったロボットがX正の方向に1ステップ移動したならば、本処理の対応する時刻にて同じ位置（X,Y,Z）にあるロボットをX負の方向に1ステップ移動させる。

[各個ロボット位置入れ替え前後の処理]
各個ロボット位置入れ替え制御を実行する前に、入れ替え対象のロボットが位置単位Mt内にあるか否かを判定し、ある場合はそのMt内の入れ替え対象ロボットを位置単位Mt’内のロボットと位置入れ替えを行う処理（Maintain_Mt_1の処理）は、以下の通りである。

[Maintain_Mt_1]
(1)入れ替え対象のロボットをロボットie1,ie2とする。ロボットie1が位置単位Mt内にあるか否かを判定する。ある場合は、フラグ_flg_ie1<-0,_flg_ie1_rot<-0とし、以下の処理をする。
(1-1)ロボットie1が位置単位Mt内の位置単位Mt’に接する位置にあり、かつMt内でロボットie1に接するロボットが２つしかない場合は、ロボットie1と、ロボットie1から見て位置単位Mtから位置単位Mt’に向かう方向にあり、かつ位置単位Mt内のロボットie2以外の任意のロボットie1’と位置を入れ替え、Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。_flg_ie1<-1とし(2)へ。位置の入れ替えには、後述のLine_Permを使用する。そうでない場合は、(1-2)を実行する。
(1-2)例えば、図９に示すように、ロボットie1の位置が、位置単位Mt内において、ロボットie1に接するロボットが２つしかない位置になるように移動させ、_flg_ie1_rot<-1とする。続いて、ロボットie1と、ロボットie1から見て位置単位Mtから位置単位Mt’に向かう方向にあり、かつ位置単位Mt内のロボットie2以外の任意のロボットie1’と位置を入れ替え、Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。_flg_ie1<-1として(2)へ。位置の入れ替えには、後述のLine_Permを使用する。
(2)ロボットie2が位置単位Mt内にあるか否かを判定する。ある場合は、フラグ_flg_ie2<-0,_flg_ie2_rot<-0とし、以下の処理をする。
(2-1)ロボットie2が位置単位Mt内の位置単位Mt’に接する位置にあり、かつ位置単位Mt内でロボットie2に接するロボットが２つしかない場合は、ロボットie2と、ロボットie2から見て位置単位Mtから位置単位Mt’に向かう方向にあり、かつ位置単位Mt内のロボットie1以外の任意のロボットie2’と位置を入れ替え、Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。_flg_ie2<-1とし終了。位置の入れ替えには、後述のEach_Robot_Position_Exchangeを使用する。そうでない場合は、(2-2)を実行する。
(2-2)例えば、図９に示すように、ロボットie2の位置が、位置単位Mt内において、ロボットie2に接するロボットが２つしかない位置になるように移動させ、_flg_ie2_rot<-1とする。続いて、ロボットie2と、ロボットie2から見て位置単位Mtから位置単位Mt’に向かう方向にあり、かつ位置単位Mt内のロボットie1以外の任意のロボットie2’と位置を入れ替え、Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。flg_ie2<-1として終了。位置の入れ替えには、後述のEach_Robot_Position_Exchangeを使用する。

Maintain_Mt_1で入れ替えを行ったロボットの位置関係を元の位置関係に戻す処理（Maintain_Mt_2の処理）は、以下の通りである。

[Maintain_Mt_2]
(1)_flg_ie2=1のとき、ロボットie2’の位置と、ロボットd(ie2)の位置を入れ替える。Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。続いて_flg_ie2_rot=1のとき、Maintain_Mt_1の（2-2）にて行った図９の動作と逆の動作を使って、ロボットd(ie2)の位置をMaintain_Mt_1実行前のロボットie2の位置に戻す。
(2)_flg_ie1=1のとき、ロボットie1’の位置と、ロボットd(ie1)の位置を入れ替える。Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。続いて_flg_ie1_rot=1のとき、Maintain_Mt_1の（1-2）にて行った図９の動作と逆の動作を使って、ロボットd(ie1)の位置をMaintain_Mt_1実行前のロボットie1の位置に戻す。

Maintain_Mt_1とMaintain_Mt_2では、Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行している。これにより、２つのボイドはTransform_to_Permutation_Mode実行により生成されたロボット構造Rcore内のボイドの初期位置に戻されることになる。なお、Reverse_Void_Toについては［ボイドの制御］にて後述する。

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

このような動作を保証するために、各個ロボット位置入れ替え制御においては、ロボット構造Rcore内にある２つのボイドを移動させつつ２つのボイドによって供給される空隙空間を使用してロボットの位置入れ替えを行う。

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

[Each_Robot_Position_Exchange]
(1)Calculate_Path_From_Origin_To_Destinationを実行する。
(2)Robot_Exchangeを実行する。
(3)Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行する。

(1)のCalculate_Path_From_Origin_To_Destinationでは、ロボット構造Rcore内におけるロボットoriginの位置からロボットdestinationの位置までをつなぐ経路を計算する（以下、入れ替え経路生成という）。(2)のRobot_Exchangeでは、ロボットoriginの位置とロボットdestinationの位置の入れ替えを行う（以下、入れ替え制御という）。(3)のReverse_Void_To では、２つのボイドを、本Each_Robot_Position_Exchange開始時の元の位置（つまり、Transform_to_Permutation_Mode実行により生成されたロボット構造Rcore内のボイドの初期位置）に戻す。

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

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

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

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

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

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

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

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

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

[ロボットの移動と位置の記録]
Move_Module(i, vx, vy, vz)はロボットiをX方向、Y方向、Z方向のうち１つの方向に所定の量（vx, vy, vz）だけ移動させるものであり、Record_Historyは、先述した通り、時刻カウンタtpにおける各ロボットiの位置を記録するものである。

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

[Record_History]
(1)(permutation_x[tp][i], permutation_y[tp][i], permutation_z[tp][i])に各ロボットiの現時点の位置を格納し、tpをインクリメントする。

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

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

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

（線要素j）
j=0のとき、t=0〜corner[0]-1の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置。
0< j< corner_numのとき、t = corner[j-1]〜corner[j]-1の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置。
j=corner_numかつcorner[j] < perm_num-2のとき、t=corner[j-1]〜perm_num-2の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置。
（最終線要素）
t = perm_num-2〜perm_num-1の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置
（角要素j）
t=corner[j]-1〜corner[j]の（path_perm_x[t], path_perm_y[t], path_perm_z[t]）に格納されているロボット位置
以下、第１の入れ替え制御処理手順Robot_Exchange_1、第２の入れ替え制御処理手順Robot_Exchange_2について説明する。まず、Robot_Exchange_1について説明する。Robot_Exchange_1は、先述の(α)ロボットoriginをロボットdestinationの位置まで移動させる制御を実現するものであり、ロボットoriginをそれが存在する線要素、角要素、最終線要素において、それぞれの要素の終点位置にあるロボットとロボットoriginの位置を入れ替えていくことでなされる。実際の制御方法は、以下の通りである。

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

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

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

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

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

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

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

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

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

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

線要素での入れ替え制御の処理は以下の通りである。

[Line_Perm(i_l, _flg)]
(1)線要素i_lの始点位置番号を_s, 終点位置番号を_gとし、Line_Perm_By_Position(_s, _g, _flg)を実行する。

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

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

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

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

以下、Rotation_X_Line、Rotation_Y_Line、Rotation_Z_Lineについて、図１１を参照しながら、説明する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

＜第一実施形態に係る行動制御システム１００＞
第一実施形態に係る行動制御システム１００は、以上に説明した各処理によって構成される。全体動作（ホモジニアス隊列制御とロボット位置入れ替え制御）について以下にまとめる。
（ホモジニアス隊列制御）
(1)非特許文献６の手法を用いて開始位置の集合Sから目標位置の集合Gまでホモジニアス隊列変形を行う。
（ロボット位置入れ替え制御）
(2)ロボット位置入れ替え制御処理手順（Robot_Position_Permutation）を実行する。つまり、すべてのロボットについて、各個ロボットの位置が目標位置に到達している状態になるまで、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）を実行する。なお、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）を実行する前後で、所定の処理を行う。この処理により、ロボット同士の非接続が発生することなく、各個ロボット位置入れ替えを実行できる。

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

図１５は第一実施形態に係る行動制御システム１００の機能ブロック図を、図１６は行動制御システム１００の処理フローの一部であるロボット位置入れ替え制御の例を示す。

行動制御システム１００は、図１５に示すように、ホモジニアス隊列制御装置９００と、制御対象物位置入れ替え制御装置１１０と、記憶部１４０と、通信部１５０と、入力部１６０とを含む。制御対象物位置入れ替え制御装置１１０は、入れ替え用構造生成部１２０と、各個制御対象物位置入れ替え制御部１３０を含む。入れ替え用構造生成部１２０は、入れ替えモード変換部１２１と、回復部１２８とを含む。各個制御対象物位置入れ替え制御部１３０は、事前処理部１３２と、入れ替え経路生成部１３３と、入れ替え制御部１３４と、ボイド初期位置移動部１３５と、事後処理部１３６とを含む。

本実施形態では、行動制御システム１００は、p(pは、16以上の整数の何れかであって、8の倍数)台のロボットの行動を制御し、p台のロボットの内の１つのロボット上に実装される。
＜入力部１６０＞
入力部１６０には、p個の開始位置の集合S={(Xr0[0],Yrs[0]),(Xr0[1],Yrs[1]),…,(Xr0[p-1],Yrs[p-1])}及びp個の目標位置の集合G={(Xre[0],Yre[0]),(Xre[1],Yre[1]),…,(Xre[p-1],Yre[p-1])}が入力され、記憶部１４０に記憶される。
＜記憶部１４０＞
記憶部１４０には、入力部１６０から入力されたp個の開始位置の集合Sとp個の目標位置の集合Gの各位置が記憶されている。その他処理の過程で生成される情報のうち、記憶する必要がある情報については適宜記憶される。
＜通信部１５０＞
行動制御システム１００が実装されているロボットも含め、すべてのロボットは、通信部１５０を介して、三次元平面上の上下左右前後方向(以下「６方向」ともいう)において隣接する他のロボットと通信することができる。
＜ホモジニアス隊列制御装置９００＞
開始位置の集合Sから目標位置の集合Gまで非特許文献６の手法を用いてホモジニアス隊列変形を行う。
＜制御対象物位置入れ替え制御装置１１０＞
制御対象物位置入れ替え制御装置１１０は、ロボット位置入れ替え制御処理手順Robot_Position_Permutationを実行して、すべてのロボットについて目標位置に到達するように制御する（Ｓ１１０）。

以下、図１６に従い制御対象物位置入れ替え制御装置１１０の動作について説明する。

制御対象物位置入れ替え制御装置１１０は、入れ替えモード変換処理手順（Transform_to_Permutation_Mode）を実行することにより、G＋Ms-Mt構造を生成する。次に、ロボットiの位置が移動先ロボット位置d(i)に一致しない（i=d(i)でない）最小のiについて、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）を行う。なお、各個ロボット位置入れ替え制御処理手順（Each_Robot_Position_Exchange）の実行の前後で事前処理手順（Maintain_Mt_1）及び事後処理手順（Maintain_Mt_2）を実行する。最後に、回復処理手順（Recover_to_Goal）を実行することにより、入れ替えモード変換処理手順（Transform_to_Permutation_Mode）の実行前の構造に戻す。

以下、入れ替え用構造生成部１２０と各個制御対象物位置入れ替え制御部１３０による処理の詳細について説明する。

入れ替えモード変換部１２１は、入れ替えモード変換処理手順Transform_to_Permutation_Modeを実行して、G＋Ms-Mt構造を生成する（Ｓ１２１）。このとき、ロボット構造Rcore（つまり、G＋Ms−Mt 構造内に含まれるp台のロボットから位置単位Mt に含まれる6 台のロボットを除いたp-6 台のロボットで構成される構造）には、２つのボイドが含まれる。これらの２つのボイドの現在位置をロボット構造Rcore内のボイドの初期位置という。

以下、現在の位置と目標位置が一致していないロボットについて入れ替え処理を実行する。

まず、各個ロボット位置入れ替え制御の実行前に、事前処理部１３２は、事前処理手順Maintain_Mt_1を実行して、入れ替え対象のロボットが位置単位Mt内にある場合、当該入れ替え対象ロボットと位置単位Mt’内の任意のロボットとの位置入れ替えを行う（Ｓ１３２）。ここで、入れ替え対象のロボットとは、上記ロボットと当該ロボットの目標位置にあるロボットのことである。

以下、入れ替え対象位置1を、事前処理部１３２により上記ロボットの位置が入れ替えられなかった場合は上記ロボットの現在位置、事前処理部１３２により入れ替えられた場合は入れ替え対象となった位置単位Mt’内のロボットがあった位置とする。また、入れ替え対象位置2を、事前処理部１３２により上記ロボットの目標位置にあるロボットの位置が入れ替えられなかった場合は上記ロボットの目標位置、事前処理部１３２により入れ替えられた場合は入れ替え対象となった前記位置単位Mt’内のロボットがあった位置とする。

さらに、入れ替え対象位置1にあるロボットを入れ替え元のロボットorigin、入れ替え対象位置2にあるロボットを入れ替え先のロボットdestinationという。

Ｓ１３３〜Ｓ１３５までの入れ替え処理は、ロボット構造Rcoreに含まれる２つのボイドを用いて、ロボット構造Rcore内で実行される。

入れ替え経路生成部１３３は、入れ替え経路生成処理手順Calculate_Path_From_Origin_To_Destinationを実行して、入れ替え元のロボットoriginと入れ替え先のロボットdestinationと間の入れ替え経路を生成する（Ｓ１３３）。入れ替え元のロボットoriginと入れ替え先のロボットdestinationと間の入れ替え経路は、図１０に示すように、複数の線要素と角要素、１つの最終線要素に分割される。この入れ替え経路は、ロボット構造Rcoreに含まれる。

入れ替え制御部１３４は、入れ替え制御処理手順Robot_Exchangeを実行して、入れ替え元のロボットoriginと入れ替え先のロボットdestinationを入れ替える（Ｓ１３４）。まず、第１の入れ替え制御処理手順Robot_Exchange_1を実行して、ロボットoriginをロボットdestinationの位置（入れ替え位置2）まで移動させる。その際、入れ替え経路を分割することで生成された線要素、角要素ごとに入れ替え制御（Line_Perm, Corner_Perm）を行っていく。各要素での入れ替え処理（Line_Perm, Corner_Perm）の中で、まずNavigate_Voidを実行し、ロボット構造Rcoreに含まれる２つのボイドを適切な位置に移動させる。また、線要素（Line_Perm）の場合は、Move_ModuleとRecord_Historyを実行することによりロボットoriginを移動させていく。このようにして、ロボットoriginをロボットdestinationの位置（入れ替え位置2）に徐々に近づけていく。つまり、Move_ModuleとRecord_Historyの実行を１セットとして、ロボットoriginをロボットdestinationの位置（入れ替え位置2）に近づけていく。

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

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

ボイド初期位置移動部１３５は、ボイド初期位置移動処理手順Reverse_Void_To(void_ini_x[0], void_ini_y[0], void_ini_z[0], void_ini_x[1], void_ini_y[1], void_ini_z[1])を実行して、入れ替え経路生成処理手順Calculate_Path_From_Origin_To_Destinationを実行する前のボイドの位置（ロボット構造Rcore内のボイドの初期位置）に２つのボイドを戻す（Ｓ１３５）。

各個ロボット位置入れ替え制御の実行後、事後処理部１３６は、事後処理手順Maintain_Mt_2を実行して、Maintain_Mt_1の実行により入れ替えを行ったロボットの位置関係を元の位置関係に戻す（Ｓ１３６）。

各個制御対象物位置入れ替え制御部１３０は、時刻ステップごとに、すべてのロボットが目標位置に到達しているかどうかを判定し（Ｓ１３７）、すべてのロボットが目標位置に到達しているときは、処理を終了する。そうでないときは、処理を継続する。つまり、ロボットiの位置が移動先ロボット位置d(i)に一致しない（i=d(i)でない）最小のiについて、Ｓ１３２〜Ｓ１３６の処理を繰り返す。

回復部１２８は、回復処理手順Recover_to_Goalを実行して、Transform_to_Permutation_Modeの実行前の構造に戻す処理をする（Ｓ１２８）。

以上に述べた処理Ｓ１１０の各ステップを毎時刻ステップごとに行う。
＜効果＞
このような構成により、多数のロボットの存在を考慮しつつ、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ障害物のある環境にてホモジニアス隊列変形動作を行った後に各ロボットを個々の目標位置に移動させることによりヘテロジニアス隊列制御を実現する場合のように、立方体形状のロボット位置の入れ替え開始前後のロボット隊列が同一という条件下で、各個のロボットをロボット同士の面せん断動作によってロボット隊列内の適切な位置にロボット台数の2乗に比例した実行時間で配置させることができる。

これにより、カメラを搭載したロボット、車輪を搭載したロボット等各ロボット個々に異なる役割があって、各個のロボットをロボット隊列内の適切な位置に配置させつつ、隊列の形状を入れ替え前後で維持制御するといった運用が可能となる。また、すべてのロボットに必要なすべての機能を実装する必要がなくなる。
＜変形例＞
本実施形態では、各格子（マス）は、立方体であるが、他の形状であってもよい。格子は左右方向、上下方向及び前後方向に連続して配置される。また、各格子は左右方向で他の二つの格子と隣接し、上下方向で他の二つの格子と隣接し、前後方向で他の二つの格子と隣接する。言い換えると、各格子は、ロボットの移動できる方向と同じ方向においてのみ、他の格子と隣接する。この条件を満たせば、各格子はどのような形状であってもよい。また、「直交」とは、厳密に「垂直に交わること」を意味しなくともよく、例えば、各格子は、平行六面体であってもよく、各格子が他の二つの格子と隣接する方向の一方を上下方向とし、他方を左右方向とすればよく、上下方向及び左右方向とからなる平面に対して平行でない方向を前後方向とすればよい。

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

本実施形態では、行動制御システム１００が、p台のロボットの内の１つのロボット上に実装される例を示したが、コンピュータ上の制御対象物に対して行動制御を行ってもよい。

本実施形態では、ロボット単位は、２×２×２の８台のロボットからなるが、M×N×Q台のロボットからなってもよい。ただし、M、N及びQはそれぞれ２以上の整数の何れかである。なお、ロボットの台数pはロボット単位に含まれるロボットの台数M×N×Qの2以上の倍数とする。つまり、p=M×N×Q×Rであり、Rは2以上の整数の何れかである。このような状態で移動を行った場合にも、２×２×２のロボット単位のときと同様に、ロボットの群れの中のいずれのロボットが一台欠けても、各ロボットはお互いに一つの辺で接しあう位置関係を崩さずに済む。なお、ロボット単位のことを一般に制御対象物単位という。

もちろん、行動制御システム１００を構成する制御対象物位置入れ替え制御装置１１０単体で実行する（つまり、ホモジニアス隊列制御装置９００とともに実行しない）ことも可能である。この場合、入れ替え開始前の位置の集合と入れ替え終了後の位置の集合は同一という条件下で入れ替えていくことになる。

また、制御対象物位置入れ替え制御装置１１０単体で実行する場合、第一実施形態の説明で用いた目標位置の集合G、面Ss、位置単位Ms、Ms’、Mt、Mt’は次のようにすればよい。目標位置の集合Gは、入れ替え開始前の位置の集合（入れ替え終了後の位置の集合）Gとする。入れ替え開始前の位置の集合Gを構成するR個の位置単位の中には必ず入れ替え開始前の位置の集合Gの外部Pと接する面を持つ位置単位があるので、そのような面をSsとする。面Ssに接する入れ替え開始前の位置の集合Gの外部P内の位置単位をMs、面Ssに接する入れ替え開始前の位置の集合G内の位置単位をMs’とする。位置単位Ms’から入れ替え開始前の位置の集合G内にてマンハッタン距離が最も大きい集合G内の位置単位をMtとし、Mtに接しMtよりMs’からのマンハッタン距離が１だけ小さい集合G内の位置単位をMt’とする。また、G＋Ms−Mt構造は、入れ替え開始前の位置の集合Gと位置単位Msから構成される構造であり、位置単位Mtには、位置単位Mt’に接しない位置に２つのボイドが接する形で含まれており、位置単位Msには位置単位Ms’に接する位置にのみ最大４つの制御対象物が存在する構造のことをいう。制御対象物構造Rcore は、G＋Ms−Mt 構造内に含まれるp台の制御対象物から位置単位Mt に含まれる6 台の制御対象物を除いたp-6 台の制御対象物で構成される構造のことをいう。

なお、制御対象物位置入れ替え制御装置１１０をホモジニアス隊列制御装置９００とともに実行（つまり、行動制御システム１００全体で実行）する場合、制御対象物位置入れ替え制御装置１１０単体で実行する場合と異なり、Pを外部とする代わりに、外部の一部であるパスとする。
＜その他の変形例＞
本発明は上記の実施形態及び変形例に限定されるものではない。例えば、上述の各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。その他、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
＜プログラム及び記録媒体＞
また、上記の実施形態及び変形例で説明した各装置における各種の処理機能をコンピュータによって実現してもよい。その場合、各装置が有すべき機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、上記各装置における各種の処理機能がコンピュータ上で実現される。

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

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

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

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

Claims (3)

1. M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、入れ替え開始前と入れ替え終了後の位置の集合（以下、集合Gという）が同一になるという条件のもと、p台の制御対象物の各々が前記集合Gの中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御装置であって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置の集合を位置単位とし、前記集合Gを構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合GはR個の位置単位からなる一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   面Ssを、前記集合Gを構成するR個の位置単位中のある位置単位が前記集合Gの外部Pと接する面、
   位置単位Msを、前記面Ssに接する前記集合Gの外部P内の位置単位、
   位置単位Ms’を、前記面Ssに接する前記集合G内の位置単位、
   位置単位Mtを、前記位置単位Ms’から前記集合G内にてマンハッタン距離が最も大きい前記集合G内の位置単位、
   位置単位Mt’を、前記位置単位Mtに接し前記位置単位Mtより前記位置単位Ms’からのマンハッタン距離が１だけ小さい前記集合G内の位置単位、
   G＋Ms−Mt構造を、前記集合Gと前記位置単位Msから構成される構造であり、前記位置単位Mtには前記位置単位Mt’に接しない位置に２つのボイドが接する形で含まれており、前記位置単位Msには前記位置単位Ms’に接する位置にのみ最大４つの制御対象物が存在する構造、
   制御対象物構造Rcoreを、前記G＋Ms−Mt 構造内に含まれるp台の制御対象物から位置単位Mt に含まれる6 台の制御対象物を除いたp-6 台の制御対象物で構成される構造とし、
   制御対象物i(iは1≦i≦pを満たす整数)が到達すべき前記集合Gの中での位置を目標位置i、前記制御対象物iの前記集合Gの中での現在の位置を現在位置iとし、
   前記G＋Ms-Mt構造を生成する入れ替えモード変換部と、
   制御対象物構造Rcore内のボイドの初期位置を、前記入れ替えモード変換部により前記G＋Ms-Mt構造が生成された際に前記制御対象物構造Rcoreに含まれる２つのボイドの位置とし、
   前記現在位置iが前記目標位置iと一致しない場合、前記制御対象物iまたは前記目標位置iにある別の制御対象物j(jは1≦j≦pを満たす整数、j≠i)のいずれかが前記位置単位Mt内にある場合、前記位置単位Mt内にある制御対象物を前記位置単位Mt’内の制御対象物と位置入れ替えを行う事前処理部と、
   入れ替え対象位置1を、前記事前処理部により前記制御対象物iの位置が入れ替えられなかった場合は前記現在位置i、前記事前処理部により前記制御対象物iの位置が入れ替えられた場合は前記制御対象物iの位置が入れ替え対象となった前記位置単位Mt’内の制御対象物があった位置、
   入れ替え対象位置2を、前記事前処理部により前記制御対象物jの位置が入れ替えられなかった場合は前記目標位置i、前記事前処理部により前記制御対象物jの位置が入れ替えられた場合は前記制御対象物jの位置が入れ替え対象となった前記位置単位Mt’内の制御対象物があった位置とし、
   前記入れ替え対象位置1から前記入れ替え対象位置2に至る、前記制御対象物構造Rcore内の入れ替え経路を生成する入れ替え経路生成部と、
   前記２つのボイドを用いて、前記入れ替え経路に沿って、前記入れ替え対象位置1にいる制御対象物と前記入れ替え対象位置2にいる制御対象物とを入れ替える入れ替え制御部と、
   前記２つのボイドを前記初期位置に戻すボイド初期位置移動部と、
   前記事前処理部により前記制御対象物iの位置が入れ替えられた場合は、前記入れ替え対象位置1にいる制御対象物と前記現在位置iにいる制御対象物とを入れ替え、前記事前処理部により前記制御対象物jの位置が入れ替えられた場合は、前記入れ替え対象位置2にいる制御対象物と前記目標位置iにいる制御対象物とを入れ替える事後処理部と、
   前記制御対象物i(iは1≦i≦pを満たす整数)が前記目標位置iにそれぞれ到達した後、前記入れ替えモード変換部により前記G＋Ms-Mt構造を生成される前の構造に戻す回復処理部とを含む
   制御対象物入れ替え制御装置。
2. M,N,Qをそれぞれ2以上の整数の何れかとし、Rを2以上の整数の何れかとし、pをM×N×Q×Rとし、制御対象物入れ替え制御装置が、入れ替え開始前と入れ替え終了後の位置の集合（以下、集合Gという）が同一になるという条件のもと、p台の制御対象物の各々が前記集合Gの中の指定された位置に配置されるように、前記制御対象物の位置を入れ替える制御対象物入れ替え制御方法であって、
   第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、M×N×Q個の位置の集合を位置単位とし、前記集合Gを構成する各位置は、前記第一方向〜第六方向の少なくとも何れかの方向において他の位置と隣接し、前記集合GはR個の位置単位からなる一塊の任意の形状を成し、
   前記制御対象物は、当該制御対象物の３次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、３次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
   面Ssを、前記集合Gを構成するR個の位置単位中のある位置単位が前記集合Gの外部Pと接する面、
   位置単位Msを、前記面Ssに接する前記集合Gの外部P内の位置単位、
   位置単位Ms’を、前記面Ssに接する前記集合G内の位置単位、
   位置単位Mtを、前記位置単位Ms’から前記集合G内にてマンハッタン距離が最も大きい前記集合G内の位置単位、
   位置単位Mt’を、前記位置単位Mtに接し前記位置単位Mtより前記位置単位Ms’からのマンハッタン距離が１だけ小さい前記集合G内の位置単位、
   G＋Ms−Mt構造を、前記集合Gと前記位置単位Msから構成される構造であり、前記位置単位Mtには前記位置単位Mt’に接しない位置に２つのボイドが接する形で含まれており、前記位置単位Msには前記位置単位Ms’に接する位置にのみ最大４つの制御対象物が存在する構造、
   制御対象物構造Rcoreを、前記G＋Ms−Mt 構造内に含まれるp台の制御対象物から位置単位Mt に含まれる6 台の制御対象物を除いたp-6 台の制御対象物で構成される構造とし、
   制御対象物i(iは1≦i≦pを満たす整数)が到達すべき前記集合Gの中での位置を目標位置i、前記制御対象物iの前記集合Gの中での現在の位置を現在位置iとし、
   前記制御対象物入れ替え制御装置が、前記G＋Ms-Mt構造を生成する入れ替えモード変換ステップと、
   制御対象物構造Rcore内のボイドの初期位置を、前記入れ替えモード変換ステップにより前記G＋Ms-Mt構造が生成された際に前記制御対象物構造Rcoreに含まれる２つのボイドの位置とし、
   前記制御対象物入れ替え制御装置が、前記現在位置iが前記目標位置iと一致しない場合、前記制御対象物iまたは前記目標位置iにある別の制御対象物j(jは1≦j≦pを満たす整数、j≠i)のいずれかが前記位置単位Mt内にある場合、前記位置単位Mt内にある制御対象物を前記位置単位Mt’内の制御対象物と位置入れ替えを行う事前処理ステップと、
   入れ替え対象位置1を、前記事前処理ステップにより前記制御対象物iの位置が入れ替えられなかった場合は前記現在位置i、前記事前処理ステップにより前記制御対象物iの位置が入れ替えられた場合は前記制御対象物iの位置が入れ替え対象となった前記位置単位Mt’内の制御対象物があった位置、
   入れ替え対象位置2を、前記事前処理ステップにより前記制御対象物jの位置が入れ替えられなかった場合は前記目標位置i、前記事前処理ステップにより前記制御対象物jの位置が入れ替えられた場合は前記制御対象物jの位置が入れ替え対象となった前記位置単位Mt’内の制御対象物あったが位置とし、
   前記制御対象物入れ替え制御装置が、前記入れ替え対象位置1から前記入れ替え対象位置2に至る、前記制御対象物構造Rcore内の入れ替え経路を生成する入れ替え経路生成ステップと、
   前記制御対象物入れ替え制御装置が、前記２つのボイドを用いて、前記入れ替え経路に沿って、前記入れ替え対象位置1にいる制御対象物と前記入れ替え対象位置2にいる制御対象物とを入れ替える入れ替え制御ステップと、
   前記制御対象物入れ替え制御装置が、前記２つのボイドを前記初期位置に戻すボイド初期位置移動ステップと、
   前記制御対象物入れ替え制御装置が、前記事前処理ステップにより前記制御対象物iの位置が入れ替えられた場合は、前記入れ替え対象位置1にいる制御対象物と前記現在位置iにいる制御対象物とを入れ替え、前記事前処理ステップにより前記制御対象物jの位置が入れ替えられた場合は、前記入れ替え対象位置2にいる制御対象物と前記目標位置iにいる制御対象物とを入れ替える事後処理ステップと、
   前記制御対象物i(iは1≦i≦pを満たす整数)が前記目標位置iにそれぞれ到達した後、前記制御対象物入れ替え制御装置が、前記入れ替えモード変換ステップにより前記G＋Ms-Mt構造を生成される前の構造に戻す回復処理ステップとを含む
   制御対象物入れ替え制御方法。
3. 請求項１に記載の制御対象物入れ替え制御装置としてコンピュータを機能させるためのプログラム。