A Grid-Based Motion Planning Approach for Coherent Groups

This paper presents a novelmotion planning approach for coherent groups with constant area, and it integrates C-Lmethod 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.


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 2 Mathematical Problems in Engineering 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 Section 2. 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.

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][15][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 gridset 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][21][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.

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 Figure 1 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.

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 ( , ,  , )  = 1, . . .,  where  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 . .,  represents the information of the th grid and  represents the total number of the grids.Obviously,  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  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  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.

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 are  dimensions.The extending strategy is divided into two steps.
Step 1. Find the center grid of  set and then place it on the position samples.The center grid of  set is represented as   (  ,   ,   ) whose information can be computed by So, at every position sample, we make   =  , ,   =  , , and   equal to the tangent direction of the medial axis at ( , ,  , ).
Step 2. After placing the center grid on ( , ,  , ), we should decide the positions of the other  − 1 grids according to the environment around ( , ,  , ), the desired configuration   , 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 duplicate C of   on ( , ,  , ) by making its center grid coincide with ( , ,  , ) and its direction aligns with the tangent direction of medial axis; then we move the grids in C which have collided with the obstacles to free space according to the compact property and the distance to ( , ,  , ).However, as these collision grids move to free space, the center of C may change.Then we should adjust the center of C so that it coincides with ( , ,  , ) once again.This is done by adjusting the boundary grids and the whole position of C iteratively.Using   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].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  sampling  and  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. shape ( sampling  ,  sampling

𝑤
) is formulated in In formulation (3) 3) can also be used to compute the deformation degree of a configuration relative to   .

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 In formulation (5), ℎ( set ) represents the estimated distance from  set to the goal. shape ( set ,   ) represents the deformation degree of  set relative to   . 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   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   will be preferred.

Local Planner and Roadmap Generation.
After establishing the evaluation function and the neighboring relationship between  sampling  and  sampling  , the local planner can be constructed, the process of which is illustrated in Algorithm 2.
In Algorithm 2,  represents a list whose elements are all the neighbor configurations of the current ones   updated by Dequeue(), that is, the first element in .Moreover, the neighbor configuration of   is generated by moving its boundary grids on the basis of the compact property or rotating   in one run step.
It must be noted that, in Section 5.1, the expression "node neighbors" means the configuration nodes that satisfy the condition in (2).In order to generate the neighbor end if (10) if end if (13) end for (14) end while (15) if SUCCESS = true (16) return the constructed path P by tracing the configuration from  sampling configurations of   , a local coordinate system is defined with its origin coinciding with the center grid of   and axis aligning with the direction of   .There are two ways to generate the neighbor configurations of   in one run step.One is moving the boundary grids in the direction of -axis or -axis with a distance unit  step and  step according to the compact property; the other is rotating   with a predefined angle unit  step .The detailed process is illustrated in Algorithm 3.
Compared with previous researchers' works, Algorithm 3 has the advantage of keeping the group's area constant in the planning.After the above procedures, the roadmap is constructed.

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 the th node of the roadmap can be represented as By allocating different values of  and , we can influence whether we prefer a path that is short but has lots of deformation relative to   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  position samples by the medial axis technique and extend each position sample into group's configuration samples in  dimensions.In the second stage, the local planner is to connect any two neighbor configuration samples.We use the  shape ( sampling  ,  sampling  ) formulation for  times to compute the minimum distance from the grid  sampling , (  ,   ,   ) to  sampling  and use Algorithm 3 to generate the neighbor configurations of   for  times at most.In the third stage, the evaluation function in PRM algorithm at every node of the roadmap is called  times.Then the complexity of our planner is ( ⋅  +  ⋅  ⋅  ⋅  +  ⋅ ) < ( 2  +  2  2 ) which is equivalent to ( 2  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.

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 Figure 5 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.  step and  step are set to the size of the grid, that is, 0.7 m.  step is set to 5 ∘ .
Input.  = { ,1 ( 1 ,   (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)  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 Algorithm 1 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.
Figure 7 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  The step of our planning approach  = 0.9;  = 0.1 in experiment 1  = 0.3;  = 0.7 in experiment 1  = 0.9;  = 0.1 in experiment 2  = 0.3;  = 0.7 in experiment 2 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 Figure 9.
In Figure 9 we can see that our approach has a better performance than C-L method.This is because the Mathematical Problems in Engineering  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 Figure 10: there are some motionless vehicles which can be regarded as parts of obstacles, and the coherent group consists of a hundred virtual individuals.
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 Figure 11.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.
Moreover, 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.

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.

𝑤 to 𝐶 sampling 𝑞 ( 17 )
else return failure(18) end if Algorithm 2: The local planner connecting any two neighbor configurations.

return 𝐿 Algorithm 3 :Figure 5 :
Figure 5: Generating the neighbors of one configuration.

Figure 7 :
Figure 7: The shape difference at every step.

2 Figure 8 :
Figure 8: The distance between the group and the goal at every step.

Figure 9 :
Figure 9: The consumed time of our approach versus C-L method in the query phase.

Figure 10 :
Figure 10: The virtual environment of the experiment.

Figure 11 :
Figure 11: The configurations of the coherent group at different times.
Flowchart of the local planner.