A Grid-Based Motion Planning Approach for Coherent Groups

Algorithm 2

The local planner connecting any two neighbor configurations.

Input. , , The desired Configuration , Obstacle space , ,

Output. Local path which connects and

(1)    Initialize a priority queue

(2)    SUCCESS = false;

(3)    Insert   into

(4)    while   is not empty and SUCCESS = false do

(5)           = Dequeue()

(6)          for every neighbor configuration   in = Generate_neighbor_configuration()

(7)                if   is collision-free with

(8)                      Insert   into according to the improved evaluation function

(9)                end if

(10)              if   =

(11)                      SUCCESS = true

(12)              end if

(13)          end for

(14)    end while

(15)    if SUCCESS = true

(16)          return the constructed path by tracing the configuration from to

(17)    else  return  failure

(18)    end if