A Grid-Based Motion Planning Approach for Coherent Groups

Algorithm 3

Generating all the neighbors of .

Input.  , Obstacle space , , ,

Output.  A neighbor list of

(1)    Initialize a queue , Initialize a list

(2)    Clone from

(3)    if all grids in boundary grid of have no collision-free neighbors

(4)        return null

(5)    end if

  Step  6–8 are used to rotate /

(6)    if   is collision-free

(7)        add into

(8)    end if

  Step  9–23 are used to moving the boundary grids of   according to the compact property  /

(9)    let represent the kth boundary grid of , num represents the total number of

boundary grid of

(10)    for ( = 1; <= num; ++)

(11)        if   have collision-free neighbors

(12)          let be the vector from to one of its free neighbors with step size of or

(13)           add + to and add all other grids in except into a queue

according to the distance to

(14)      while   is not empty

(15)                    = Dequeue(Q)

(16)                  if   + is collision-free, add + to

(17)              else add + to , where is the new direction which ensures the gird-set

satisfying the compact property

(18)                  end if

(19)      end while

(20)          add into

(21)        end if

(22)    end for

(23)    return