JP2016146159A - 行動制御システム、その方法及びプログラム - Google Patents
行動制御システム、その方法及びプログラム Download PDFInfo
- Publication number
- JP2016146159A JP2016146159A JP2015159322A JP2015159322A JP2016146159A JP 2016146159 A JP2016146159 A JP 2016146159A JP 2015159322 A JP2015159322 A JP 2015159322A JP 2015159322 A JP2015159322 A JP 2015159322A JP 2016146159 A JP2016146159 A JP 2016146159A
- Authority
- JP
- Japan
- Prior art keywords
- robot
- positions
- move
- target
- adjacent
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 380
- 230000009471 action Effects 0.000 title claims abstract description 118
- 230000015572 biosynthetic process Effects 0.000 claims abstract description 148
- 230000008569 process Effects 0.000 claims description 336
- 238000001125 extrusion Methods 0.000 claims description 211
- 238000012545 processing Methods 0.000 claims description 159
- 238000005755 formation reaction Methods 0.000 claims description 145
- 239000011800 void material Substances 0.000 claims description 137
- 230000007704 transition Effects 0.000 claims description 36
- 238000011161 development Methods 0.000 claims description 33
- 230000009466 transformation Effects 0.000 abstract description 40
- 238000004364 calculation method Methods 0.000 description 43
- 238000004891 communication Methods 0.000 description 21
- 101100425598 Solanum lycopersicum tm-1 gene Proteins 0.000 description 16
- 230000001131 transforming effect Effects 0.000 description 12
- 238000007906 compression Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 230000007423 decrease Effects 0.000 description 6
- 230000000694 effects Effects 0.000 description 6
- 230000006835 compression Effects 0.000 description 5
- 125000005842 heteroatom Chemical group 0.000 description 5
- 238000012986 modification Methods 0.000 description 5
- 230000004048 modification Effects 0.000 description 5
- 238000013519 translation Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 230000001276 controlling effect Effects 0.000 description 4
- 239000000284 extract Substances 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 230000001174 ascending effect Effects 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 240000007594 Oryza sativa Species 0.000 description 1
- 235000007164 Oryza sativa Nutrition 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 235000009566 rice Nutrition 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
まず、行動制御システム及び方法の理論的背景について説明する。以下、行動制御の対象である制御対象物が、ロボットである場合を例に挙げて説明するが、制御対象物は、制御の対象となり得るものであれば、ロボット以外であってもよい。
多数のロボットが協調して開始位置における隊列形成状態から、各ロボットが接した状態を維持しつつ移動を行い、目標位置での隊列形成を行う任務は、例えば図1に例示するような、互いに接する辺同志をスライドさせて移動していくことが可能な正方形型のロボットの使用を想定し、図2に示すように、開始位置の集合Sから目標位置の集合Gまで複数のロボットの移動によって実現するものである。
それぞれのロボットi(iはロボット番号を表す、i=0,1,2,3,…,p-1)の初期位置(開始位置)を(Xr0[i],Yr0[i])とし、目標位置を(Xre[j],Yre[j])(jは目標位置の番号を表す、j=0,1,2,3,…,q-1、p=q+a)とするとき、本問題は、開始位置に配置されたロボットが、目標位置まで移動するための行動計画を求めることと定義できる。
行動主体は部屋に配置されている各ロボットとなる。ロボットiの行動aは、静止、上下左右方向への1格子分の移動の計5種類のうちのいずれかを取る。例えば、a[i]∈{0,1,2,3,4}として、
0:静止
1:二次元平面上でX座標値が増える方向(図2で右方向)に1格子だけ移動する
2:二次元平面上でY座標値が増える方向(図2で下方向)に1格子だけ移動する
3:二次元平面上でX座標値が減る方向(図2で左方向)に1格子だけ移動する
4:二次元平面上でY座標値が減る方向(図2で上方向)に1格子だけ移動する
とする。
このような任務環境における状態空間は、ロボット数×2の次元数の状態を持ち、かつ選択可能な行動数は、ロボットの行動(=5通り)のロボット数乗だけ存在する。例えば、ロボット数が50で、部屋の縦横方向の格子数がそれぞれ20であるとすれば状態数は20の100乗個にもなり、探索計算に要する資源の量は膨大なものとなる。さらにロボット数が1台増えるごとに、その状態数は400倍増加していくことになる。
本実施形態では、これらの計算負荷の問題を解決するための方策の一つとして、ロボットの動作の過程を3つの段階にわけて考える。一つ目は、目標位置の集合Gの隊列を形成するロボットを開始位置の集合Sから位置Psを経て開始位置の集合Sの外に押し出すロボット押し出し過程であり、二つ目は、押し出されたロボット群を、目標位置の集合Gの隊列形状に遷移させるための中間形態に変形する中間遷移過程であり、三つ目はその中間形態から、目標位置の集合Gの隊列形態に展開動作を行う最終展開過程である。これらのうち、一つ目の過程でのみ動的計画法による探索計算を行う。
これらの一つ目のロボット押し出し過程にて、本実施形態では、ボイド制御の考え方を導入する。まず、ここでいうボイドとは、あるロボットが別の位置に移動した後に、元いた位置にできる空隙のことである。別の言い方をすると、ボイドとは、ロボットの移動する方向と反対の方向に移動する仮想的な存在である。こうした群ロボットの隊列形成問題においては、複数のロボットの動作に着目するがゆえに、その探索計算量が爆発してしまうが、視点を変えて、ボイドの動きに着目すれば、多数のロボットの動作計画の問題を単一のボイドの動作計画として考えることができ、探索計算負荷の軽減に適している。
ロボットの押し出し過程として、本実施形態では、2通りの過程を用意し、そのいずれかを使用することにする。一つ目は、横方向押し出し過程であり、二つ目は縦方向押し出し過程である。
ロボット押し出し過程として、横方向押し出し過程を使用した場合のロボットの動作の様子を図2及び図6〜10に示す。横方向押し出し過程は図2、6及び7である。横方向押し出し過程では、位置Psからロボットをある数だけ押し出す動作と、押し出されたロボットを右方向(Xの正方向)に1マスずらす動作の繰り返しでロボットを開始位置の集合Sの外に押し出す。このように押し出されたロボットの隊形(以下「押し出し隊列」ともいう)は、目標位置の集合Gに位置するロボットの隊形(以下「目標隊列」ともいう)へ容易に変形することができる。
(1) 各目標位置(Xre[j],Yre[j])をG_line(i_y)に振り分ける。例えば、図2の場合、G_line(1)={(0,1)}、G_line(2)={(0,2)}、G_line(3)={(0,3)}、G_line(4)={(-1,4),(0,4),(1,4),(8,4),(9,4),(10,4),(11,4)}、…、G_line(13)={(-4,13),(-3,13),(3,13),(4,13),}である。
(2) i_y←1とする。カウンタ変数cnt←0とする。
(3) G_line(i_y)に属する要素があれば、そのもっともX座標の小さな目標位置jのjp[j]の値をcntとし(jp[j]←cnt)、cntをインクリメントする(cnt←cnt+1)。さらに、変数x_min[i_y]の値を目標位置jのx座標値とし(x_min[i_y]←Xre[j])、(4)へ移行する。要素がなければ(5)へ移行する。例えば、図2の場合、x_min[1]←0,x_min[2]←0,x_min[3]←0,x_min[4]←-1,…,x_min[13]←-4とする。
(5) 現、i_y値を変数y_maxに記録し(6)へ移行する。例えば、図2の場合、y_max←14とする。
(6) j=0〜q-1の全ての値について、Yp[jp[j]]←Yre[j]とする。この処理により、押し出し隊列内位置のy座標値Yp[jp[j]]を目標位置jのy座標値Yre[j]とする。
(8) Xp[0]←0とする。G_line(1)内に、一つしか要素がないなら(10)へ移行する。そうでないなら、(9)へ移行する。なお、Xp[0]は位置Ps(0,0)にy方向において隣接する、目標位置の集合G側のマスであり、位置Psの定義上、Xp[0]←0となる。なお、位置Psは、開始位置の集合と目標位置の集合とが接している位置であって、開始位置の集合側の位置であって、X座標が最も小さい位置であり、原点(0,0)である。
(9) G_line(1)の要素のうち、cnt+1番目にX座標が小さい目標位置があるとき、それを目標位置jとして、Xp[jp[j]]←cntとする。cntをインクリメントし(9)を繰りかえす。ないとき(10)へ移行する。
(11) G_line(i_y)の要素数n[i_y]が、G_line(i_y-1)の要素数n[i_y-1]より大きいとき、G_line(i_y)の要素のうち、1番目にX座標が小さい目標位置を目標位置jとして、Xp[jp[j]]←Xp[jp[j]-n[i_y-1]]とする。左端を揃える処理に相当する。G_line(i_y)の要素数n[i_y]が、G_line(i_y-1)の要素数n[i_y-1]以下の時、G_line(i_y)の要素のうち、1番目にX座標が小さい目標位置を目標位置jとして、Xp[jp[j]]←Xp[jp[j]-1]-(n[i_y]-1)とする。右端を揃える処理に相当する。
(12) G_line(i_y)の要素のうち、目標位置jの次にX座標が小さい目標位置があれば、それを次の目標位置jとして、Xp[jp[j]]←Xp[jp[j]-1]+1とし、(12)を繰りかえす。なければ、(13)へ移行する。
(13) i_yをインクリメントし、i_y=y_maxならば終了する。そうでなければ、(11)へ移行する。(6)〜(13)の処理を行うことで、図7の押し出し隊列におけるjp[j]の値は図11のようになる。
<押し出し順序計算処理>
(1) Xp_max←Xp[q-1],Yp_max←Yp[q-1]とする(図11参照)。cnt←1とし、jp[j]=q-1となるjをjk[0]の値に格納する(図12参照)。
(2) Xp_max=Xp[jp_next]かつYp_max-1=Yp[jp_next]となるjp_next値を探す。つまり、押し出し隊列において位置(Xp_max,Yp_max)に位置するロボットのYの負方向にロボットjp_nextが存在するか否かを判定する。
見つかれば、jp[j]=jp_nextとなるjをjk[cnt]に格納する(jk[cnt]←j)。Xp_max←Xp[jp_next]、Yp_max←Yp[jp_next]とし、cntをインクリメントする。
なければ、Xp_max-1=Xp[jp_next]を満たすjp_nextの中で、最もYp[jp_next]値の大きいものを選び、jp[j]=jp_nextとなるjをjk[cnt]に格納する(jk[cnt]←j)。Xp_max←Xp[jp_next]、Yp_max←Yp[jp_next]とし、cntをインクリメントする。
(3) cnt=qでなければ、(2)を繰り返す。であれば終了。
<押し出し制御処理>
(1) k←0とする。
(2) 位置Psにあるロボットをロボットiとし、robot_target_index[i]←jk[k]とする。
(3) k=0のときは、なにもせず(4)へ移行する。
k≠0のときは、Xp[jp[jk[k]]]=Xp[jp[jk[k-1]]]であるかどうかを判定する。
Xp[jp[jk[k]]]=Xp[jp[jk[k-1]]]のとき、(4)へ移行する。
Xp[jp[jk[k]]]≠Xp[jp[jk[k-1]]]のとき、開始位置の集合S内にない全てのロボットをa=1方向に1マス移動させて、(4)へ移行する。
(5) (4)で生じたボイドの位置(ロボットi_pushed_2の元あった位置)を位置void_endとする。
(6) 位置void_endを、ロボットi_pushed_1とロボットi_pushed_2以外のS内のロボットで埋めていくことで、もっとも位置void_endから離れた位置のロボットを現在位置より、1マス分位置void_endに近づける。
(8) xk←0とする。Xp_max←Xp[q-1],Yp_max←Yp[q-1]とする。
(9) xk=Xr[i]となるS内にないロボットiをひとつ選択する。
(10) (9)で選択されたロボットiのy座標Yr[i]とYp[jp[robot_target_index[i]]]を比較し、等しくない場合は、Xr[i]≧xkとなる全てのS外のロボットをa=2方向に1マス移動させ(9)へ移行する。等しい場合は、xkをインクリメントし(11)へ移行する。
(11) xk>Xp_maxでなければ(9)へ移行し、そうであれば終了する。この(8)〜(11)の処理により、図6の隊列は、図7の押し出し隊列に変形する。
<ボイド制御処理Robot_Void_Control>
(1)前回、本処理Robot_Void_Controlが実行された際のi_tail値を変数pre_i_tailに格納する(pre_i_tail←i_tail)。本処理が実行されるのがこれが最初ならば、pre_i_tail←-1とする。
(2)各ロボットが位置void_endからどれだけの距離離れているかを計算する。以下、この処理をロボット序列計算処理Robot_Joretu_Decisionともいう。ロボット序列計算処理Robot_Joretu_Decisionでは、各位置の序列変数値Joretu[Xr][Yr]を計算する。ロボット序列計算処理Robot_Joretu_Decisionについては後述する。
a_tail=1のとき:void_startのX座標を2×Xr_unit[j_start]+1、Y座標をYr[i_tail]とする。
a_tail=2のとき:void_startのX座標をXr[i_tail]、Y座標を2×Yr_unit[j_start]+1、とする。
a_tail=3のとき:void_startのX座標を2×Xr_unit[j_start]、Y座標をYr[i_tail]とする。
a_tail=4のとき:void_startのX座標をXr[i_tail]、Y座標を2×Yr_unit[j_start]、とする。
Xr[i1]=2×Xr_unit[j]
Yr[i1]=2×Yr_unit[j]
Xr[i2]=2×Xr_unit[j]+1
Yr[i2]=2×Yr_unit[j]
Xr[i3]=2×Xr_unit[j]
Yr[i3]=2×Yr_unit[j]+1
Xr[i4]=2×Xr_unit[j]+1
Yr[i4]=2×Yr_unit[j]+1
である。
(5)位置void_startから、位置void_endにいたるボイドの移動経路を計算する。以下、この処理をボイド経路計算処理Void_Trajectory_Decisionともいう。
(6) (5)で計算されたボイドの移動経路を、void_endから_void_startに逆にたどり、ボイドを埋めていく。以下、この処理をボイド移動処理Void_Motionともいう。
(ロボット序列計算処理Robot_Joretu_Decision)
ロボット序列計算処理Robot_Joretu_Decisionは、ダイクストラ法などを用いることができる。例えば以下のとおりである。
(1)各ロボットにて、上下左右4方向に隣接するロボットの番号を調べる。ロボットiの行動aの移動方向にロボットi_nextが隣接して存在する場合、next[a][i]変数の値をi_nextに設定する。ロボットが存在しないときは、next[a][i]変数の値をp(ロボットの台数を表す)に設定する。
(2)状態空間内(Xr,Yr)各位置の序列変数値Joretu[Xr][Yr]の値をpに初期化する(Joretu[Xr][Yr]←p)。
(4)void_endの位置と、void_endから、行動a_headの移動方向に隣接してたどれるロボットの位置以外の、各ロボットiの、全行動aについて、next[a][j]の値がpではない場合の、Joretu[Xr[next[a][i]]][Yr[next[a][i]]]の値を調べ、Joretu[Xr[next[a][i]]][Yr[next[a][i]]]の値が-1でない場合で、かつ、その最小値に1を加えた値が、現在のJoretu[Xr[i]][Yr[i]]よりも小さい場合は、その値をJoretu[Xr[i]][Yr[i]]に代入する(Joretu[Xr[i]][Yr[i]]←Joretu[Xr[next[a][i]]][Yr[next[a][i]]]+1)。
(5) (4)の処理にて、Joretu値の更新がなくなるまで、(4)を繰り返す。
ボイド経路計算処理Void_Trajectory_Decisionは、以下のとおりである。
(1)位置void_startの位置をボイド軌道(X,Y)=(void_trj_x[h],void_trj_y[h])(h=0,1,2,3…)の最初の位置とする。
(2)軌道番号h_trj←0とする。
(3)現ボイド軌道位置(void_trj_x[h_trj],void_trj_y[h_trj])から、全方向にて隣接する位置にあるロボットiのうち、そのロボットiの位置における序列値Joretu[Xr[i]][Yr[i]]が、-1以外であって、かつ、Joretu[void_trj_x[h_trj]][void_trj_y[h_trj]]よりも小さいロボットiの位置を次のボイド軌道位置(void_trj_x[h_trj+1],void_trj_y[h_trj+1])とする。その際の、ボイドの移動の方向と同じ行動の番号をボイド行動履歴a_void[h_trj]に格納する。h_trjをインクリメントする。
(4)現ボイド軌道位置が、位置void_endに等しくなるまで(3)を繰り返す。
(ボイド移動処理Void_Motion)
ボイド移動処理Void_Motionは、例えば、以下のとおりである。計算されたボイド軌道に沿って、同じ方向に動くロボットを一度に移動させている。もちろん、一度に移動させず一台ずつ動かしてもよい。
(1)h_trj_buffer←h_trj-1とする。
(2)h_trjをデクリメントし、a_void[h_trj]の値を、a_void_bufferに格納する。
(3)h_trj=0でなければ、h_trjをデクリメントし、a_void[h_trj]=a_void_bufferであるか否かを判定し、a_void[h_trj]=a_void_bufferのとき、(3)を繰りかえす。そうでなければ(a_void[h_trj]≠a_void_bufferのとき)、ボイド軌道(void_trj_x[h_trj_buffer],void_trj_y[h_trj_buffer])から、(void_trj_x[h_trj+1],void_trj_y[h_trj+1])までの位置にあるロボットを皆、a_void_bufferに格納された行動値で移動させ(4)にいく。この処理により、同じ方向に動くロボットを一度に移動させている。h_trj=0のとき、ボイド軌道(void_trj_x[h_trj_buffer],void_trj_y[h_trj_buffer])から、(void_trj_x[0],void_trj_y[0])までの位置にあるロボットをa_void[0]に格納された行動値で移動させ(5)にいく。
(4)h_trjをインクリメントし、(1)に戻る。
(5)ロボットi_tailから行動a_tailの向きに隣接し、位置void_startに至るまでの位置にあるロボットを皆、行動a_tailで移動させる。なお、この移動は、一台ずつではなく、全て一緒に移動させる必要がある。この処理により位置void_startにあるボイドをロボットi_tailの位置に一気に移動させ、ロボットi_tailに隣接するロボットが無くなることを防ぐ。
[縦方向押し出し過程]
ロボット押し出し過程として、縦方向押し出し過程を使用した場合のロボットの動作の様子を図2、図16、図17及び図8〜10に示す。縦方向押し出し過程は図2、図16及び図17である。本過程では、まず、ロボットがY軸に平行な列をなすように押し出し、そのあと押し出される各ロボットが、Y軸に平行な列をなすロボットをガイドにして、それぞれの押し出し隊列内でのY座標の高さまで移動していき、その後X軸正方向に枝を伸ばしていくように押し出し隊列を形成する。以下、処理の詳細について説明する。
(2) yk←1とする。
(3) 位置Psにあるロボットを選択し、ロボットi_pushed_1とする。
(4) ロボットi_pushed_1とX座標が同じで、Y座標が一つ小さな値のロボットをロボットi_pushed_2とする。ロボットi_pushed_1とロボットi_pushed_2を行動a=2の方向に1マス移動させる。同時に、すでに開始位置の位置の集合Sの外のロボットを全て、行動a=2方向に1マス移動させる。ykをインクリメントする。
(5) (4)で生じたボイド(ロボットi_pushed_2の元あった位置)を位置void_endとする。
(7) yk>y_maxのときは、S外の全てのロボットをa=1の方向へX座標一つ分移動し、(8)へ移行する。そうでないならば、(3)へ移行する。なお、yk>y_maxのとき、Paxisと接し、Y軸に平行な列が完成する(図16参照)。
(8) yk←1とする。
(9) 目標位置jのうち、Y座標の値がykであるものの個数をカウントし、cnt_yk[yk]とする。例えば、図16の場合、yk=1のときcnt_yk[1]=1、yk=2のときcnt_yk[2]=1、yk=3のときcnt_yk[3]=1、yk=4のときcnt_yk[4]=7、…、yk=13のときcnt_yk[13]=4である。
(11) X座標が0で、Y座標がykに等しいロボットがあるかどうか判定する。あるなら、Y座標がykに等しいロボットをすべてa=1方向に1マス移動する。
(12) 開始位置の集合Sの外で、X座標が0で、Y座標がykより小さいロボットをa=2方向に1マス移動させる。
(13) 現yk値にて、位置Psから開始位置の集合Sの外へ押し出したロボット数が、cnt_yk[yk]より少ないならば、(14)へ移行する。そうでないなら(10)へ移行する。
(15) (14)で生じたボイド(ロボットi_pushed_2の元あった位置)を位置void_endとする。
(16) 位置void_endを、ロボットi_pushed_1とロボットi_pushed_2以外のS内のロボットで埋めていくことで、もっともを位置void_endから離れた位置のロボットを現在位置より、1マス分位置void_endに近づける。例えば、前述のボイド制御処理Robot_Void_Controlを行う。(10)へ移行する。
(17) yk>y_maxのときは終了。そうでないならば、(9)へ移行する
以上の処理のうち、(1)〜(7)は、図2の開始隊列を図16の隊列に変形するためのものであり、(8)以降は、図16の隊列を図17の押し出し隊列に変形するためのものである。続いて、縦方向押し出し過程の場合の押し出し位置対応付け処理について述べる。
以下の手順の内、(1)〜(6)は横方向押し出し過程における押し出し位置対応付け処理と同様である。
(1) 各目標位置(Xre[j],Yre[j])をG_line(i_y)に振り分ける。
(2) i_y←1とする。カウンタ変数cnt←0とする。
(3) G_line(i_y)に属する要素があれば、そのもっともX座標の小さな目標位置jのjp[j]の値をcntとし(jp[j]←cnt)、cntをインクリメントする(cnt←cnt+1)。さらに、変数x_min[i_y]の値を目標位置jのx座標値とし(x_min[i_y]←Xre[j])、(4)へ移行する。要素がなければ(5)へ移行する。
(4) G_line(i_y)内に、目標位置jの次にX座標が小さな位置があれば、それを目標位置jとする。目標位置jのjp[j]の値をcntとし(jp[j]←cnt)、cntをインクリメントして(cnt←cnt+1)、(4)を繰りかえす。なければ、変数x_max[i_y]の値を目標位置jのx座標値とし(x_max[i_y]←Xre[j])、i_yをインクリメントして(i_y←i_y+1)、(3)に戻る。
(6) j=0〜q-1の全ての値について、Yp[jp[j]]←Yre[j]とする。
(7) カウンタ変数cnt←1とする。
(8) Xp[0]←1とする。G_line(1)内に、一つしか要素がないなら(10)へ移行する。そうでないなら、(9)へ移行する。
(9) G_line(1)の要素のうち、cnt+1番目にX座標が小さい目標位置があるとき、それを目標位置jとして、Xp[jp[j]]←cnt+1とする。cntをインクリメントし(9)を繰りかえす。ないとき(10)へ移行する。
(11) G_line(i_y)の要素のうち、目標位位置のX座標が最小のものをjとし、Xp[jp[j]]←1とする。各行i_yの左端をx=1に揃える。
(12) G_line(i_y)の要素のうち、目標位置jの次にX座標が小さい目標位置があれば、それを次の目標位置jとして、Xp[jp[j]]←Xp[jp[j]-1]+1とし(12)を繰りかえす。なければ、(13)へ移行する。
(13) i_yをインクリメントし、i_y=y_maxならば(14)へ移行する。そうでなければ、(11)へ移行する。
(14) 押し出しの済んだ各ロボットiについて(Xr[i],Yr[i])=(Xp[jp[j]],Yp[jp[j]])となるjを探し、robot_target_index[i]←jとする。
この過程では、まず、目標位置の集合G(図19参照)を中間形態隊列位置(図21参照)に変形させるための処理を計算する。ここでいう中間形態隊列は、ちょうど目標隊列(図10参照)の左右から見えない壁が目標隊列等を圧縮するように迫ってきて、その壁に押されて目標隊列がX座標0の点を通りy方向に平行な軸に向かって、集まった隊列(図8参照)であり、中間形態隊列位置は中間形態隊列を成すロボットの位置である(図19〜21参照)。そのような中間形態隊列は、押し出し隊列(図7及び図17参照)からの変形が容易であり、かつ、中間形態隊列から目標隊列への変形は、この圧縮処理の逆再生であるから、ロボット同士の接続もその変形過程で違反が生じないというメリットがある。
<中間形態隊列生成処理>
(1) 目標位置jのうち、X座標が最大のもののX座標の値を壁のX座標値X_wallに格納する。例えば、図19の場合、X_wall←11とする。
(2) すべての行動a値、目標位置j値についてdest_next[a][j]←−1に初期化する。(Xr_hist[j][0],Yr_hist[j][0])←(Xre[j],Yre[j])とし、t←1とする。
(3) 各目標位置jの行動a=3方向(左方向)に隣接する目標位置の番号を変数dest_next[3][j]に格納する。ある目標位置jについて、左隣の位置が目標位置であれば、その目標位置の番号をdest_next[3][j]に格納する。
(4) Xre[j]=X_wallであるロボットjすべてについて以下を行う。
a. まずj_0←jとする。
b. dest_next[3][j_0]=-1となるまで、j_0の値をdest_next[3][j_0]の値に置き換える処理(j_0←dest_next[3][j_0])を繰り返す。要は、左端に存在する目標位置を見つけ、その目標位置の番号をj_0に格納する。
c. b.の結果得られるj_0について、Xre[j_0]>0であるならば、目標位置j、j_0とその二つの目標位置に挟まれる全ての目標位置を左方向に1マス移動させる。
(6) 目標位置jのうち、X座標が最小のもののX座標値を壁のX座標値X_wallに格納する。例えば、図19の場合、X_wall←−5とする。
(7) すべての行動a値、目標位置j値にてdest_next[a][j]←−1に初期化する。
(8) 各目標位置jの行動a=1方向(右方向)に隣接する目標位置の番号をdest_next[1][j]変数に格納する。ある目標位置jについて、右隣の位置が目標位置であれば、その目標位置の番号をdest_next[1][j]に格納する。
a. まずj_0←jとする。
b. dest_next[1][j_0]=-1となるまで、j_0の値をdest_next[1][j_0]の値に置き換える処理(j_0←dest_next[1][j_0])を繰り返す。要は、右端に存在する目標位置を見つけ、その目標位置の番号をj_0に格納する。
c. b.の結果得られるj_0について、Xre[j_0]<0であるならば、目標位置j、j_0とその二つの目標位置jに挟まれる全ての目標位置を右方向に1マス移動させる。
(10) (Xr_hist[j][t],Yr_hist[j][t])←(Xre[j],Yre[j])とする。tをインクリメントする。X_wallをデクリメントする。X_wall=0ならばt_all=t-1として終了し、そうでなければ(8)へ移行する。(6)〜(10)以上の処理により、X座標x=0よりも左側に存在する目標位置を、X座標0を通るy方向に平行な軸に集める。図20の中間形態隊列の生成途中の状態を図21の中間形態隊列に変形する。
<中間遷移処理>
(1) yk←1とする。
(2) Y座標の値がykに等しいロボットiを一台選択する。
(3) Xr[i]>Xr_hist[robot_target_index[i]][t_all]ならば、Y座標がyk以上のロボットすべてを行動a=3方向(左方向)に1マス移動させる。Xr[i]<Xr_hist[robot_target_index[i]][t_all]ならば、Y座標がyk以上のロボットすべてを行動a=1方向(右方向)に1マス移動させる。Xr[i]=Xr_hist[robot_target_index[i]][t_all]ならば、ykをインクリメントし、(4)へ移行する。そうでないなら(2)に戻る。
(4) Y座標の値がykに等しいロボットがあるなら(2)へ移行する、ないなら終了する。
以上の処理により、図7または図17の押し出し隊列から図8の中間形態隊列に変形する。
最終展開処理は、変数(Xr_hist[j][t],Yr_hist[j][t])に格納された目標位置jの移動の履歴をt=t_allからt=0まで逆にたどらせればよい。
<最終展開処理>
(1) t←t_all-1とする。
(2) 各ロボットiについて、(Xr_hist[robot_target_index[i]][t],Yr_hist[robot_target_index[i]][t])と(Xr[i],Yr[i])の値を比較し、移動先が、(Xr_hist[robot_target_index[i]][t],Yr_hist[robot_target_index[i]][t])になるような行動で1マス移動する。tをデクリメントする。
(3) t<0ならば終了、そうでないなら(2)へ移行する。
以上の処理により、図8の中間形態隊列から図9を経て図10の目標位置の集合に変形する。
<第一実施形態に係る行動制御システム100>
図22は第一実施形態に係る行動制御システム100の機能ブロック図を、図23はその処理フローの例を示す。行動制御システム100は、図22に示すように、行動選択部120と、隣接状態判定部124と、位置更新部123と、位置判定部126と、記憶部140と、通信部150と、入力部160とを含む。行動選択部120は押し出し過程処理部120Aと中間遷移過程処理部120Bと最終展開過程処理部120Cとを含む。
入力部160には、p台のロボットiのそれぞれの初期位置(Xr0[i],Yr0[i])(言い換えると、p個の開始位置の集合S={(Xr0[0],Yr0[0]), (Xr0[1],Yr0[1]), …, (Xr0[p-1],Yr0[p-1])})及びq個の目標位置の集合G={(Xre[0],Yre[0]), (Xre[1],Yre[1]),…,(Xre[q-1],Yre[q-1])}が入力され、記憶部140に記憶される。
行動制御システム100が実装されているロボットも含め、全てのロボットは、通信部150を介して、二次元平面上の上下左右方向(以下「4方向」ともいう)において隣接する他のロボットと通信することができる。
行動選択部120は、上述の方法で、p台のロボットを制御する(s120)。なお、押し出し過程処理部120A、中間遷移過程処理部120B、最終展開過程処理部120Cでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部140に記憶しておく。行動選択部120は、記憶部140から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。
行動選択部120の押し出し過程処理部120Aは、記憶部140からp個の開始位置の集合S及びq個の目標位置の集合を取り出し、p個の開始位置の集合Sに位置する少なくともq台のロボットを移動させ、押し出し隊列を成すように制御し、押し出し隊列における各ロボットの位置を中間遷移過程処理部120Bに出力する。
横方向押し出し過程の場合、押し出し隊列は、y座標値i_yに位置するロボットの個数n[i_y]と、y座標値i_y-1に位置するロボットの個数n[i_y-1]との大小関係に応じて、y座標値i_yにおいてx軸方向において連続して隣接しているロボットの一端または他端のロボットと、y座標値i_y-1においてx軸方向において連続して隣接しているロボットの一端または他端のロボットとを第二方向において合わせた隊列である。本実施形態では、個数n[i_y]が個数n[i_y-1]よりも大きいとき、y座標値i_yにおいてx軸方向において連続して隣接しているロボットの左端とy座標値i_y-1においてx軸方向において連続して隣接しているロボットの左端とを合わせ、個数n[i_y]が個数n[i_y-1]以下のとき、y座標値i_yにおいてx軸方向において連続して隣接しているロボットの右端とy座標値i_y-1においてx軸方向において連続して隣接しているロボットの右端とを合わせた隊列を押し出し隊列としている。
押し出し過程処理部120Aは、記憶部140からp個の開始位置の集合S及びq個の目標位置の集合を取り出し、<縦方向押し出し過程>により、ロボットがy軸に平行な列をなすように、開始位置の集合からロボットを移動させ、その後、y軸に平行な列に沿って、他のロボットをy軸の正方向に、中間形態隊列内でのy座標値の位置まで移動させ、その後、x軸の正方向に移動させることで、押し出し隊列を形成する。その後、<縦押し出し位置対応付け処理>により、押し出し隊列におけるロボットの位置と目標位置におけるロボットの位置とを対応付ける。
行動選択部120の中間遷移過程処理部120Bは、q個の目標位置の集合を取り出し、<中間形態隊列生成処理>により、あるx座標値を通る、y軸に平行な軸(本実施形態ではx=0)に向かって、目標位置の集合に含まれる位置を平行移動させる(圧縮する)ことで、中間形態隊列を成すロボットの位置を求める。このとき、平行な軸に対する距離が大きい、目標位置の集合に含まれる位置から順に、平行移動させる。平行移動させる際に、目標位置の集合に含まれる他の位置と隣接する場合には、その隣接する位置と一緒に平行移動させる。目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部120Cに出力する。
行動選択部120の最終展開過程処理部120Cは、中間遷移過程処理部120Bから受け取った、目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを使用して、[最終展開過程]により、中間形態隊列を成すロボットに対して、目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から中間形態隊列を成すロボットの位置を求める際の平行移動方向とは反対の方向にロボットを移動させることで、中間形態隊列を成すロボットを目標位置の集合に移動させる。
位置更新部123は、各i=0,1,…,p-1について、i番目のロボットの現在の位置(Xr[i],Yr[i])において、行動選択部120で決定した行動を実行した場合のロボットの移動後(行動後)の位置(Xr'[i],Yr'[i])を計算し、計算された(Xr'[i],Yr'[i])で記憶部140に格納されたi番目のロボットの位置を更新する(S123)。言い換えれば、位置更新部123は、行動選択部120で決定した行動に基づいて、ロボットが行動した場合に想定される位置(以下、「想定位置」ともいう)を計算し、ロボットの位置を更新し記憶部140に格納する。
隣接状態判定部124は、ロボットの2次元平面上の上下左右の隣接する位置に、他のロボットが存在するか否かを判定し、(S124),判定結果を記憶部140に格納する。
位置判定部126は、隣接状態判定部124の判定結果を用いて、行動後位置を求め、行動後位置(Xr"[i],Yr"[i])と想定位置(Xr'[i],Yr'[i])とが一致するか否かを判定する(S126)。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。この場合、行動後位置(Xr"[i],Yr"[i])と想定位置(Xr'[i],Yr'[i])との少なくとも一方を補正すればよい。補正方法としては様々な手法が考えられる。例えば、移動した全てのロボットに対して、制御前の位置に戻るように指示し、行動後位置(Xr"[i],Yr"[i])を補正してもよいし、想定位置(Xr'[i],Yr'[i])を行動後位置(Xr"[i],Yr"[i])に合わせて補正してもよい。
<効果>
このような構成により、ロボットに複雑な機構を備えなくとも、ロボット同士が接したままの状態を維持しつつ任意の開始位置における隊列形成状態から、他の任意の目標位置における隊列形成状態へ変形動作を行うことを可能とすることができる。また、計算負荷がロボット数の階乗に比例せず、実時間で実行可能な低計画計算負荷での自己位置座標定義型隊列形成アルゴリズム獲得ができる。また、静止しているロボットに対する相対的な位置を判定することで、絶対的な位置を取得することができるため、付加的な位置計測用の装備を必要としない。または少なくとも一台のGPSを備えるロボットか、開始状態にて位置の定義されたロボットに対する相対的な位置を判定することで、絶対的な位置を取得することができるため、全てのロボットがGPS等を備えなくともよい。
開始位置の集合Sは図4の条件((1)開始位置の集合Sが芯の部分から飛び出している部分は芯の部分に隣接するマス単位に含まれるものとする。(2)開始位置の集合Sが芯の部分から飛び出している部分は、かぎ型のように飛び出したL字型とはならないものとする。(3)芯となるロボット単位一つに対し、飛び出している部分が接するロボット単位の辺は一つ以内に収めるものとする。)を満たさずともよく、一塊の任意の形状を成せばよい。前述の通り、目標位置の集合Gは一塊の任意の形状であればよいため、図4の条件を満たす一塊の任意の形状を目標位置の集合Gに変形する際の行動制御を、逆の手順で、逆の方向に移動させることで、接続状態を維持したまま、目標位置の集合Gから図4の条件を満たす一塊の任意の形状に変更することができる。よって、図4の条件を満たさない、一塊の任意の形状を成す開始位置の集合S'を、一旦、図4の条件を満たす一塊の任意の形状に変形し(図4の条件を満たす一塊の任意の形状から一塊の任意の形状を成す開始位置の集合S'に変形する手順を考え、逆の手順で、逆の方向に移動させることで変形する)、変形後の形状(図4の条件を満たす一塊の任意の形状)を、一塊の任意の形状を成す目標位置の集合Gに変形すればよい(本実施形態における開始位置の集合が変形後の形状に対応するものとして計算すればよい)。ただし、開始位置の集合Sは少なくとも図5Aの隊列を内部に含むか、その一部が図5Aの隊列に変形できる形状であることを必要とする。
第一実施形態と異なる部分を中心に説明する。
まず、本実施形態における行動制御システム及び方法の理論的背景について説明する。
[問題設定]
多数のロボットが協調して開始位置における隊列形成状態から、各ロボットが接した状態を維持しつつ移動を行い、目標位置での隊列形成を行う任務は、例えば図25に例示するような、互いに接する面同志をスライドさせて移動していくことが可能な立方体型のロボットの使用を想定し、図26に示すように、開始位置の集合Sから目標位置の集合Gまで複数のロボットの移動によって実現するものである。
それぞれのロボットi(iはロボット番号を表す、i=0,1,2,3,…,p-1)の初期位置(ここでは中間位置の集合Mから目標位置の集合Gへの変形を行うため、中間位置を初期位置とも呼ぶ)を(Xr0[i],Yr0[i],Zr0[i])とし、目標位置を(Xre[j],Yre[j],Zre[j])(jは目標位置の番号を表す、j=0,1,2,3,…,q-1、p=q+a)、開始位置を(Xrs[j],Yrs[j],Zrs[j])(jは開始位置の番号を表す、j=0,1,2,3,…,q-1、p=q+a)とするとき、本問題は、中間位置に配置されたロボットが、目標位置まで移動するための行動計画を求めることと定義できる。
Xr[i1]=2xXr_unit[j]
Yr[i1]=2xYr_unit[j]
Zr[i1]=2xZr_unit[j]
Xr[i2]=2xXr_unit[j]+1
Yr[i2]=2xYr_unit[j]
Zr[i2]=2xZr_unit[j]
Xr[i3]=2xXr_unit[j]
Yr[i3]=2xYr_unit[j]+1
Zr[i3]=2xZr_unit[j]
Xr[i4]=2xXr_unit[j]+1
Yr[i4]=2xYr_unit[j]+1
Zr[i4]=2xZr_unit[j]
Xr[i5]=2xXr_unit[j]
Yr[i5]=2xYr_unit[j]
Zr[i5]=2xZr_unit[j]+1
Xr[i6]=2xXr_unit[j]+1
Yr[i6]=2xYr_unit[j]
Zr[i6]=2xZr_unit[j]+1
Xr[i7]=2xXr_unit[j]
Yr[i7]=2xYr_unit[j]+1
Zr[i7]=2xZr_unit[j]+1
Xr[i8]=2xXr_unit[j]+1
Yr[i8]=2xYr_unit[j]+1
Zr[i8]=2xZr_unit[j]+1
である。
行動主体は部屋に配置されている各ロボットとなる。ロボットiの行動aは、静止、上下左右前後方向への1格子分の移動の計7種類のうちのいずれかを取る。例えば、a[i]∈{0,1,2,3,4,5,6}として、
0:静止
1:三次元空間内でX座標値が増える方向(右方向)に1格子だけ移動する
2:三次元空間内でY座標値が増える方向(下方向)に1格子だけ移動する
3:三次元空間内でX座標値が減る方向(左方向)に1格子だけ移動する
4:三次元空間内でY座標値が減る方向(上方向)に1格子だけ移動する
5:三次元空間内でZ座標値が増える方向(前方向)に1格子だけ移動する
6:三次元空間内でZ座標値が減る方向(後方向)に1格子だけ移動する
とする。
このような任務環境における状態空間は、ロボット数×3の次元数の状態を持ち、かつ選択可能な行動数は、ロボットの行動(=7通り)のロボット数乗だけ存在する。例えば、ロボット数が50で、部屋の縦横高さ方向の格子数がそれぞれ20であるとすれば状態数は20の150乗個にもなり、探索計算に要する資源の量は膨大なものとなる。さらにロボット数が1台増えるごとに、その状態数は8000倍増加していくことになる。
本実施形態では、これらの計算負荷の問題を解決するための方策の一つとして、ロボットの動作の過程を2つの段階にわけて考える。一つ目は、目標位置の集合Gの隊列を形成するロボットを中間位置の集合Mから位置Psか、位置Prを経て中間位置の集合Mの外に押し出すロボット押し出し過程であり、二つ目はその押し出された形態から、目標位置の集合Gの隊列形態に展開動作を行う最終展開過程である。一つ目の過程においては、以下に示すボイド制御を行う。
これらの一つ目のロボット押し出し過程にて、本実施形態では、ボイド制御の考え方を導入する。まず、ここでいうボイドとは、あるロボットが別の位置に移動した後に、元いた位置にできる空隙のことである。別の言い方をすると、ボイドとは、ロボットの移動する方向と反対の方向に移動する仮想的な存在である。こうした群ロボットの隊列形成問題においては、複数のロボットの動作に着目するがゆえに、その探索計算量が爆発してしまうが、視点を変えて、ボイドの動きに着目すれば、多数のロボットの動作計画の問題を単一のボイドの動作計画として考えることができ、探索計算負荷の軽減に適している。
ロボットの押し出し過程は、3段階の過程からなるものとする。一つ目は、位置Psから、Z軸正方向にロボットを押し出して、押し出されたロボットが位置Psから立つ柱の形状を形成する上方向押し出し過程である(図28参照)。二つ目は、位置PrからZ軸正方向に押し出されたロボットが、その柱の形状にそって上昇した後、しかるべきZ座標値において、その柱にあるロボットを利用して、Y軸の負方向に一ステップだけ移動することで、旗上の形態を形成する旗押し出し過程である(図29参照)。三つ目は、位置PrからZ軸正方向に押し出されたロボットが、シード位置単位Useed上を移動することで、旗押し出し過程で形成された旗面(押し出し済みのロボットのX軸正方向(X=0)にある面で構成された平面)に接する位置(本実施形態では位置(1,0,2))まで移動し、旗面上をしかるべき、Z座標値、Y座標値まで移動した後に、旗面上にあるロボットを利用して、X軸負方向へ1ステップ移動することで、最終展開過程の開始状態である圧縮形態(押し出し終了時形状)を形成するための最終押し出し過程である(図30参照)。
各押し出し過程で共通の中間位置の集合Mからのロボットの押し出しの制御は、ロボットの隊列制御開始前にあらかじめ実行される尾部マス単位決定用動作計画、隊列形成動作中に実行される各ロボット押し出し動作決定用動作計画からなる。本制御を押し出しボイド制御と呼ぶことにする。本制御における、ロボット中間位置の集合Mに含まれるマス単位の集合(中間位置単位の集合ともいう)をMUとする。
ロボットが動作を開始する前にあらかじめ、2つの動作計画のために使用する任務空間内の各位置単位のシード位置単位Useedからのマンハッタン距離(1つの「マス」におけるマンハッタン距離ではなく、8つのマスからなる「マス単位」におけるマンハッタン距離)の計算を行う。ただし、UseedにX方向に隣接して存在する2つのマス単位Unext1=(1,0,0)、Unext2=(2,0,0)のシード位置単位Useedからのマンハッタン距離はそれぞれ1,2に固定し、Useed、Unext1、Unext2以外のマス単位は、形式上、Unext2以外とは、接していないものとして扱う。言い換えると、Useed、Unext1、Unext2以外のマス単位は、Unext2を経由してUseed、Unext1と間接的に接する。そのために、まず、任務空間内の各位置単位(X,Y,Z)にて、シード位置単位Useedからの各位置単位へのマンハッタン距離δ[X][Y][Z]を以下の計算手続きで求める。なお、以下において、説明を簡単にするため「位置単位」を単に「位置」ともいい、「シード位置単位Useed」を単に「シード位置Useed」ともいう。
(1)中間位置単位の集合MU内の各位置(X,Y,Z)において、next[a][X][Y][Z]を用意し、隣接する行動aの方向に中間位置単位の集合MU内に含まれてUnext1,Useed以外である位置(X,Y,Z)が存在しない場合は、next[a][X][Y][Z]←0とし、それ以外の場合はnext[a][X][Y][Z]←1とする。なお、中間位置単位の集合MUは、ロボット単位で構成された長方形形状の平板上の隊列を形成するため、Z軸方向には隣接するロボット単位が存在しないことは明らかである。そのため、行動aは1〜4についてのみ計算すればよい。
(2)中間位置単位の集合MU内の各位置(X,Y,Z)のマンハッタン距離δ[X][Y][Z]を中間位置単位の集合MU内の位置単位数より大きな値s_maxに初期化する。
(3)シード位置Useedのマンハッタン距離δを0に、Unext1のマンハッタン距離δを1に、Unext2のマンハッタン距離δを2に初期化する。
(4)位置Unext2,Unext1,Useed以外の、中間位置単位の集合MU内の各位置(X,Y,Z)の全行動aについて、next[a][X][Y][Z]の値が0ではない場合の、行動aによって位置(X,Y,Z)から移動した先の位置(X',Y',Z')で位置Unext1,Useed以外のもののうち、マンハッタン距離δ[X'][Y'][Z']を調べ、その最小値に1を加えた値が、現在のδ[X][Y][Z]よりも小さい場合は、その値をδ[X][Y][Z]に代入する。
(5)上述の(4)の処理にて、δ[X][Y][Z]値の更新がなくなるまで、(4)を繰り返す。
中間位置単位の集合MUに含まれる各ロボット位置単位(Xr_unit[j],Yr_unit[j],Zr_unit[j])において、マンハッタン距離δ値が最大のものを選択して、尾部ロボット単位Tとする。また、常にシード位置Useedを以下の処理におけるロボット単位Hとする。
以下、各ロボット動作決定用動作計画の動作について述べる。この動作計画では、8つのロボットからなる「ロボット単位」ではなく、1つの「ロボット」における動作計画を行う。各ロボット動作決定用動作計画においては、ロボット単位Hから一つのマス単位分のロボットを押し出すたびに、計8つのボイドが発生するので、まずは、それら発生した各々のボイドについてそのボイドがロボット単位Hから尾部ロボット単位Tまでたどるべき経路を計算する。
[Void_Trajectory_Decision]
(1)各ボイドvoid[iv_prev+k]の発生位置を以下のようにする。
k≦npのとき、上方向押し出し過程(図28参照)が終わっていないため、ロボットiは位置Psから押し出される。このとき、void[iv_prev+k]の発生位置は、ロボットi_move_psの移動前の位置である。
k>npのとき、上方向押し出し過程(図28参照)が終わっているため、ロボットiは位置Prから押し出される(図29、図30)。このとき、void[iv_prev+k]の発生位置は、ロボットi_move_prの移動前の位置である。
(2)各ボイドの発生位置(ロボットi_move_psまたはロボットi_move_prの移動前の位置)を各ボイドの軌道の終点void_end[iv_prev+k]とする。
(4)(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])をT'、(void_unit_trj_x[2],void_unit_trj_y[2],void_unit_trj_z[2])をT"とする。T'は尾部ロボット単位Tに隣接するロボット単位であり、T"はロボット単位T'に隣接するロボット単位である。
void[iv_prev+1]、void[iv_prev+2]の目標位置→ロボットi_tail_0の位置
void[iv_prev+3]、void[iv_prev+4]の目標位置→ロボットi_tail_1の位置
void[iv_prev+5]、void[iv_prev+6]の目標位置→ロボットi_tail_2の位置
void[iv_prev+7]、void[iv_prev+8]の目標位置→ロボットi_tail_3の位置
(6)T"内における各ボイドの発生位置と同じ位置(ロボット単位HをT"の位置に平行移動させた場合のロボット単位H内の各ボイドの発生位置)から、T'内の各ボイド目標位置までの軌道(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])を後述するVoid_Local_Trajectory_Decisionで決定する。tの値は、T'内の目標位置で0としてT"内の各ボイド発生位置に近づくにつれて大きくなるようにボイドの動きと逆向きに設定する。
(8)it←2とする。
(9)(void_unit_trj_x[2+it/2-1],void_unit_trj_y[2+it/2-1],void_unit_trj_z[2+it/2-1])がロボット単位Hの位置に一致していないならば、(void_unit_trj_x[2+(it)/2-1],void_unit_trj_y[2+(it)/2-1],void_unit_trj_z[2+(it)/2-1])から(void_unit_trj_x[2+it/2],void_unit_trj_y[2+it/2],void_unit_trj_z[2+it/2])へ移動する方向に、(void_trj_x[iv_prev+k][ts+it-2],void_trj_y[iv_prev+k][ts+it-2],void_trj_z[iv_prev+k][ts+it-2])から1ステップ移動した位置を(void_trj_x[iv_prev+k][ts+it-1],void_trj_y[iv_prev+k][t+it-1],void_trj_z[iv_prev+k][t+it-1])とし、2ステップ移動した位置を(void_trj_x[iv_prev+k][ts+it],void_trj_y[iv_prev+k][ts+it],void_trj_z[iv_prev+k][ts+it])とし、itを2回インクリメントする。(void_unit_trj_x[2+it/2-1],void_unit_trj_y[2+it/2-1],void_unit_trj_z[2+it/2-1])がロボット単位Hの位置に一致するまで(9)を繰り返す。(void_unit_trj_x[2+it/2-1],void_unit_trj_y[2+it/2-1],void_unit_trj_z[2+it/2-1])がロボット単位Hの位置に一致しているならば、軌道内点数trj_num[iv_prev+k]←ts+it-2として終了する。
Void_Local_Trajectory_Decisionの処理は、以下のとおりである。まず、ロボットが動作を開始する前に、あらかじめ、以下の6つの位置関係にある尾部ロボット単位T及びロボット単位T'において、ロボット単位T'に5通りのロボット単位T"[kt](kt=1,2,3,4,5)が接しているとして、ロボット単位T'内の尾部ロボット単位Tに接しない位置とロボット単位T"の全ての位置について、ロボット単位T'内の尾部ロボット単位Tに接しない4つの位置からのマンハッタン距離δlocal[vs][al](X,Y,Z)(vs=0,1,2,3。al=1,2,3,4,5,6)を全て計算する。ここでは、8台のロボットからなるロボット単位ではなく、1台のロボットのマンハッタン距離を求める。なお、alはロボット単位Tの位置からロボット単位T'の位置に向かう6つの方向の行動であり、vsはロボット単位T'内の尾部ロボット単位Tに接しない4つのロボット(その位置)のインデックスである。
(2)al=2,T=(0,0,0),T'=(0,1,0),T"[1]=(1,1,0),T"[2]=(-1,1,0),T"[3]=(0,2,0),T"[4]=(0,1,1),T"[5]=(0,1,-1)
(3)al=3,T=(0,0,0),T'=(-1,0,0),T"[1]=(-1,1,0),T"[2]=(-1,-1,0),T"[3]=(-2,0,0),T"[4]=(-1,0,1),T"[5]=(-1,0,-1)
(4)al=4,T=(0,0,0),T'=(0,-1,0),T"[1]=(1,-1,0),T"[2]=(-1,-1,0),T"[3]=(0,-2,0),T"[4]=(0,-1,1),T"[5]=(0,-1,-1)
(5)al=5,T=(0,0,0),T'=(0,0,1),T"[1]=(0,1,1),T"[2]=(0,-1,1),T"[3]=(0,0,2),T"[4]=(1,0,1),T"[5]=(-1,0,1)
(6)al=6,T=(0,0,0),T'=(0,0,-1),T"[1]=(0,1,-1),T"[2]=(0,-1,-1),T"[3]=(0,0,-2),T"[4]=(1,0,-1),T"[5]=(-1,0,-1)
[Calculate_delta_local]
(1)ロボット単位T'内の尾部ロボット単位Tに接しない位置とロボット単位T"の全ての位置(X,Y,Z)において、next_local[al][a][X][Y][Z]を用意し、位置(X,Y,Z)に隣接するロボット単位T'内の尾部ロボット単位Tに接しない位置か、ロボット単位T"内の位置があるならば、next_local[al][a][X][Y][Z]←1とし、それ以外の場合はnext_local[al][a][X][Y][Z]←0とする。
(2)ロボット単位T'内の尾部ロボット単位Tに接しない位置とロボット単位T"の全ての位置(X,Y,Z)について、各位置(X,Y,Z)のδlocal[vs][al](X,Y,Z)値を44(T'内4つ、T"内8x5つの位置の数)より大きな値s_maxに初期化する。
target_local[0][1]=(3,0,0)
target_local[1][1]=(3,0,1)
target_local[2][1]=(3,1,0)
target_local[3][1]=(3,1,1)
target_local[0][2]=(0,3,0)
target_local[1][2]=(0,3,1)
target_local[2][2]=(1,3,0)
target_local[3][2]=(1,3,1)
target_local[0][3]=(-2,0,0)
target_local[1][3]=(-2,0,1)
target_local[2][3]=(-2,1,0)
target_local[3][3]=(-2,1,1)
target_local[0][4]=(0,-2,0)
target_local[1][4]=(0,-2,1)
target_local[2][4]=(1,-2,0)
target_local[3][4]=(1,-2,1)
target_local[0][5]=(0,0,3)
target_local[1][5]=(0,1,3)
target_local[2][5]=(1,0,3)
target_local[3][5]=(1,1,3)
target_local[0][6]=(0,0,-2)
target_local[1][6]=(0,1,-2)
target_local[2][6]=(1,0,-2)
target_local[3][6]=(1,1,-2)
(4)目標位置target_local[vs][al]以外の、各位置(X,Y,Z)の全行動aについて、next_local[al][a][X][Y][Z]の値が0ではない場合の、δlocal[vs][al](X,Y,Z)の値を調べ、その最小値に1を加えた値が、現在のδlocal[vs][al](X,Y,Z)よりも小さい場合は、その値をδlocal[vs][al](X,Y,Z)に代入する。δlocal[vs][al](X,Y,Z)値の更新がなくなるまで、(4)を繰り返す。
[Void_Local_Trajectory_Decision]
(1)ロボット単位T'の位置を(XT',YT',ZT')、ロボット単位T"の位置を(XT",YT",ZT")として、ロボット単位Tの位置からロボット単位T'の位置に向かう方向の行動をal値とする。i_tail_0の位置をtarget_local[0][al](vs←0とする),i_tail_1の位置をtarget_local[1][al](vs←1とする),i_tail_2の位置をtarget_local[2][al](vs←2とする),i_tail_3の位置をtarget_local[3][al](vs←3とする)とする。
もしal=1ならば
vt"←(xv[iv_prev+k]-xrh*2+2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=2ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2+2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=3ならば
vt"←(xv[iv_prev+k]-xrh*2-2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=4ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2-2+Ydt*2,zv[iv_prev+k]-zrh*2+Zdt*2)
とし、もしal=5ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2+2+Zdt*2)
とし、もしal=6ならば
vt"←(xv[iv_prev+k]-xrh*2+Xdt*2,yv[iv_prev+k]-yrh*2+Ydt*2,zv[iv_prev+k]-zrh*2-2+Zdt*2)
とする。
(4)位置(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])が、target_local[vs][al]に一致しないとき、
位置(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])に隣接し、δlocal[vs][al]の値が(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])の位置のδlocal[vs][al]の値より小さい位置を(void_local_x[iv_prev+k][tl+1],void_local_y[iv_prev+k][tl+1],void_local_z[iv_prev+k][tl+1])とし、tlをインクリメントする。(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])が、target_local[vs][al]に一致するまで(4)を繰り返す。位置(void_local_x[iv_prev+k][tl],void_local_y[iv_prev+k][tl],void_local_z[iv_prev+k][tl])が、target_local[vs][al]に一致するとき、(5)へ移行する。
(5)(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_local_x[iv_prev+k][tl-t],void_local_y[iv_prev+k][tl-t],void_local_z[iv_prev+k][tl-t])とし、ts←tlとする。
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]-2+XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=2ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]XT’,void_trj_y[iv_prev+k][t]-2 +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=3ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]+2+XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=4ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]XT’,void_trj_y[iv_prev+k][t]+2 +YT’,void_trj_z[iv_prev+k][t] +ZT’)
とする。
もしal=5ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]+XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] -2+ZT’)
とする。
もしal=6ならば
(void_trj_x[iv_prev+k][t],void_trj_y[iv_prev+k][t],void_trj_z[iv_prev+k][t])←(void_trj_x[iv_prev+k][t]XT’,void_trj_y[iv_prev+k][t] +YT’,void_trj_z[iv_prev+k][t] +2+ZT’)
とする。
[Void_Unit_Trajectory_Decision]
(1)尾部ロボット単位Tの位置を(void_unit_trj_x[0],void_unit_trj_y[0],void_unit_trj_z[0])とする。tu←1とし、尾部ロボット単位Tよりも小さなマンハッタン距離δを持つ隣接のロボット単位の位置を(void_unit_trj_x[tu],void_unit_trj_y[tu],void_unit_trj_z[tu])とする。
(2)もし、(void_unit_trj_x[tu],void_unit_trj_y[tu],void_unit_trj_z[tu])がロボット単位Hでなければ、tuをインクリメントし、位置(void_unit_trj_x[tu-1],void_unit_trj_y[tu-1],void_unit_trj_z[tu-1])にあるロボット単位よりも小さなマンハッタン距離δを持つ隣接のロボット単位の位置を(void_unit_trj_x[tu],void_unit_trj_y[tu],void_unit_trj_z[tu])とする。(void_unit_trj_x[tu],void_unit_trj_y[tu],void_unit_trj_z[tu])がロボット単位Hの位置になるまで(2)を繰り返す。
[Void_Trajectory_Decision_Seed]
(1)各ボイドvoid[iv_prev+k]の発生位置を以下のようにする。
k≦npのとき、void[iv_prev+k]の発生位置は、ロボットi_move_psの移動前の位置である。
k>npのとき、void[iv_prev+k]の発生位置は、ロボットi_move_prの移動前の位置である。
(2)各ボイドの発生位置を各ボイドの軌道の終点void_end[iv_prev+k]とする。
T=Unext2のとき
(void_unit_trj_x[0],void_unit_trj_y[0],void_unit_trj_z[0])←(2,0,0)
(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])←(1,0,0)
(void_unit_trj_x[2],void_unit_trj_y[2],void_unit_trj_z[2])←(0,0,0)
とし、T=Unext1のとき
(void_unit_trj_x[0],void_unit_trj_y[0],void_unit_trj_z[0])←(1,0,0)
(void_unit_trj_x[1],void_unit_trj_y[1],void_unit_trj_z[1])←(0,0,0)
とする。なお、tuの値は、尾部ロボット単位Tからロボット単位Hまでの道筋上の点を表し、尾部ロボット単位Tの位置をtu=0としてHに近づくにつれて大きくなるようにボイドの動きと逆向きに設定する。
(5)T'内にあり、尾部ロボット単位T内の位置に接しない位置にあり、Y座標が1、Z座標が0の点を各ボイドの目標位置とする。例えば、
T=Unext2のとき→(2,1,0)
T=Unext1のとき→(1,1,0)
とする。
(6)各ボイドの目標位置を(void_trj_x[0],void_trj_y[0],void_trj_z[0])とする。t←0とする。
(7)tをインクリメントして、void_trj_x[t-1]をデクリメントした値が0以上ならば、(void_trj_x[t],void_trj_y[t],void_trj_z[t])←(void_trj_x[t-1]-1,void_trj_y[t-1],void_trj_z[t-1])とする。void_trj_x[t-1]をデクリメントした値が0より小さいならば、(8)へ移行する。
(8)k≦npのとき、tをインクリメントして、(void_trj_x[t],void_trj_y[t],void_trj_z[t])=(0,0,0)とする。
(9)tをインクリメントして、trj_num[iv_prev+k]←tとする。al←3とする。
以下、上記の各ロボット動作決定用動作計画を使用して、ボイドをロボット単位Hの位置から尾部ロボット単位Tの位置に移動させる制御方法を示す。図33に示すように、ロボット単位T'内の尾部ロボット単位Tに接する位置にあるロボット位置に空白が生じないように、ロボット単位Hでのボイド発生と尾部ロボット単位Tでのボイド追い出しをしていることで、ロボット群全体の接続は保たれている。ロボット単位Hとロボット単位T'が直接、接しないように、尾部ロボット単位Tの選択アルゴリズムにおいて、尾部ロボット単位Tの位置のδ(ロボット単位Hと尾部ロボット単位Tとのマンハッタン距離)が3以上という条件を設定している。本制御方法で、各ロボット単位内において、ボイドが一つ以内に収まるように制御されている。
[Void_Control]
(1)本処理が最初に実行されたとき、もしくは内部変数i_motion_flg=0のとき、i_motion_flg←1とし、iv_prev<(p-24)-1のときにVoid_Trajectory_Decisionを、q-1>iv_prev≧(p-24)-1のときにVoid_Trajectory_Decision_Seedを実行し、trj_cnt[iv_prev+1],…,trj_cnt[iv_prev+8],を-1にする。q-1-iv_prevが8以上のときは、iv_prevの値を8回インクリメントする。そうでないときは、iv_prevの値をq-1-iv_prev回インクリメントする。
発生番号の大きいもの(iv=iv_prev)から以下の処理をする。
(2-i)trj_cnt[iv]=-10のとき→void[iv]について何もしない。
(2-ii)trj_cnt[iv]<trj_num[iv]かつtrj_cnt[iv]≧0のとき→
i=trj_cnt[iv]とし、以下の処理を行う:
位置nv(void_trj_x[trj_num[iv]-i-1],void_trj_y[trj_num[iv]-i-1],void_trj_z[trj_num[iv]-i-1])が含まれるロボット単位Nvに他のvoidがないこと、自分より番号の小さいvoidが、すべてロボット単位Nvよりもマンハッタン距離δの大きいロボット単位に所属していることを確認する。確認できたなら、nvの位置にあるロボットを位置(void_trj_x[trj_num[iv]-i],void_trj_y[trj_num[iv]-i],void_trj_z[trj_num[iv]-i])に移動させる。trj_cnt[iv]をインクリメントする。確認できないならなにもしない。
(2-iii)trj_cnt[iv]=trj_num[iv]のとき→
現在のvoid[iv]の位置から、alの方向の逆方向に隣接していて、たどれるロボットを全て、al方向に1ステップ移動させて、trj_cnt[iv]←‐10とする。iv≧p-24のときで、ivが奇数のとき、i_motion_flgを3にする。
(2-iv)trj_cnt[iv]=‐1のとき→
ロボット単位Hが8つのロボットで満たされている場合、void[iv]の発生位置とX,Y座標が同じ位置にあるZ座標が2以上のロボットを全てa_headの方向に1ステップ移動させる。また、void[iv]の発生位置にあるロボットと、そのロボットに行動a_headの方向で隣接しているロボット(位置PsかPr)をa_headの方向に1ステップ移動させる。trj_cnt[iv]←0とする。満たされてない場合なにもしない。
Y座標が0、Z座標が0の全てのロボットのZ座標をインクリメントし、iv<q-1ならば、i_motion_flgを1にして終了する。
(4)i_motion_flg=4のとき、尾部ロボット単位T内のロボットについて以下の動作を行う。
Y座標が0、Z座標が1の全てのロボットのY座標をインクリメントし、i_motion_flgを5にして終了する。
(5)i_motion_flg=3のとき、尾部ロボット単位T内のロボットについて以下の動作を行う。
Y座標が1、Z座標が1の全てのロボットのZ座標をデクリメントし、i_motion_flgを4にして終了する。
以下の押し出し過程における各時刻ステップtmでの各ロボットiの位置を格納する変数を(motion_x[tm][i],motion_y[tm][i],motion_z[tm][i])とする。
[Motion_Data_Update]
(motion_x[tm][i],motion_y[tm][i],motion_z[tm][i])に各ロボットiの現時点の位置を格納し、tmをインクリメントする。
(1)押し出し済みの柱の高さを表す変数として、i_pole←0とする。tm←0とする。[Motion_Data_Update]を実行する。
(2)Zmax-1-i_pole>8のとき、np←8として、上述のVoid_Controlを実行する。Zmax-1-i_pole≦8のとき、np←Zmax-1-i_poleとして、Void_Controlを実行する。Z座標が2以上のロボットの総数をi_poleに代入する。Motion_Data_Updateを実行する。
(3)Zmax-1=i_poleとなるまで、(2)を繰り返す。
旗押し出し過程(図29参照)で、形成される旗形状は、前過程で生成済みの柱における各Z座標値(2からZmaxまで)の高さの位置からY軸負の向きに連続して接しているロボットの数_y_num[Z]で表現される。Z=2から、Zmaxまでの_y_num[Z]の総数を_y_allとする。_y_num[Z]は、後述の最終展開過程で計算される。なお、Zmax及び_y_num[Z]の値は、予め与えられる目標位置の集合から求めることができる。旗押し出し過程は、位置Prから押し出されたロボットが、押し出し済みの柱面をたどりながら、自身のたどり着くべきZ座標値まで、柱に沿って上昇して、たどり着くべきZ座標値にたどり着いたら、同じZ座標値のロボット全てと同時に、Y軸負方向に位置を1ステップ移動させる動作を、旗形状が完成するまで繰り返す処理である。以下の処理で実行される。
(1)変数_z←0とする。押し出し済みロボット数_i_pushed←0とする。
(2)_z<Zmax-1の間、(3)〜(7)以下を繰り返す。
(3)_y←0とし、_y<_y_num[_z+2]の間、(4)〜(6)を繰り返す。
(4)後述のUpdate_Y_Branchを実行する。Z座標が_z+2の位置にあるロボットの総数をカウントし、変数_yに、その数-1を格納する。
(5)_i_pushed<_y_allのとき、np←0としてVoid_Controlを実行する(位置Prから押し出し)。VoidControlが新たなロボットを押し出したならばi_pushedをインクリメントする。_i_pushed=_y_allのとき、X座標が0、Y座標が1、Z座標が2以上のロボットをZ軸正方向に1ステップ移動させる。
(6)[Motion_Data_Update]を実行する。
(7)_zをインクリメントする。
Update_Y_Branchでは、たどり着くべきZ座標値にたどり着いたロボットを、同じZ座標値のロボット全てと同時に、Y軸負方向に位置を1ステップ移動させる。
(1)位置(0,1,_z+2)にロボットがあるとき、Z座標値が_z+2のロボットを全てY方向負の向きに1ステップ移動させる。
(2)[Motion_Data_Update]を実行する。
最終押し出し過程で形成される形状は、各Z値におけるXY平面内のフロア形状を表す変数_x_num[Y][Z](Y=0〜_y_num[Z], Z=2〜Zmax)によって表現される。_x_numの値は後述の最終展開過程で計算される。なお、_x_numの値は、予め与えられる目標位置の集合から求めることができる。_x_numの値には、各Y、Z値をとるロボットがX軸に平行な向きに前過程で形成された旗形状に連続して接している個数が格納されている。最終押し出し過程が終了した後の形状を、押し出し終了時形状と呼ぶことにする。最終押し出し過程は、位置Prから押し出されたロボットが、前過程で形成された旗形状を辿りながら、自身がたどり着くべきY座標値、Z座標値まで、旗形状に沿って移動し(本実施形態では、まず、自身がたどり着くべきZ座標値にたどり着いた後で、自身がたどり着くべきY座標値に移動する)、たどり着くべきY座標値、Z座標値にたどり着いたら、同じY座標値、Z座標値のロボット全てと同時に、X軸負方向に位置を1ステップ移動させる動作を押し出し終了時形状が完成するまで繰り返す処理である。以下の処理で実行される。
(1)i_pushed←0とする。_z←0とする。
(2)_z<Zmax-1の間、(3)〜(9)以下を繰り返す。
(3)_y←0とし、_y≦_y_num[_z+2]の間、(4)〜(8)を繰り返す。
(4)_x←0、押し出し済みロボット数を格納する変数_x_all←0とし、_x<_x_num[_y][_z+2]の間、(5)〜(7)を繰り返す。
(5)後述のUpdate_X_Floorを実行する。実行後Y=_y、Z=_z+2の位置にあるロボットの個数-1を_xに格納する。
(6)_x_all<_x_num[_y][_z+2]のとき、np←0として、Void_Controlを実行する。実行後、位置Prからロボットが押し出された場合、_x_allをインクリメントし、押し出されたロボットの番号iを変数pushed_floor[i_pushed]に格納して、後述のSet_Pushed_Cube_Trajectoryを実行し、i_pushedをインクリメントする。
(7)[Motion_Data_Update]を実行する。
(8)_yをインクリメントする。
(9)_zをインクリメントする。
(10)生成済みボイドを全て追い出すまでVoid_Controlを繰り返す。
(1)処理Set_Pushed_Cube_Trajectoryでは、ロボットpushed_floor[i_pushed]の時刻tpでの目標動作action_floor[i_pushed][tp]を設定する。例えば、以下のように設定する。
action_floor[i_pushed][0]←1、
action_floor[i_pushed][1]←4、
(1-1)2≦tp<_z+2で、
action_floor[i_pushed][tp]←5、
(1-2)_z+2≦tp<_z+2+_yで、
action_floor[i_pushed][tp]←4、
(1-3)tp=_z+2+_yで、
action_floor[i_pushed][tp]←3、
(1-4)tp>_z+2+_yで、
action_floor[i_pushed][tp]←0、
とする。
(2)time_pushed_robot[i_pushed]←0とする。
処理Update_X_Floorでは、処理Set_Pushed_Cube_Trajectoryで設定された目標動作action_floor[i_pushed][tp]を実行する。例えば、以下のように実行する。
(1)_p=0〜i_pushed-1の全ての_pについて、(2)を実行する。
(2)action_floor[_p][time_pushed_robot[_p]]の値が
0のとき:何もしない
1のとき:ロボットpushed_floor[_p]のX座標をインクリメントする。time_pushed_robot[_p]をインクリメントする。
(3)[Motion_Data_Update]を実行する。
図34を用いて、最終展開過程について説明する。なお、図34Eは押し出し終了時形状の例を、図34Aは目標位置の集合に位置する仮想的なロボット(以下、仮想ロボットともいう)の例を示す。最終展開過程においては、まず、X軸に垂直な仮想面が、目標位置(Xre[j],Yre[j],Zre[j])にある仮想的なロボットをX軸正方向無限遠からX軸負方向に移動して、仮想面に当たった仮想ロボットを押して、すべてのロボットがX座標値が0以下になるように圧縮し(図34B、処理Extend_Process_X_Minusに相当)、続いて、X軸に垂直な仮想面が、目標位置にある仮想的なロボットをX軸負方向無限遠からX軸正方向に移動して、仮想面に当たった仮想ロボットを押して、X座標が0ではないすべてのロボットについて、ロボットのX軸正方向に接するロボットが存在している状態になるように圧縮し(図34C、処理Extend_Process_X_Plusに相当)、続いて、Y軸に垂直な仮想面が、目標位置にある仮想的なロボットをY軸正方向無限遠からY軸負方向に移動して、仮想面に当たった仮想ロボットを押して、すべてのロボットがY座標値が0以下になるように圧縮し(図34D、処理Extend_Process_Y_Minusに相当)、続いて、Y軸に垂直な仮想面が、目標位置にある仮想的なロボットをY軸負方向無限遠からY軸正方向に移動して、仮想面に当たった仮想ロボットを押して、Y座標が0ではないすべてのロボットについて、ロボットのY軸正方向に接するロボットが存在している状態になるように圧縮する(図34E、処理Extend_Process_Y_Plusに相当)ときの各ロボットの動作を計算する過程である。本過程によって、目標位置の集合Gの形状が、最終押し出し過程を終えた後のロボットの形状(押し出し終了時形状)と同一になるように変形される。そのための処理は以下のとおりである。
(1)tm←0とする。仮想ロボットiの位置を目標位置(Xre[i],Yre[i],Zre[i])に設定する。[Motion_Data_Update]を実行する。後述の[Extend_Process_X_Minus]を実行する。
(2)後述の[Extend_Process_X_Plus]を実行する。
(3)後述の[Extend_Process_Y_Minus]を実行する。
(4)後述の[Extend_Process_Y_Plus]を実行する。
(5)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、各Z座標値_zでの、X座標が0である仮想ロボットの数-1を_y_num[_z]に、各Z座標値_zでの、Y座標値が_yである仮想ロボットの数-1を_x_num[_y][_z]に格納する。
(1)全仮想ロボットでのX座標の最大値をXmaxとする。
(2)_xw=Xmaxとして、_xw>0の間、(3)〜(5)を繰り返す。
(3)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(4)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(4-1)X座標値が_xwである全ての仮想ロボットiについて、destination_to_be_compressed[i]←1とする。
(4-2)仮想ロボットiからX軸負方向に途切れず隣接している全ての仮想ロボットinについて、destination_to_be_compressed[in]←1とする。
(4-3)_flg_c←1とする。
(5)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をデクリメントし、[Motion_Data_Update]を実行する。_xwをデクリメントする。
(1)現時刻tmでの仮想ロボット位置のX座標の最小値をXminとし、_xw=Xminとして、_xw<0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(3-1)仮想ロボットiからX軸正方向に途切れず隣接している全ての仮想ロボットinの中にX座標が0のものがない場合、それら隣接の仮想ロボットi〜inのdestination_to_be_compressedの値を1とし、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をインクリメントし、[Motion_Data_Update]を実行する。_xwをインクリメントする。
(1)現時刻tmでの仮想ロボット位置のY座標の最大値をYmaxとし、_yw=Ymaxとして、_yw>0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1)Y座標値が_ywである全ての仮想ロボットiについて、destination_to_be_compressed[i]←1とする。
(3-2)仮想ロボットiからY軸負方向に途切れず隣接している全ての仮想ロボットinについて、destination_to_be_compressed[in]←1とする。
(3-3)_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をデクリメントし、[Motion_Data_Update]を実行する。_ywをデクリメントする。
(1)現時刻tmでの仮想ロボット位置のY座標の最小値をYminとし、_yw=Yminとして、_yw<0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1)仮想ロボットiからY軸正方向に途切れず隣接している全ての仮想ロボットinの中にY座標が0のものがない場合、それら隣接の仮想ロボットi〜inのdestination_to_be_compressedの値を1とし、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をインクリメントし、[Motion_Data_Update]を実行する。_ywをインクリメントする。
以上、中間位置の集合Mにあるロボットが、隊列制御によって、目標位置の集合Gに移動するまでの動作を計算する処理Transform_from_M_to_Gは以下のようになる。
[Transform_from_M_to_G]
(1)Extend_Processを実行する。t=0〜tm-1において、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を変数(compressed_x[t][i],compressed_y[t][i],compressed_z[t][i])に格納する。tmの値を変数tcに格納する。
(2)Upward_Pushingを実行する。
(3)Flag_Pushingを実行する。
(4)Last_Pushingを実行する。
(6)時刻t=tm〜tm+tc-1について、以下の処理を行う。
(6-1)全てのロボットiについて(motion_x[t][i],motion_y[t][i],motion_z[t][i])←(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])とする。
(6-2)全ての仮想ロボットjについて(motion_x[t][destination_robot_index[j]],motion_y[t][destination_robot_index[j]],motion_z[t][destination_robot_index[j]])←(compress_x[j][tc-1-(t-tm)],compress_y[j][tc-1-(t-tm)],compress_z[j][tc-1-(t-tm)])とする。
(7)t=0〜tm+tc-1において、変数(transform_M_to_G_x[t][i],transform_M_to_G_y[t][i],transform_M_to_G_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。t_m_to_g←tm+tc-1とする。
開始位置の集合Sから中間位置の集合Mへの隊列変形は、上述の中間位置の集合Mから目標位置の集合Gへの隊列変形過程[Transform_from_M_to_G]における(1)から(4)を目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。その処理にて計算された変形の時系列結果を逆再生すればよい。
[Transformation_from_S_to_M]
(1)Extend_Processを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。t=0〜tm-1において、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を変数(compressed_x[t][i],compressed_y[t][i],compressed_z[t][i])に格納する。tmの値を変数tcに格納する。
(2)Upward_Pushingを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。
(3)Flag_Pushingを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。
(4)Last_Pushingを目標位置の集合Gに当たる箇所を全て開始位置の集合Sに置き換えて実行する。
(5)全ての仮想ロボットjに対し、(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])=(compress_x[j][tc-1],compress_y[j][tc-1],compress_z[j][tc-1])となるロボットiを特定し、destination_robot_index_2[j]の値に格納する。
(6-1)全てのロボットiについて(motion_x[t][i],motion_y[t][i],motion_z[t][i])←(motion_x[tm-1][i],motion_y[tm-1][i],motion_z[tm-1][i])とする。
(6-2)全ての仮想ロボットjについて(motion_x[t][destination_robot_index_2[j]],motion_y[t][destination_robot_index_2[j]],motion_z[t][destination_robot_index_2[j]])←(compress_x[j][tc-1-(t-tm)],compress_y[j][tc-1-(t-tm)],compress_z[j][tc-1-(t-tm)])とする。
(7)t=0〜tm+tc-1において、変数(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])に、(motion_x[tm+tc-1-t][i],motion_y[tm+tc-1-t][i],motion_z[tm+tc-1-t][i])の値を格納する。t_s_to_m←tm+tc-1とする。
以上の処理を結合して、開始位置の集合Sから目標位置の集合Gまでの変形過程が求められる。
[Transformation_from_S_to_G_Homo]
(1)[Transformation_from_M_to_G]を実行する。
(2)[Transformation_from_S_to_M]を実行する。
(3)t=0〜t_s_to_mにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])の値を格納する。
(4)t=t_s_to_m+1〜t_s_to_m+1+t_m_to_gにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M_to_G_x[t-(t_s_to_m+1)][i],transform_M_to_G_y[t-(t_s_to_m+1)][i],transform_M_to_G_z[t-(t_s_to_m+1)][i])の値を格納する。
開始位置の集合S内の位置jと目標位置の集合G内の位置jは、同じロボットによって占有されるものとする。そのような場合、開始位置の集合S内の特定の位置jにあったロボットiが、目標位置の集合G内の決まった特定位置jに移動するためには、先述のTrandformation_from_S_to_G_Homoの処理に加えて、中間位置の集合Mにあるロボットの位置の入れ替えを行う必要がある。そのための入れ替え処理を[Transformation_M_to_M]として、開始位置の集合Sから目標位置の集合Gまでのヘテロ隊列変形処理は以下のとおりである。
(1)[Transformation_from_M_to_G]を実行する。
(2)[Transformation_from_S_to_M]を実行する。
(3)[Transformation_from_M_to_M]を実行する。
(4)t=0〜t_s_to_mにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_S_to_M_x[t][i],transform_S_to_M_y[t][i],transform_S_to_M_z[t][i])の値を格納する。
(5)t=t_s_to_m+1〜t_s_to_m+1+t_m_to_mにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M_to_M_x[t-(t_s_to_m+1)][i],transform_M_to_M_y[t-(t_s_to_m+1)][i],transform_M_to_M_z[t-(t_s_to_m+1)][i])の値を格納する。
(6)t=t_s_to_m+1+t_m_to_m+1〜t_s_to_m+1+t_m_to_m+1+t_m_to_gにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i]t,ransform_S_to_G_z[t][i])に、(transform_M_to_G_x[t-(t_s_to_m+1+t_m_to_m+1)][cube_after[i]],transform_M_to_G_y[t-(t_s_to_m+1+t_m_to_m+1)][cube_after[i]],transform_M_to_G_z[t-(t_s_to_m+1+t_m_to_m+1)][cube_after[i]])の値を格納する。
処理Transformation_M_to_Mでは、開始位置の集合S内の特定の位置jにあったロボットiが目標位置の集合G内の決まった特定位置jに移動するために、中間位置の集合Mにあるロボットの位置を入れ替える。
(1)中間位置の集合M内にあるすべてのロボットiに対して、cube_id[i],cube_id_inv[i]の値を、qより大きな値に設定する。本処理実行前の中間位置の集合M内のロボットiの位置をバッファ、(cube_buffer_x[i],cube_buffer_y[i],cube_buffer_z[i])に格納する。tm←0とする。Motion_Data_Updateを実行する。
(2)すべての開始位置の集合S内の位置jと目標位置の集合G内の位置jに対して、cube_id[destination_robot_index_2[j]]の値をdestination_robot_index[j]に設定する(cube_id[destination_robot_index_2[j]]←destination_robot_index[j])。さらに、すべての開始位置の集合S内の位置jと目標位置の集合G内の位置jに対して、cube_id_inv[destination_robot_index[j]]の値をdestination_robot_index_2[j]に設定する(cube_id_inv[destination_robot_index[j]]←destination_robot_index_2[j])。これにより、開始位置の集合S内の位置jから移動して、中間位置の集合M内の位置destination_robot_index_2[j]に移動したロボットが、目標位置の集合G内の位置jへの移動の前に中間位置の集合M内の位置にて、destination_robot_index[j]に移動しなけれならないという関連づけが行われたことになる。
(4)全てのロボット番号iについて、変数already[i]を0に設定する。
(5)全てのロボット番号iについてiの小さい順に(6)〜(11)を実行する。
(6)_i←cube_id[i]、_flg_id←0として、i=_iでない限り(7)〜(10)を繰り返す。
(7)_flg_id=0のとき、_flg_id←1として、_iにiの値を代入する。
(8)変数cube_after[_i]←cube_id[_i]と設定する。
(9)後述のChange_Heteroを実行する。
(10)_already[_i]←1とする。_i←cube_id[_i]とする。
(11)_already[i]←1とする。
処理Change_Heteroでは、中間位置の集合M内の位置destination_robot_index_2[j]にあるロボットが、中間位置の集合M内の位置destination_robot_index[j]に移動するように制御する。例えば以下のように制御する。
(1)i=_iのとき、(2)〜(12)を実行する。そうでないとき、(13)〜(23)を実行する。
(2)ロボット_iのZ座標値>0のとき、(3)〜(7)を実行する。そうでないとき(8)〜(12)を実行する。
(3)Xr[_i]=cube_buffer_x[cube_id[_i]]とYr[_i]=cube_buffer_y[cube_id[_i]]との何れかを満たさないとき、(4)〜(5)を実行する。そうでないとき(何れも満たすとき)、(6)〜(7)を実行する。
(5)ロボットcube_id[_i]のZ座標が1のとき、ロボットcube_id[_i]にZ軸負方向で接するロボットとともに1ステップZ軸正方向に移動させる。そうでないときは、ロボットcube_id[_i]にZ軸正方向で接するロボットとともに1ステップZ軸正方向に移動させる。Motion_Data_Updateを実行する。後述のHorizontal_ChangeとVertical_Changeを実行する。
(6)ロボット_iとロボットcube_id[_i]をともにZ軸正方向に1移動させる。Motion_Data_Updateを実行する。
(7)ロボット_iのX座標が偶数ならば、ロボット_iをX軸正方向に1ステップ移動させる。そうでなければ、ロボット_iをX軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(8)Xr[_i]=cube_buffer_x[cube_id[_i]]とYr[_i]=cube_buffer_y[cube_id[_i]]との何れかを満たさないとき、(9)〜(10)を実行する。そうでないとき(何れも満たすとき)、(11)〜(12)を実行する。
(10)ロボットcube_id[_i]のZ座標が1のとき、ロボットcube_id[_i]にZ軸負方向で接するロボットとともに1ステップZ軸負方向に移動させる。そうでないときは、ロボットcube_id[_i]にZ軸正方向で接するロボットとともに1ステップZ軸負方向に移動させる。Motion_Data_Updateを実行する。後述のHorizontal_ChangeとVertical_Changeを実行する。
(11)ロボット_iとロボットcube_id[_i]をともにZ軸負方向に1移動させる。Motion_Data_Updateを実行する。
(12)ロボット_iのX座標が偶数ならば、ロボット_iをX軸正方向に1ステップ移動させる。そうでなければ、ロボット_iをX軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(13)ロボット_iのZ座標値>0のとき、(14)〜(18)を実行する。そうでないとき(19)〜(23)を実行する。
(15)cube_id[_i]の値が、iでもinit_cubeでもないとき、(16)〜(17)を実行する。そうでないとき(18)を実行する。
(16)cube_buffer_z[cube_id[_i]]>0のとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸負方向で接しているロボットをともにZ軸正方向に1ステップ移動させる。そうでないとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸正方向で接しているロボットをともにZ軸正方向に1ステップ移動させる。
(17)Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(18)cube_id[_i]=iのとき、Vertical_Single_Changeを実行し、そうでないとき(cube_id[_i]=init_cube)、Vertical_Single_Change_Invを実行する。
(20)cube_id[_i]の値が、iでもinit_cubeでもないとき、(21)〜(22)を実行する。そうでないとき(23)を実行する。
(21)cube_buffer_z[cube_id[_i]]>0のとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸負方向で接しているロボットをともにZ軸負方向に1ステップ移動させる。そうでないとき、ロボットcude_id[_i]と、ロボットcude_id[_i]にZ軸正方向で接しているロボットをともにZ軸負方向に1ステップ移動させる。
(22)Motion_Data_Updateを実行する。Vertical_Changeを実行する。
(23)cube_id[_i]=iのとき、Vertical_Single_Changeを実行し、そうでないとき、Vertical_Single_Change_Invを実行する。
処理Horizontal_Changeでは、ロボット_iを移動先のロボットcube_id[_i]の位置のX座標、Y座標の何れかにおいて隣接する位置に移動させる。ここでは、移動先の位置のX座標値がcube_buffer_x[cube_id[_i]]であり、Y座標値がcube_buffer_y[cube_id[_i]]であり、ロボット_iと移動先の位置との位置関係に応じて、ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]±1となるように、または、Y座標値がcube_buffer_y[cube_id[_i]]±1となるように、ロボット_iを移動させる。例えば以下のように制御する。
(1)ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]に等しくかつロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に等しいとき、以下を実行して終了する。そうでないなら(2)へ移行する。
ロボット_iのX座標値が偶数ならば、ロボット_iをX軸正方向に1ステップ移動させ、奇数ならばX軸負方向に1移動させる。Motion_Data_Updateを実行する。
ロボット_iのX座標値がcube_buffer_x[cube_id[_i]]に等しくなるまで、ロボット_iをX軸正方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。続いて、ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]との差の絶対値が1に等しくなるまで、ロボット_iをロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に近づく方向に、Y軸方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。
ロボット_iのX座標値とcube_buffer_x[cube_id[_i]]との差の絶対値が1に等しくなるまで、ロボット_iをX軸正方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。
ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]が等しくなるまで、ロボット_iをロボット_iのY座標値がcube_buffer_y[cube_id[_i]]に近づく方向に、Y軸方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。続いて、ロボット_iのX座標値とcube_buffer_x[cube_id[_i]]との差の絶対値が1に等しくなるまで、ロボット_iをX軸負方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。
ロボット_iのX座標値とcube_buffer_x[cube_id[_i]]との差の絶対値が1に等しくなるまで、ロボット_iをX軸負方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。
ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]との差の絶対値が1に等しくなるまで、ロボット_iをY軸負方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。
ロボット_iのY座標値とcube_buffer_y[cube_id[_i]]との差の絶対値が1に等しくなるまで、ロボット_iをY軸正方向に1ステップ移動させMotion_Data_Updateを実行することを繰り返す。
処理Vertical_Changeでは、ロボット_iを移動先の位置のX座標、Y座標の何れかにおいて隣接する位置から、移動先の位置に移動させる。
(1)Zr[_i]=2かつZr[cube_id[_i]]=2のとき、以下を実行し終了する。そうでないなら(2)へ移行する。
(1-1)もしXr[_i]=Xr[cube_id[_i]]ならば(1-1-1)に移行し、そうでないなら(1-1-2)へ移行する。
(1-1-1)もしXr[_i]が偶数ならば(1-1-1-1)に移行し、そうでないなら(1-1-1-2)へ移行する。
(1-1-1-1)ロボット_iとロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[cube_id[_i]]ならば、(1-1-1-1-1)に移行し、それ以外ならば、(1-1-1-1-2)に移行する。
(1-1-1-1-1)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-1-1-2)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-1-2)ロボット_iとロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[cube_id[_i]]ならば(1-1-1-2-1)に移行し、それ以外ならば(1-1-1-2-2)に移行する。
(1-1-1-2-1)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-1-2-2)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2)(Xr[_i]=Xr[cube_id[_i]]以外の場合)もしYr[_i]が偶数ならば、(1-1-2-1)に移行し、それが以外の場合(1-1-2-2)に移行する。
(1-1-2-1)ロボット_iとロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-1-1)に移行し、それ以外の場合(1-1-2-1-2)に移行する。
(1-1-2-1-1)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2-1-2)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2-2)ロボット_iとロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-2-1)に移行し、それ以外の場合(1-1-2-2-2)に移行する。
(1-1-2-2-1)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(1-1-2-2-2)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸負方向に接するロボットをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1)ロボットcube_id[_i]にZ軸正方向に接するロボットをロボット_pairとする。
もしXr[_i]=Xr[_pair]ならば(2-1-1)に移行し、それ以外の場合(2-1-2)に移行する。
(2-1-1)もしXr[_i]が偶数ならば(2-1-1-1)に移行し、それ以外の場合(2-1-1-2)に移行する。
(2-1-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[_pair]ならば(2-1-1-1-1)に移行し、それ以外の場合(2-1-1-1-2)に移行する。
(2-1-1-1-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-1-1-2)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-1-2)(Xr[_i]が偶数ではない場合)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[_pair]ならば(2-1-1-2-1)に移行し、それ以外の場合(2-1-1-2-2)に移行する。
(2-1-1-2-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-1-2-2)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2)(Xr[_i]=Xr[_pair]ではない場合)もしYr[_i]が偶数ならば(2-1-2-1)に移行し、それ以外の場合(2-1-2-2)に移行する。
(2-1-2-1)ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[_pair]ならば(2-1-2-1-1)に移行し、それ以外の場合(2-1-2-1-2)に移行する。
(2-1-2-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2-1-2)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2-2)(Yr[_i]が偶数ではない場合)ロボット_pairをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[_pair]ならば(2-1-2-2-1)に移行し、それ以外の場合(2-1-2-2-2)に移行する。
(2-1-2-2-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2-1-2-2-2)ロボット_pairをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_pairをZ軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1)もしXr[_i]=Xr[cube_id[_i]]ならば(3-1-1)に移行し、それ以外の場合(3-1-2)に移行する。
(3-1-1)もしXr[_i]が偶数ならば(3-1-1-1)に移行し、それ以外の場合(3-1-1-2)に移行する。
(3-1-1-1)ロボット_iとロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[cube_id[_i]]ならば(3-1-1-1-1)に移行し、それ以外の場合(3-1-1-1-2)に移行する。
(3-1-1-1-1)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-1-1-2)ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-1-2)(Xr[_i]が偶数ではない場合)ロボット_iとロボットcube_id[_i]をX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしYr[_i]>Yr[cube_id[_i]]ならば(3-1-1-2-1)に移行し、それ以外の場合(3-1-1-2-2)に移行する。
(3-1-1-2-1)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-1-2-2)ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2)(Xr[_i]=Xr[cube_id[_i]]ではない場合)もしYr[_i]が偶数ならば(3-1-2-1)に移行し、それ以外の場合(3-1-2-2)に移行する。
(3-1-2-1)ロボット_iとロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(3-1-2-1-1)に移行し、それ以外の場合(3-1-2-1-2)に移行する。
(3-1-2-1-1)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2-1-2)ロボット_iをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2-2)ロボット_iとロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
もしXr[_i]>Xr[cube_id[_i]]ならば(3-1-2-2-1)に移行し、それ以外の場合(3-1-2-2-2)に移行する。
(3-1-2-2-1)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(3-1-2-2-2)ロボット_iをY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iとロボット_iにZ軸正方向に接するロボットをZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボットcube_id[_i]をY軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。
(4-1)ロボットcube_id[_i]にZ軸負方向に接するロボットをロボット_pairとする。もしXr[_i]=Xr[_pair]ならば(4-1-1)に移行し、それ以外の場合(4-1-2)に移行する。
(4-1-1)もしXr[_i]が偶数ならば(4-1-1-1)に移行し、それ以外の場合(4-1-1-2)に移行する。
(4-1-1-1)ロボット_pairをX軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。もしYr[_i]>Yr[_pair]ならば(4-1-1-1-1)に移行し、それ以外の場合(4-1-1-1-2)に移行する。
(1)Xr[_i]-cube_buffer_x[cube_id[_i]]の値が、0のとき(1-1)に移行し、1のとき(1-2)に移行し、-1のとき(1-3)に移行する。
(1-1)Yr[_i]-cube_buffer_y[cube_id[_i]]の値が1のときに、ロボット_iをY軸負方向に1ステップ移動し、-1のとき、Y軸正方向に1ステップ移動する。
(1-2)ロボット_iをX軸負方向に1ステップ移動する。
(1-3)ロボット_iをX軸正方向に1ステップ移動する。
(2)Motion_Data_Updateを実行する。
(3)Zr[_i]>0のとき、ロボット_iとロボット_iにZ軸負方向に接するロボットをともに、Z軸負の向きに1ステップ移動させる。そうでないときは、ロボット_iとロボット_iにZ軸正方向に接するロボットをともに、Z軸正の向きに1ステップ移動させる。
(4)Motion_Data_Updateを実行する。
(1)Zr[_i]=2かつZr[cube_id[_i]]=1のとき、(1-1)へ移行し、それ以外の場合(2)に移行する。
(1-1)もしXr[_i]=Xr[cube_id[id]]ならば(1-1-1)に移行し、それ以外の場合(1-1-2)に移行する。
(1-1-1)もしXr[_i]が偶数ならば(1-1-1-1)に移行し、それ以外の場合(1-1-1-2)に移行する。
(1-1-2-2)(Yr[_i]が偶数ではない場合)ロボットcube_id[_i]をZ軸正方向に1ステップ移動する。Motion_Data_Updateを実行する。ロボット_iをY軸負方向に1ステップ移動する。ロボットcube_id[_i]をY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。もしXr[_i]>Xr[cube_id[_i]]ならば(1-1-2-2-1)に移行し、それ以外の場合(1-1-2-2-2)に移行する。
(2)Zr[_i]=-1かつZr[cube_id[_i]]=0のとき、(2-1)に移行し、処理を終了する。
図22は第二実施形態に係る行動制御システム100の機能ブロック図を、図23はその処理フローの例を示す。行動制御システム100は、図22に示すように、行動選択部120と、隣接状態判定部124と、位置更新部123と、位置判定部126と、記憶部140と、通信部150と、入力部160とを含む。行動選択部120は押し出し過程処理部120Aと中間遷移過程処理部120Bと最終展開過程処理部120Cと位置入れ替え部120Dとを含む。
入力部160には、p台のロボットiのそれぞれの中間位置(Xr0[i],Yr0[i],Zr0[i])(言い換えると、p個の中間位置の集合M={(Xr0[0],Yr0[0],Zr0[0]),(Xr0[1],Yr0[1],Zr0[1]),…,(Xr0[p-1],Yr0[p-1],Zr0[p-1])})及び、q個の開始位置の集合S={(Xrs[0],Yrs[0]),(Xrs[1],Yrs[1]),…,(Xrs[q-1],Yrs[q-1])}、q個の目標位置の集合G={(Xre[0],Yre[0]),(Xre[1],Yre[1]),…,(Xre[q-1],Yre[q-1])}が入力され、記憶部140に記憶される。
行動制御システム100が実装されているロボットも含め、全てのロボットは、通信部150を介して、二次元平面上の上下左右前後方向(以下「6方向」ともいう)において隣接する他のロボットと通信することができる。
行動選択部120は、上述の方法で、p台のロボットを制御する(s120)。なお、押し出し過程処理部120A、中間遷移過程処理部120B、最終展開過程処理部120C、位置入れ替え部120Dでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部140に記憶しておく。行動選択部120は、記憶部140から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。
行動選択部120の押し出し過程処理部120Aは、記憶部140からp個の中間位置の集合M及びq個の目標位置の集合G、q個の開始位置の集合Sを取り出し、p個の中間位置の集合Mに位置する少なくともq台のロボットを移動させ、押し出し終了時形状を成すように制御し、押し出し終了時形状における各ロボットの位置を最終展開過程処理部120Cに出力する。
行動選択部120の中間遷移過程処理部120Bは、q個の目標位置の集合Gを取り出し、(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ(圧縮する)、(2)第一方向における座標値が座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ(圧縮する)、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ(圧縮する)、(4)第二方向における座標値が座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させることで、押し出し終了時形状を成すロボットの位置を求める。例えば、第一方向、第二方向、第三方向、第四方向をそれぞれx軸正方向、y軸正方向、x軸負方向、y軸負方向とし、座標値O1をx=0とし、座標値O2をy=0としたときに、上述の[最終展開過程]を実行することで、押し出し終了時形状を成すロボットの位置を求めることができる。目標位置の集合Gに含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部120Cに出力する。
行動選択部120の最終展開過程処理部120Cは、押し出し過程処理部120Aから押し出し終了時形状における各ロボットの位置を取得し、中間遷移過程処理部120Bから目標位置の集合Gに含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを取得する。そして、最終展開過程処理部120Cは、押し出し終了時形状を成すロボットに対して、目標位置の集合に含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の順番とは逆の順番で、目標位置の集合に含まれる位置から押し出し終了時形状を成すロボットの位置を求める際の平行移動方向とは反対の方向にロボットを移動させることで、押し出し終了時形状を成すロボットを目標位置の集合に移動させる。
位置入れ替え部120Dは、目標位置の集合Gから中間位置の集合Mに変形した際の、ロボットiの中間位置の集合Mにおける位置を最終展開過程処理部120Cから取得する。
位置更新部123は、各i=0,1,…,p-1について、i番目のロボットの現在の位置(Xr[i],Yr[i],Zr[i])において、行動選択部120で決定した行動を実行した場合のロボットの移動後(行動後)の位置(Xr'[i],Yr'[i],Zr'[i])を計算し、計算された(Xr'[i],Yr'[i],Zr'[i])で記憶部140に格納されたi番目のロボットの位置を更新する(S123)。言い換えれば、位置更新部123は、行動選択部120で決定した行動に基づいて、ロボットが行動した場合に想定される位置(以下、「想定位置」ともいう)を計算し、ロボットの位置を更新し記憶部140に格納する。
隣接状態判定部124は、ロボットの2次元平面上の上下左右の隣接する位置に、他のロボットが存在するか否かを判定し、(S124),判定結果を記憶部140に格納する。
位置判定部126は、隣接状態判定部124の判定結果を用いて、行動後位置を求め、行動後位置(Xr"[i],Yr"[i],Zr"[i])と想定位置(Xr'[i],Yr'[i],Zr'[i])とが一致するか否かを判定する(S126)。なお、一致しない場合には、移動するように制御されたロボットが何らかのトラブルにより、制御通りに移動できなかったと考えられる。この場合、行動後位置(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])に合わせて補正してもよい。
このような構成により、三次元空間において、第一実施形態と同様の効果を得ることができる。また、中間位置の集合Mの形態を8個のロボットからなるマス単位で構成されたL字もしくは、長方形の平板とすることで処理Transformation_M_to_Mを簡略化している。
本実施形態では、各格子(マス)は、立方体であるが、他の形状であってもよい。格子は左右方向、上下方向及び前後方向に連続して配置される。また、各格子は左右方向で他の二つの格子と隣接し、上下方向で他の二つの格子と隣接し、前後方向で他の二つの格子と隣接する。言い換えると、各格子は、ロボットの移動できる方向と同じ方向においてのみ、他の格子と隣接する。この条件を満たせば、各格子はどのような形状であってもよい。また、「直交」とは、厳密に「垂直に交わること」を意味しなくともよく、例えば、各格子は、平行六面体であってもよく、各格子が他の二つの格子と隣接する方向の一方を上下方向とし、他方を左右方向とすればよく、上下方向及び左右方向とからなる平面に対して平行でない方向を前後方向とすればよい。
第二実施形態と異なる部分を中心に説明する。
[開始、目標および中間位置の集合]
第三実施形態においては、第二実施形態とは中間位置の集合の形状がことなる。第三実施形態における中間位置の集合をM2とする。中間位置の集合M2の形状の条件は、天井が、2×2の4マスによるロボット単位によって構成されており、天井の厚みは1つのロボット分でよい代わりに、かならず天井の下に接するロボットが1つ以上存在していることである(図37参照)。なお、天井にあるロボットi([Xri],[Yri],[Zri])の下に、そのロボットと同じX,Y座標値を持つロボットが存在している場合、それらのロボットは皆、天井からぶら下がって、天井のロボットiに接していなければならない。第三実施形態における開始位置の集合Sと目標位置の集合Gには、4つのロボットが田の字状に連結した形状からなるシードロボットが含まれる。開始位置の集合SにおいてはそのZ軸座標値が最小の位置に、目標位置の集合GにおいてはそのZ軸座標値が最大の位置にX座標、Y座標がともに0の位置にシードロボットが位置する(図35参照)。なお、開始位置Sと目標位置Gにおけるシードロボットの座標位置は同一である。
(1)開始位置の集合Sから押し出し終了時形状1への圧縮過程:[Extend_Process_S]
(2)押し出し終了時形状1から中間位置の集合M2への変形過程:[Make_Ceiling]
(3)押し出し終了時形状2から目標位置の集合Gへの展開過程:[Extend_Process_G]
(4)中間位置の集合M2から押し出し終了時形状2への変形過程:[Translate_from_M2_to_Mobile_G]
(5)中間位置の集合M2におけるロボット入れ替え過程:[Permutation_in_M2]
[開始位置の集合Sから押し出し終了時形状1への圧縮過程]
開始位置の集合Sの形状から押し出し終了時形状1への圧縮過程[Extend_Process_S]は、第二実施形態における最終展開過程にZ軸上方向への以下の圧縮過程を追加したものである。
(1)現時刻tmでの仮想ロボット位置のZ座標の最小値をZmin、Z座標の最大値をZmaxとし、_zw←Zminとして、_zw<Zmaxの間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Z座標値が_zwである仮想ロボットiについて、以下を実行する。
(3-1)仮想ロボットiからZ軸正方向に途切れず隣接している全ての仮想ロボットinの中にZ座標がZmaxのものがない場合、仮想ロボットiから、仮想ロボットinの間の全ての仮想ロボットのdestination_to_be_compressed値を1とし、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのZ座標値をインクリメントし、[Motion_Data_Update]を実行する。_zwをインクリメントする。
[Extend_Process_S]
(1)tm←0とし、各仮想ロボットiの位置を開始位置の集合Sとし[Motion_Data_Update]を実行する。[Extend_Process_X_Minus]を実行する(図36Aから図36Bへの変形に相当)。
(2)[Extend_Process_X_Plus]を実行する(図36Bから図36Cへの変形に相当)。
(3)[Extend_Process_Y_Minus]を実行する(図36Cから図36Dへの変形に相当)。
(4)[Extend_Process_Y_Plus]を実行する(図36Dから図36Eへの変形に相当)。
(5)[Extend_Process_Z_Plus]を実行する(図36Eから図36Fへの変形に相当)。
(6)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、最小値をZmin、各Z座標値_zでの、X座標が0であるロボットの数-1を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-1を_x_num[-_y][ _z-Zmin]に格納する。なお、[Extend_Process_Z_Plus]を実行後の、ロボット群の成す形状を押し出し終了時形状1という。
押し出し終了時形状1から中間位置の集合M2への変形過程(図36Fから図37への変形に相当)は、以下のプロセスである。
(1)押し出し終了時形状1の天井(Z座標が最大値(Zmax)をとるロボットがなす面)に存在するロボット数が4の倍数になるように天井にないロボットのいくつかを天井へ移動させる(図38参照)。
(2)X,Y,Z軸正方向に圧縮を行う。
(3)天井形状が、4つのロボットで構成されるマス単位で作られた長方形になるように変形する(図39、図40参照)。
(4)X軸、Y軸、Z軸正方向で圧縮を行い、ロボットによりX=0、Y=0、Z=最大値の平面が構成されるようにする。
[Make_Ceiling]
(1)後述の[Move_Odd_Robots]を実行する。
(2)[Extend_Process_X_Plus]を実行する。
(3)[Extend_Process_Y_Plus]を実行する。
(4)[Extend_Process_Z_Plus]を実行する。
(5)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、各Z座標値_zでの、X座標が0であるロボットの数-1を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-1を_x_num[-_y][ _z-Zmin]に格納する。
(6)後述の[Make_2x2_Rectangle_Ceiling]を実行する。
(7)[Extend_Process_X_Plus]を実行する。
(8)[Extend_Process_Y_Plus]を実行する。
(9)[Extend_Process_Z_Plus]を実行する。
(10)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax、各Z座標値_zでの、X座標が0であるロボットの数-1を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-1を_x_num[-_y][ _z-Zmin]に格納する。tm-1を変数push_time_startに格納する。
(11)[Extend_Process_S]開始時に開始位置の集合S内の各位置にあった仮想ロボットiが[Extend_Process_S][Make_Ceiling]により移動した先の、押し出し終了時形状2内の位置として、各仮想ロボットiの位置を変数(Xsp[i],Ysp[i],Zsp[i])に格納する。
(12)t=0,push_time_startの各時刻において、変数(transform_S_to_M2_x[t][i],transform_S_to_M2_y[t][i],transform_S_to_M2_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。
(1)Z座標がZmaxであるロボット数をカウントし、変数ceiling_cntに格納する。ceiling_cntの値を4で割った余りの値を、変数add_numに格納する。
(2)_z=(Zmax-Zmin+1)−1から_z=0まで、(3)〜(4)を繰り返す。
(3)_y=_y_num[_z]から_y=0まで、(4)を繰り返す。
(4)もし、_x_num[_z][_y]が、1以上かつadd_numが0でないならば、以下を実行する。(動作例を図38に示す。)
(4-1)変数add_x←0とする。add_xが_x_num[_z][_y]より小さく、add_numが0でない間、(4-2)〜(4-5)を繰り返す。
(4-2)Y座標が_y,Z座標が_zのロボットを全て、X軸正方向に1ステップ移動させる(図38Aから図38Bへの変形に相当)。[Motion_Data_Update]を実行する。このとき、X座標が1、Y座標が_y,Z座標が_zのロボットをロボットipとする。
(4-3)add_xをインクリメントし、add_numをデクリメントする。
(4-4)ロボットipを、Z軸正方向に1ステップ移動させ[Motion_Data_Update]を実行することを、ロボットipのZ座標がZmaxになるまで繰り返す(図38Bから図38Cへの変形に相当)。
(4-5)Y座標がロボットipのY座標,Z座標がZ_maxのロボットを全て、X軸負方向に1ステップ移動させる(図38Cから図38Dへの変形に相当)。[Motion_Data_Update]を実行する。
(5)_z=Zmax−1から_z=0まで、(6)を繰り返す。
(6)もし、_y_num[_z]が、1以上かつadd_numが0でないならば、以下を実行する。
(6-1)変数add_x←0とする。add_xが_y_num[_z]より小さく、add_numが0でない間、(6-2)〜(6-5)を繰り返す。
(6-2)X座標が0,Z座標が_zのロボットを全て、Y軸正方向に1ステップ移動させる。[Motion_Data_Update]を実行する。このとき、X座標が0、Y座標が1,Z座標が_zのロボットをロボットipとする。
(6-3)add_xをインクリメントし、add_numをデクリメントする。
(6-4)ロボットipを、Z軸正方向に1ステップ移動させ[Motion_Data_Update]を実行することを、ロボットipのZ座標がZmaxになるまで繰り返す。
(6-5)X座標がロボットipのX座標,Z座標がZ_maxのロボットを全て、Y軸負方向に1ステップ移動させる。[Motion_Data_Update]を実行する。
(1)天井のX方向、Y方向の長さを決定する。
(1-1)天井に含まれる(Z座標値がZmaxの)ロボットの数をカウントし、ceiling_cntに格納する。ceiling_cntの値を4で割った値に入れ替える(ceiling_cnt←ceiling_cnt/4)。
(1-2)変数ic=1からic=ceiling_cnt-1までの中で、ceiling_cntを割った余りが0となるicを抽出し、ceiling_cntを抽出したicで割った値をjc(=ceiling_cnt/ic)とし、icとjcの組合せの中で(言い換えると、ceiling_cntを被除数とし、icを除数とし、剰余が0のときの除数icと商jcとの組合せの中で)、2×jcがy_num[Zmax-Zmin]より小さく、かつ、2×icがx_num[Zmax-Zmin][0]より小さくなる組合せを抽出し、抽出した組合せの中で、icとjcの差の絶対値が最小となるものを長方形の横縦長さとして選ぶ。選んだicを変数_latに、jcを変数_lonに格納する。
(2)Y軸方向の入れ替えを行う(図39参照)。
(2-1)天井に含まれるロボットのうち、X座標が-_xのロボットの数をカウントし変数y_ceiling_num[_x]に格納する。
(2-2)変数_x=0から_x=-x_num[Zmax-Zmin][0]まで、(2-3)〜(2-8)を繰り返す。
(2-3)y_ceiling_num[_x]が2×_lonより大きい間(2-4)〜(2-8)を繰り返す。
(2-4)X座標が-_xのロボットを全てY軸正方向に1ステップ移動させる(図39Aから図39Bへ変形に相当)。[Motion_Data_Update]を実行する。
(2-5)天井に含まれるロボットのうち、X座標が-_xのロボットの数をカウントし変数y_ceiling_num[_x]に格納する。
(2-6)変数_xx=_x+1から_xx=-x_num[Zmax-Zmin][0]までで、y_ceiling_num[_xx]が2×_lonより小さい最小の_xxについて、(2-7)〜(2-8)を実行する。
(2-7)Y座標が1のロボットを全て、X軸負方向に1ステップ移動させ、[Motion_Data_Update]を実行することを、_xx-_x回実行する(図39Bから図39Cへ変形に相当)。
(2-8)X座標が-_xxのロボットを全て、Y軸負方向に1ステップ移動させ(図39Cから図39Dへ変形に相当)、[Motion_Data_Update]を実行する。
(3)X軸方向の入れ替えを行う(図40参照)。
(3-1)天井に含まれるロボットのうち、Y座標が-_yのロボットの数をカウントし変数x_ceiling_num[_y]に格納する。
(3-2)変数_y=0から_y=-y_num[Zmax-Zmin]まで、(3-3)〜(3-8)を繰り返す。
(3-3)x_ceiling_num[_y]が2×_latより大きい間(3-4)〜(3-8)を繰り返す。
(3-4)Y座標が-_yのロボットを全てX軸正方向に1ステップ移動させる(図40Aから図40Bへの変形に相当)。[Motion_Data_Update]を実行する。
(3-5)天井に含まれるロボットのうち、Y座標が-_yのロボットの数をカウントし変数x_ceiling_num[_y]に格納する。
(3-6)変数_yy=_y+1から_yy=-y_num[Zmax-Zmin]までで、x_ceiling_num[_yy]が2×_latより小さい最小の_yyについて、(2-7)〜(2-8)を実行する。
(3-7)X座標が1のロボットを全て、Y軸負方向に1ステップ移動させ、[Motion_Data_Update]を実行することを、_yy-_y回実行する(図40Bから図40Cへの変形に相当)。
(3-8)Y座標が-_yyのロボットを全て、X軸負方向に1ステップ移動させ(図40Cから図40Dへの変形に相当)、[Motion_Data_Update]を実行する。
押し出し終了時形状2から目標位置の集合Gへ変形する最終展開過程[Extend_Process_G]は、第二実施形態における最終展開過程にZ軸下方向へ圧縮過程を追加した過程となる。よって、押し出し終了時形状2は、Z軸に垂直な仮想面が、第二実施形態における押し出し終了時形状をZ軸正方向無限遠からZ軸負方向に移動して、仮想面に当たったロボットを押して、Z座標がZminではないすべてのロボットについて、ロボットのZ軸負方向に接するロボットが存在している状態になるように圧縮した形状である。ただし、圧縮後の隊形がX座標、Y座標が正の空間に押し込まれるため、X軸、Y軸方向の圧縮の方向が第二実施形態と異なる。
(1)tm←0とし、各仮想ロボットiの位置を目標位置の集合Gとし、[Extend_Process_X_Minus_Inv]を実行する(図41Aから図41Bへの変形に相当)。
(2)後述の[Extend_Process_X_Plus_Inv]を実行する(図41Bから図41Cへの変形に相当)。
(3)後述の[Extend_Process_Y_Minus_Inv]を実行する(図41Cから図41Dへの変形に相当)。
(4)後述の[Extend_Process_Y_Plus_Inv]を実行する(図41Dから図41Eへの変形に相当)。
(5)後述の[Extend_Process_Z_Minus]を実行する(図41Eから図41Fへの変形に相当)。
(6)現時刻tmでの仮想ロボットiの位置で、Z座標の最大値をZmax_g、最小値をZmin_g、各Z座標値_zでの、X座標が0である仮想ロボットの数-1を_y_num_g[_z-_Zmin_g]に、各Z座標値_zでの、Y座標値が_yである仮想ロボットの数-1を_x_num_g[_y][ _z-_Zmin_g]に格納する。tm-1の値を変数push_time_goalに格納する。
(7)目標位置の集合G内の各位置iが圧縮により移動した先の、押し出し終了時形状2内の位置iを変数(Xgp[i],Ygp[i],Zgp[i])に格納する。
(8)t=0〜push_time_goalの各時刻において、変数(transform_P2_to_G_x[t][j],transform_P2_to_G_y[t][j],transform_P2_to_G_z[t][j])に、(motion_x[push_time_goal-t][j],motion_y[push_time_goal-t][j],motion_z[push_time_goal-t][j])の値を格納する。
(1)現時刻tmでの仮想ロボット位置のZ座標の最小値をZmin、Z座標の最大値をZmaxとし、_zw=Zmax-1として、_zw>Zminの間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Z座標値が_zwである仮想ロボットiについて、以下を実行する。
(3-1)仮想ロボットiからZ軸負方向に途切れず隣接している全ての仮想ロボットinの中にZ座標がZminのものがない場合、仮想ロボットiから、仮想ロボットinの間の全ての仮想ロボットのdestination_to_be_compressed値を1として、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのZ座標値をデクリメントし、[Motion_Data_Update]を実行する。_zwをデクリメントする。
(1)全仮想ロボットでのX座標の最小値をXminとする。
(2)_xw=Xminとして、_xw<0の間、(3)〜(5)を繰り返す。
(3)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(4)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(4-1)destination_to_be_compressed[i]←1とする。
(4-2)仮想ロボットiからX軸正方向に途切れず隣接している全ての仮想ロボットinのdestination_to_be_compressed[in]←1とする。
(4-3)_flg_c←1とする。
(5)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をインクリメントし、[Motion_Data_Update]を実行する。_xwをインクリメントする。
(1)現時刻tmでの仮想ロボット位置のX座標の最大値をXmaxとし、_xw=Xmaxとして、_xw>0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)X座標値が_xwである仮想ロボットiについて、以下を実行する。
(3-1)仮想ロボットjからX軸負方向に途切れず隣接している全ての仮想ロボットinの中にX座標が0のものがない場合、隣接する仮想ロボットi〜inのdestination_to_be_compressed[in]←1として、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのX座標値をデクリメントし、[Motion_Data_Update]を実行する。_xwをデクリメントする。
(1)現時刻tmでの仮想ロボット位置のY座標の最小値をYminとし、_yw=Yminとして、_yw<0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1)destination_to_be_compressed[i]←1とする。
(3-2)仮想ロボットjからY軸正方向に途切れず隣接している全ての仮想ロボットinのdestination_to_be_compressed[in]←1とする。
(3-3)_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をインクリメントし、[Motion_Data_Update]を実行する。_ywをインクリメントする。
(1)現時刻tmでの仮想ロボット位置のY座標の最大値をYmaxとし、_yw=Ymaxとして、_yw>0の間、(2)〜(4)を繰り返す。
(2)全ての仮想ロボットiについて変数destination_to_be_compressed[i]←0とする。_flg_c←0とする。
(3)Y座標値が_ywである仮想ロボットiについて、以下を実行する。
(3-1)仮想ロボットiからY軸正方向に途切れず隣接している全ての仮想ロボットinの中にY座標が0のものがない場合、それら隣接の仮想ロボットi〜inのdestination_to_be_compressed[in]←1として、_flg_c←1とする。
(4)_flg_c=1のとき、destination_to_be_compressed[i]=1である全ての仮想ロボットのY座標値をデクリメントし、[Motion_Data_Update]を実行する。_ywをデクリメントする。
本過程では、まず中間位置の集合M2からシードロボットをZ座標が最低となる位置に移動させる(後述するMake_seed_M2に相当)。続いて、中間位置の集合M2にあるロボットを第二実施形態における押し出し過程の逆再生の形で、押し出し終了時形状2へ押し出していく。すなわち、中間位置の集合M2にあるロボットは、まず、X=0の面からX軸正方向に押し出し可能なロボットから、1ずつ押し出されていき、X=0の面にそってシードロボットに接する位置まで移動していく。X=0の面からX軸正方向に押し出し可能なロボットがなくなったら、続いて、X=0、Y=0の柱からY軸正方向に押し出し可能なロボットから、1ずつ押し出されていき、柱にそってシードロボットに接する位置まで移動していく。シードロボットに接する位置に達したロボットは、接しているシードロボットをZ軸下方に押しのけて自らがシードロボットの位置に収まる。押しのけられた元シードであったロボットは、押し出し終了時形状2内の位置に移動していく。その移動は、第二実施形態で示した押し出し過程と同じ移動形態(柱形成→旗形成→最終押し出し形態形成)である。
[Translate_from_M2_to_Mobile_G]
(1)tm←0とし、各ロボットiの位置を(Xsp[i],Ysp[i],Zsp[i])とし、Motion_Data_Updateを実行する。後述の[Make_Seed_M2]を実行する(図42Aから図42Bへの変形に相当)。
(2)後述の[Robot_Translation]を実行する(図42Bから図42Eへの変形に相当)。
(3)シードロボット以外の全ロボットを、X軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(4)シードロボット以外の全ロボットを、Y軸負方向に1ステップ移動させる((3)(4)の処理を併せて図42Eから図42Fへの変形に相当)。Motion_Data_Updateを実行する。tm-1をtranslate_timeに格納する。
(5)t=0〜translate_timeの各時刻において、変数(transform_M2_to_P2_x[t][i],transform_M2_to_P2_y[t][i],transform_M2_to_P2_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。
(6)各ロボットiの位置を(Xtr[i],Ytr[i],Ztr[i])に格納する。
(1)Z座標値がZmaxのロボットを全て、X軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(2)Z座標値がZmaxのロボットを全て、Y軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する((1)(2)の処理を併せて図43Aから図43Bへの変形に相当)。
(3)Z座標値がZmaxで、Y座標値が-1以下のロボットを全て、X軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(4)Z座標値がZmaxで、X座標値が-1以下のロボットを全て、Y軸負方向に1ステップ移動させる。Motion_Data_Updateを実行する((3)(4)の処理を併せて図43Bから図43Cへの変形に相当)。
(5)[Extend_Process_X_Plus]を実行する。
(6)位置(1,0,Zmax),(1,1,Zmax),(0,1,Zmax)にあるロボットをそれぞれのZ座標値がZ座標の最小値Zminに等しくなるまで、1ステップZ軸負方向に移動させ、Motion_Data_Updateを実行することを繰り返す((5)(6)の処理を併せて図43Cから図43Dへの変形に相当)。
(7)現時刻tmでのロボットiの位置で、Z座標の最大値をZmax、Z座標の最小値をZmin、各Z座標値_zでの、X座標が0であるロボットの数-1を_y_num[_z-Zmin]に、各Z座標値_zでの、Y座標値が_yであるロボットの数-1を_x_num[-_y][_z-Zmin]に格納する。
(1)_z=Zmax- Zminから、_z=1までの全ての値で、(2)〜(8)を繰り返す。
(2)_y=y_num[_z]から、_y=0までの全ての値で、(3)〜(8)を繰り返す。
(3)_x=x_num[_z][_y]から、_x=0までの全ての値で、(4)〜(8)を繰り返す。
(4)位置(0,-_y,_z+Zmin)にあるロボットをipとする。
(5)ロボットipとY,Z座標が同じロボットを全て、X軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(6)ロボットipのY座標が0になるまで、ロボットipをY軸正方向に移動させ、Motion_Data_Updateを実行することを繰り返す。
(7)ロボットipがシードロボットの一つに接するまで、ロボットipをZ軸負方向に移動させ、Motion_Data_Updateを実行することを繰り返す。
(8)ロボットipについて、後述のTravel_Goalを実行する。
(9) _z=Zmax- Zminから、_z=1までの全ての値で、(10)〜(14)を繰り返す。
(10)_y=y_num[_z]から、_y=1までの全ての値で、(11)〜(14)を繰り返す。
(11)位置(0,0,_z+Zmin)にあるロボットをipとする。
(12)ロボットipとZ座標が同じロボットを全て、Y軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(13)ロボットipがシードロボットの一つに接するまで、ロボットipをZ軸負方向に移動させ、Motion_Data_Updateを実行することを繰り返す。
(14)ロボットipについて、後述のTravel_Goalを実行する。
(15)_z=Zmaxから、_z=Zmin+1までの全ての値で、(16)〜(17)を繰り返す。
(16)位置(0,0,Zmin+1)にあるロボットをipとする。
(17)ロボットipについて、後述のTravel_Goalを実行する。
(1)最初に本処理が実行されたときに、変数_pole_cnt,_flag_cnt,_flag_y,_flag_z,_flag_x,_flag_zz,_flag_yy,_floor_cntを0に初期化する。最初に本処理が実行されたときに、変数_flag_allに、押し出し終了時形状2内の、X座標が0で、Y座標が負のロボットの総和を、変数_floor_allにX座標が負のロボットの総和を格納する。
(2-1)ロボットipのX座標が1、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。
(2-2)ロボットipのX座標が0、Y座標が1ならば、ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。
(2-3)ロボットipのX座標が0、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。
(2-4)ロボットipとX,Y座標値が同じで、ロボットipと連なっている全てのロボットをZ軸負方向に移動させる。Motion_Data_Updateを実行する。_pole_cntをインクリメントする。
(3-1)_flag_y<_y_num_g[_flag_z+Zmin_g]ならば、(3-2)〜(3-8)を実行する。そうでないならば、(3-8)のみ実行する。
(3-2)ロボットipのX座標が0、Y座標が1ならば、ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。ロボットipの上方に隣接する全てのロボットをY軸負方向に移動させる。Motion_Data_Updateを実行する。
(3-3)ロボットipのX座標が1、Y座標が1ならば、ロボットipの上方に隣接する全てのロボットをY軸負方向に移動させる。Motion_Data_Updateを実行する。
(3-4)ロボットipのX座標が0、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをX軸正方向に移動させる。Motion_Data_Updateを実行する。
(3-5)ロボットipとX,Y座標値が同じで、ロボットipと連なっている全てのロボットをZ軸負方向に移動させる。Motion_Data_Updateを実行する。
(3-6)ロボットipがZ軸負方向で接しているロボットをigとし、ロボットigをZ軸負方向に移動させ、Motion_Data_Updateを実行することを、ロボットigのZ座標が_flag_z+Zmin_gになるまで繰り返す。
(3-7)ロボットigとZ座標が等しいロボットを全て、Y軸正方向に1ステップ移動させる。_flag_cntをインクリメントする。
(3-8)_flag_yをインクリメントし、_flag_y>y_num_g[_flag_z+Z_min_g]ならば、_flag_zをインクリメントし、_flag_yを0にする。
(4-1)ロボットipのX座標が1、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。ロボットipの上方に隣接する全てのロボットをX軸負方向に移動させる。Motion_Data_Updateを実行する。
(4-2)ロボットipのX座標が1、Y座標が1ならば、ロボットipの上方に隣接する全てのロボットをX軸負方向に移動させる。Motion_Data_Updateを実行する。
(4-3)ロボットipのX座標が0、Y座標が0ならば、ロボットipの上方に隣接する全てのロボットをY軸正方向に移動させる。Motion_Data_Updateを実行する。
(4-4)ロボットipとX,Y座標値が同じで、ロボットipに連なる全てのロボットをZ軸負方向に移動させる。Motion_Data_Updateを実行する。
(4-5)ロボットipがZ軸負方向で接しているロボットをigとし、ロボットigをZ軸負方向に移動させ、Motion_Data_Updateを実行することを、ロボットigのZ座標が_flag_zz+Zmin_gになるまで繰り返す。
(4-6)ロボットigをY軸正方向に移動させ、Motion_Data_Updateを実行することを、ロボットigのZ座標が_flag_yy+1になるまで繰り返す。
(4-7)ロボットigとY,Z座標が等しいロボットを全て、X軸正方向に1ステップ移動させる。_flag_cntをインクリメントする。
(4-8)_flag_xをインクリメントし、_flag_x>x_num_g[_flag_zz+Z_min_g][_flag_yy]ならば、_flag_yyをインクリメントし、さらに_flag_yy>y_num_g[_flag_zz+Z_min_g]ならば、_flag_yy←0とし、_flag_zzをインクリメントする。_flag_xを0にする。
中間位置の集合M2の形状に関する条件により、中間位置の集合M2の天井を構成する各ロボットのうち、Z軸負方向に隣接するロボットを持つかどうかについては、天井を構成するロボットのうち、少なくとも一つがそれに当てはまるという保証しかない。図44に示すように、本過程でのロボット入れ替え動作においては、入れ替え元のロボットi_originが天井の上部に押し出されたのちに、天井の上を入れ替え元ロボットi_originが入れ替え先ロボットi_targetの近傍まで移動する動作が含まれるため、入れ替え元ロボットi_originを選ぶ際に、必ず、Z軸負方向に隣接するロボットを持つロボットを入れ替え元のロボットi_originとして選択しなければならないという制限がある。そのため、本処理では、最初に入れ替え元のロボットi_originとしてそのような、Z軸負方向に隣接するロボットを持つロボットを選択する。そして、その後の入れ替え動作の後に、入れ替え元のロボットi_originが元あった位置(X_origin,Y_origin,Z_origin)を入れ替え先とするロボットi_origin_2が、位置(X_origin,Y_origin,Z_origin)にたどりついたのちは、ロボットi_origin_2がその後の入れ替え動作における仮想的な、入れ替え元ロボットとしてふるまうようにする。すなわち、以下のような流れになる。
(2)ロボットi_originを入れ替え先ロボットi_target[0]の位置に移動させ、ロボットi_target[0]をロボットi_target[0]が元あった場所から追い出す。i_perm←0とする。
(3)以下の繰り返し処理(4)を実行する。
(4)ロボットi_target[i_perm]をロボットi_target[i_perm]の入れ替え先ロボットi_target[i_perm+1]の位置に移動させる。i_target[i_perm+1]=i_originでない場合は、ロボットi_target[i_perm+1]をロボットi_target[i_perm+1]が元あった場所から追い出す。i_permをインクリメントし、(4)を繰り返す。i_target[i_perm+1]=i_originであるなら終了し(5)へ。
(5) i_origin_2←i_target[i_perm]とし、入れ替えの済んでいない任意のロボットをi_target[i_perm]とする。入れ替えの済んでいないロボットがなくなるまで(7)〜(10)を繰り返す。
(7)ロボットi_origin_2を入れ替え先ロボットi_target[i_perm]の位置に移動させ、ロボットi_target[i_perm]をロボットi_target[i_perm]が元あった場所から追い出す。
(8)以下の繰り返し処理(9)を実行する。
(9)ロボットi_target[i_perm]をロボットi_target[i_perm]の入れ替え先ロボットi_target[i_perm+1]の位置に移動させ、ロボットi_target[i_perm+1]をロボットi_target[i_perm+1]が元あった場所から追い出す。追い出されたロボットがロボットi_origin_2であるならば終了して(10)へ、そうでないなら(9)を繰り返す。
(10)ロボットi_origin_2を(5)の処理が行われた時点の位置に戻す。
[Permutation_in_M2]
(1)後述のPermutation_Initializationを実行する。
(2)後述のPermutaiton_Originを実行する。
(3)後述のPermutaiton_Othersを実行する。
(4)tm-1の値を、permutation_timeに格納し、t=0〜permutation_timeの各時刻において、変数(transform_M2_to_M2_x[t][i],transform_M2_to_M2_y[t][i],transform_M2_to_M2_z[t][i])に、(motion_x[t][i],motion_y[t][i],motion_z[t][i])の値を格納する。
(1)全てのロボットi(i=0,…,p-1)で、変数already[i]を0に、cube_id[i]をpに、cube_after[i]をpに初期化する。
(2)全てのi(i=0,…,p-1)について、位置(Xgp[i],Ygp[i],Zgp[i])と等しくなる位置(Xtr[j],Ytr[j],Ztr[j])を特定し、ロボットiの入れ替え先を示す変数cube_id[i]の値をjとする。
(3)(Xsp[i],Ysp[i],Zsp[i])の値を(cube_buffer_x[i],cube_buffer_y[i],cube_buffer_z[i])に格納する。
(4)tm←0とし、各ロボットの位置を(Xsp[i],Ysp[i],Zsp[i])とする。Motion_Data_Updateを実行する。
(1)天井に存在し、Z軸負方向に接するロボットを持つロボットciを一つ選択する。
(2)flg_ci←0とし、_i=cube_id[ci]とする。
(3)_iがciに等しくない間、(4)〜(7)を繰り返す。
(4)flg_ci=0の場合、_i←ci、flg_ci←1とする。
(5)cube_after[_i]←cube_id[_i]とする。
(6-1)X座標がcube_buffer_x[ci]に、Y座標がcube_buffer_y[ci]に等しいロボットiiを一台特定し、_dest←iiとして、[Horizontal_Change_Odd]を実行する。
(6-2)ロボット_iのX座標と、cube_buffer_x[ci]の差が1の場合は、ロボット_iをX軸負方向に1ステップ移動させる。―1の場合は、ロボット_iをX軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(6-3)ロボット_iのY座標と、cube_buffer_y[ci]の差が1の場合は、ロボット_iをY軸負方向に1ステップ移動させる。―1の場合は、ロボット_iをY軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(6-4)ロボット_iのZ座標値がZmaxになるまで、ロボット_iとX,Y座標が同じ全てのロボットをZ軸負方向に1ステップ移動し、Motion_Data_Updateを実行することを繰り返す。
(6-5)変数seed_perm←_i,zero_perm=ciとする。
(8)already[ci]←1とする。
(1)全てのロボットciに対して、already[i]=0のとき(2)以下を実行する。
(2)flg_ci←0、flg_oo=0とし、_i=cube_id[ci]とする。
(3)ci=_iでない場合、
flg_oo←1とし、変数x0にロボットseed_permのX座標を、変数y0にロボットseed_permのY座標を、バッファリングする。[Change_Hetero_Ceiling(seed_perm,seed_perm,ci)]を実行する。
(4)_iがciに等しくない間、(5)〜(8)を繰り返す。
(5)flg_ci=0の場合、_i←ci、flg_ci←1とする。
(6)cube_after[_i]←cube_id[_i]とする。
(7)ciがcube_id[_i]に等しくない場合、後述の[Change_Hetero_Ceiling(seed_perm,_i,cube_id[_i])]を実行する。等しい場合、[Change_Hetero_Ceiling(seed_perm,_i,seed_perm)]を実行する。
(8)already[_i]←1とし、_i=cube_id[_i]とする。
(9)already[ci]←1とする。
(10-1)X座標がx0に、Y座標がy0に、等しいロボットiiを1つ選んで、_dest←iiとし、[Horizontal_Change_Odd]における_iをseed_permに置き換えて、[Horizontal_Change_Odd]を実行する。
(10-2)ロボットseed_permのX座標と、ロボットiiのX座標との差が1の場合は、ロボットseed_permをX軸負方向に1ステップ移動させる。―1の場合は、ロボットseed_permをX軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(10-3)ロボットseed_permのY座標と、ロボットiiのY座標との差が1の場合は、ロボットseed_permをY軸負方向に1ステップ移動させる。―1の場合は、ロボットseed_permをY軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(10-4)ロボットseed_permと同じX,Y座標値のロボットを全て、Z軸負方向に1ステップ移動させ、Motion_Data_Updateを実行することを、ロボットseed_permと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
[Horizontal_Change_Odd]は、第二実施形態の[Horizontal_Change]にて、cube_id[_i]を_destに置き換えて、ステップ(1)を以下のものに置き換えた処理である。
(1)ロボット_iのX座標値がcube_buffer_x[_dest]に等しくかつロボット_iのY座標値がcube_buffer_y[_dest]に等しいとき、以下を実行して終了する。そうでないなら(2)へ移行する。
ロボット_iのX座標値が偶数ならば、ロボット_iをX軸負方向に1ステップ移動させ、奇数ならばX軸正方向に1移動させる。Motion_Data_Updateを実行する。
(1)_io=_iの場合、_iと同じX,Y座標値のロボットを全て、Z軸正方向に1ステップ移動させる。Motion_Data_Updateを実行する。
(2)_dest←_idとし[Horizontal_Change_Odd]を実行する。
(3)ロボット_iのZ座標とロボット_idのZ座標が等しくなるまで、_idと同じX,Y座標値のロボットを全て、Z軸正方向に1ステップ移動させ、Motion_Data_Updateを実行することを繰り返す。
(4)後述の[Vertical_Change_Ceiling]を実行する。
(5) ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxより小さいならば、ロボット_iと同じX,Y座標値のロボットを全て、Z軸正方向に1ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxより大きいならば、ロボット_iと同じX,Y座標値のロボットを全て、Z軸負方向に1ステップ移動させ、Motion_Data_Updateを実行することを、ロボット_iと同じX,Y座標値のロボットの中で最大のZ座標値のもののZ座標がZmaxになるまで繰り返す。
(1)ロボット_iとロボット_idのX座標が等しい時、(1-1)に移行し、それ以外の場合(2)に移行する。
ロボット_idとX、Y座標が等しく、ロボット_idよりZ座標が大きい全てのロボットをY軸負方向に1ステップ移動する。Motion_Data_Updateを実行する。
(2)もしロボット_iのY座標が奇数ならば(2-1)に移行し、それ以外の場合(2-2)に移行する。
[Transformation_from_S_to_G_Hetero_2]
(1)[Extend_Process_S]を実行する。
(2)[Make_Ceiling]を実行する。
(3)[Extend_Process_G]を実行する。
(4)[Translate_from_M2_to_Mobile_G]を実行する。
(5)[Permutation_in_M2]を実行する。
(6)t=0〜push_time_startにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_S_to_M2_x[t][i],transform_S_to_M2_y[t][i],transform_M_to_M2_z[t][i])の値を格納する。
(7)t=push_time_start+1〜push_time_start+1+permutation_timeにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M2_to_M2_x[t-(push_time_start+1)][i],transform_M2_to_M2_y[t-(push_time_start+1)][i],transform_M2_to_M2_z[t-(push_time_start+1)][i])の値を格納する。
(8)t=push_time_start+1+permutation_time+1
〜push_time_start+1+permutation_time+1+translate_timeにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_M2_to_P2_x[t-(push_time_start+1+permutation_time+1)][cube_after[i]],transform_M2_to_P2_y[t-(push_time_start+1+permutation_time+1)][cube_after[i]],transform_M2_to_P2_z[t-(push_time_start+1+permutation_time+1)][cube_after[i]])の値を格納する。
(9)t=push_time_start+1+permutation_time+1+translate_time+1〜push_time_start+1+permutation_time+1+translate_time+1+push_time_goalにおいて、変数(transform_S_to_G_x[t][i],transform_S_to_G_y[t][i],transform_S_to_G_z[t][i])に、(transform_P2_to_G_x[t-(push_time_start+1+permutation_time+1+translate_time+1)][i],transform_P2_to_G_y[t-(push_time_start+1+permutation_time+1+translate_time+1)][i],transform_P2_to_G_z[t-(push_time_start+1+permutation_time+1+translate_time+1)][i])の値を格納する。
図22は第三実施形態に係る行動制御システム100の機能ブロック図を、図23はその処理フローの例を示す。行動制御システム100は、図22に示すように、行動選択部120と、隣接状態判定部124と、位置更新部123と、位置判定部126と、記憶部140と、通信部150と、入力部160とを含む。行動選択部120は押し出し過程処理部120Aと中間遷移過程処理部120Bと最終展開過程処理部120Cと位置入れ替え部120Dと中間位置変形部120Eを含む。
行動選択部120の処理内容以外は第二実施形態と同様である。
行動選択部120は、上述の方法で、p台のロボットを制御する(s120)。なお、押し出し過程処理部120A、中間遷移過程処理部120B、最終展開過程処理部120C、位置入れ替え部120D、中間位置変形部120Eでは、ロボットの動作開始前に、各ロボットの動作の順序、方向を予め計算し、記憶部140に記憶しておく。行動選択部120は、記憶部140から各時刻における各ロボットの動作を取り出し、各時刻において移動させるロボットに対して移動方向を示す制御信号を送信し、各ロボットはその制御信号に従って動作する。
行動選択部120の中間遷移過程処理部120Bは、q個の目標位置の集合Gを取り出し、(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ(圧縮する)、(2)第一方向における座標値が座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ(圧縮する)、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ(圧縮する)、(4)第二方向における座標値が座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させ、(5)第五方向における座標値が座標値O3ではない目標位置の集合に含まれる位置を、第五方向において隣接する目標位置の集合に含まれる位置が存在するように、第六方向に平行移動させることで、押し出し終了時形状2を成すロボットの位置を求める。例えば、第一方向、第二方向、第三方向、第四方向、第五方向、第六方向をそれぞれx軸正方向、y軸正方向、x軸負方向、y軸負方向、z軸正方向、z軸負方向とし、座標値O1をx=0とし、座標値O2をy=0とし座標値O3をz=Zminとしたときに、上述の[Extend_Process_G]を実行することで、押し出し終了時形状2を成すロボットの位置を求めることができる。目標位置の集合Gに含まれる位置から押し出し終了時形状2を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを最終展開過程処理部120Cに出力する。
中間位置変形部120Eは、押し出し終了時形状1を成すロボットの位置を受け取り、上述の[Make_Ceiling]を実行することで、中間位置の集合M2を成すロボットの位置を求め、押し出し終了時形状1を中間位置の集合M2に変形する際の各ロボットの移動の順番と方向とを求める。
行動選択部120の押し出し過程処理部120Aは、中間位置の集合M2を成すロボットの位置を受け取り、上述の[Translate_from_M2_to_Mobile_G]を実行することで、押し出し終了時形状2を成すロボットの位置を求め、中間位置の集合M2を押し出し終了時形状2にに変形する際の各ロボットの移動の順番と方向とを求める。
行動選択部120の最終展開過程処理部120Cは、押し出し過程処理部120Aから押し出し終了時形状2における各ロボットの位置を取得し、中間遷移過程処理部120Bから目標位置の集合Gに含まれる位置から押し出し終了時形状2を成すロボットの位置を求める際の各ロボットの移動の順番と方向とを取得する。そして、最終展開過程処理部120Cは、押し出し終了時形状2を成すロボットに対して、目標位置の集合Gに含まれる位置から押し出し終了時形状2を成すロボットの位置を求める際の順番とは逆の順番で、目標位置の集合Gに含まれる位置から押し出し終了時形状2を成すロボットの位置を求める際の平行移動方向とは反対の方向にロボットを移動させることで、押し出し終了時形状2を成すロボットを目標位置の集合Gに移動させる。
位置入れ替え部120Dは、目標位置の集合Gから押し出し終了時形状2に変形した際の、ロボットiの押し出し終了時形状2における位置を押し出し過程処理部120Aから取得する。
このような構成により、第二実施形態と同様の効果を得ることができる。なお、第一実施形態では、中間位置の集合Mの形は2×2×2の8マスのロボット単位で構成される矩形またはL字の平板形状であるが、第三実施形態では、中間位置の集合M2の形の条件は2×2の4マスのロボット単位で構成される長方形からなる天井または床を持っていさえすればよく(要はZ軸方向における最大値または最小値となる位置を通る平面上にロボット単位で構成される矩形の平板状を成すようにロボットが配置されていればよい)、それに伴い、変形過程が単純化することができる。なお、第二実施形態の変形例と同様の変形が可能である。
本発明は上記の実施形態及び変形例に限定されるものではない。例えば、上述の各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。その他、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。
また、上記の実施形態及び変形例で説明した各装置における各種の処理機能をコンピュータによって実現してもよい。その場合、各装置が有すべき機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、上記各装置における各種の処理機能がコンピュータ上で実現される。
Claims (13)
- p>qとし、p個の開始位置の集合に配置されたp台の制御対象物をq個の目標位置の集合に移動させるための行動制御を行う行動制御システムであって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第四方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、
制御対象物は、当該制御対象物の2次元平面上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、及び、第四方向において隣接する第四位置とし、静止するか、または、二次元平面上の第一〜第四位置の何れかに移動するように制御されるものとし、
第一方向におけるある位置を通る、第二方向に平行な軸に向かって、前記平行な軸に対する距離が大きい、目標位置の集合に含まれる位置から順に、前記平行な軸に対する距離が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、平行移動させることで、中間形態隊列を成す制御対象物の位置を求める中間遷移過程処理部と、
前記中間形態隊列を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理部とを含む、
行動制御システム。 - 請求項1の行動制御システムであって、
前記中間形態隊列と第二方向における同じ位置に位置する制御対象物の個数が同じで、かつ、第二方向における同じ位置に位置する制御対象物全てが第一方向において連続して隣接している隊列を押し出し隊列とし、少なくともq台の制御対象物を移動させ、押し出し隊列を成すように制御する押し出し過程処理部を含み、
前記中間遷移過程処理部は、前記押し出し隊列を、第二方向の位置毎に第一方向に平行移動させることで、前記中間形態隊列に変形する、
行動制御システム。 - 請求項2の行動制御システムであって、
前記押し出し隊列は、第二方向における位置i_yに位置する制御対象物の個数n[i_y]と、第二方向における前記位置i_yに隣接する位置i_y-1に位置する制御対象物の個数n[i_y-1]との大小関係に応じて、前記位置i_yにおいて第一方向において連続して隣接している制御対象物の一端または他端の制御対象物と、前記位置i_y-1において第一方向において連続して隣接している制御対象物の一端または他端の制御対象物とを第二方向において合わせた隊列であり、
前記押し出し過程処理部は、前記押し出し隊列の第一方向における同じ位置に位置するの制御対象物の個数分だけ、開始位置の集合から制御対象物を第二方向に移動させ、その後、開始位置の集合の外にある制御対象物を全て第一方向に移動させる処理を繰り返し、その後、第二方向に移動させることで、押し出し隊列を形成する、
行動制御システム。 - 請求項2の行動制御システムであって、
前記押し出し過程処理部は、制御対象物が第二方向に平行な列をなすように、開始位置の集合から制御対象物を移動させ、その後、前記第二方向に平行な列に沿って、他の制御対象物を第二方向に、前記中間形態隊列内での第二方向の位置まで移動させ、その後、第一方向に移動させることで、押し出し隊列を形成する、
行動制御システム。 - p>qとし、p個の開始位置の集合に配置されたp台の制御対象物をq個の目標位置の集合に移動させるための行動制御を行う行動制御方法であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第四方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、
制御対象物は、当該制御対象物の2次元平面上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、及び、第四方向において隣接する第四位置とし、静止するか、または、二次元平面上の第一〜第四位置の何れかに移動するように制御されるものとし、
中間遷移過程処理部が、第一方向におけるある位置を通る、第二方向に平行な軸に向かって、前記平行な軸に対する距離が大きい、目標位置の集合に含まれる位置から順に、前記平行な軸に対する距離が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、平行移動させることで、中間形態隊列を成す制御対象物の位置を求める中間遷移過程処理ステップと、
最終展開過程処理部が、前記中間形態隊列を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記中間形態隊列を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理ステップとを含む、
行動制御方法。 - 請求項1から4の何れかの行動制御システムとしてコンピュータを機能させるためのプログラム。
- p>qとし、p台の制御対象物をq個の開始位置の集合からq個の目標位置の集合に移動させるための行動制御を行う行動制御システムであって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の3次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、3次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ、(2)第一方向における座標値が前記座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ、(4)第二方向における座標値が前記座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させることで、押し出し終了時形状を成す制御対象物の位置を求める中間遷移過程処理部と、
前記押し出し終了時形状を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理部とを含む、
行動制御システム。 - 請求項7の行動制御システムであって、
前記中間遷移過程処理部は、前記座標値O1及び前記座標値O2において制御対象物が平面を成すように制御対象物の位置を求める、
行動制御システム。 - 請求項7または請求項8の行動制御システムであって、
p>qとし、p台の制御対象物を、q個の開始位置の集合からp個の中間位置の集合に移動させ、p個の中間位置の集合からq個の目標位置の集合に移動させ、
M,N,Qをそれぞれ2以上の整数の何れかとし、中間位置の集合において、p台の前記制御対象物は、M×N×Q台毎に1つの制御対象物単位を構成し、1つの制御対象物単位を構成するM×N×Q台の制御対象物はそれぞれ3つ以上の方向において当該制御対象物単位を構成する他の制御対象物と隣接し、
Rを3以上の整数の何れかとし、p=M×N×Q×Rとし、中間位置の集合は、第五方向におけるある位置を通る、第一方向と第二方向との成す第1-2平面上にR個の制御対象物単位がL字または矩形の平板状を成すように配置された形状である、
行動制御システム。 - 請求項9の行動制御システムであって、
制御対象物の移動に伴って生じる、または、制御対象物の移動する方向と反対の方向に移動する仮想的な存在をボイドとし、p台の制御対象物を、p個の中間位置の集合から押し出し終了時形状に変形させる際に、中間位置の集合の中から外へ制御対象物を移動させるときに中間位置の集合の中に生じたボイドがボイドが生じた位置単位から、中間位置の集合の最も遠い位置に存在する制御対象物に移動するように制御する、
行動制御システム。 - 請求項7または請求項8の行動制御システムであって、
p>qとし、p台の制御対象物を、q個の開始位置の集合からp個の中間位置の集合に移動させ、p個の中間位置の集合からq個の目標位置の集合に移動させ、
M,Nをそれぞれ2以上の整数の何れかとし、中間位置の集合において、M×N台毎に1つの制御対象物単位を構成し、Rを1以上の整数の何れかとし、中間位置の集合は、第五方向における最大値または最小値となる位置を通る、第一方向と第二方向との成す第1-2平面上にR個の制御対象物単位が矩形の平板状を成すように配置された形状である、
行動制御システム。 - p>qとし、p台の制御対象物をq個の開始位置の集合からq個の目標位置の集合に移動させるための行動制御を行う行動制御方法であって、
第一方向に対して平行でない方向を第二方向とし、第一方向に対して反対の方向を第三方向とし、第二方向に対して反対の方向を第四方向とし、第一方向と第二方向との成す平面に対して平行でない方向を第五方向とし、第五方向に対して反対の方向を第六方向とし、各開始位置及び各目標位置は、それぞれ前記第一方向〜第六方向の少なくとも何れかの方向において他の開始位置及び目標位置と隣接し、目標位置の集合及び開始位置の集合はそれぞれ一塊の任意の形状を成し、
前記制御対象物は、当該制御対象物の3次元空間上の第一方向において隣接する第一位置、第二方向において隣接する第二位置、第三方向において隣接する第三位置、第四方向において隣接する第四位置とし、第五方向において隣接する第五位置とし、第六方向において隣接する第六位置とし、静止するか、または、3次元空間上の第一〜第六位置の何れかに移動するように制御されるものとし、
(1)第一方向におけるある座標値O1以下となるように、第一方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第一方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第三方向に平行移動させ、(2)第一方向における座標値が前記座標値O1ではない目標位置の集合に含まれる位置を、第一方向において隣接する目標位置の集合に含まれる位置が存在するように、第一方向に平行移動させ、(3)第二方向におけるある座標値O2以下となるように、第二方向における座標値が大きい、目標位置の集合に含まれる位置から順に、第二方向における座標値が小さい、目標位置の集合に含まれる位置と隣接する場合にはその隣接する位置と一緒に、第四方向に平行移動させ、(4)第二方向における座標値が前記座標値O2ではない目標位置の集合に含まれる位置を、第二方向において隣接する目標位置の集合に含まれる位置が存在するように、第二方向に平行移動させることで、押し出し終了時形状を成す制御対象物の位置を求める中間遷移過程処理ステップと、
前記押し出し終了時形状を成す制御対象物に対して、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の順番とは逆の順番で、前記目標位置の集合に含まれる位置から前記押し出し終了時形状を成す制御対象物の位置を求める際の平行移動方向とは反対の方向に制御対象物を移動させる最終展開過程処理ステップとを含む、
行動制御方法。 - 請求項7から11の何れかの行動制御システムとしてコンピュータを機能させるためのプログラム。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2015016074 | 2015-01-29 | ||
JP2015016074 | 2015-01-29 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2016146159A true JP2016146159A (ja) | 2016-08-12 |
JP6392187B2 JP6392187B2 (ja) | 2018-09-19 |
Family
ID=56686151
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2015159322A Active JP6392187B2 (ja) | 2015-01-29 | 2015-08-12 | 行動制御システム、その方法及びプログラム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6392187B2 (ja) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20230018658A1 (en) * | 2019-12-16 | 2023-01-19 | Nippon Telegraph And Telephone Corporation | Action control apparatus, method and program for the same |
WO2023195065A1 (ja) * | 2022-04-05 | 2023-10-12 | 日本電信電話株式会社 | 制御装置、方法及びプログラム |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05119835A (ja) * | 1991-10-29 | 1993-05-18 | Kawasaki Heavy Ind Ltd | ロボツト装置 |
JP2014079819A (ja) * | 2012-10-12 | 2014-05-08 | Nippon Telegr & Teleph Corp <Ntt> | ロボット協調搬送計画装置、方法、プログラム |
JP2014211667A (ja) * | 2013-04-17 | 2014-11-13 | 日本電信電話株式会社 | ロボット協調搬送計画装置、方法及びプログラム |
-
2015
- 2015-08-12 JP JP2015159322A patent/JP6392187B2/ja active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05119835A (ja) * | 1991-10-29 | 1993-05-18 | Kawasaki Heavy Ind Ltd | ロボツト装置 |
JP2014079819A (ja) * | 2012-10-12 | 2014-05-08 | Nippon Telegr & Teleph Corp <Ntt> | ロボット協調搬送計画装置、方法、プログラム |
JP2014211667A (ja) * | 2013-04-17 | 2014-11-13 | 日本電信電話株式会社 | ロボット協調搬送計画装置、方法及びプログラム |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20230018658A1 (en) * | 2019-12-16 | 2023-01-19 | Nippon Telegraph And Telephone Corporation | Action control apparatus, method and program for the same |
WO2023195065A1 (ja) * | 2022-04-05 | 2023-10-12 | 日本電信電話株式会社 | 制御装置、方法及びプログラム |
Also Published As
Publication number | Publication date |
---|---|
JP6392187B2 (ja) | 2018-09-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Murray et al. | Robot motion planning on a chip. | |
CN102682103B (zh) | 一种面向海量激光雷达点云模型的三维空间索引方法 | |
Roy et al. | Stochastic models for unit-load operations in warehouse systems with autonomous vehicles | |
Merschformann et al. | RAWSim-O: A simulation framework for robotic mobile fulfillment systems | |
JP6879233B2 (ja) | 制御装置、方法及びプログラム | |
CN108805261A (zh) | 基于八叉树的卷积神经网络 | |
JP6392187B2 (ja) | 行動制御システム、その方法及びプログラム | |
Xu et al. | Local motion simulation using deep reinforcement learning | |
JP6489923B2 (ja) | 行動制御システム、及びそのプログラム | |
Konobrytskyi et al. | 5-Axis tool path planning based on highly parallel discrete volumetric geometry representation: Part I contact point generation | |
GB2227106A (en) | Detecting collision | |
JP6559582B2 (ja) | 行動制御システム、その方法及びプログラム | |
CN103871086A (zh) | 基于fpga构建的分层次栅格转矢量处理方法 | |
JP6685957B2 (ja) | 制御対象物位置入れ替え制御装置、制御対象物位置入れ替え制御方法、プログラム | |
JP6939396B2 (ja) | 隊列制御装置、隊列制御方法、およびプログラム | |
JP6633467B2 (ja) | 行動制御システム、行動制御方法、プログラム | |
Qin et al. | Performance Analysis of Multi-Tote Storage and Retrieval Autonomous Mobile Robot Systems | |
Березовський | 3D models of switching patterns of switching plants on elements by Berezovsky. | |
CN109993338A (zh) | 一种链路预测方法及装置 | |
JP6946933B2 (ja) | 隊列制御装置、隊列制御方法、およびプログラム | |
CN109901164B (zh) | 一种合成孔径雷达的分布式后向投影成像方法 | |
JP6939395B2 (ja) | 制御装置、方法及びプログラム | |
JP6881352B2 (ja) | 制御装置、方法及びプログラム | |
WO2021124410A1 (ja) | 行動制御装置、その方法及びプログラム | |
JP6553000B2 (ja) | 制御対象物位置入れ替え制御装置、制御対象物位置入れ替え制御方法、プログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20170829 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20180619 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20180621 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20180807 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20180821 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20180822 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6392187 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |