A Grid-Based Motion Planning Approach for Coherent Groups

Algorithm 1

The extending strategy of generating configuration samples.

Input. The desired Configuration , Obstacle space ,   

Output. Configuration_Samples

  Step  1–9 are used to place a copy of on and move the grids which collide with

to the free space  /

(1)    Clone from and ensure that the center grid of coincides with and the

direction of is aligned with the tangent direction of medial axis at

(2)    for   =

(3)          if   collides with

(4)            find the boundary grid of which is represented as nearest to and

collision-free with

(5)                  find the neighbor grid of which is represented as nearest to

and collision-free with

(6)            move to

(7)          end if

(8)          update  

(9)    end for

  Step  10–22 are used to iteratively  adjust the center of to   /

(10)    compute the center of which is represented as

(11)    if the distance between and is bigger than a small positive value

(12)        if   don’t collide with

(13)            move to

in which is the vector from

to

(14)          return   =

(15)        else find the farthest boundary grid from which is denoted as

satisfying the condition that

(16)                  find the farthest boundary grid from which is denoted as

satisfying the condition that

(17)                    find the neighbor grid of which is farthest to and is

represented as

(18)                 move to

(19)                    update  

(20)                    go to Step  10 until the distance between and is small enough

(21)        end if

(22)    end if