# A Grid-Based Motion Planning Approach for Coherent Groups

Authors: Yue-wen Fu; Meng Li; Jia-hong Liang; Xiao-qian Hu
Journal: Mathematical Problems in Engineering
 (2014)

---

## Abstract

This paper presents a novel motion planning approach for coherent groups with constant area, and it integrates C-L method into the probabilistic roadmap algorithm with sampling on the medial axis (MAPRM). In the preprocessing phase, the group is discretized into a grid-set which represents the configuration of the group. Then, a number of samples are generated on workspace by medial axis technique. These samples are extended into group’s configuration nodes of the roadmap using an extending strategy. Also, the group's deformation degree relative to the desired shape is introduced to improve the evaluation function. It gives users more flexibility to determine the respective weights of the group’s deformation degree and its distance to the goal in the query phase. After that, a novel local planner is constructed to connect any two neighbor configurations by using C-L method and the improved evaluation function. Experiments show that our approach is able to find paths for the coherent group efficiently and keep its area invariant when moving toward the goal.


---

## Body

## 1. Introduction

Human crowd is a fascinating social phenomenon which has been constantly investigated by experts from various areas. Crowd simulation can be considered as a multiagent system [1, 2]. And it is enjoying considerable success in numerous applied domains, most notably in evacuation scenarios where simulated crowd behaviors can help to improve the safety of interior building designs [3, 4]. In crowd simulation, groups are the most important entities that are worth intensive studies. Groups can be defined as semipermanent collections of individuals sharing navigation goals who attempt to maintain spatial cohesion with each other. But these groups may split up when moving toward the goal. By contrast, coherent groups are a class of groups which maintain permanent gathering and would not split up [5].At present, coherent groups have gained tremendous momentum in military and safety training applications. This type of coherent groups can be seen as deformable objects with area conservation. And investigating the motion planning problem for these deformable objects has an important meaning in computer animation, crowd simulation, and group behavior generating. For example, we need to simulate the behavior of a squad of soldiers which have a desired formation with a constant area on the whole and constant space between any two soldiers when marching toward the goal. The formation of the soldier group can reflect their tactical strategy that can provide different functions for distinct physical effects, which could be a key factor in defeating the enemy in a battle. In addition, it can be used to simulate different motions of the wild congregated animals in computer animations to create special visual effects, for example, a crowd of migratory geese that herd to the south in perfect V formation. However, most current motion planning approaches cannot control the area of the group as a constant easily, and how to plan paths for these deformable objects with area conservation is still a challenging problem in these fields.As one of the motion planning methods, C-L method proposed by Chang and Li [6] provides an excellent approach to planning the path of deformable objects and maintaining its size conservation when moving toward the goal. Unfortunately, the C-L method is computationally expensive because it did not use the environment’s information efficiently. Fortunately, a probabilistic roadmap planner using the medial axis technique (MAPRM) which generates samples on the medial axis of the workspace has an additional benefit in this situation to fully take advantage of the environment’s information [7, 8].Consequently, in this paper, a novel approach will be described which is used to plan a path to connect the initial and goal configuration of the coherent group by combining MAPRM algorithm and C-L method for size conservation. Rather than planning the motion for individual entities, the goal of this paper is planning the motion for the group as a whole, in which the group is modeled as a deformable grid-set with a constant area and planned for this particular grid-set.This paper is organized as follows. The previous works related to group motion planning are introduced in Section2. And the framework of our approach, including the flowchart of the local planner, is proposed in Section 3. In Section 4, we first describe the sampling mechanism on medial axis briefly and discretization approach for coherent group. Then we describe how to generate configuration nodes of the roadmap by extending the position samples on medial axis. Section 5 shows the detailed designing process of the local planner. Experiments and results are given in Section 6. Finally, we draw some conclusions and outline future work in Section 7.


## 2. Related Work

In the virtual environment the most common approach to simulating group movement is to use flocking which was introduced by Reynolds [9]. Bayazit et al. [10] combined flocking with the probabilistic roadmap algorithm (PRM) to guide the group’s motion toward the goal. In their works, the roadmap provided a convenient abstract representation of global information in complex environments and associated rules with roadmap nodes which enables local customization of behaviors. Li and Chou [11] proposed a novel centralized planning approach that moves the robots in groups formed dynamically with a sphere-tree structure. Li et al. [12] combined basic flocking algorithm with leader-follower model to simulate group’s motion. At the same time, in order to plan the path of the group, they tried to model the shape of the group’s region by estimating the longitudinal depth of the group. However the above methods cannot ensure the group’s coherence when moving toward the goal and do not consider the groups as deformable objects.Guibas et al. [13] proposed a probabilistic roadmap planner for flexible objects with a workspace medial-axis-based sampling approach (MAPRM). In their method, the medial axis of the workspace was computed at the preprocessing stage and the configurations of the flexible objects were fitted at random points along the medial axis. However, this method is only suitable for planning the path for the objects like sheets of metal or plastic flexible pipes which only can be bent. But the coherent group can deform to arbitrary shapes.Overmars et al. [14–16] introduced a new approach to motion planning for coherent groups of entities. In their methods, the group was represented as a deformable shape of sufficient volume, such as deformable rounded rectangle or hinged rectangle. Then, an extension of the probabilistic roadmap method was used to plan the motion for this deformable shape. In Kamphuis’s method, the designers must take care to guarantee that the area of the deformable shape remained the same during its motion. However, the area of the group cannot be controlled as a constant easily in their method and the shape of the group cannot be simplified into rectangle in the complex environment.Because of the defects of the above methods, Chang and Li [6] proposed a novel method to investigate the motion planning problem for deformable objects with size conservation which is named as C-L method in this paper. In C-L method, the deformable object was firstly discretized into a series of small grids. Together, these grids formed a grid-set that represented the configuration of the deformable object. Then, C-L method planned paths for the whole grid-set to generate the paths of the deformable objects. So, these grids became the smallest units to be operated by C-L method in which a new configuration was generated according to the current information of the environment and the configuration of the grid-set. At the same time, the deformation degree of the object which represented the difference between the current configuration of the object and the desired configuration was introduced to improve the evaluation function of the searching algorithm. Later, Chang and Li [17, 18] made the crowd conform to a specific shape while avoiding collisions with other agents in the crowd or with obstacles by combining C-L method with fuzzy controller. The C-L method was used to generate the global motion of the group, while the fuzzy controller was used to move the agents in the group to conform to a desired shape.Chen et al. presented a modelling and simulation of extra-large-scale crowd evacuation using massively parallel computing technology [19]. Each individual in the scenario is modeled as an adaptable and autonomous agent driven by a weight-based decision-making mechanism. The simulation is intended to characterize the individuals’ adaptable behaviors. Their approach adopts GPGPU to successfully sustain massively parallel modeling and simulation of a huge evacuation scenario consisting of hundreds of thousands of individuals. Their approaches for handling big scientific data with GPGPU are also applicable for dynamic data driven applications (DDDAS) of online simulation studies [20–22].Planning paths for deformable objects or flexible objects has gained various applications in industrial automation, computer generated animation, and virtual environments where the deformable properties of objects need to be considered for the creation of realistic motion. In this paper, we restrict ourselves to motion planning approaches to compute paths for a special flexible object which is the coherent group with area conservation. The group’s contour changes in different environments; however the group’s area must keep constant when moving along the planned path.


## 3. Planning Framework

In general, this study was inspired by the discretization technique in C-L method [17, 18], medial axis technique [7], and Kamphuis and Overmars’ work [14, 16]. The goal of our approach is twofold. On one hand we should make the group move toward the goal with optimal evaluation function; on the other hand the group can be changed into any shape according to the environment and the group’s area must be kept constant in this process. In this paper, the evaluation function is the weighted sum of the distance to the goal and the difference between the group’s configuration and the desired configuration. Optimal evaluation function means that the value of evaluation function is the smallest at every run step. Our approach consists of the following steps.(1)
Specify the initial, goal, and desired shape of the group and discretize these shapes into grid-sets which, respectively, represent the group’s initial, goal, and desired configurations.
(2)
Generate a series of position samples on the medial axis of the workspace and extend these position samples into group’s configuration samples which will be seen as the nodes of the roadmap.
(3)
Design the local planner based on C-L method and the improved evaluation function. Then, construct a roadmap of possible motions for the deformable configuration of the group between any two neighbor nodes using this local planner.
(4)
To answer a motion query, use the roadmap to compute a collision-free path for the deformable configuration.The flowchart of our approach is illustrated in Figure1 and the detailed process of the local planner is illustrated in Figure 2 in which we combine C-L method and improved evaluation function. Our local planner has two advantages: one is that the configuration nodes extended from position samples can provide guidance for C-L method and the other is that C-L method can ensure the constant of the group’s area.Figure 1
Flowchart of our approach.Figure 2
Flowchart of the local planner.


## 4. Generate Configuration Nodes

### 4.1. Medial Axis Sampling Technique and Discretization of the Group

Firstly, the medial axis technique for sampling strives to generate samples on the medial axis of the workspace [17, 18]. This approach has been used previously in motion planning for deformable objects [13]. The technique is as follows: generate a uniformly random point in workspace (or 2D configuration space in [18]). According to whether this point lies in free space or not, compute different retraction directions and then retract this point to medial axis. Repeat this process until a series of position samples on medial axis of the workspace are generated. These position samples are represented as (x
w
,
i
,
y
w
,
i) i
=
1
,
…
,
N where N represents the total number of the position samples. The fact that medial axis technique provides the largest clearance from obstacles for the group is advantageous. Secondly, the group’s shape is discretized into a grid-set, denoted by C
set
=
G
1
x
1
,
y
1
,
θ
1
,
G
2
x
2
,
y
2
,
θ
2
,
…
,
G
M
x
M
,
y
M
,
θ
M, in which G
i
x
i
,
y
i
,
θ
i
i
=
1
,
…
,
M represents the information of the ith grid and M represents the total number of the grids. Obviously, C
set is the configuration of the coherent group. In the grid-set, there are two types of grids: inner grids and boundary grids. The inner grids are the ones whose neighbor grids are also the grids of C
set, while this is not true for the boundary grids. Moreover, we suppose that the group is compact; that is, no grid is isolated and no hole exists in C
set and the directions of all the grids are the same. An isolated grid means that there is no common border or vertex between it and any other ones. Figure 3 graphically illustrates the compact group versus incompact ones. Obviously, the compact property ensures the group’s coherence.Compact group versus incompact ones.
(a)
Compact group
(b)
Incompact group
(c)
Incompact group


### 4.2. The Extending Strategy of Generating Configuration Nodes

The goal of this section is to extend the 2D position samples into group’s configuration samples which areM dimensions. The extending strategy is divided into two steps.Step 1.
Find the center grid ofC
set and then place it on the position samples. The center grid of C
set is represented as G
c
x
c
,
y
c
,
θ
c whose information can be computed by
(1)
x
c
,
y
c
,
θ
c
=
argmin
x
j
,
y
j
,
θ
j
∈
G
set
∑
i
=
1
M
x
i
M
-
x
j
2
+
∑
i
=
1
M
y
i
M
-
y
j
2
.
So, at every position sample, we make x
c
=
x
w
,
i, y
c
=
y
w
,
i, and θ
c equal to the tangent direction of the medial axis at (x
w
,
i
,
y
w
,
i).Step 2.
After placing the center grid on (x
w
,
i
,
y
w
,
i), we should decide the positions of the other M
-
1 grids according to the environment around (x
w
,
i
,
y
w
,
i), the desired configuration C
P, and the compact property of the group. In this process, the desired configuration is regarded as a template to provide guidance for generating configuration samples.
At first, we place a duplicateC
~
P of C
P on (x
w
,
i
,
y
w
,
i) by making its center grid coincide with (x
w
,
i
,
y
w
,
i) and its direction aligns with the tangent direction of medial axis; then we move the grids in C
~
P which have collided with the obstacles to free space according to the compact property and the distance to (x
w
,
i
,
y
w
,
i). However, as these collision grids move to free space, the center of C
~
P may change. Then we should adjust the center of C
~
P so that it coincides with (x
w
,
i
,
y
w
,
i) once again. This is done by adjusting the boundary grids and the whole position of C
~
P iteratively. Using C
P as the template to guide the generation of configuration samples, it can reduce the differences between the desired configuration and the generated configuration. The detailed algorithm for generating the configuration samples is illustrated in Algorithm 1 and Figure 4 gives graphic depictions of the results computed by Algorithm 1.
It is important to note that the medial axis is prone to having small “twigs” in the obstacle boundaries. These are of no use during the planning stage and are discarded if their length is less than a specified threshold [13].Algorithm 1:The extending strategy of generating configuration samples.
Input. The desired Configuration C
P, Obstacle space C
obs, (
x
w
,
i
,
y
w
,
i
)  i
∈
{
1
,
…
,
N
}
Output. Configuration_Samples
C
i
sampling
=
{
G
1
sampling
(
x
1
,
y
1
,
θ
1
)
,
G
2
sampling
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
M
sampling
(
x
M
,
y
M
,
θ
M
)
} i
∈
{
1
,
…
,
N
}
/
*  Step  1–9 are used to place a copy of
C
P
on
(
x
w
,
i
,
y
w
,
i
)
and move the grids which collide with
C
obs
to the free space  *
/
(1)    CloneC
~
P from C
P and ensure that the center grid of C
~
P coincides with (
x
w
,
i
,
y
w
,
i
) and the
direction ofC
~
P is aligned with the tangent direction of medial axis at (
x
w
,
i
,
y
w
,
i
)
(2)for  j = 1
,
…
,
M
(3)if  G
~
j
(
x
j
,
y
j
,
θ
j
) collides with C
obs
(4)            find the boundary grid ofC
~
P which is represented as G
~
j
,
boundary
nearest nearest to (
x
w
,
i
,
y
w
,
i
) and
collision-free withC
obs
(5)                  find the neighbor grid  ofG
~
j
,
boundary
nearest which is represented as G
~
j
,
boundary
nearest_neighbor nearest to
(
x
w
,
i
,
y
w
,
i
) and collision-free with C
obs
(6)            moveG
~
j
(
x
j
,
y
j
,
θ
j
) to G
~
j
,
boundary
nearest_neighbor
(7)end if
(8)update  C
~
P
(9)end for
/
*  Step  10–22 are used to iteratively  adjust the center of
C
~
P
to
(
x
w
,
i
,
y
w
,
i
)  *
/
(10)    compute the center ofC
~
P which is represented as G
~
c
(
x
c
,
y
c
,
θ
c
)
(11)if the distance between G
~
c
(
x
c
,
y
c
,
θ
c
) and (
x
w
,
i
,
y
w
,
i
) is bigger than a small positive value
(12)if  {
G
~
1
(
x
1
,
y
1
,
θ
1
)
+
ν
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
+
ν
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
+
ν
} don’t collide with C
obs
(13)            move{
G
~
1
(
x
1
,
y
1
,
θ
1
)
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
} to
{
G
~
1
(
x
1
,
y
1
,
θ
1
)
+
ν
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
+
ν
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
+
ν
} in which ν is the vector from
G
~
c
(
x
c
,
y
c
,
θ
c
) to (
x
w
,
i
,
y
w
,
i
)
(14)return  C
i
sampling = {
G
~
1
(
x
1
,
y
1
,
θ
1
)
+
ν
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
+
ν
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
+
ν
}
(15)else find the farthest boundary grid from G
~
c
(
x
c
,
y
c
,
θ
c
) which is denoted as G
~
farthest
α
(
x
,
y
,
θ
)
satisfying the condition that(
G
~
farthest
α
(
x
,
y
,
θ
)
-
G
~
c
(
x
c
,
y
c
,
θ
c
)
)
⋅
ν
<
0
(16)                  find the farthest boundary grid fromG
~
c
(
x
c
,
y
c
,
θ
c
) which is denoted as G
~
farthest
β
(
x
,
y
,
θ
)
satisfying the condition that(
G
~
farthest
β
(
x
,
y
,
θ
)
-
G
~
c
(
x
c
,
y
c
,
θ
c
)
)
⋅
ν
>
0
(17)                    find the neighbor grid ofG
~
farthest
β
(
x
,
y
,
θ
) which is farthest to G
~
c
(
x
c
,
y
c
,
θ
c
) and is
represented asG
~
farthest
β
,
neighbor
(
x
,
y
,
θ
)
(18)                 moveG
~
farthest
α
(
x
,
y
,
θ
) to G
~
farthest
β
,
neighbor
(
x
,
y
,
θ
)
(19)update  C
~
P
(20)go to Step  10 until the distance between G
~
c
(
x
c
,
y
c
,
θ
c
) and (
x
w
,
i
,
y
w
,
i
) is small enough
(21)end if
(22)end ifFigure 4
Generated configuration nodes of the roadmap.


## 4.1. Medial Axis Sampling Technique and Discretization of the Group

Firstly, the medial axis technique for sampling strives to generate samples on the medial axis of the workspace [17, 18]. This approach has been used previously in motion planning for deformable objects [13]. The technique is as follows: generate a uniformly random point in workspace (or 2D configuration space in [18]). According to whether this point lies in free space or not, compute different retraction directions and then retract this point to medial axis. Repeat this process until a series of position samples on medial axis of the workspace are generated. These position samples are represented as (x
w
,
i
,
y
w
,
i) i
=
1
,
…
,
N where N represents the total number of the position samples. The fact that medial axis technique provides the largest clearance from obstacles for the group is advantageous. Secondly, the group’s shape is discretized into a grid-set, denoted by C
set
=
G
1
x
1
,
y
1
,
θ
1
,
G
2
x
2
,
y
2
,
θ
2
,
…
,
G
M
x
M
,
y
M
,
θ
M, in which G
i
x
i
,
y
i
,
θ
i
i
=
1
,
…
,
M represents the information of the ith grid and M represents the total number of the grids. Obviously, C
set is the configuration of the coherent group. In the grid-set, there are two types of grids: inner grids and boundary grids. The inner grids are the ones whose neighbor grids are also the grids of C
set, while this is not true for the boundary grids. Moreover, we suppose that the group is compact; that is, no grid is isolated and no hole exists in C
set and the directions of all the grids are the same. An isolated grid means that there is no common border or vertex between it and any other ones. Figure 3 graphically illustrates the compact group versus incompact ones. Obviously, the compact property ensures the group’s coherence.Compact group versus incompact ones.
(a)
Compact group
(b)
Incompact group
(c)
Incompact group


## 4.2. The Extending Strategy of Generating Configuration Nodes

The goal of this section is to extend the 2D position samples into group’s configuration samples which areM dimensions. The extending strategy is divided into two steps.Step 1.
Find the center grid ofC
set and then place it on the position samples. The center grid of C
set is represented as G
c
x
c
,
y
c
,
θ
c whose information can be computed by
(1)
x
c
,
y
c
,
θ
c
=
argmin
x
j
,
y
j
,
θ
j
∈
G
set
∑
i
=
1
M
x
i
M
-
x
j
2
+
∑
i
=
1
M
y
i
M
-
y
j
2
.
So, at every position sample, we make x
c
=
x
w
,
i, y
c
=
y
w
,
i, and θ
c equal to the tangent direction of the medial axis at (x
w
,
i
,
y
w
,
i).Step 2.
After placing the center grid on (x
w
,
i
,
y
w
,
i), we should decide the positions of the other M
-
1 grids according to the environment around (x
w
,
i
,
y
w
,
i), the desired configuration C
P, and the compact property of the group. In this process, the desired configuration is regarded as a template to provide guidance for generating configuration samples.
At first, we place a duplicateC
~
P of C
P on (x
w
,
i
,
y
w
,
i) by making its center grid coincide with (x
w
,
i
,
y
w
,
i) and its direction aligns with the tangent direction of medial axis; then we move the grids in C
~
P which have collided with the obstacles to free space according to the compact property and the distance to (x
w
,
i
,
y
w
,
i). However, as these collision grids move to free space, the center of C
~
P may change. Then we should adjust the center of C
~
P so that it coincides with (x
w
,
i
,
y
w
,
i) once again. This is done by adjusting the boundary grids and the whole position of C
~
P iteratively. Using C
P as the template to guide the generation of configuration samples, it can reduce the differences between the desired configuration and the generated configuration. The detailed algorithm for generating the configuration samples is illustrated in Algorithm 1 and Figure 4 gives graphic depictions of the results computed by Algorithm 1.
It is important to note that the medial axis is prone to having small “twigs” in the obstacle boundaries. These are of no use during the planning stage and are discarded if their length is less than a specified threshold [13].Algorithm 1:The extending strategy of generating configuration samples.
Input. The desired Configuration C
P, Obstacle space C
obs, (
x
w
,
i
,
y
w
,
i
)  i
∈
{
1
,
…
,
N
}
Output. Configuration_Samples
C
i
sampling
=
{
G
1
sampling
(
x
1
,
y
1
,
θ
1
)
,
G
2
sampling
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
M
sampling
(
x
M
,
y
M
,
θ
M
)
} i
∈
{
1
,
…
,
N
}
/
*  Step  1–9 are used to place a copy of
C
P
on
(
x
w
,
i
,
y
w
,
i
)
and move the grids which collide with
C
obs
to the free space  *
/
(1)    CloneC
~
P from C
P and ensure that the center grid of C
~
P coincides with (
x
w
,
i
,
y
w
,
i
) and the
direction ofC
~
P is aligned with the tangent direction of medial axis at (
x
w
,
i
,
y
w
,
i
)
(2)for  j = 1
,
…
,
M
(3)if  G
~
j
(
x
j
,
y
j
,
θ
j
) collides with C
obs
(4)            find the boundary grid ofC
~
P which is represented as G
~
j
,
boundary
nearest nearest to (
x
w
,
i
,
y
w
,
i
) and
collision-free withC
obs
(5)                  find the neighbor grid  ofG
~
j
,
boundary
nearest which is represented as G
~
j
,
boundary
nearest_neighbor nearest to
(
x
w
,
i
,
y
w
,
i
) and collision-free with C
obs
(6)            moveG
~
j
(
x
j
,
y
j
,
θ
j
) to G
~
j
,
boundary
nearest_neighbor
(7)end if
(8)update  C
~
P
(9)end for
/
*  Step  10–22 are used to iteratively  adjust the center of
C
~
P
to
(
x
w
,
i
,
y
w
,
i
)  *
/
(10)    compute the center ofC
~
P which is represented as G
~
c
(
x
c
,
y
c
,
θ
c
)
(11)if the distance between G
~
c
(
x
c
,
y
c
,
θ
c
) and (
x
w
,
i
,
y
w
,
i
) is bigger than a small positive value
(12)if  {
G
~
1
(
x
1
,
y
1
,
θ
1
)
+
ν
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
+
ν
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
+
ν
} don’t collide with C
obs
(13)            move{
G
~
1
(
x
1
,
y
1
,
θ
1
)
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
} to
{
G
~
1
(
x
1
,
y
1
,
θ
1
)
+
ν
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
+
ν
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
+
ν
} in which ν is the vector from
G
~
c
(
x
c
,
y
c
,
θ
c
) to (
x
w
,
i
,
y
w
,
i
)
(14)return  C
i
sampling = {
G
~
1
(
x
1
,
y
1
,
θ
1
)
+
ν
,
G
~
2
(
x
2
,
y
2
,
θ
2
)
+
ν
,
…
,
G
~
M
(
x
M
,
y
M
,
θ
M
)
+
ν
}
(15)else find the farthest boundary grid from G
~
c
(
x
c
,
y
c
,
θ
c
) which is denoted as G
~
farthest
α
(
x
,
y
,
θ
)
satisfying the condition that(
G
~
farthest
α
(
x
,
y
,
θ
)
-
G
~
c
(
x
c
,
y
c
,
θ
c
)
)
⋅
ν
<
0
(16)                  find the farthest boundary grid fromG
~
c
(
x
c
,
y
c
,
θ
c
) which is denoted as G
~
farthest
β
(
x
,
y
,
θ
)
satisfying the condition that(
G
~
farthest
β
(
x
,
y
,
θ
)
-
G
~
c
(
x
c
,
y
c
,
θ
c
)
)
⋅
ν
>
0
(17)                    find the neighbor grid ofG
~
farthest
β
(
x
,
y
,
θ
) which is farthest to G
~
c
(
x
c
,
y
c
,
θ
c
) and is
represented asG
~
farthest
β
,
neighbor
(
x
,
y
,
θ
)
(18)                 moveG
~
farthest
α
(
x
,
y
,
θ
) to G
~
farthest
β
,
neighbor
(
x
,
y
,
θ
)
(19)update  C
~
P
(20)go to Step  10 until the distance between G
~
c
(
x
c
,
y
c
,
θ
c
) and (
x
w
,
i
,
y
w
,
i
) is small enough
(21)end if
(22)end ifFigure 4
Generated configuration nodes of the roadmap.


## 5. Local Planner

### 5.1. The Node Neighbors and Shape Differences

Based on the medial axis technique and Algorithm1, N configuration samples C
1
sampling
,
C
2
sampling
,
…
,
C
N
sampling have been generated which will be regarded as the nodes of the roadmap. The role of the local planner is to connect any two neighbor configuration samples and then construct the roadmap. Suppose C
q
sampling
=
{
G
q
,
1
sampling
x
1
,
y
1
,
θ
1
,
…
,
G
q
,
M
sampling
x
M
,
y
M
,
θ
M
} and C
w
sampling
=
{
G
w
,
1
sampling
x
1
,
y
1
,
θ
1
,
…
,
G
w
,
M
sampling
x
M
,
y
M
,
θ
M
} are two neighbour configuration samples. Making the judgment that C
q
sampling is the neighbor node of C
w
sampling is based on the two conditions in
(2)
x
c
,
w
sampling
-
x
c
,
q
sampling
2
+
y
c
,
w
sampling
-
y
c
,
q
sampling
2
WW
≤
max
⁡
_
distance
,
S
shape
C
q
sampling
,
C
w
sampling
≤
max
⁡
_
deformation
,
where (x
c
,
q
sampling
,
y
c
,
q
sampling) and (x
c
,
w
sampling
,
y
c
,
w
sampling) represent the center grid of C
q
sampling and C
w
sampling, respectively. max
⁡
_
distance and max
⁡
_
deformation are the predefined thresholds. S
shape
(
C
q
sampling
,
C
w
sampling
) represents the shape difference between C
q
sampling and C
w
sampling.The two conditions in (2) represent two criteria deciding which configuration samples we should try to connect using the local planner. One of the criteria we use is the distance of the center grid in the workspace between C
q
sampling and C
w
sampling. If they are too far apart, the chance of finding a free path between them is small. The second criterion is the shape difference between them. If they are bigger than a predefined threshold, the chance of finding a path between them is also small. S
shape
(
C
q
sampling
,
C
w
sampling
) is formulated in
(3)
S
shape
C
q
sampling
,
C
w
sampling
=
∑
j
=
1
M
r
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
C
w
sampling
.In formulation (3), r
(
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
C
w
sampling
) is used to compute the minimum distance from the grid G
q
,
j
sampling
x
j
,
y
j
,
θ
j to C
w
sampling and is formulated in
(4)
r
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
C
w
sampling
=
0
W
if
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
is
in
C
w
sampling
min
⁡
distance
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
G
w
,
k
sampling
x
k
,
y
k
,
θ
k
:
minW
∀
G
w
,
k
sampling
x
k
,
y
k
,
θ
k
∈
C
w
sampling
0
W
if
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
is
outside
C
w
sampling
.
Formulation (3) denotes that S
shape
(
C
w
sampling
,
C
q
sampling
) is the sum of all the distances from M grids in C
q
sampling to C
w
sampling. The bigger the S
shape
(
C
w
sampling
,
C
q
sampling
) is, the larger the shape difference between C
q
sampling and C
w
sampling is. Obviously, (3) can also be used to compute the deformation degree of a configuration relative to C
P.


### 5.2. Improved Evaluation Function

In C-L method, an improved evaluation function is proposed which is worthy of being used for reference and is formulated in(5)
f
C
set
=
α
·
h
C
set
+
β
·
S
shape
C
set
,
C
P
.In formulation (5), h
(
C
set
) represents the estimated distance from C
set to the goal. S
shape
C
set
,
C
P represents the deformation degree of C
set relative to C
P. α and β, respectively, represent the weight of the distance to the goal and the weight of deformation degree. They also satisfy the restriction of α
+
β
=
1.Different allocations ofα and β produce diverse evaluation function values which will have a great effect on the configuration to be extended at the next run step. Therefore, α and β reflect the priorities between maintaining similarity to C
P and the distance to the goal. When we increase α, the next extended configuration will be prone to the one which has smaller distance to the goal. Conversely, the configuration which has a smaller shape difference with C
P will be preferred.


### 5.3. Local Planner and Roadmap Generation

After establishing the evaluation function and the neighboring relationship betweenC
q
sampling and C
w
sampling, the local planner can be constructed, the process of which is illustrated in Algorithm 2.Algorithm 2:The local planner connecting any two neighbor configurations.
Input. C
q
sampling, C
w
sampling, The desired Configuration C
P, Obstacle space C
obs, α, β
Output. Local path P which connects C
q
sampling and C
w
sampling
(1)    Initialize a priority queueQ
(2)    SUCCESS =false;
(3)Insert  C
q
sampling into Q
(4)while  Q is not empty and SUCCESS = false  do
(5)C
Q
=
{
G
Q
,
1
(
x
1
,
y
1
,
θ
1
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
)
} = Dequeue(Q)
(6)for every neighbor configuration  C
Q
neighbor in L = Generate_neighbor_configuration(C
Q)
(7)if  C
Q
neighbor is collision-free with C
obs
(8)Insert  C
Q
neighbor into Q according to the improved evaluation function f
(
C
Q
neighbor
)
(9)end if
(10)if  C
Q
neighbor = C
w
sampling
(11)                      SUCCESS =true
(12)end if
(13)end for
(14)end while
(15)if SUCCESS = true
(16)returnthe constructed path P by tracing the configuration from C
w
sampling to C
q
sampling
(17)else  return  failure
(18)end ifIn Algorithm2, L represents a list whose elements are all the neighbor configurations of the current ones C
Q updated byDequeue(Q), that is, the first element in Q. Moreover, the neighbor configuration of C
Q is generated by moving its boundary grids on the basis of the compact property or rotating C
Q in one run step.It must be noted that, in Section5.1, the expression “node neighbors” means the configuration nodes that satisfy the condition in (2). In order to generate the neighbor configurations of C
Q, a local coordinate system is defined with its origin coinciding with the center grid of C
Q and x-axis aligning with the direction of C
Q. There are two ways to generate the neighbor configurations of C
Q in one run step. One is moving the boundary grids in the direction of x-axis or y-axis with a distance unit x
step and y
step according to the compact property; the other is rotating C
Q with a predefined angle unit θ
step. The detailed process is illustrated in Algorithm 3.Algorithm 3:Generating all the neighbors of C
Q.
Input.  C
Q
=
{
G
Q
,
1
(
x
1
,
y
1
,
θ
1
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
)
}, Obstacle space C
obs, x
step, y
step, θ
step
Output.  A neighbor list L of C
Q
(1)    Initialize a queueQ, Initialize a list L
(2)    CloneC
Q
′ from C
Q
(3)if all grids in boundary grid of C
Q
′ have no collision-free neighbors
(4)return null
(5)end if
/
*
Step  6–8 are used to rotate
C
Q
*
/
(6)if  {
G
Q
,
1
(
x
1
,
y
1
,
θ
1
±
θ
step
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
±
θ
step
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
±
θ
step
)
} is collision-free
(7)        add{
G
Q
,
1
(
x
1
,
y
1
,
θ
1
±
θ
step
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
±
θ
step
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
±
θ
step
)
} into L
(8)end if
/
*
Step  9–23 are used to moving the boundary grids of
C
Q
according to the compact property
*
/
(9)    letC
Q
(
k
) represent the kth boundary grid of C
Q, num(
C
Q
) represents the total number of
boundary grid ofC
Q
(10)for (k = 1; k <= num(
C
Q
); k++)
(11)if  C
Q
(
k
) have collision-free neighbors
(12)          letμ be the vector from C
Q
(
k
) to one of its free neighbors with step size of x
step or
y
step
(13)           addC
Q
(
k
) + μ to C
Q
′ and add all other grids in C
Q except C
Q
(
k
) into a queue Q
according to the distance toC
Q
(
k
)
(14)while  Q is not empty
(15)Q
k = Dequeue(Q)
(16)if  Q
k + μ is collision-free, add Q
k + μ to C
Q
′
(17)else add Q
k + μ
′ to C
Q
′, where μ
′ is the new direction which ensures the gird-set
satisfying the compact property
(18)end if
(19)end while
(20)          addC
Q
′ into L
(21)end if
(22)end for
(23)return  LCompared with previous researchers’ works, Algorithm3 has the advantage of keeping the group’s area constant in the planning. After the above procedures, the roadmap is constructed.


### 5.4. Querying

When performing a query, we connect the start and goal configurations to the roadmap and determine the shortest path in the roadmap between them. The querying procedure in our approach is the same as the one in classic PRM algorithm. The difference is that, in every node of the roadmap, we make use of the improved evaluation function once again. So the evaluation function at theith node of the roadmap can be represented as
(6)
f
C
i
sampling
=
α
·
h
C
i
sampling
+
β
·
S
shape
C
i
sampling
,
C
P
,
i
∈
1
,
N
.By allocating different values ofα and β, we can influence whether we prefer a path that is short but has lots of deformation relative to C
P or a path that is longer but with less deformation.Complexity Analysis. The computational cost of our approach comes from the above three stages. In the stage of generating configuration nodes, we generate N position samples by the medial axis technique and extend each position sample into group’s configuration samples in M dimensions. In the second stage, the local planner is to connect any two neighbor configuration samples. We use the S
shape
(
C
q
sampling
,
C
w
sampling
) formulation for M times to compute the minimum distance from the grid G
q
,
j
sampling
x
j
,
y
j
,
θ
j to C
w
sampling and use Algorithm 3 to generate the neighbor configurations of C
Q for M times at most. In the third stage, the evaluation function in PRM algorithm at every node of the roadmap is called M times. Then the complexity of our planner is O
N
·
M
+
N
·
N
·
M
·
M
+
N
·
M
<
O
N
2
M
+
N
2
M
2 which is equivalent to O
N
2
M
2.Through the analysis of the corresponding computational complexity, the computational cost of the classic C-L method is vast; however, the configuration nodes extended from position samples in the first stage can provide guidance for the local planner and reduce the complexity of our approach.


## 5.1. The Node Neighbors and Shape Differences

Based on the medial axis technique and Algorithm1, N configuration samples C
1
sampling
,
C
2
sampling
,
…
,
C
N
sampling have been generated which will be regarded as the nodes of the roadmap. The role of the local planner is to connect any two neighbor configuration samples and then construct the roadmap. Suppose C
q
sampling
=
{
G
q
,
1
sampling
x
1
,
y
1
,
θ
1
,
…
,
G
q
,
M
sampling
x
M
,
y
M
,
θ
M
} and C
w
sampling
=
{
G
w
,
1
sampling
x
1
,
y
1
,
θ
1
,
…
,
G
w
,
M
sampling
x
M
,
y
M
,
θ
M
} are two neighbour configuration samples. Making the judgment that C
q
sampling is the neighbor node of C
w
sampling is based on the two conditions in
(2)
x
c
,
w
sampling
-
x
c
,
q
sampling
2
+
y
c
,
w
sampling
-
y
c
,
q
sampling
2
WW
≤
max
⁡
_
distance
,
S
shape
C
q
sampling
,
C
w
sampling
≤
max
⁡
_
deformation
,
where (x
c
,
q
sampling
,
y
c
,
q
sampling) and (x
c
,
w
sampling
,
y
c
,
w
sampling) represent the center grid of C
q
sampling and C
w
sampling, respectively. max
⁡
_
distance and max
⁡
_
deformation are the predefined thresholds. S
shape
(
C
q
sampling
,
C
w
sampling
) represents the shape difference between C
q
sampling and C
w
sampling.The two conditions in (2) represent two criteria deciding which configuration samples we should try to connect using the local planner. One of the criteria we use is the distance of the center grid in the workspace between C
q
sampling and C
w
sampling. If they are too far apart, the chance of finding a free path between them is small. The second criterion is the shape difference between them. If they are bigger than a predefined threshold, the chance of finding a path between them is also small. S
shape
(
C
q
sampling
,
C
w
sampling
) is formulated in
(3)
S
shape
C
q
sampling
,
C
w
sampling
=
∑
j
=
1
M
r
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
C
w
sampling
.In formulation (3), r
(
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
C
w
sampling
) is used to compute the minimum distance from the grid G
q
,
j
sampling
x
j
,
y
j
,
θ
j to C
w
sampling and is formulated in
(4)
r
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
C
w
sampling
=
0
W
if
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
is
in
C
w
sampling
min
⁡
distance
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
,
G
w
,
k
sampling
x
k
,
y
k
,
θ
k
:
minW
∀
G
w
,
k
sampling
x
k
,
y
k
,
θ
k
∈
C
w
sampling
0
W
if
G
q
,
j
sampling
x
j
,
y
j
,
θ
j
is
outside
C
w
sampling
.
Formulation (3) denotes that S
shape
(
C
w
sampling
,
C
q
sampling
) is the sum of all the distances from M grids in C
q
sampling to C
w
sampling. The bigger the S
shape
(
C
w
sampling
,
C
q
sampling
) is, the larger the shape difference between C
q
sampling and C
w
sampling is. Obviously, (3) can also be used to compute the deformation degree of a configuration relative to C
P.


## 5.2. Improved Evaluation Function

In C-L method, an improved evaluation function is proposed which is worthy of being used for reference and is formulated in(5)
f
C
set
=
α
·
h
C
set
+
β
·
S
shape
C
set
,
C
P
.In formulation (5), h
(
C
set
) represents the estimated distance from C
set to the goal. S
shape
C
set
,
C
P represents the deformation degree of C
set relative to C
P. α and β, respectively, represent the weight of the distance to the goal and the weight of deformation degree. They also satisfy the restriction of α
+
β
=
1.Different allocations ofα and β produce diverse evaluation function values which will have a great effect on the configuration to be extended at the next run step. Therefore, α and β reflect the priorities between maintaining similarity to C
P and the distance to the goal. When we increase α, the next extended configuration will be prone to the one which has smaller distance to the goal. Conversely, the configuration which has a smaller shape difference with C
P will be preferred.


## 5.3. Local Planner and Roadmap Generation

After establishing the evaluation function and the neighboring relationship betweenC
q
sampling and C
w
sampling, the local planner can be constructed, the process of which is illustrated in Algorithm 2.Algorithm 2:The local planner connecting any two neighbor configurations.
Input. C
q
sampling, C
w
sampling, The desired Configuration C
P, Obstacle space C
obs, α, β
Output. Local path P which connects C
q
sampling and C
w
sampling
(1)    Initialize a priority queueQ
(2)    SUCCESS =false;
(3)Insert  C
q
sampling into Q
(4)while  Q is not empty and SUCCESS = false  do
(5)C
Q
=
{
G
Q
,
1
(
x
1
,
y
1
,
θ
1
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
)
} = Dequeue(Q)
(6)for every neighbor configuration  C
Q
neighbor in L = Generate_neighbor_configuration(C
Q)
(7)if  C
Q
neighbor is collision-free with C
obs
(8)Insert  C
Q
neighbor into Q according to the improved evaluation function f
(
C
Q
neighbor
)
(9)end if
(10)if  C
Q
neighbor = C
w
sampling
(11)                      SUCCESS =true
(12)end if
(13)end for
(14)end while
(15)if SUCCESS = true
(16)returnthe constructed path P by tracing the configuration from C
w
sampling to C
q
sampling
(17)else  return  failure
(18)end ifIn Algorithm2, L represents a list whose elements are all the neighbor configurations of the current ones C
Q updated byDequeue(Q), that is, the first element in Q. Moreover, the neighbor configuration of C
Q is generated by moving its boundary grids on the basis of the compact property or rotating C
Q in one run step.It must be noted that, in Section5.1, the expression “node neighbors” means the configuration nodes that satisfy the condition in (2). In order to generate the neighbor configurations of C
Q, a local coordinate system is defined with its origin coinciding with the center grid of C
Q and x-axis aligning with the direction of C
Q. There are two ways to generate the neighbor configurations of C
Q in one run step. One is moving the boundary grids in the direction of x-axis or y-axis with a distance unit x
step and y
step according to the compact property; the other is rotating C
Q with a predefined angle unit θ
step. The detailed process is illustrated in Algorithm 3.Algorithm 3:Generating all the neighbors of C
Q.
Input.  C
Q
=
{
G
Q
,
1
(
x
1
,
y
1
,
θ
1
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
)
}, Obstacle space C
obs, x
step, y
step, θ
step
Output.  A neighbor list L of C
Q
(1)    Initialize a queueQ, Initialize a list L
(2)    CloneC
Q
′ from C
Q
(3)if all grids in boundary grid of C
Q
′ have no collision-free neighbors
(4)return null
(5)end if
/
*
Step  6–8 are used to rotate
C
Q
*
/
(6)if  {
G
Q
,
1
(
x
1
,
y
1
,
θ
1
±
θ
step
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
±
θ
step
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
±
θ
step
)
} is collision-free
(7)        add{
G
Q
,
1
(
x
1
,
y
1
,
θ
1
±
θ
step
)
,
G
Q
,
2
(
x
2
,
y
2
,
θ
2
±
θ
step
)
,
…
,
G
Q
,
M
(
x
M
,
y
M
,
θ
M
±
θ
step
)
} into L
(8)end if
/
*
Step  9–23 are used to moving the boundary grids of
C
Q
according to the compact property
*
/
(9)    letC
Q
(
k
) represent the kth boundary grid of C
Q, num(
C
Q
) represents the total number of
boundary grid ofC
Q
(10)for (k = 1; k <= num(
C
Q
); k++)
(11)if  C
Q
(
k
) have collision-free neighbors
(12)          letμ be the vector from C
Q
(
k
) to one of its free neighbors with step size of x
step or
y
step
(13)           addC
Q
(
k
) + μ to C
Q
′ and add all other grids in C
Q except C
Q
(
k
) into a queue Q
according to the distance toC
Q
(
k
)
(14)while  Q is not empty
(15)Q
k = Dequeue(Q)
(16)if  Q
k + μ is collision-free, add Q
k + μ to C
Q
′
(17)else add Q
k + μ
′ to C
Q
′, where μ
′ is the new direction which ensures the gird-set
satisfying the compact property
(18)end if
(19)end while
(20)          addC
Q
′ into L
(21)end if
(22)end for
(23)return  LCompared with previous researchers’ works, Algorithm3 has the advantage of keeping the group’s area constant in the planning. After the above procedures, the roadmap is constructed.


## 5.4. Querying

When performing a query, we connect the start and goal configurations to the roadmap and determine the shortest path in the roadmap between them. The querying procedure in our approach is the same as the one in classic PRM algorithm. The difference is that, in every node of the roadmap, we make use of the improved evaluation function once again. So the evaluation function at theith node of the roadmap can be represented as
(6)
f
C
i
sampling
=
α
·
h
C
i
sampling
+
β
·
S
shape
C
i
sampling
,
C
P
,
i
∈
1
,
N
.By allocating different values ofα and β, we can influence whether we prefer a path that is short but has lots of deformation relative to C
P or a path that is longer but with less deformation.Complexity Analysis. The computational cost of our approach comes from the above three stages. In the stage of generating configuration nodes, we generate N position samples by the medial axis technique and extend each position sample into group’s configuration samples in M dimensions. In the second stage, the local planner is to connect any two neighbor configuration samples. We use the S
shape
(
C
q
sampling
,
C
w
sampling
) formulation for M times to compute the minimum distance from the grid G
q
,
j
sampling
x
j
,
y
j
,
θ
j to C
w
sampling and use Algorithm 3 to generate the neighbor configurations of C
Q for M times at most. In the third stage, the evaluation function in PRM algorithm at every node of the roadmap is called M times. Then the complexity of our planner is O
N
·
M
+
N
·
N
·
M
·
M
+
N
·
M
<
O
N
2
M
+
N
2
M
2 which is equivalent to O
N
2
M
2.Through the analysis of the corresponding computational complexity, the computational cost of the classic C-L method is vast; however, the configuration nodes extended from position samples in the first stage can provide guidance for the local planner and reduce the complexity of our approach.


## 6. Results

In this section we will show some experiment results of the approach described above. The experiment environment is in 3D, and we use two experiments to test our approach. The experiment environments are illustrated in Figure5 in which there are four corridors A, B, C, and D, respectively. The widths of them are 2.3 m, 3.7 m, 2.3 m, and 0.8 m. In experiments 1 and 2, the desired shape of the group is a square which size is 3.5
m
×
3.5
m. And we discretize the group into 25 grids with the size of 0.7
m
×
0.7
m. The initial and goal configurations are placed in Figure 5. x
step and y
step are set to the size of the grid, that is, 0.7
m. θ
step is set to 5
°.Figure 5
Generating the neighbors of one configuration.Firstly, the medial axis technique is used to generate 100 position samples. And 12 position samples are discarded because they are too close to the obstacle boundaries. Then we use Algorithm1 to extend the remaining ones into 88 configuration samples, part of which are illustrated in Figure 6. In experiments 1 and 2, we set α
=
0.9, β
=
0.1 and α
=
0.3, β
=
0.7, respectively. Experiment 1 and experiment 2 were implemented using Microsoft Visual C++ using the Solid collision detection package [23] and run on an Intel Core i5-2400 CPU @ 3.10 Ghz with 4 GB internal memory. The results are depicted in Figures 7 and 8.Some representative configuration samples.
(a)
α
=
0.9, β
=
0.1
(b)
α
=
0.3, β
=
0.7
(c)
α
=
0.9, β
=
0.1
(d)
α
=
0.3, β
=
0.7Figure 7
The shape difference at every step.Figure 8
The distance between the group and the goal at every step.Figure7 depicts the shape difference between the group and the desired configuration at every step of our planning approach. Figure 8 depicts the distance between the group and the goal at every step. The experiment results show that when α
=
0.9, β
=
0.1, our approach selects the path across corridor A in experiment 1 and the path across corridor D in experiment 2. When α
=
0.3, β
=
0.7, our approach selects the path across corridor B in experiment 1 and the path across corridor C in experiment 2. These two paths are longer but have less deformation than the paths across corridors A and D. In Figure 7 we can see that because the initial configuration is different from the desired configuration, the group tries to make its shape approach the desired shape in the beginning of the planning steps. So, the shape difference declines before entering the corridors. When the group enters the corridors, the deformation begins. In the worst case (corridor D), the maximum shape difference equals 85 m. In the best case (corridor B), because its width is bigger than the size of the desired shape, the shape difference remains zero after reducing to zero. Therefore, increasing β will make the group less deformed when passing through narrow corridors. However, this will lead to the increase of the distance from the group to the goal in some planning steps, as can be seen in Figure 8.Moreover, the computational cost of C-L method is compared with our approach in the query phase in experiments 1 and 2. The results are depicted in Figure9.Figure 9
The consumed time of our approach versus C-L method in the query phase.In Figure9 we can see that our approach has a better performance than C-L method. This is because the configuration samples which are regarded as nodes of the roadmap provide guidance in planning the group’s path. Further, the computational time of our approach mainly comes from the collision detection of the boundary grids. So if there are two paths whose lengths are the same, the one which has more deformations will need more computational time.Furthermore, in order to prove the effectiveness of our approach, we perform an experiment in a more complicated environment with more grids. The scenario can be depicted in Figure10: there are some motionless vehicles which can be regarded as parts of obstacles, and the coherent group consists of a hundred virtual individuals.Figure 10
The virtual environment of the experiment.Both the proposed approach and C-L method are used to plan a path connecting the initial and goal configurations of the coherent group. And the visualization simulation results of our approach in 3D space are illustrated in Figure11. The experimental results show that the coherent group can find a path to reach the destination in the goal configuration and be collision-free with the obstacles.The configurations of the coherent group at different times.
(a)
T
1
=
0 s
(b)
T
2
=
242 s
(c)
T
3
=
491 s
(d)
T
4
=
716 sMoreover, the computational cost of our approach is compared with C-L method in this experiment, and the comparison result is 627 s to 703 s, which means that our approach has a better performance than C-L method.


## 7. Conclusions

We present a novel planning method for the coherent group whose area is constant when moving toward the goal. Our approach has two advantages: compared with the method which is based on classic PRM or its variants, our approach embeds the discretization way of C-L method in local planner to keep the area invariant in the planning. Compared with C-L method, the medial axis technique in the preprocessing phase can provide guidance for generating roadmaps, which decreases the blindness of the C-L method. Moreover, different weights ofα and β can produce different paths which have diverse features. And the computational cost of our approach means that it is suitable for planning paths for the coherent group.



---

*Source: 416913-2014-12-29.xml*