# Optimal Acceleration-Velocity-Bounded Trajectory Planning in Dynamic Crowd Simulation

Authors: Fu Yue-wen; Li Meng; Liang Jia-hong; Hu Xiao-qian
Journal: Journal of Applied Mathematics
 (2014)

---

## Abstract

Creating complex and realistic crowd behaviors, such as pedestrian navigation behavior with dynamic obstacles, is a difficult and time consuming task. In this paper, we study one special type of crowd which is composed of urgent individuals, normal individuals, and normal groups. We use three steps to construct the crowd simulation in dynamic environment. The first one is that the urgent individuals move forward along a given path around dynamic obstacles and other crowd members. An optimal acceleration-velocity-bounded trajectory planning method is utilized to model their behaviors, which ensures that the durations of the generated trajectories are minimal and the urgent individuals are collision-free with dynamic obstacles (e.g., dynamic vehicles). In the second step, a pushing model is adopted to simulate the interactions between urgent members and normal ones, which ensures that the computational cost of the optimal trajectory planning is acceptable. The third step is obligated to imitate the interactions among normal members using collision avoidance behavior and flocking behavior. Various simulation results demonstrate that these three steps give realistic crowd phenomenon just like the real world.


---

## Body

## 1. Introduction

Crowds, ubiquitous in the real world from groups of humans to flocks of insects, are vital features to model in a virtual environment. The simulation of pedestrians is a difficult challenge that is beginning to capture the attention of researchers and practitioners in evacuation simulation and urban planning. The field of computer graphics, in which virtual human animation has been an important research interest for decades, has contributed technologies fundamental to the computer-assisted visualization, including the automatic animation of pedestrian [1–3]. Until now, various simulation models and architectures have been developed. Whether the behaviors of the crowd are realistic or not has many relations with the methods used in constructing the crowd models. This paper focuses on one special crowd phenomenon which is significant in real world. The crowd which is composed of urgent members and normal ones has the following characteristics: the urgent members are the people who move forward along a fixed, given path for something urgent to do. The normal ones are the people who move in normal ways, just like most of the people in their daily lives. In real world, urgent people do not allow others to get in their way. Consequently, if the normal members are in a predefined range of the urgent ones, the latter will push them away. However, we must ensure that the urgent members are collision-free with the dynamic obstacles in the environment. Moreover, the interactions among the normal members are important issues to improve the authenticity of the crowd behaviors. For this special crowd, all of these propose demands for the studies of its behaviors.Given the observations in real world, how is it possible that the urgent people can safely navigate through crowds and dynamic obstacles along a predefined path? The key insight is that the urgent people typically engage in moving forward just considering the dynamic obstacle: they push the normal members away from their trajectories to make room for their navigations.In our work, we have been developing an autonomous, self-animating model of pedestrian crowd capable of performing a broad variety of natural activities in urban dynamic environments. Because the acceleration and velocity of real person are bounded, we have adopted an optimal acceleration-velocity-bounded trajectory planning method in planning area and a comprehensive artificial life approach (e.g., collision avoidance method and flocking method) to addressing the problem of pedestrian behaviors. Our approach is inspired most heavily by the work of Johnson and Hauser [4] on optimal trajectory planning and by Foudil and Noureddine [5] on collision avoidance modeling in crowd simulation with priority rules. Also, Olfati-Saber’s work on flocking model is used for reference [6].In summary, our main contributions exist in three aspects.(1)
Construct the behaviors of the urgent members with an optimal acceleration-velocity-bounded trajectory planning method. This novel method ensures that the urgent members reach their destinations as soon as possible and are collision-free with the dynamic obstacles.
(2)
Simulate the interactions between the urgent members and the normal ones. A novel pushing model is adopted to ensure that the normal members will not get in the way of the urgent ones. In this way, the normal members will not be treated as dynamic obstacles which promotes that the computational cost of the trajectory planning is acceptable for real time application.
(3)
Imitate the interactions among the normal members. These interactions play a major role in synthesizing the animations of the crowd. The key insight is that the normal members typically engage in joint collision avoidance: they adapt their moving to each other to make room for navigation. So the collision avoidance behavior and the flocking behavior are exploited.The above three aspects are combined together to generate realistic behaviors for our special crowd. The components of the crowd are represented in Figure1.Figure 1
Components of the crowd behaviors.As illustrated in Figure1, the crowd behaviors consist of four parts: optimal trajectory planning, pushing model, collision avoidance behavior, and flocking behavior, each of which is responsible for dealing with different interactions. By designing and combining the respective behaviors of these four parts, realistic and complex behavior phenomena are achieved.The organization of the rest of this paper is as follows. At first, Section2 describes background and related works in trajectory planning, collision avoidance model, and flocking behavior. In Section 3, we give the detailed process of the optimal acceleration-velocity-bounded trajectory planning method and the pushing model. In Section 4, the collision avoidance model and flocking behavior for crowd simulation are presented briefly. After performing experiments with 3D virtual environments, simulation results and conclusions are stated at the end of this paper.


## 2. Related Work

In this paper, the behavior of the urgent members is a trajectory planning problem which has a vast difference with path planning. In respect of optimal trajectory planning problem with dynamic constraints in dynamic environments, Fraichard first addressed optimal trajectory planning in dynamic environments, that is, motion planning for robot subject to dynamic constraints and moving in a workspace with moving obstacles [7]. A novel concept of state-time space was introduced and a near-time-optimal trajectory was obtained by searching a set of canonical trajectories. The state-time space for planning in a dynamic environment is the Cartesian product of configuration space and time, that is, C
×
T. Fraichard defined the canonical trajectories as the ones which had piecewise constant acceleration and changed its value at discrete times [7]. Fiorini and Shiller also presented a two-stage approach to plan a local time-optimal trajectory for a manipulator arm in the dynamic environments [8]. An optimization-based planning was used by Park et al. to handle the planning problem in arbitrary dynamic environments [9]. However, the dynamic constraints were ignored in their work [9]. Fiorini and Shiller presented a global optimal motion planning for Mars Rover which accounted for traversability and dynamic stability [10]. Recently, Maček et al. adopted rapidly exploring random trees and B-splines to develop a collision-free trajectory planning method in dynamic urban-like scenarios. This method can run automatically with the natural acceleration/deceleration phases according to the dynamic obstacles along the predefined path. The advantage of this method is that the vehicle velocities can be searched with a minimum time. Maček et al. have used kinodynamic planning techniques to explore the vehicle’s action space [11]. While these above algorithms are able to find paths as well as velocity profiles, they have the defect of ignoring the acceleration constraint or obtaining near-optimal trajectories.More recently, Johnson and Hauser have presented an optimal, exact, polynomial-time planner for optimal bounded-acceleration trajectories along a fixed, given path with dynamic obstacles [4]. In their method, the planner uses a velocity interval propagation technique to compute reachable velocity sets at obstacle vertices in the path-time space. In this process, the acceleration and velocity constraints are considered in path-velocity plane. This approach gives us great promotions in fulfillment of this paper to generate the optimal trajectories of urgent members. However, in this method the goal velocity is an interval rather than a fixed value. This is not the case in real life; for example, the goal velocity of the virtual human at the goal position is equal to zero. So the optimal trajectory planning in this paper is more complex and intractable.In respect of collision avoidance behaviors for individuals and groups, techniques for simulating a crowd as a single entity have been proposed, as well as those which consider each person in the crowd separately. Collision avoidance is one of the crucial problems in crowd simulation. Without collision avoidance, crowd simulation does not look realistic. In crowd simulation, static and dynamic objects are the two types of obstacle that must concern us. The motionless objects can be seen as static obstacles, while dynamic objects refer to human, animals, and vehicles in the urban environments [12]. However, devising collision avoidance behavior among a number of individuals is more complex than designing the one between just two individuals. Morini et al. presented a short-term avoidance algorithm [13]. Aiming at the applications of densely populated urban environment, Tecchia and Chrysanthou proposed a simple and fast collision detection method. In their method, two different approaches for detecting and avoiding collision of the moving objects were used. Feurtey proposed an algorithm for collision avoidance which was planned from their current position to the goal based on the predicting and modifying trajectories. So in their method, the agents could predict their way by using the position and speed information of the obstacles [13]. Musse and Thalmann developed a multiresolution collision avoidance behavior for group interrelationship in multiresolution [14]. Foudil and Noureddine proposed a novel collision avoidance behavior for crowd simulation with priority rules. This method also provides us with great inspirations for our paper to generate various collision avoidance strategies for normal members [5].In respect of flocking behavior for groups, many researchers used flocking algorithm to model flock, herd, and large crowds of people. Flocking is one of the pioneer techniques used to model a crowd of animals or humans. Flocking is basically composed of three rules: separation-steer away from neighbors and obstacle, cohesion-steer towards the centre of nearby entities, and alignment-steer in the average velocity and direction with the neighbors. In recent years, various distributed control algorithms have been proposed for the motion of multiple dynamic agents and the multiagent systems. For example, the flocking problem with one virtual leader was considered by Shi et al. [15]. Su et al. investigated the problem of controlling a group of autonomous agents to track multiple virtual leaders. They proved that the agents with the same virtual leader attained the same velocity, and the center and average velocity of the whole group converged to the weighted center and average velocity of all the leaders [16]. Olfati-Saber proposed a groundbreaking work on flocking for multiagent dynamic systems. He proposed three types of flocking algorithm. The first one is the basic flocking algorithm with only separation, cohesion, and alignment. Then one single leader agent was added to obtain the second algorithm. His third algorithm was obligated to deal with flocking problem in the presence of multiple obstacles. Also, a systematic method is provided for analyzing the split/rejoin maneuver of flocking [6].All these above techniques can be seen as the lamp to light up our researches in generating realistic crowd behaviors of this paper.


## 3. Optimal Acceleration-Velocity-Bounded Trajectory Planning

### 3.1. Preliminaries

Suppose the virtual humans are simplified into cylinder with a unified radius. It is assumed that they move in the workspaceW
=
R
2. The configuration of a virtual human is uniquely defined by the triple (
x
,
y
,
θ
)
∈
C, where C
=
R
2
×
[
0
,
2
π
] denotes the configuration space. Suppose one urgent individual A is given a fixed parameterized geometric path S which is a continuous sequence of configurations, that is, a straight line or a curve in the configuration space. In this paper, we assume that S must be piecewise of class C
1 (a curve is of class C
n if it is differentiable n times and if its nth derivative is continuous). Because the urgent individual moves along S, we can reduce its configuration to a single variable s which satisfies q
(
s
)
:
[
0,1
]
→
C. s
(
t
) is a time scaling function s
:
[
0
,
t
f
]
→
[
0,1
] which assigns value s to each time t
∈
[
0
,
t
f
]. s
·
L
S represents the distance traveled along S in which L
S represents the total length of S. Moreover, the time scaling s
(
t
) should be twice-differentiable and monotonic (s
˙
(
t
)
≥
0 for all t
∈
[
0
,
t
f
]). The twice-differentiability of s
(
t
) ensures that the urgent individual’s acceleration q
¨
(
t
) is well defined and bounded [17].The dynamic planning problem occurs in the path-velocity-time state space, that is,s
×
s
˙
×
t space which is denoted as PVT [4]. So, the state of one urgent individual x
=
(
s
,
s
˙
,
t
) consists of variable s
∈
[
0,1
], velocity s
˙, and time t
∈
[
0
,
t
max
⁡
]. For a given q
(
s
), the following holds:
(1)
q
˙
=
d
q
d
s
s
˙
,
q
¨
=
d
2
q
d
s
2
s
˙
2
+
d
q
d
s
s
¨
.
The dynamic constraints of the urgent individuals must satisfy velocity and acceleration bounds:
(2)
0
≤
q
˙
x
2
+
q
˙
y
2
≤
v
max
⁡
,
q
˙
x
≥
0
,
q
˙
y
≥
0
,
0
≤
q
¨
x
2
+
q
¨
y
2
≤
a
max
⁡
.
Substituting (2) into (1), we get the following dynamic constraints on variable s:
(3)
s
˙
∈
[
s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
,
s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
]
,
(4)
s
¨
∈
[
s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
,
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
]
with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
≥
0 and s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
<
0
<
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
). A trajectory x
(
t
)
=
(
s
(
t
)
,
s
˙
(
t
)
,
t
) defined over t
∈
[
t
0
,
t
f
] in state space is dynamically feasible if and only if (3) and (4) are satisfied at the same time. Then, we say that a goal state x
f is dynamically reachable from initial state x
0 if a dynamically feasible trajectory x
(
t
) over interval [
t
0
,
t
f
] such that x
(
t
0
)
=
x
0 and x
(
t
f
)
=
x
f is existed. Note that there is a one-to-one correspondence between the trajectory x
(
t
) and its time scaling function s
(
t
).LetO
i
i
∈
{
1
,
…
,
m
} be the set of moving obstacles. Let O
i
(
t
) denote the region of W occupied by O
i at time t and A
(
q
(
s
(
t
)
)
) the region of W occupied by A at position q
(
s
(
t
)
) along S. Then the obstacle region in state space is defined as
(5)
X
obs
=
{
(
s
,
s
˙
,
t
)
∣
∃
i
∈
{
1
,
…
,
m
}
,
A
(
q
(
s
(
t
)
)
)
∩
O
i
(
t
)
≠
∅
}
.Obviously, theX
obs corresponds to (
s
(
t
)
,
t
) points in s
×
t space. The trajectory x
(
t
) is collision-free if (
s
(
t
)
,
t
)
∉
X
obs for t
∈
[
t
0
,
t
f
].Above all, we can formally state the problem which is to be solved. Let(
s
0
,
s
˙
0
,
0
) be the start state of A and (
s
f
,
s
˙
f
,
t
f
) its goal state.A trajectoryΓ
:
[
0,1
]
→
PVT is a solution to the problem if and only if(1)
Γ
(
0
)
=
(
s
0
,
s
˙
0
,
0
), Γ
(
1
)
=
(
s
f
,
s
˙
f
,
t
f
), and s
˙
0
,
s
˙
f
≥
0,
(2)
Γ is dynamically feasible and collision-free for t
∈
[
0
,
t
f
] and t
f
≤
t
max
⁡. t
max
⁡ is the maximal time given for the planner,
(3)
s
˙
f is a specific value (e.g., s
˙
f
=
0) rather than a velocity interval.We are interested in finding an exact time-optimal trajectory, that is, a trajectory such thatt
f should be minimal. Moreover, s
˙
f is an interval in Johnson’s method rather than a specific value. So the optimal trajectory planning in this paper is more complex and intractable.


### 3.2. Optimal Acceleration-Velocity-Bounded Trajectory Planning

The method that we have developed in order to solve the above problem is initially motivated by the work described in [4]. We improve Johnson’s method in order to find out the exact time-optimal trajectory between an initial and a specific goal state. The flowchart of our method is illustrated in Figure 2.Figure 2
Flowchart of optimal trajectory planning.Next we will describe every part of Figure2 in detail.


#### 3.2.1. Compute Reachable Velocity Sets

(I) Compute Reachable Velocity Sets from One Specified Initial State. The goal of this section is to facilitate the computing reachable velocity intervals from one specified initial state. Without loss of generality, we suppose the specified initial state is x
I
=
(
s
I
,
s
˙
I
,
t
I
) which can be any state in {
(
s
0
(
t
0
)
,
s
˙
0
(
t
0
)
,
t
0
)
,
…
,
(
s
2
m
+
1
(
t
2
m
+
1
)
,
s
˙
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
)
}. The dynamically reachable set of velocities R
(
t
;
x
I
) from x
I at any point in time t
≥
t
I is the set of velocities attainable at t from x
I via dynamically feasible trajectories. It can be computed in s
×
s
˙ space. In order to simplify the computation, we assume that S is a straight line. Then (3) and (4) can be simplified into
(6)
s
˙
∈
[
0
,
v
max
⁡
L
S
]
,
that
is
,
s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
=
0
;
s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
=
v
max
⁡
L
S
,
(7)
s
¨
∈
[
-
a
max
⁡
L
S
,
a
max
⁡
L
S
]
,
that
is
,
s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
=
-
a
max
⁡
L
S
;
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
=
a
max
⁡
L
S
.
Johnson showed that R
(
t
;
x
I
) is bounded by at most six curves [4] which are denoted in Figure 3 in our paper.Figure 3
Reachable velocity sets from one initial state.In Figure3, parabolic segment (a) corresponds to the process in which A firstly takes the minimal acceleration for time t
s and then switches to maximal acceleration for time t
-
t
s. It must be noted that the terminal velocity of the process of deceleration and the one of the process of acceleration must satisfy formulation (6). The states on (a) can be formulated as in the following equation:
(8)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
(
t
-
t
s
)
+
1
2
s
¨
max
⁡
(
t
-
t
s
)
2
,
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
(
t
-
t
s
)
,
t
I
+
t
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
(
t
-
t
s
)
)
,
s
˙
I
+
s
¨
min
⁡
t
s
≥
s
˙
min
⁡
,
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
(
t
-
t
s
)
≤
s
˙
max
⁡
.
Similarly, parabolic segment (d) corresponds to the process, takes the maximal acceleration for time t
s, and then switches to minimal acceleration for time t
-
t
s. The states on (d) can be formulated as
(9)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
(
t
-
t
s
)
+
1
2
s
¨
min
⁡
(
t
-
t
s
)
2
,
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
(
t
-
t
s
)
,
t
I
+
t
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
(
t
-
t
s
)
)
,
s
˙
I
+
s
¨
max
⁡
t
s
≤
s
˙
max
⁡
,
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
(
t
-
t
s
)
≥
s
˙
min
⁡
.
Parabolic segment (c) corresponds to the process, takes the maximal acceleration until reaching s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) at the time t
v, progresses with s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t
s elapse, and then decelerates for time t
-
t
s. The states on (c) can be formulated as
(10)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
v
+
1
2
s
¨
max
⁡
t
v
2
+
s
˙
max
⁡
(
t
s
-
t
v
)
+
s
˙
max
⁡
(
t
-
t
s
)
+
1
2
s
¨
min
⁡
(
t
-
t
s
)
2
,
s
˙
max
⁡
+
s
¨
min
⁡
(
t
-
t
s
)
,
t
I
+
t
)
,
t
v
=
(
s
˙
max
⁡
-
s
˙
I
s
¨
max
⁡
)
;
s
˙
I
+
s
¨
max
⁡
t
s
≥
s
˙
max
⁡
.
Similarly, parabolic segment (f) corresponds to the process, takes the minimal acceleration until reaching s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), progresses with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t
s elapse, and then accelerates for time t
-
t
s. The states on (f) can be formulated as
(11)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
v
+
1
2
s
¨
min
⁡
t
v
2
+
s
˙
min
⁡
(
t
s
-
t
v
)
+
s
˙
min
⁡
(
t
-
t
s
)
+
1
2
s
¨
max
⁡
(
t
-
t
s
)
2
,
s
˙
min
⁡
+
s
¨
max
⁡
(
t
-
t
s
)
,
t
I
+
t
)
,
t
v
=
(
s
˙
min
⁡
-
s
˙
I
s
¨
min
⁡
)
;
s
˙
I
+
s
¨
min
⁡
t
s
≤
s
˙
min
⁡
.
Line segment (b) corresponds to the process, takes the minimal acceleration for time t
s, and then switches to maximal acceleration until reaching s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), finally progressing with s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t elapse. The states on (b) can be formulated as
(12)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
t
v
+
1
2
s
¨
max
⁡
t
v
2
+
s
˙
max
⁡
(
t
-
t
s
-
t
v
)
,
s
˙
max
⁡
,
t
I
+
t
)
,
t
v
=
(
s
˙
max
⁡
-
s
˙
I
-
s
¨
min
⁡
t
s
s
¨
max
⁡
)
;
s
˙
I
+
s
¨
min
⁡
t
s
≥
s
˙
min
⁡
;
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
t
v
≥
s
˙
max
⁡
.
Similarly, line segment (e) corresponds to the process, takes the maximal acceleration for time t
s, and then switches to minimal acceleration until reaching s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), finally progressing with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t elapse. The states on (e) can be formulated as
(13)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
t
v
+
1
2
s
¨
min
⁡
t
v
2
+
s
˙
min
⁡
(
t
-
t
s
-
t
v
)
,
s
˙
min
⁡
,
t
I
+
t
)
,
t
v
=
(
s
˙
min
⁡
-
s
˙
I
-
s
¨
max
⁡
t
s
s
¨
min
⁡
)
;
s
˙
I
+
s
¨
max
⁡
t
s
≤
s
˙
max
⁡
;
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
t
v
≤
s
˙
min
⁡
.All these six curves enclose the reachable velocity setR
(
t
;
x
I
) from one single initial state x
I
=
(
s
I
,
s
˙
I
,
t
I
) which is graphically showed as the shadow region in Figure 3.(II) Compute Reachable Velocity Interval at a Point in
s
×
t
Space from
x
I
=
(
s
I
,
s
˙
I
,
t
I
)
. After obtaining the reachable velocity set in s
×
s
˙ space, we can then compute the reachable velocity interval at a point (
s
G
(
t
G
)
,
t
G
) in s
×
t space from the initial state x
I
=
(
s
I
(
t
I
)
,
s
˙
I
,
t
I
). We use {
s
˙
G
(
t
G
)
} to represent the reachable velocity interval at (
s
G
(
t
G
)
,
t
G
). We use R
(
t
G
-
t
I
;
x
I
) to compute {
s
˙
G
(
t
G
)
} in the following formulation:
(14)
{
s
˙
G
(
t
G
)
}
=
[
s
˙
_
G
,
s
˙
-
G
]
=
R
(
t
G
-
t
I
;
x
I
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
G
}
in which s
˙
-
G and s
˙
_
G represent the upper and lower bounds of velocity interval {
s
˙
G
(
t
G
)
}, respectively. s
˙
-
G is defined as the intersection of a boundary curve of R
(
t
G
-
t
I
;
x
I
) with s
=
s
G.We only need to consider three boundary cures in computings
˙
-
G (curves (a), (b), and (f)) and s
˙
_
G (curves (c), (d), and (e)), as illustrated in Figure 4.Figure 4
Reachable velocity sets from one initial state.In this process, we determine the switching timet
s by substituting s
=
s
G into (8), (11), and (12) for s
˙
-
G and (9), (10), and (13) for s
˙
_
G, respectively, and solving the resulting quadratic equation. At the same time, the constraints in (8)~(13) are validated to determine if some trajectories exist from x
I
=
(
s
I
,
s
˙
I
,
t
I
) to (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
).(III) VIP Algorithm Computing Reachable Velocity Interval from an Initial Velocity Interval. After computing the reachable velocity interval from an initial specified velocity, we can extend it to compute the reachable velocity interval from an initial velocity interval which forms the backbone of optimal trajectory planner. It takes (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
) and a target point (
s
H
(
t
H
)
,
t
H
) in s
×
t space as input. It outputs a range of target velocities {
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
] reachable from (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
). {
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
] is computed in the following steps.Step 1.
Compute the reachable velocity intervalV
1
=
R
(
t
H
-
t
G
;
x
G
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
H
} for initial velocity s
˙
G
=
s
˙
-
G from (
s
G
(
t
G
)
,
s
˙
-
G
,
t
G
).Step 2.
Compute the reachable velocity intervalV
2
=
R
(
t
H
-
t
G
;
x
G
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
H
} for initial velocity s
˙
G
=
s
˙
_
G from (
s
G
(
t
G
)
,
s
˙
_
G
,
t
G
).Step 3.
The goal of this step is maximizing the terminal velocity at(
s
H
(
t
H
)
,
t
H
). It does so by constructing a parabolic trajectory with acceleration s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
) that interpolates (
s
G
(
t
G
)
,
t
G
) and (
s
H
(
t
H
)
,
t
H
). If the initial velocity s
˙
G
,
max
⁡
interpolate of this interpolating parabolic trajectory belongs to {
s
˙
G
(
t
G
)
}, then V
3
=
{
s
˙
G
,
max
⁡
interpolate
}.Step 4.
The goal of this step is minimizing the terminal velocity at(
s
H
(
t
H
)
,
t
H
). It also does so by constructing a parabolic trajectory with acceleration s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
) that interpolates (
s
G
(
t
G
)
,
t
G
) and (
s
H
(
t
H
)
,
t
H
). If the initial velocity s
˙
G
,
min
⁡
interpolate of this interpolating parabolic trajectory belongs to {
s
˙
G
(
t
G
)
}, then V
4
=
{
s
˙
G
,
min
⁡
interpolate
}.Step 5.
Finally, the output velocity interval{
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
]
=
V
1
∪
V
2
∪
V
3
∪
V
4. Note that either V
1, V
2, V
3, or V
4 may be empty.Because the goal of the method in this section is propagating the velocity intervals, from one velocity interval to the other as long as we know the initial and terminal point ins
×
t space, we call this method velocity interval propagation (VIP algorithm). Steps 1~4 can be represented as {
s
˙
H
(
t
H
)
}
=
VIP
(
(
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
)
,
(
s
H
(
t
H
)
,
t
H
)
) in short.


#### 3.2.2. Computing the Optimal Trajectory

Suppose there arem dynamic obstacles in the environment; then only 2
m vertices are important to our optimal planning which correspond to the upper left and lower right vertex of the obstacles [4]. Let (
s
1
(
t
1
)
,
t
1
)
,
…
,
(
s
2
m
(
t
2
m
)
,
t
2
m
) be the sequence of upper left and lower right obstacle vertices such that t
0
≤
t
i
≤
t
f for all i
=
1
,
…
,
2
m. Then we sort these 2
m vertices by increasing t coordinate and denote (
s
0
(
t
0
)
,
s
˙
0
(
t
0
)
,
t
0
)
=
(
s
0
,
s
˙
0
,
0
) and (
s
2
m
+
1
(
t
2
m
+
1
)
,
s
˙
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
)
=
(
s
f
,
s
˙
f
,
t
f
). Our planner consists of four stages.In the first stage, using the VIP algorithm, we can get the velocity interval from any vertex of one dynamic obstacle to any vertex of another one for(
s
0
(
t
0
)
,
t
0
)
,
…
,
(
s
2
m
(
t
2
m
)
,
t
2
m
); that is, we carry out {
s
˙
j
(
t
j
)
}
=
VIP
(
(
s
i
(
t
i
)
,
{
s
˙
i
(
t
i
)
}
,
t
i
)
,
(
s
j
(
t
j
)
,
t
j
)
) for all i
,
j
∈
[
1,2
m
]
;
i
≠
j.In the second stage, we get2
m channels and 2
m terminal velocity intervals of these channels. These terminal velocity intervals can be merged into 2
m velocity intervals {
{
s
˙
1
,
union
(
t
1
)
}
,
…
,
{
s
˙
2
m
,
union
(
t
2
m
)
}
} corresponding to the 2
m vertices with union operation.In the third stage, we use VIP algorithm to connect(
s
2
m
(
t
2
m
)
,
t
2
m
) with (
s
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
) by taking these 2
m velocity intervals as input to determine the terminal velocity interval at given terminal time. We use discrete t
i
,
f
k to represent the given terminal time:
(15)
t
i
,
f
k
=
t
i
+
k
·
Δ
t
k
=
1
,
…
,
⌊
t
max
⁡
-
t
i
Δ
t
⌋
.Then we can get the terminal velocity interval at every given terminal timet
i
,
f
k by calling {
s
˙
k
(
t
i
,
f
k
)
}
=
VIP
(
(
s
i
(
t
i
)
,
{
s
˙
i
,
union
(
t
i
)
}
,
t
i
)
,
(
s
k
(
t
i
,
f
k
)
,
t
i
,
f
k
)
)
i
=
1
,
…
,
2
m
;
k
=
1
,
…
,
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋.In the fourth stage, we judge ifs
˙
f is in these ∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋ terminal velocity intervals or not. If true, we return the trajectory with minimal terminal time t
i
,
f
k which is the optimal trajectory and its final velocity equals s
˙
f. If not, the planner returns failure. It means that no trajectory exists between the initial state and the goal state of A which satisfies the dynamic constraints and is collision-free with dynamic obstacles.Complexity Analysis. The computational cost of the planner mainly comes from the first and the third stage. In the first stage, we must call the VIP algorithm for 2
m times. In the third stage, VIP algorithm is called ∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋ times. Then the complexity of our planner is O
(
2
m
+
∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋
)
<
O
(
2
m
+
2
m
⌊
t
max
⁡
/
Δ
t
⌋
) which is equivalent to O
(
2
m
). This computational cost is vast for big m values. However, there are only several dynamic obstacles in our environment and this is why we apply pushing behavior to deal with the interaction between the urgent members and the normal members.


### 3.3. Pushing Model

Pushing model is used to model the interactions between the urgent members and the normal ones; that is, the normal members will be pushed away when they are in a predefined range of the urgent ones. We use the four following rules to model pushing behavior between any normal individuali and urgent one j:
(16)
If
d
i
j
=
0
000000000000000
Then
V
i
j
→
=
-
V
max
⁡
If
0
<
d
i
j
<
μ
00000000000
.
Then
V
i
j
→
=
-
M
·
V
max
⁡
·
e
-
d
i
j
+

V
0
If
|
V
i
j
→
|
>
V
max
⁡
2
-
(
V
i
j
→
⊥
)
2
0
Then
V
i
j
→
=
-
V
max
⁡
2
-
(
V
i
j
→
⊥
)
2
If
d
i
j
≥
μ
0000000000000.00
Then
V
i
j
→
=
V
0
in which d
i
j denotes the distance between i and j. μ represents the threshold of the interaction distance. V
i
j
→ represents the velocity of i at the direction of vector i
j
→. V
i
j
→
⊥ represents the velocity of i at the perpendicular direction of vector i
j
→. V
max
⁡ indicates the maximal velocity at which a normal individual moves. And V
0 represents the velocity of i before the interaction between i and j happens.This novel pushing model ensures that the urgent members will never collide with the normal ones. So we do not need to consider the normal members in the process of optimal trajectory planning whose cost time will greatly reduce owing to only the dynamic obstacles being considered.


## 3.1. Preliminaries

Suppose the virtual humans are simplified into cylinder with a unified radius. It is assumed that they move in the workspaceW
=
R
2. The configuration of a virtual human is uniquely defined by the triple (
x
,
y
,
θ
)
∈
C, where C
=
R
2
×
[
0
,
2
π
] denotes the configuration space. Suppose one urgent individual A is given a fixed parameterized geometric path S which is a continuous sequence of configurations, that is, a straight line or a curve in the configuration space. In this paper, we assume that S must be piecewise of class C
1 (a curve is of class C
n if it is differentiable n times and if its nth derivative is continuous). Because the urgent individual moves along S, we can reduce its configuration to a single variable s which satisfies q
(
s
)
:
[
0,1
]
→
C. s
(
t
) is a time scaling function s
:
[
0
,
t
f
]
→
[
0,1
] which assigns value s to each time t
∈
[
0
,
t
f
]. s
·
L
S represents the distance traveled along S in which L
S represents the total length of S. Moreover, the time scaling s
(
t
) should be twice-differentiable and monotonic (s
˙
(
t
)
≥
0 for all t
∈
[
0
,
t
f
]). The twice-differentiability of s
(
t
) ensures that the urgent individual’s acceleration q
¨
(
t
) is well defined and bounded [17].The dynamic planning problem occurs in the path-velocity-time state space, that is,s
×
s
˙
×
t space which is denoted as PVT [4]. So, the state of one urgent individual x
=
(
s
,
s
˙
,
t
) consists of variable s
∈
[
0,1
], velocity s
˙, and time t
∈
[
0
,
t
max
⁡
]. For a given q
(
s
), the following holds:
(1)
q
˙
=
d
q
d
s
s
˙
,
q
¨
=
d
2
q
d
s
2
s
˙
2
+
d
q
d
s
s
¨
.
The dynamic constraints of the urgent individuals must satisfy velocity and acceleration bounds:
(2)
0
≤
q
˙
x
2
+
q
˙
y
2
≤
v
max
⁡
,
q
˙
x
≥
0
,
q
˙
y
≥
0
,
0
≤
q
¨
x
2
+
q
¨
y
2
≤
a
max
⁡
.
Substituting (2) into (1), we get the following dynamic constraints on variable s:
(3)
s
˙
∈
[
s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
,
s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
]
,
(4)
s
¨
∈
[
s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
,
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
]
with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
≥
0 and s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
<
0
<
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
). A trajectory x
(
t
)
=
(
s
(
t
)
,
s
˙
(
t
)
,
t
) defined over t
∈
[
t
0
,
t
f
] in state space is dynamically feasible if and only if (3) and (4) are satisfied at the same time. Then, we say that a goal state x
f is dynamically reachable from initial state x
0 if a dynamically feasible trajectory x
(
t
) over interval [
t
0
,
t
f
] such that x
(
t
0
)
=
x
0 and x
(
t
f
)
=
x
f is existed. Note that there is a one-to-one correspondence between the trajectory x
(
t
) and its time scaling function s
(
t
).LetO
i
i
∈
{
1
,
…
,
m
} be the set of moving obstacles. Let O
i
(
t
) denote the region of W occupied by O
i at time t and A
(
q
(
s
(
t
)
)
) the region of W occupied by A at position q
(
s
(
t
)
) along S. Then the obstacle region in state space is defined as
(5)
X
obs
=
{
(
s
,
s
˙
,
t
)
∣
∃
i
∈
{
1
,
…
,
m
}
,
A
(
q
(
s
(
t
)
)
)
∩
O
i
(
t
)
≠
∅
}
.Obviously, theX
obs corresponds to (
s
(
t
)
,
t
) points in s
×
t space. The trajectory x
(
t
) is collision-free if (
s
(
t
)
,
t
)
∉
X
obs for t
∈
[
t
0
,
t
f
].Above all, we can formally state the problem which is to be solved. Let(
s
0
,
s
˙
0
,
0
) be the start state of A and (
s
f
,
s
˙
f
,
t
f
) its goal state.A trajectoryΓ
:
[
0,1
]
→
PVT is a solution to the problem if and only if(1)
Γ
(
0
)
=
(
s
0
,
s
˙
0
,
0
), Γ
(
1
)
=
(
s
f
,
s
˙
f
,
t
f
), and s
˙
0
,
s
˙
f
≥
0,
(2)
Γ is dynamically feasible and collision-free for t
∈
[
0
,
t
f
] and t
f
≤
t
max
⁡. t
max
⁡ is the maximal time given for the planner,
(3)
s
˙
f is a specific value (e.g., s
˙
f
=
0) rather than a velocity interval.We are interested in finding an exact time-optimal trajectory, that is, a trajectory such thatt
f should be minimal. Moreover, s
˙
f is an interval in Johnson’s method rather than a specific value. So the optimal trajectory planning in this paper is more complex and intractable.


## 3.2. Optimal Acceleration-Velocity-Bounded Trajectory Planning

The method that we have developed in order to solve the above problem is initially motivated by the work described in [4]. We improve Johnson’s method in order to find out the exact time-optimal trajectory between an initial and a specific goal state. The flowchart of our method is illustrated in Figure 2.Figure 2
Flowchart of optimal trajectory planning.Next we will describe every part of Figure2 in detail.


### 3.2.1. Compute Reachable Velocity Sets

(I) Compute Reachable Velocity Sets from One Specified Initial State. The goal of this section is to facilitate the computing reachable velocity intervals from one specified initial state. Without loss of generality, we suppose the specified initial state is x
I
=
(
s
I
,
s
˙
I
,
t
I
) which can be any state in {
(
s
0
(
t
0
)
,
s
˙
0
(
t
0
)
,
t
0
)
,
…
,
(
s
2
m
+
1
(
t
2
m
+
1
)
,
s
˙
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
)
}. The dynamically reachable set of velocities R
(
t
;
x
I
) from x
I at any point in time t
≥
t
I is the set of velocities attainable at t from x
I via dynamically feasible trajectories. It can be computed in s
×
s
˙ space. In order to simplify the computation, we assume that S is a straight line. Then (3) and (4) can be simplified into
(6)
s
˙
∈
[
0
,
v
max
⁡
L
S
]
,
that
is
,
s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
=
0
;
s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
=
v
max
⁡
L
S
,
(7)
s
¨
∈
[
-
a
max
⁡
L
S
,
a
max
⁡
L
S
]
,
that
is
,
s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
=
-
a
max
⁡
L
S
;
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
=
a
max
⁡
L
S
.
Johnson showed that R
(
t
;
x
I
) is bounded by at most six curves [4] which are denoted in Figure 3 in our paper.Figure 3
Reachable velocity sets from one initial state.In Figure3, parabolic segment (a) corresponds to the process in which A firstly takes the minimal acceleration for time t
s and then switches to maximal acceleration for time t
-
t
s. It must be noted that the terminal velocity of the process of deceleration and the one of the process of acceleration must satisfy formulation (6). The states on (a) can be formulated as in the following equation:
(8)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
(
t
-
t
s
)
+
1
2
s
¨
max
⁡
(
t
-
t
s
)
2
,
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
(
t
-
t
s
)
,
t
I
+
t
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
(
t
-
t
s
)
)
,
s
˙
I
+
s
¨
min
⁡
t
s
≥
s
˙
min
⁡
,
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
(
t
-
t
s
)
≤
s
˙
max
⁡
.
Similarly, parabolic segment (d) corresponds to the process, takes the maximal acceleration for time t
s, and then switches to minimal acceleration for time t
-
t
s. The states on (d) can be formulated as
(9)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
(
t
-
t
s
)
+
1
2
s
¨
min
⁡
(
t
-
t
s
)
2
,
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
(
t
-
t
s
)
,
t
I
+
t
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
(
t
-
t
s
)
)
,
s
˙
I
+
s
¨
max
⁡
t
s
≤
s
˙
max
⁡
,
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
(
t
-
t
s
)
≥
s
˙
min
⁡
.
Parabolic segment (c) corresponds to the process, takes the maximal acceleration until reaching s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) at the time t
v, progresses with s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t
s elapse, and then decelerates for time t
-
t
s. The states on (c) can be formulated as
(10)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
v
+
1
2
s
¨
max
⁡
t
v
2
+
s
˙
max
⁡
(
t
s
-
t
v
)
+
s
˙
max
⁡
(
t
-
t
s
)
+
1
2
s
¨
min
⁡
(
t
-
t
s
)
2
,
s
˙
max
⁡
+
s
¨
min
⁡
(
t
-
t
s
)
,
t
I
+
t
)
,
t
v
=
(
s
˙
max
⁡
-
s
˙
I
s
¨
max
⁡
)
;
s
˙
I
+
s
¨
max
⁡
t
s
≥
s
˙
max
⁡
.
Similarly, parabolic segment (f) corresponds to the process, takes the minimal acceleration until reaching s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), progresses with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t
s elapse, and then accelerates for time t
-
t
s. The states on (f) can be formulated as
(11)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
v
+
1
2
s
¨
min
⁡
t
v
2
+
s
˙
min
⁡
(
t
s
-
t
v
)
+
s
˙
min
⁡
(
t
-
t
s
)
+
1
2
s
¨
max
⁡
(
t
-
t
s
)
2
,
s
˙
min
⁡
+
s
¨
max
⁡
(
t
-
t
s
)
,
t
I
+
t
)
,
t
v
=
(
s
˙
min
⁡
-
s
˙
I
s
¨
min
⁡
)
;
s
˙
I
+
s
¨
min
⁡
t
s
≤
s
˙
min
⁡
.
Line segment (b) corresponds to the process, takes the minimal acceleration for time t
s, and then switches to maximal acceleration until reaching s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), finally progressing with s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t elapse. The states on (b) can be formulated as
(12)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
t
v
+
1
2
s
¨
max
⁡
t
v
2
+
s
˙
max
⁡
(
t
-
t
s
-
t
v
)
,
s
˙
max
⁡
,
t
I
+
t
)
,
t
v
=
(
s
˙
max
⁡
-
s
˙
I
-
s
¨
min
⁡
t
s
s
¨
max
⁡
)
;
s
˙
I
+
s
¨
min
⁡
t
s
≥
s
˙
min
⁡
;
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
t
v
≥
s
˙
max
⁡
.
Similarly, line segment (e) corresponds to the process, takes the maximal acceleration for time t
s, and then switches to minimal acceleration until reaching s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), finally progressing with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t elapse. The states on (e) can be formulated as
(13)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
t
v
+
1
2
s
¨
min
⁡
t
v
2
+
s
˙
min
⁡
(
t
-
t
s
-
t
v
)
,
s
˙
min
⁡
,
t
I
+
t
)
,
t
v
=
(
s
˙
min
⁡
-
s
˙
I
-
s
¨
max
⁡
t
s
s
¨
min
⁡
)
;
s
˙
I
+
s
¨
max
⁡
t
s
≤
s
˙
max
⁡
;
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
t
v
≤
s
˙
min
⁡
.All these six curves enclose the reachable velocity setR
(
t
;
x
I
) from one single initial state x
I
=
(
s
I
,
s
˙
I
,
t
I
) which is graphically showed as the shadow region in Figure 3.(II) Compute Reachable Velocity Interval at a Point in
s
×
t
Space from
x
I
=
(
s
I
,
s
˙
I
,
t
I
)
. After obtaining the reachable velocity set in s
×
s
˙ space, we can then compute the reachable velocity interval at a point (
s
G
(
t
G
)
,
t
G
) in s
×
t space from the initial state x
I
=
(
s
I
(
t
I
)
,
s
˙
I
,
t
I
). We use {
s
˙
G
(
t
G
)
} to represent the reachable velocity interval at (
s
G
(
t
G
)
,
t
G
). We use R
(
t
G
-
t
I
;
x
I
) to compute {
s
˙
G
(
t
G
)
} in the following formulation:
(14)
{
s
˙
G
(
t
G
)
}
=
[
s
˙
_
G
,
s
˙
-
G
]
=
R
(
t
G
-
t
I
;
x
I
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
G
}
in which s
˙
-
G and s
˙
_
G represent the upper and lower bounds of velocity interval {
s
˙
G
(
t
G
)
}, respectively. s
˙
-
G is defined as the intersection of a boundary curve of R
(
t
G
-
t
I
;
x
I
) with s
=
s
G.We only need to consider three boundary cures in computings
˙
-
G (curves (a), (b), and (f)) and s
˙
_
G (curves (c), (d), and (e)), as illustrated in Figure 4.Figure 4
Reachable velocity sets from one initial state.In this process, we determine the switching timet
s by substituting s
=
s
G into (8), (11), and (12) for s
˙
-
G and (9), (10), and (13) for s
˙
_
G, respectively, and solving the resulting quadratic equation. At the same time, the constraints in (8)~(13) are validated to determine if some trajectories exist from x
I
=
(
s
I
,
s
˙
I
,
t
I
) to (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
).(III) VIP Algorithm Computing Reachable Velocity Interval from an Initial Velocity Interval. After computing the reachable velocity interval from an initial specified velocity, we can extend it to compute the reachable velocity interval from an initial velocity interval which forms the backbone of optimal trajectory planner. It takes (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
) and a target point (
s
H
(
t
H
)
,
t
H
) in s
×
t space as input. It outputs a range of target velocities {
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
] reachable from (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
). {
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
] is computed in the following steps.Step 1.
Compute the reachable velocity intervalV
1
=
R
(
t
H
-
t
G
;
x
G
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
H
} for initial velocity s
˙
G
=
s
˙
-
G from (
s
G
(
t
G
)
,
s
˙
-
G
,
t
G
).Step 2.
Compute the reachable velocity intervalV
2
=
R
(
t
H
-
t
G
;
x
G
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
H
} for initial velocity s
˙
G
=
s
˙
_
G from (
s
G
(
t
G
)
,
s
˙
_
G
,
t
G
).Step 3.
The goal of this step is maximizing the terminal velocity at(
s
H
(
t
H
)
,
t
H
). It does so by constructing a parabolic trajectory with acceleration s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
) that interpolates (
s
G
(
t
G
)
,
t
G
) and (
s
H
(
t
H
)
,
t
H
). If the initial velocity s
˙
G
,
max
⁡
interpolate of this interpolating parabolic trajectory belongs to {
s
˙
G
(
t
G
)
}, then V
3
=
{
s
˙
G
,
max
⁡
interpolate
}.Step 4.
The goal of this step is minimizing the terminal velocity at(
s
H
(
t
H
)
,
t
H
). It also does so by constructing a parabolic trajectory with acceleration s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
) that interpolates (
s
G
(
t
G
)
,
t
G
) and (
s
H
(
t
H
)
,
t
H
). If the initial velocity s
˙
G
,
min
⁡
interpolate of this interpolating parabolic trajectory belongs to {
s
˙
G
(
t
G
)
}, then V
4
=
{
s
˙
G
,
min
⁡
interpolate
}.Step 5.
Finally, the output velocity interval{
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
]
=
V
1
∪
V
2
∪
V
3
∪
V
4. Note that either V
1, V
2, V
3, or V
4 may be empty.Because the goal of the method in this section is propagating the velocity intervals, from one velocity interval to the other as long as we know the initial and terminal point ins
×
t space, we call this method velocity interval propagation (VIP algorithm). Steps 1~4 can be represented as {
s
˙
H
(
t
H
)
}
=
VIP
(
(
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
)
,
(
s
H
(
t
H
)
,
t
H
)
) in short.


### 3.2.2. Computing the Optimal Trajectory

Suppose there arem dynamic obstacles in the environment; then only 2
m vertices are important to our optimal planning which correspond to the upper left and lower right vertex of the obstacles [4]. Let (
s
1
(
t
1
)
,
t
1
)
,
…
,
(
s
2
m
(
t
2
m
)
,
t
2
m
) be the sequence of upper left and lower right obstacle vertices such that t
0
≤
t
i
≤
t
f for all i
=
1
,
…
,
2
m. Then we sort these 2
m vertices by increasing t coordinate and denote (
s
0
(
t
0
)
,
s
˙
0
(
t
0
)
,
t
0
)
=
(
s
0
,
s
˙
0
,
0
) and (
s
2
m
+
1
(
t
2
m
+
1
)
,
s
˙
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
)
=
(
s
f
,
s
˙
f
,
t
f
). Our planner consists of four stages.In the first stage, using the VIP algorithm, we can get the velocity interval from any vertex of one dynamic obstacle to any vertex of another one for(
s
0
(
t
0
)
,
t
0
)
,
…
,
(
s
2
m
(
t
2
m
)
,
t
2
m
); that is, we carry out {
s
˙
j
(
t
j
)
}
=
VIP
(
(
s
i
(
t
i
)
,
{
s
˙
i
(
t
i
)
}
,
t
i
)
,
(
s
j
(
t
j
)
,
t
j
)
) for all i
,
j
∈
[
1,2
m
]
;
i
≠
j.In the second stage, we get2
m channels and 2
m terminal velocity intervals of these channels. These terminal velocity intervals can be merged into 2
m velocity intervals {
{
s
˙
1
,
union
(
t
1
)
}
,
…
,
{
s
˙
2
m
,
union
(
t
2
m
)
}
} corresponding to the 2
m vertices with union operation.In the third stage, we use VIP algorithm to connect(
s
2
m
(
t
2
m
)
,
t
2
m
) with (
s
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
) by taking these 2
m velocity intervals as input to determine the terminal velocity interval at given terminal time. We use discrete t
i
,
f
k to represent the given terminal time:
(15)
t
i
,
f
k
=
t
i
+
k
·
Δ
t
k
=
1
,
…
,
⌊
t
max
⁡
-
t
i
Δ
t
⌋
.Then we can get the terminal velocity interval at every given terminal timet
i
,
f
k by calling {
s
˙
k
(
t
i
,
f
k
)
}
=
VIP
(
(
s
i
(
t
i
)
,
{
s
˙
i
,
union
(
t
i
)
}
,
t
i
)
,
(
s
k
(
t
i
,
f
k
)
,
t
i
,
f
k
)
)
i
=
1
,
…
,
2
m
;
k
=
1
,
…
,
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋.In the fourth stage, we judge ifs
˙
f is in these ∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋ terminal velocity intervals or not. If true, we return the trajectory with minimal terminal time t
i
,
f
k which is the optimal trajectory and its final velocity equals s
˙
f. If not, the planner returns failure. It means that no trajectory exists between the initial state and the goal state of A which satisfies the dynamic constraints and is collision-free with dynamic obstacles.Complexity Analysis. The computational cost of the planner mainly comes from the first and the third stage. In the first stage, we must call the VIP algorithm for 2
m times. In the third stage, VIP algorithm is called ∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋ times. Then the complexity of our planner is O
(
2
m
+
∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋
)
<
O
(
2
m
+
2
m
⌊
t
max
⁡
/
Δ
t
⌋
) which is equivalent to O
(
2
m
). This computational cost is vast for big m values. However, there are only several dynamic obstacles in our environment and this is why we apply pushing behavior to deal with the interaction between the urgent members and the normal members.


## 3.2.1. Compute Reachable Velocity Sets

(I) Compute Reachable Velocity Sets from One Specified Initial State. The goal of this section is to facilitate the computing reachable velocity intervals from one specified initial state. Without loss of generality, we suppose the specified initial state is x
I
=
(
s
I
,
s
˙
I
,
t
I
) which can be any state in {
(
s
0
(
t
0
)
,
s
˙
0
(
t
0
)
,
t
0
)
,
…
,
(
s
2
m
+
1
(
t
2
m
+
1
)
,
s
˙
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
)
}. The dynamically reachable set of velocities R
(
t
;
x
I
) from x
I at any point in time t
≥
t
I is the set of velocities attainable at t from x
I via dynamically feasible trajectories. It can be computed in s
×
s
˙ space. In order to simplify the computation, we assume that S is a straight line. Then (3) and (4) can be simplified into
(6)
s
˙
∈
[
0
,
v
max
⁡
L
S
]
,
that
is
,
s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
=
0
;
s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
)
=
v
max
⁡
L
S
,
(7)
s
¨
∈
[
-
a
max
⁡
L
S
,
a
max
⁡
L
S
]
,
that
is
,
s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
=
-
a
max
⁡
L
S
;
s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
)
=
a
max
⁡
L
S
.
Johnson showed that R
(
t
;
x
I
) is bounded by at most six curves [4] which are denoted in Figure 3 in our paper.Figure 3
Reachable velocity sets from one initial state.In Figure3, parabolic segment (a) corresponds to the process in which A firstly takes the minimal acceleration for time t
s and then switches to maximal acceleration for time t
-
t
s. It must be noted that the terminal velocity of the process of deceleration and the one of the process of acceleration must satisfy formulation (6). The states on (a) can be formulated as in the following equation:
(8)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
(
t
-
t
s
)
+
1
2
s
¨
max
⁡
(
t
-
t
s
)
2
,
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
(
t
-
t
s
)
,
t
I
+
t
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
(
t
-
t
s
)
)
,
s
˙
I
+
s
¨
min
⁡
t
s
≥
s
˙
min
⁡
,
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
(
t
-
t
s
)
≤
s
˙
max
⁡
.
Similarly, parabolic segment (d) corresponds to the process, takes the maximal acceleration for time t
s, and then switches to minimal acceleration for time t
-
t
s. The states on (d) can be formulated as
(9)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
(
t
-
t
s
)
+
1
2
s
¨
min
⁡
(
t
-
t
s
)
2
,
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
(
t
-
t
s
)
,
t
I
+
t
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
(
t
-
t
s
)
)
,
s
˙
I
+
s
¨
max
⁡
t
s
≤
s
˙
max
⁡
,
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
(
t
-
t
s
)
≥
s
˙
min
⁡
.
Parabolic segment (c) corresponds to the process, takes the maximal acceleration until reaching s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) at the time t
v, progresses with s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t
s elapse, and then decelerates for time t
-
t
s. The states on (c) can be formulated as
(10)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
v
+
1
2
s
¨
max
⁡
t
v
2
+
s
˙
max
⁡
(
t
s
-
t
v
)
+
s
˙
max
⁡
(
t
-
t
s
)
+
1
2
s
¨
min
⁡
(
t
-
t
s
)
2
,
s
˙
max
⁡
+
s
¨
min
⁡
(
t
-
t
s
)
,
t
I
+
t
)
,
t
v
=
(
s
˙
max
⁡
-
s
˙
I
s
¨
max
⁡
)
;
s
˙
I
+
s
¨
max
⁡
t
s
≥
s
˙
max
⁡
.
Similarly, parabolic segment (f) corresponds to the process, takes the minimal acceleration until reaching s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), progresses with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t
s elapse, and then accelerates for time t
-
t
s. The states on (f) can be formulated as
(11)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
v
+
1
2
s
¨
min
⁡
t
v
2
+
s
˙
min
⁡
(
t
s
-
t
v
)
+
s
˙
min
⁡
(
t
-
t
s
)
+
1
2
s
¨
max
⁡
(
t
-
t
s
)
2
,
s
˙
min
⁡
+
s
¨
max
⁡
(
t
-
t
s
)
,
t
I
+
t
)
,
t
v
=
(
s
˙
min
⁡
-
s
˙
I
s
¨
min
⁡
)
;
s
˙
I
+
s
¨
min
⁡
t
s
≤
s
˙
min
⁡
.
Line segment (b) corresponds to the process, takes the minimal acceleration for time t
s, and then switches to maximal acceleration until reaching s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), finally progressing with s
˙
max
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t elapse. The states on (b) can be formulated as
(12)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
min
⁡
t
s
2
+
(
s
˙
I
+
s
¨
min
⁡
t
s
)
t
v
+
1
2
s
¨
max
⁡
t
v
2
+
s
˙
max
⁡
(
t
-
t
s
-
t
v
)
,
s
˙
max
⁡
,
t
I
+
t
)
,
t
v
=
(
s
˙
max
⁡
-
s
˙
I
-
s
¨
min
⁡
t
s
s
¨
max
⁡
)
;
s
˙
I
+
s
¨
min
⁡
t
s
≥
s
˙
min
⁡
;
s
˙
I
+
s
¨
min
⁡
t
s
+
s
¨
max
⁡
t
v
≥
s
˙
max
⁡
.
Similarly, line segment (e) corresponds to the process, takes the maximal acceleration for time t
s, and then switches to minimal acceleration until reaching s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
), finally progressing with s
˙
min
⁡
(
s
,
a
max
⁡
,
v
max
⁡
) until time t elapse. The states on (e) can be formulated as
(13)
x
t
I
+
t
=
(
s
I
+
s
˙
I
t
s
+
1
2
s
¨
max
⁡
t
s
2
+
(
s
˙
I
+
s
¨
max
⁡
t
s
)
t
v
+
1
2
s
¨
min
⁡
t
v
2
+
s
˙
min
⁡
(
t
-
t
s
-
t
v
)
,
s
˙
min
⁡
,
t
I
+
t
)
,
t
v
=
(
s
˙
min
⁡
-
s
˙
I
-
s
¨
max
⁡
t
s
s
¨
min
⁡
)
;
s
˙
I
+
s
¨
max
⁡
t
s
≤
s
˙
max
⁡
;
s
˙
I
+
s
¨
max
⁡
t
s
+
s
¨
min
⁡
t
v
≤
s
˙
min
⁡
.All these six curves enclose the reachable velocity setR
(
t
;
x
I
) from one single initial state x
I
=
(
s
I
,
s
˙
I
,
t
I
) which is graphically showed as the shadow region in Figure 3.(II) Compute Reachable Velocity Interval at a Point in
s
×
t
Space from
x
I
=
(
s
I
,
s
˙
I
,
t
I
)
. After obtaining the reachable velocity set in s
×
s
˙ space, we can then compute the reachable velocity interval at a point (
s
G
(
t
G
)
,
t
G
) in s
×
t space from the initial state x
I
=
(
s
I
(
t
I
)
,
s
˙
I
,
t
I
). We use {
s
˙
G
(
t
G
)
} to represent the reachable velocity interval at (
s
G
(
t
G
)
,
t
G
). We use R
(
t
G
-
t
I
;
x
I
) to compute {
s
˙
G
(
t
G
)
} in the following formulation:
(14)
{
s
˙
G
(
t
G
)
}
=
[
s
˙
_
G
,
s
˙
-
G
]
=
R
(
t
G
-
t
I
;
x
I
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
G
}
in which s
˙
-
G and s
˙
_
G represent the upper and lower bounds of velocity interval {
s
˙
G
(
t
G
)
}, respectively. s
˙
-
G is defined as the intersection of a boundary curve of R
(
t
G
-
t
I
;
x
I
) with s
=
s
G.We only need to consider three boundary cures in computings
˙
-
G (curves (a), (b), and (f)) and s
˙
_
G (curves (c), (d), and (e)), as illustrated in Figure 4.Figure 4
Reachable velocity sets from one initial state.In this process, we determine the switching timet
s by substituting s
=
s
G into (8), (11), and (12) for s
˙
-
G and (9), (10), and (13) for s
˙
_
G, respectively, and solving the resulting quadratic equation. At the same time, the constraints in (8)~(13) are validated to determine if some trajectories exist from x
I
=
(
s
I
,
s
˙
I
,
t
I
) to (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
).(III) VIP Algorithm Computing Reachable Velocity Interval from an Initial Velocity Interval. After computing the reachable velocity interval from an initial specified velocity, we can extend it to compute the reachable velocity interval from an initial velocity interval which forms the backbone of optimal trajectory planner. It takes (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
) and a target point (
s
H
(
t
H
)
,
t
H
) in s
×
t space as input. It outputs a range of target velocities {
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
] reachable from (
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
). {
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
] is computed in the following steps.Step 1.
Compute the reachable velocity intervalV
1
=
R
(
t
H
-
t
G
;
x
G
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
H
} for initial velocity s
˙
G
=
s
˙
-
G from (
s
G
(
t
G
)
,
s
˙
-
G
,
t
G
).Step 2.
Compute the reachable velocity intervalV
2
=
R
(
t
H
-
t
G
;
x
G
)
∩
{
(
s
,
s
˙
)
∣
s
=
s
H
} for initial velocity s
˙
G
=
s
˙
_
G from (
s
G
(
t
G
)
,
s
˙
_
G
,
t
G
).Step 3.
The goal of this step is maximizing the terminal velocity at(
s
H
(
t
H
)
,
t
H
). It does so by constructing a parabolic trajectory with acceleration s
¨
max
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
) that interpolates (
s
G
(
t
G
)
,
t
G
) and (
s
H
(
t
H
)
,
t
H
). If the initial velocity s
˙
G
,
max
⁡
interpolate of this interpolating parabolic trajectory belongs to {
s
˙
G
(
t
G
)
}, then V
3
=
{
s
˙
G
,
max
⁡
interpolate
}.Step 4.
The goal of this step is minimizing the terminal velocity at(
s
H
(
t
H
)
,
t
H
). It also does so by constructing a parabolic trajectory with acceleration s
¨
min
⁡
(
s
,
s
˙
,
a
max
⁡
,
v
max
⁡
) that interpolates (
s
G
(
t
G
)
,
t
G
) and (
s
H
(
t
H
)
,
t
H
). If the initial velocity s
˙
G
,
min
⁡
interpolate of this interpolating parabolic trajectory belongs to {
s
˙
G
(
t
G
)
}, then V
4
=
{
s
˙
G
,
min
⁡
interpolate
}.Step 5.
Finally, the output velocity interval{
s
˙
H
(
t
H
)
}
=
[
s
˙
_
H
,
s
˙
-
H
]
=
V
1
∪
V
2
∪
V
3
∪
V
4. Note that either V
1, V
2, V
3, or V
4 may be empty.Because the goal of the method in this section is propagating the velocity intervals, from one velocity interval to the other as long as we know the initial and terminal point ins
×
t space, we call this method velocity interval propagation (VIP algorithm). Steps 1~4 can be represented as {
s
˙
H
(
t
H
)
}
=
VIP
(
(
s
G
(
t
G
)
,
{
s
˙
G
(
t
G
)
}
,
t
G
)
,
(
s
H
(
t
H
)
,
t
H
)
) in short.


## 3.2.2. Computing the Optimal Trajectory

Suppose there arem dynamic obstacles in the environment; then only 2
m vertices are important to our optimal planning which correspond to the upper left and lower right vertex of the obstacles [4]. Let (
s
1
(
t
1
)
,
t
1
)
,
…
,
(
s
2
m
(
t
2
m
)
,
t
2
m
) be the sequence of upper left and lower right obstacle vertices such that t
0
≤
t
i
≤
t
f for all i
=
1
,
…
,
2
m. Then we sort these 2
m vertices by increasing t coordinate and denote (
s
0
(
t
0
)
,
s
˙
0
(
t
0
)
,
t
0
)
=
(
s
0
,
s
˙
0
,
0
) and (
s
2
m
+
1
(
t
2
m
+
1
)
,
s
˙
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
)
=
(
s
f
,
s
˙
f
,
t
f
). Our planner consists of four stages.In the first stage, using the VIP algorithm, we can get the velocity interval from any vertex of one dynamic obstacle to any vertex of another one for(
s
0
(
t
0
)
,
t
0
)
,
…
,
(
s
2
m
(
t
2
m
)
,
t
2
m
); that is, we carry out {
s
˙
j
(
t
j
)
}
=
VIP
(
(
s
i
(
t
i
)
,
{
s
˙
i
(
t
i
)
}
,
t
i
)
,
(
s
j
(
t
j
)
,
t
j
)
) for all i
,
j
∈
[
1,2
m
]
;
i
≠
j.In the second stage, we get2
m channels and 2
m terminal velocity intervals of these channels. These terminal velocity intervals can be merged into 2
m velocity intervals {
{
s
˙
1
,
union
(
t
1
)
}
,
…
,
{
s
˙
2
m
,
union
(
t
2
m
)
}
} corresponding to the 2
m vertices with union operation.In the third stage, we use VIP algorithm to connect(
s
2
m
(
t
2
m
)
,
t
2
m
) with (
s
2
m
+
1
(
t
2
m
+
1
)
,
t
2
m
+
1
) by taking these 2
m velocity intervals as input to determine the terminal velocity interval at given terminal time. We use discrete t
i
,
f
k to represent the given terminal time:
(15)
t
i
,
f
k
=
t
i
+
k
·
Δ
t
k
=
1
,
…
,
⌊
t
max
⁡
-
t
i
Δ
t
⌋
.Then we can get the terminal velocity interval at every given terminal timet
i
,
f
k by calling {
s
˙
k
(
t
i
,
f
k
)
}
=
VIP
(
(
s
i
(
t
i
)
,
{
s
˙
i
,
union
(
t
i
)
}
,
t
i
)
,
(
s
k
(
t
i
,
f
k
)
,
t
i
,
f
k
)
)
i
=
1
,
…
,
2
m
;
k
=
1
,
…
,
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋.In the fourth stage, we judge ifs
˙
f is in these ∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋ terminal velocity intervals or not. If true, we return the trajectory with minimal terminal time t
i
,
f
k which is the optimal trajectory and its final velocity equals s
˙
f. If not, the planner returns failure. It means that no trajectory exists between the initial state and the goal state of A which satisfies the dynamic constraints and is collision-free with dynamic obstacles.Complexity Analysis. The computational cost of the planner mainly comes from the first and the third stage. In the first stage, we must call the VIP algorithm for 2
m times. In the third stage, VIP algorithm is called ∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋ times. Then the complexity of our planner is O
(
2
m
+
∑
i
=
1
2
m
⌊
(
t
max
⁡
-
t
i
)
/
Δ
t
⌋
)
<
O
(
2
m
+
2
m
⌊
t
max
⁡
/
Δ
t
⌋
) which is equivalent to O
(
2
m
). This computational cost is vast for big m values. However, there are only several dynamic obstacles in our environment and this is why we apply pushing behavior to deal with the interaction between the urgent members and the normal members.


## 3.3. Pushing Model

Pushing model is used to model the interactions between the urgent members and the normal ones; that is, the normal members will be pushed away when they are in a predefined range of the urgent ones. We use the four following rules to model pushing behavior between any normal individuali and urgent one j:
(16)
If
d
i
j
=
0
000000000000000
Then
V
i
j
→
=
-
V
max
⁡
If
0
<
d
i
j
<
μ
00000000000
.
Then
V
i
j
→
=
-
M
·
V
max
⁡
·
e
-
d
i
j
+

V
0
If
|
V
i
j
→
|
>
V
max
⁡
2
-
(
V
i
j
→
⊥
)
2
0
Then
V
i
j
→
=
-
V
max
⁡
2
-
(
V
i
j
→
⊥
)
2
If
d
i
j
≥
μ
0000000000000.00
Then
V
i
j
→
=
V
0
in which d
i
j denotes the distance between i and j. μ represents the threshold of the interaction distance. V
i
j
→ represents the velocity of i at the direction of vector i
j
→. V
i
j
→
⊥ represents the velocity of i at the perpendicular direction of vector i
j
→. V
max
⁡ indicates the maximal velocity at which a normal individual moves. And V
0 represents the velocity of i before the interaction between i and j happens.This novel pushing model ensures that the urgent members will never collide with the normal ones. So we do not need to consider the normal members in the process of optimal trajectory planning whose cost time will greatly reduce owing to only the dynamic obstacles being considered.


## 4. Collision Avoidance Behaviors and Flocking Behaviors

### 4.1. Collision Avoidance Behaviors

In this section, we present the collision avoidance behaviors on the basis of Foudil and Noureddine’s work. As a general approach, this method can be combined with different crowd and multiagent simulation algorithms. In each time step of the simulation, we need to predict if every agent will collide with other agents in the crowd. Then we must determine the type of collision which may happen. In real life, there are three possible types of collision [5]. The first type istoward collision behavior. It happens when two agents are moving head-on toward each other. The second one isaway collision. It happens when one agent who is behind the other one is moving with a bigger velocity and whose moving direction is consistent with the line from the back agent to the front one. The third collision behavior isglancing collision which happens when two agents are walking in roughly the same direction.These three types of collision behaviors are shown in Figure5.Three types of the collision behaviors.
(a)
Toward collision
(b)
Away collision
(c)
Glancing collisionWe use a series of rules to realize these three collision avoidance behaviors separately. Intoward collision behavior, we use the following strategy to avoid the forthcoming collision: the agent who has a low priority can select waiting or changing its moving direction and the agent who has an upper priority selects keeping its moving on with unchanged direction and velocity. In respect toaway collision, the back agent can select slowing its velocity or changing its moving direction to any side. We deal withglancing collision with the same manner as thetoward collision behavior.Particularly, when these three collision avoidance behaviors conflict with each other, we resolve this problem by predefining the priority order of them. In this paper, the priority order of these three types of collision avoidance behaviors is toward collision, away collision, and glancing collision from high to low.


### 4.2. Flocking Behaviors

Flocking behaviors are used to model the collective gathering behaviors of the normal groups. Aiming at the agents group consisting of one leader agent andN follower ones, we use a distributed control method to simulate the flocking behavior [15, 16]. The motion of each virtual agent is described by two integrators as
(17)
p
˙
i
(
t
)
=
v
i
(
t
)
v
˙
i
(
t
)
=
a
i
(
t
)
i
=
1,2
,
…
,
N
,
where p
i, v
i, and a
i are denoted as the position, velocity, and acceleration of agent i, respectively. Similarly, the virtual leader has the following dynamics of motion:
(18)
p
˙
γ
(
t
)
=
v
γ
(
t
)
,
v
˙
γ
(
t
)
=
a
γ
(
t
)
,
where p
γ, v
γ, and a
γ represent the position, velocity, and acceleration of leader agent, respectively. The control algorithm of the flocking method is given as
(19)
a
i
=
-
∑
j
∈
N
i
(
t
)
∇
p
i
ψ
α
(
∥
p
i
-
p
j
∥
σ
)
+
∑
j
∈
N
i
(
t
)
a
i
j
(
t
)
(
v
j
-
v
i
)
+
a
γ
i
-
c
1
(
p
i
-
p
γ
i
)
-
c
2
(
v
i
-
v
γ
i
)
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
i
=
1
,
…
,
N
c
1
,
c
2
>
0
,
where N
i
(
t
)
=
{
j
:
∥
p
i
-
p
j
∥
≤
R
,
j
=
1,2
,
…
,
N
,
j
≠
i
} is the set of spatial neighbors of agent i. R represents the interaction range. ∥
·
∥
σ is the Euclidean norm and ∥
z
∥
σ
=
(
1
/
ε
)
[
1
+
ε
∥
z
∥
2
-
1
]
ε
>
0 for a vector z. ψ
α is the artificial nonnegative smooth pairwise potential function whose characteristics depend on the relative distances between agent i and its neighbors: ψ
α reaches its maximal value as ∥
p
i
-
p
j
∥
σ
→
0, and ψ
α acquires its unique minimal value at a predefined distance ∥
d
∥
σ. When ∥
p
i
-
p
j
∥
σ
<
∥
d
∥
σ, agent i attains repulsion force from j by ∇
p
i
ψ
α
(
∥
p
i
-
p
j
∥
σ
). When ∥
p
i
-
p
j
∥
σ
=
∥
d
∥
σ, the repulsion force and attraction force between i and j become balance. When ∥
d
∥
σ
<
∥
p
i
-
p
j
∥
σ
<
∥
R
∥
σ, agent i attains attraction force from j. And when ∥
p
i
-
p
j
∥
σ
≥
∥
R
∥
σ, ψ
α becomes constant; then no force exists between i and j. c
1 and c
2 are positive constants which reflect the influence degree from leader’s position and velocity. a
i
j
(
t
) is the adjacent weight coefficient.


## 4.1. Collision Avoidance Behaviors

In this section, we present the collision avoidance behaviors on the basis of Foudil and Noureddine’s work. As a general approach, this method can be combined with different crowd and multiagent simulation algorithms. In each time step of the simulation, we need to predict if every agent will collide with other agents in the crowd. Then we must determine the type of collision which may happen. In real life, there are three possible types of collision [5]. The first type istoward collision behavior. It happens when two agents are moving head-on toward each other. The second one isaway collision. It happens when one agent who is behind the other one is moving with a bigger velocity and whose moving direction is consistent with the line from the back agent to the front one. The third collision behavior isglancing collision which happens when two agents are walking in roughly the same direction.These three types of collision behaviors are shown in Figure5.Three types of the collision behaviors.
(a)
Toward collision
(b)
Away collision
(c)
Glancing collisionWe use a series of rules to realize these three collision avoidance behaviors separately. Intoward collision behavior, we use the following strategy to avoid the forthcoming collision: the agent who has a low priority can select waiting or changing its moving direction and the agent who has an upper priority selects keeping its moving on with unchanged direction and velocity. In respect toaway collision, the back agent can select slowing its velocity or changing its moving direction to any side. We deal withglancing collision with the same manner as thetoward collision behavior.Particularly, when these three collision avoidance behaviors conflict with each other, we resolve this problem by predefining the priority order of them. In this paper, the priority order of these three types of collision avoidance behaviors is toward collision, away collision, and glancing collision from high to low.


## 4.2. Flocking Behaviors

Flocking behaviors are used to model the collective gathering behaviors of the normal groups. Aiming at the agents group consisting of one leader agent andN follower ones, we use a distributed control method to simulate the flocking behavior [15, 16]. The motion of each virtual agent is described by two integrators as
(17)
p
˙
i
(
t
)
=
v
i
(
t
)
v
˙
i
(
t
)
=
a
i
(
t
)
i
=
1,2
,
…
,
N
,
where p
i, v
i, and a
i are denoted as the position, velocity, and acceleration of agent i, respectively. Similarly, the virtual leader has the following dynamics of motion:
(18)
p
˙
γ
(
t
)
=
v
γ
(
t
)
,
v
˙
γ
(
t
)
=
a
γ
(
t
)
,
where p
γ, v
γ, and a
γ represent the position, velocity, and acceleration of leader agent, respectively. The control algorithm of the flocking method is given as
(19)
a
i
=
-
∑
j
∈
N
i
(
t
)
∇
p
i
ψ
α
(
∥
p
i
-
p
j
∥
σ
)
+
∑
j
∈
N
i
(
t
)
a
i
j
(
t
)
(
v
j
-
v
i
)
+
a
γ
i
-
c
1
(
p
i
-
p
γ
i
)
-
c
2
(
v
i
-
v
γ
i
)
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
h
i
=
1
,
…
,
N
c
1
,
c
2
>
0
,
where N
i
(
t
)
=
{
j
:
∥
p
i
-
p
j
∥
≤
R
,
j
=
1,2
,
…
,
N
,
j
≠
i
} is the set of spatial neighbors of agent i. R represents the interaction range. ∥
·
∥
σ is the Euclidean norm and ∥
z
∥
σ
=
(
1
/
ε
)
[
1
+
ε
∥
z
∥
2
-
1
]
ε
>
0 for a vector z. ψ
α is the artificial nonnegative smooth pairwise potential function whose characteristics depend on the relative distances between agent i and its neighbors: ψ
α reaches its maximal value as ∥
p
i
-
p
j
∥
σ
→
0, and ψ
α acquires its unique minimal value at a predefined distance ∥
d
∥
σ. When ∥
p
i
-
p
j
∥
σ
<
∥
d
∥
σ, agent i attains repulsion force from j by ∇
p
i
ψ
α
(
∥
p
i
-
p
j
∥
σ
). When ∥
p
i
-
p
j
∥
σ
=
∥
d
∥
σ, the repulsion force and attraction force between i and j become balance. When ∥
d
∥
σ
<
∥
p
i
-
p
j
∥
σ
<
∥
R
∥
σ, agent i attains attraction force from j. And when ∥
p
i
-
p
j
∥
σ
≥
∥
R
∥
σ, ψ
α becomes constant; then no force exists between i and j. c
1 and c
2 are positive constants which reflect the influence degree from leader’s position and velocity. a
i
j
(
t
) is the adjacent weight coefficient.


## 5. Experimental Results and Discussion

In our simulation, we employ a human animation software package called DI-Guy, which is commercially available from Boston Dynamics Inc. We control the moving of the urgent members and normal members using SDK interface by C++ programs. The scenario can be depicted as follows: in a city populated with a majority of civilians and 2 moving cars. The civilians consist of three urgent individuals and 50 normal members comprising 30 normal individuals and 2 normal groups with 10 members, respectively. The cars’ moving characteristics are known previously. The priority values of the 50 normal agents are selected randomly from the interval[
0,1
]. The interaction range R
=
10
m and the predefined distance d
=
8
m. The values of a
i
j and ψ
α are set the same as those in [6], and c
1
=
0.81, c
2
=
2
c
1, and N
=
10. The dynamic environment of the experiments is depicted in Figure 6.Figure 6
The dynamic environment of the experiments.In Figure6, we set a
max
⁡
=
1.2
m
/
s
2 and v
max
⁡
=
8
m
/
s. Length of the specified paths of urgent individuals is L
S
1
=
20
m and L
S
2
=
30
m and L
S
3
=
25
m. The first dynamic obstacle (moving cars) moves at a constant speed of 10
m
/
s (Car1) and the second moves at a constant speed of 5
m
/
s (Car2). The s
×
t spaces of the three urgent individuals in the dynamic environment are depicted in Figure 7.Thes
×
t spaces of urgent individuals.
(a)
Urgent individual 1
(b)
Urgent individual 2
(c)
Urgent individual 3Figure8 illustrates the reachable sets and the optimal trajectory for each urgent individual.The optimal trajectory, time, and the velocity interval of urgent individuals.
(a)
Urgent individual 1
(b)
Urgent individual 2
(c)
Urgent individual 3The visualization simulation results of the crowd behaviors in 3D space are illustrated in Figure9.Visualization simulation results of the crowd behaviors.
(a)
t
=
2.41
s
(b)
t
=
4.2
s
(c)
t
=
6.5
s
(d)
t
=
7.5
sFrom Figure8, we can see that the gradient in s
×
t space is +
∞ which means that the terminal velocity of the urgent individual is zero, and this satisfies the requirement of the problem definition in Section 3.1; for example, in Figure 8(b), urgent individual 2 takes the maximal acceleration until reaching A point before Car1 arrives at his path and then switches to deacceleration for the path A-B in case of colliding with Car2. After that, he continues to take the maximal acceleration for path B-C and then deaccelerates to zero from C point to the terminal point of the whole path. Figure 9 depicts realistic and believable crowd behaviors in city environment: the urgent individual can reach the destination along a given path in minimal time and be collision-free with dynamic cars. Simultaneously, the agents of the two normal groups show real gathering behaviors according to the flocking method and the normal individuals try to keep away from the urgent ones and avoid collision with each other.The computation cost of our approach is illustrated in Figure10 when we change the numbers of the dynamic obstacles. Similarly, Figure 11 depicts the cost time of our approach according to the number of the normal members in the crowd.Figure 10
Cost time of our approach according to the number of the dynamic obstacles.Figure 11
Cost time of our approach according to the number of the normal members in the crowd.From Figure10, we can see that the cost time of our approach is influenced vastly by the number of dynamic obstacles in the environment. However the experiment results in Figure 11 show that the cost time of our approach changes slightly according to the number of the normal members. This means that the increase in the number of the normal members does not generate much computational cost. And all the above results are in accordance with previous analyses.


## 6. Conclusions

Although there have been some research studies on optimal trajectory planning for various purposes, few efforts have been conducted to simulate the realistic crowd behaviors with it, such as pedestrian navigation behavior in dynamic environment. In this paper, we first present an optimal acceleration-velocity-bounded trajectory planning method along a fixed, given path with dynamic obstacles. We use it to generate the optimal trajectory of the urgent members under the dynamic constraints. This planner ensures that the moving time of the urgent members is minimal by using a velocity interval propagation algorithm to compute reachable velocity sets at obstacle vertices ins
×
t space. Moreover, the cost time of this optimal trajectory planning method is acceptable by applying a novel pushing model. Finally, combining with the collision avoidance behavior and the flocking behavior, the crowd simulation with dynamic vehicles is implemented. So, the potential of our approach for planning the optimal trajectories and modeling the social behaviors of the crowd is promising.



---

*Source: 501689-2014-08-31.xml*