# Long-Term Dynamic Window Approach for Kinodynamic Local Planning in Static
and Crowd Environments

Zhiqiang Jian1, Songyi Zhang1, Lingfeng Sun2, Wei Zhan2, Nanning Zheng1†, and
Masayoshi Tomizuka2 1Z. Jian, S. Zhang, and N. Zheng are with the Institute of
Artificial Intelligence and Robotics, Xi’an Jiaotong University, Xi’an,
Shaanxi 710049, P.R. China; flztiii<EMAIL_ADDRESS><EMAIL_ADDRESS>Sun, W. Zhan, and M. Tomizuka are with the
Department of Mechanical Engineering, University of California, Berkeley, CA
94720, USA; lingfengsun, wzhan<EMAIL_ADDRESS>Zheng is the
corresponding author.

###### Abstract

Local planning for a differential wheeled robot is designed to generate
kinodynamic feasible actions that guide the robot to a goal position along the
navigation path while avoiding obstacles. Reactive, predictive, and learning-
based methods are widely used in local planning. However, few of them can fit
static and crowd environments while satisfying kinodynamic constraints
simultaneously. To solve this problem, we propose a novel local planning
method. The method applies a long-term dynamic window approach to generate an
initial trajectory and then optimizes it with graph optimization. The method
can plan actions under the robot’s kinodynamic constraints in real time while
allowing the generated actions to be safer and more jitterless. Experimental
results show that the proposed method adapts well to crowd and static
environments and outperforms most state-of-the-art approaches.

##  I INTRODUCTION

Differential wheeled robot planning can be achieved by global and local
planners. The global planner generates a navigation path to a goal point. The
local planner continuously generates actions that guide the robot to follow
the navigation path until it reaches the goal point. During this process, the
actions generated by the local planner must meet the kinodynamic constraints
and keep the robot safe and jitterless.

Tab. I shows several key features that should be satisfied for applying local
planning methods in differential wheeled robots. First, planning methods need
to obey the differential constraints and the acceleration limitation. Then,
these methods should fit the static environments, which means they need to be
able to deal with irregular borders. Meanwhile, they should also fit the crowd
environments, which means they should be able to interact with multiple moving
agents. Moreover, planning methods should be long-sighted, which means they
need to give a long horizon planning result. Finally, planning methods should
be able to track the navigation path to ensure the robot converges to the
destination. However, as shown in Tab. I, most current local planning methods
can only meet some of the above-mentioned features.

Therefore, we propose a novel local planning method satisfying all the
features in Tab. I. First, our method constructs time-varying distance fields
[1] from the agents and occupancy grid map. Then, a Long-Term Dynamic Window
Approach (LT-DWA) is proposed to generate a long-time horizon state-cost tree.
Finally, a path with the least cost in the tree is selected and optimized
using the Elastic-Band Model Predictive Control (EB-MPC) method to obtain the
planned trajectory.

TABLE I: Features for local planning methods applied in the differential mobile robots. Method |  | Differential
---
Constraints
| Acc.
---
Limit
| Static
---
Env.
| Crowd
---
Env.
| Long
---
Sighted
| Track
---
Nav.
DWA [2] | ✓ | ✓ | ✓ | ✕ | ✕ | ✕
PCL-LSTM [3] | ✓ | ✓ | ✓ | ✕ | ✓ | ✕
ESA [4] | ✓ | ✕ | ✕ | ✓ | ✓ | ✕
SOADRL [5] | ✕ | ✕ | ✓ | ✓ | ✓ | ✕
TEB [6] | ✓ | ✓ | ✓ | ✕ | ✓ | ✓
KCP [7] | ✓ | ✓ | ✓ | ✕ | ✓ | ✕
DC [8] | ✓ | ✕ | ✕ | ✓ | ✓ | ✕
Timed-ESDF [9] | ✕ | ✕ | ✓ | ✓ | ✓ | ✕
Our method | ✓ | ✓ | ✓ | ✓ | ✓ | ✓

In conclusion, this paper has the following contributions:

  * •

The LT-DWA is proposed to generate the initial state sequence.

  * •

Time-varying distance fields are combined with the MPC [10] to formulate the
planning problem and the EB method [11] is applied to solve it.

  * •

The proposed local planner is open-
sourced111https://github.com/flztiii/LT_DWA. It can be applied to static and
crowd environments and outperforms current planning methods.

##  II RELATED WORK

The local planner can be achieved by reactive, predictive, and learning-based
methods. Reactive methods directly build the mapping from the robot’s current
state to action, including Dynamic Window Approach (DWA) [2], Reciprocal
Velocity Obstacles (RVO) [12], and RouteGAN[13]. Predictive methods generate a
continuous sequence of actions based on the robot’s current state and
predicted future conditions. For example, the Timed Elastic Band (TEB) method
proposed by Rösmann _et al._ [6], the Dynamic Channel (DC) method proposed by
Cao _et al._ [8], and the Timed-ESDF method proposed by Zhu _et al._ [9] are
all predictive methods. Learning-based methods use large amounts of data to
map from the robot’s current state to its action through imitation learning or
reinforcement learning. Learning-based methods such as SARL [14], RGL [15],
DSRNN [16], and ESA [4] show state-of-the-art performance in crowd
environments by considering the crowd’s interaction [17, 18].

MPC-formed planning: Modeling the local planning problem in MPC form and then
solving the problem through optimization is an effective approach [19, 10,
20]. The mathematical models established by these methods are often non-
convex, so the initial guess significantly influences the planning result.
However, obtaining a feasible initial guess in a crowd environment is
challenging. For example, Brito _et al._ ’s method [10] uses the expansion of
the previous planning result as the initial guess for the next planning
episode, which is difficult to guarantee the feasibility of the initial guess
in a crowd environment. Therefore, in our method, we introduce a robust method
for generating a feasible initial guess and combine it with the MPC-formed
planning method.

DWA methods: The DWA has many applications as a planning method that considers
kinodynamic and environmental constraints simultaneously. Its fundamental idea
is to sample in the feasible control space and then evaluate in the state
space. Brock _et al._ [21] improve the DWA’s evaluation function. Ogren _et
al._ [22] combine the DWA with global planning and prove the global
convergence of their method. However, none of their methods can address the
short-sight of the DWA in a single planning episode. A solution is to use
multi-step DWA, but another problem will arise: the exponential expansion of
the state space. If the exponential expansion of the state space of the multi-
step DWA can be solved, it can be applied to generating the feasible initial
guess, which our method does.

Distance field: The distance field adequately represents the environment and
is widely used by various planners [23, 1, 24]. Oleynikova _et al._ ’s method
[23] introduces the distance field in the static environment. Chen _et al._ ’s
and Ngo _et al._ ’s methods [1, 24] are proposed to construct the distance
field in the crowd environment and prove its effectiveness. Therefore, we
introduce the distance field into the MPC-formed planning method to improve
the effectiveness of the planner.

##  III PROBLEM FORMULATION

The state and system dynamics of the robot are defined as follows.

$\displaystyle\mathbf{s}=(x,y,\theta,v,\omega)^{T},\quad\mathbf{u}=(a_{v},a_{\omega})^{T},$
(1)
$\displaystyle\dot{\mathbf{s}}=f(t,\mathbf{s},\mathbf{u})=(v\cos{\theta},v\sin{\theta},\omega,a_{v},a_{\omega})^{T},$

where, $x$ and $y$ indicate the position in 2D space, $\theta$ is orientation,
$v$ and $\omega$ are linear and angular velocities, $a_{v}$ and $a_{\omega}$
are linear and angular accelerations.

The robot’s radius is defined as $R$. The robot’s state at time $t_{0}$ is
defined as $\mathbf{s}_{\mathrm{init}}$. Since all the inputs can be
transformed into the coordinate system with the robot state as the origin,
without losing generality, it can be considered that
$\mathbf{s}_{\mathrm{init}}=\mathbf{0}^{T}\times v_{\mathrm{init}}\times
w_{\mathrm{init}}$. The navigation path connecting the robot’s current and
goal points is defined as $\mathcal{P}=\\{\mathbf{p}_{p}\\}_{p=0}^{P}$, where
$\mathbf{p}_{p}=(x_{\mathbf{p}_{p}},y_{\mathbf{p}_{p}},\theta_{\mathbf{p}_{p}})^{T}$
indicates the $p_{\mathrm{th}}$ point’s position and orientation on the
navigation path. The agents are defined as
$\mathcal{O}=\\{\mathbf{o}_{o}\\}_{o=0}^{O}$, where
$\mathbf{o}_{o}=(x_{\mathbf{o}_{o}},y_{\mathbf{o}_{o}},vx_{\mathbf{o}_{o}},vy_{\mathbf{o}_{o}},r_{\mathbf{o}_{o}})^{T}$
indicates the $o_{\mathrm{th}}$ agent’s center position, velocity, and radius.
The occupancy grid map is defined as
$\mathcal{B}=\\{\mathbf{b}_{b}\\}_{b=0}^{B}$, where
$\mathbf{b}_{b}=(x_{\mathbf{b}_{b}},y_{\mathbf{b}_{b}})^{T}$ indicates the
$b_{\mathrm{th}}$ occupied grid’s position.

In this way, the local planning problem can be described as follows. The local
planning is to calculate the state sequence
$\mathbf{s}(t),t\in[t_{0},t_{0}+T]$ within a fixed time $T$, so that
$\mathbf{s}(t)$ minimizes the cost function
$c(\mathbf{s}(t),\mathcal{P},\mathcal{O},\mathcal{B},T)$ while satisfying Eq.
1. For simplification, the problem is discretized in the time domain, where
$T$ is divided into $N$ frames time segments $\Delta T$. Then, $\mathbf{s}(t)$
can be defined as $\mathcal{S}=\\{\mathbf{s}_{i}\\}_{i=0}^{N}$
($\mathbf{s}_{i}=\mathbf{s}(t_{0}+i\Delta T)$), and the local planning problem
can be defined as follows.

$\displaystyle\min\limits_{\mathcal{S}}\quad
c(\mathcal{S},\mathcal{P},\mathcal{O},\mathcal{B})$ (2)
$\displaystyle\mathrm{s.t.}\quad\left\\{\begin{array}[]{l}\mathbf{s}_{0}=\mathbf{0}^{T}\times
v_{\mathrm{init}}\times w_{\mathrm{init}},\\\
\mathbf{s}_{i+1}=\mathbf{s}_{i}+\Delta Tf(t_{0}+i\Delta
T,\mathbf{s},\mathbf{u}),\\\ ~{}~{}~{}~{}~{}~{}~{}~{}~{}~{}\forall
i\in[0,N-1],\\\
\mathbf{s}_{i}\in\mathrm{SE}(2)\times[v_{\mathrm{min}},v_{\mathrm{max}}]\times[\omega_{\mathrm{min}},\omega_{\mathrm{max}}],\\\
\mathbf{u}_{i}\in[a_{v}^{\mathrm{min}},a_{v}^{\mathrm{max}}]\times[a_{\omega}^{\mathrm{min}},a_{\omega}^{\mathrm{max}}],\end{array}\right.$

where, function $f(\cdot)$ is the same as Eq. 1, $v_{\mathrm{min}}$ and
$v_{\mathrm{max}}$ are the robot’s minimum and maximum linear velocities,
$\omega_{\mathrm{min}}$ and $\omega_{\mathrm{max}}$ are the robot’s minimum
and maximum angular velocities, $a_{v}^{\mathrm{min}}$ and
$a_{v}^{\mathrm{max}}$ are the robot’s minimum and maximum linear
accelerations, $a_{\omega}^{\mathrm{min}}$ and $a_{\omega}^{\mathrm{max}}$ are
the robot’s minimum and maximum angular accelerations. In our method, the cost
function $c(\cdot)$ is the weighted summation of the collision risk cost
$c_{\mathrm{c}}(\cdot)$, navigation following cost $c_{\mathrm{n}}(\cdot)$,
and jitter cost $c_{\mathrm{j}}(\cdot)$, which are defined in section IV. The
collision risk cost aims to improve the robot’s safety, which means a lower
collision rate and a longer distance to the borders. The navigation following
cost punishes the robot from deviating from the navigation path. The jitter
cost aims to reduce the robot’s jitter, which means a smaller change in the
orientation, linear velocity, and angular velocity.

##  IV METHOD

###  IV-A Framework

Figure 1: This figure shows the local planner’s framework. The example of the
time-varying distance fields is shown in the upper left. The example of the
reference navigation path is shown in the bottom left, where the green line
indicates the navigation path, the blue line indicates the reference
navigation path, the colorful circles indicate agents, and the red arrows
indicate the agents’ velocities. The example of the LT-DWA is shown at the
bottom, where the left side indicates the expanded states in the different
frames, the blue curves on the right side indicate the projection of the
state-cost tree on the Cartesian plane, and the red curve indicates the
selected state sequence. The example of EB-MPC is shown on the right, where
the upper part indicates the connection relationship of a node in the graph
optimization, and the green curve in the lower part indicates the optimized
state sequence.

The framework shown in Fig. 1 is proposed to solve the local planning problem.
In step one (Section B), the local planner generates the $N$-frames time-
varying distance fields $\\{d_{i}(\cdot)\\}_{i=0}^{N}$.
($d_{i}(\cdot):\mathbb{R}^{2}\\!\rightarrow\\!\mathbb{R}$ is a mapping from
the Cartesian plane to the real number set.) Then, the planner calculates the
reference navigation path $\\{\mathbf{p}_{i}\\}_{i=0}^{N}$.
($\mathbf{p}_{i}=(x_{\mathbf{p}_{i}},y_{\mathbf{p}_{i}},\theta_{\mathbf{p}_{i}})^{T}\in\mathrm{SE}(2)$.)
$\\{\mathbf{p}_{i}\\}_{i=0}^{N}$ and $\\{d_{i}(\cdot)\\}_{i=0}^{N}$ are
required in the following state cost and optimization objective function
calculations. In step two (Section C), the planner applies LT-DWA to generate
an initial state sequence. In step three (Section D), the planner uses the EB-
MPC method to optimize the initial state sequence. According to the optimized
state sequence, the controller generates control commands. The local planner
updates at a fixed frequency until the robot reaches the goal point.

###  IV-B Reference Navigation Path and Time-Varying Distance Fields

Figure 2: In this figure, the left part shows the scenario and the right part
shows the first frame of its corresponding time-varying distance fields. The
colorful circles indicate agents, the red arrows indicate agents’ velocities,
and the black region indicates the occupancy grid map.

The reference navigation path $\\{\mathbf{p}_{i}\\}_{i=0}^{N}$ is obtained
according to the navigation path $\mathcal{P}$. The point on the navigation
path, which has the minimum euclidean distance to the robot’s current state
$\mathbf{0}^{T}$ on the Cartesian plane, is selected as the reference
navigation path’s beginning point $\mathbf{p}_{0}$. Then, the
$i_{\mathrm{th}}$ point $\mathbf{p}_{i}$ on the reference navigation path is
the point on the navigation path, whose arc length to $\mathbf{p}_{0}$ along
the navigation path is $i\Delta
Tv_{\mathrm{max}}\max(\cos\theta_{\mathbf{p}_{0}},0)$.
$\theta_{\mathbf{p}_{0}}$ is the orientation gap between $\mathbf{p}_{0}$ and
$\mathbf{0}^{T}$. This expression expects that when the difference between the
orientation of the robot and the navigation path is significant, the robot
should adjust its orientation first. Otherwise, it should follow the
navigation path at the maximum speed.

The time-varying distance fields $\\{d_{i}(\cdot)\\}_{i=0}^{N}$ are calculated
according to the agents $\mathcal{O}$ and the occupancy grid map
$\mathcal{B}$. The $i_{\mathrm{th}}$ frame distance field $d_{i}(\cdot)$ is
determined by the agent distance field $d_{i}^{\mathcal{O}}(\cdot)$ and the
occupancy grid map distance field $d_{i}^{\mathcal{B}}(\cdot)$, whose
calculation is as follows.

$d_{i}(\cdot)=\max(w_{\mathrm{do}}d_{i}^{\mathcal{O}}(\cdot),w_{\mathrm{db}}d_{i}^{\mathcal{B}}(\cdot)),$

where, $w_{\mathrm{do}}$ and $w_{\mathrm{db}}$ are preset weights.

$d_{i}^{\mathcal{O}}(\cdot)$ is calculated as follows, referring to Chen _et
al._ ’s method [1].

$d_{i}^{\mathcal{O}}(x,y)\\!=\\!\max_{\forall
o\in[0,O]}\exp^{-\big{(}\frac{l_{x}^{o}(x,y,i)^{2}}{2\sigma_{x}^{2}}+\frac{l_{y}^{o}(x,y,i)^{2}}{2\sigma_{y}^{2}}\big{)}},$

where,

$\displaystyle l^{o}(x,y,i)=\Big{\|}\begin{pmatrix}x\\\
y\end{pmatrix}-\begin{pmatrix}x_{\mathbf{o}_{o}}+i\Delta
Tvx_{\mathbf{o}_{o}}\\\ y_{\mathbf{o}_{o}}+i\Delta
Tvy_{\mathbf{o}_{o}}\end{pmatrix}\Big{\|},$
$\displaystyle\alpha=\operatorname{atan2}\Big{(}\frac{y\\!-\\!y_{\mathbf{o}_{o}}\\!-\\!i\Delta
Tvy_{\mathbf{o}_{o}}}{x\\!-\\!x_{\mathbf{o}_{o}}\\!-\\!i\Delta
Tvx_{\mathbf{o}_{o}}}\Big{)}-\operatorname{atan2}\Big{(}\frac{vy_{\mathbf{o}_{o}}}{vx_{\mathbf{o}_{o}}}\Big{)},$
$\displaystyle
l_{x}^{o}(x,y,i)\\!=\\!l^{o}(x,y,i)\\!*\\!\cos\alpha,l_{y}^{o}(x,y,i)\\!=\\!l^{o}(x,y,i)\\!*\\!\sin\alpha,$
$\displaystyle\sigma_{x}=\left\\{\begin{array}[]{l}\\!\frac{1}{3}\\!\big{(}r_{\mathbf{o}_{o}}\\!+\\!R\\!+\\!\eta\\!+\\!\beta\big{\|}\begin{pmatrix}vx_{\mathbf{o}_{o}}\\\
vy_{\mathbf{o}_{o}}\end{pmatrix}\big{\|}\big{)},\quad-\frac{\pi}{2}\\!<\\!\alpha\\!<\\!\frac{\pi}{2},\\\
\\!\frac{1}{3}\\!(r_{\mathbf{o}_{o}}\\!+\\!R\\!+\\!\eta),\quad\mathrm{else},\\\
\end{array}\right.$
$\displaystyle\sigma_{y}=\frac{1}{3}(r_{\mathbf{o}_{o}}+R+\eta),$

where, $\eta$ and $\beta$ are preset parameters. As shown in Fig. 2, this
distance field comprises multiple two-dimensional normal distributions with
offset. The normal distributions’ centers are $i_{\mathrm{th}}$ frame agents’
centers, whose ranges are determined by $\eta$ and offsets are determined by
the agents’ velocities and $\beta$.

$d_{i}^{\mathcal{B}}(\cdot)$ is calculated as follows.

$\displaystyle d_{i}^{\mathcal{B}}(x,y)=(\mathrm{Relu}(\eta-\delta))^{2},$
$\displaystyle\delta=\min_{b}\mathrm{Relu}\Big{(}\big{\|}\begin{pmatrix}x-x_{\mathbf{b}_{b}}\\\
y-y_{\mathbf{b}_{b}}\end{pmatrix}\big{\|}-R\Big{)},\quad\forall b\in[0,B],$

where, $\eta$ is the same preset parameter. As shown in Fig 2, this distance
field is a quadratically decreasing function with the distance to the boundary
of the occupancy grid map. The value of this distance field is zero, if the
distance to the boundary of the occupancy grid map is larger than $\eta+R$.

###  IV-C Long-Term Dynamic Window Approach

After obtaining the reference navigation path $\\{\mathbf{p}\\}_{i=0}^{N}$ and
the time-varying distance fields $\\{d_{i}(\cdot)\\}_{i=0}^{N}$, the next step
is to build a state-cost tree $\mathcal{T}$ to obtain the initial state
sequence $\mathcal{S}_{\mathrm{init}}$ for the following optimization. The
tree is a set of $N$ layers, and the tree’s $i_{\mathrm{th}}$ layer
$\mathcal{T}_{i}$ is the set of $i_{\mathrm{th}}$ frame nodes. Each node $n$
has three attributes: state (State$[n]$), cost (Cost$[n]$), and parent node
(Parent$[n]$). The construction of the state-cost tree is shown in Alg. 1.

Algorithm 1 Long Term Dynamic Window Approach

1:Reference Navigation Path $\\{\mathbf{p}\\}_{i=0}^{N}$, Time-Varying
Distance Fields $\\{d_{i}(\cdot)\\}_{i=0}^{N}$.

2:State-Cost Tree $\mathcal{T}$.

3:Add Layer $\mathcal{T}_{0}=\\{$Node($\mathbf{0}^{T}$, 0, null)$\\}$ into
empty Tree $\mathcal{T}$.

4:for $i=1,2,\cdots,N$ do

5: $\mathcal{T}_{i}$ $\leftarrow$ $\varnothing$.

6: for Node $n$ $\in$ Layer $\mathcal{T}_{i-1}$ do

7: $\mathcal{S}_{\mathrm{c}}$ $\leftarrow$ expandStates($n$).

8: for State $\mathbf{s}$ $\in$ $\mathcal{S}_{\mathrm{c}}$ do

9: Push Node($\mathbf{s}$, 0, $n$) to Layer $\mathcal{T}_{i}$.

10: if len($\mathcal{T}_{i}$) $>$ $K^{{}^{\prime}}$ then

11: $\mathcal{T}_{i}$ $\leftarrow$ voxelSampling($\mathcal{T}_{i}$).

12: for Node $n$ $\in$ Layer $\mathcal{T}_{i}$ do

13: Cost$[n]$ $=$ Cost$[$Parent$[n]$$]+$ calcCost(State$[n]$,
$\mathbf{p}_{i}$, $d_{i}(\cdot)$, $i$).

14: if $\mathcal{T}_{i}$ is not $\varnothing$ then

15: Add Layer $\mathcal{T}_{i}$ into Tree $\mathcal{T}$.

16: else

17: break

The root layer $\mathcal{T}_{0}$ can be obtained according to the robot’s
current state. Then, $\mathcal{T}_{i}$ is obtained as the exemplary diagram in
Fig. 3. All the nodes in the previous layer $\mathcal{T}_{i-1}$ are traversed.
For each node $n$, state expansion is performed according to $n$’s state
State$[n]$. In this way, an expanded state set $\mathcal{S}_{\mathrm{c}}$ can
be generated. The DWA achieves state expansion. In detail, given a state
$\mathbf{s}$, the velocity space boundary of the states in the next frame can
be determined according to the robot’s velocity and acceleration limitations.
The limited velocity space is uniformly sampled, and $V\times V$ samples can
be obtained. For each sample, an expanded state can be calculated according to
Eq. 1. Collision-free states are selected from these expanded states, which
make up $\mathcal{S}_{\mathrm{c}}$. A new node without cost is added to
$\mathcal{T}_{i}$ for each state in $\mathcal{S}_{\mathrm{c}}$.

Figure 3: This figure shows an exemplary diagram of how to construct the layer
of the tree. The diagram includes state expansion, voxel sampling, and cost
calculation.

When the states of all nodes in the previous frame are expanded, the number of
nodes in $\mathcal{T}_{i}$ is significant. Assuming that the DWA can expand
$K$ states from one state each time, the time complexity of calculating the
tree with $N$ frames is $O(K^{N})$. When $N$ is large, this time complexity is
undoubtedly unacceptable. In order to solve this problem, after using the DWA
to expand all the states in the current frame, we perform voxel sampling to
ensure that the total number of states in the next frame is always around
$K^{{}^{\prime}}$. In this way, the time complexity is $O(K^{{}^{\prime}}N)$.
The voxel sampling process is as follows. The $\mathrm{SE}(2)$ space boundary
of all nodes in $\mathcal{T}_{i}$ is calculated, and subsequently, the
$\mathrm{SE}(2)$ space within the boundary is voxelized to $W\times W\times W$
voxels. Each node is located in one voxel. A node is randomly sampled in each
voxel, and the sampling result can be obtained.

There are two reasons why voxelization is performed in the $\mathrm{SE}(2)$
space instead of the state space. The first is to reduce the space dimension
and achieve lower computational complexity. The second is that the attributes
of the state in the $\mathrm{SE}(2)$ space are more important than those in
the velocity space. The former has a direct relationship to the robot’s
safety, while the latter only has an indirect relationship. Fig. 4 shows an
example of the robot’s state distribution and the sampled state distribution
in the $\mathrm{SE}(2)$ space at the end of the long horizon. It can be seen
from the figure that the blue points distribution is consistent with the red
points distribution. Therefore, the representation of the robot’s
$\mathrm{SE}(2)$ properties at the horizon’s end can be achieved using voxel
sampling.

Figure 4: This figure shows an example of the robot’s state distribution and
the sampled state distribution in the SE(2) space at the end of the long
horizon. The red points represent the robot’s state distribution, and the blue
points represent the sampled state distribution using voxel sampling.

After voxel sampling, the number of nodes in $\mathcal{T}_{i}$ will be
acceptable for real-time performance. At this time, the cost of each node $n$
in $\mathcal{T}_{i}$ can be calculated, which includes two parts. One is the
cost of its parent node, and the other is its state $\mathbf{s}$’s cost. When
calculating the cost of $\mathbf{s}$, only the collision risk cost
$c_{\mathrm{c}}(\cdot)$ and the navigation following cost
$c_{\mathrm{n}}(\cdot)$ are considered. The reason is that
$c_{\mathrm{n}}(\cdot)$ and $c_{\mathrm{c}}(\cdot)$ are related to
$\mathrm{s}$’s $\mathrm{SE}(2)$ space attributes, while the jitter cost
$c_{\mathrm{j}}(\cdot)$ is only related to the velocity space attributes. In
the previous step, the sampling is performed in the $\mathrm{SE}(2)$ space, so
the calculation of $c_{\mathrm{j}}(\cdot)$ does not make much sense here. In
conclusion, given the $i_{\mathrm{th}}$ frame point on the reference
navigation path
$\mathbf{p}_{i}=(x_{\mathbf{p}_{i}},y_{\mathbf{p}_{i}},\theta_{\mathbf{p}_{i}})^{T}$
and the $i_{\mathrm{th}}$ frame time-varying distance field $d_{i}(\cdot)$,
the cost of the $i_{\mathrm{th}}$ frame state
$\mathbf{s}=(x_{\mathbf{s}},y_{\mathbf{s}},\theta_{\mathbf{s}},v_{\mathbf{s}},\omega_{\mathbf{s}})^{T}$
can be calculated as follows.

$\displaystyle\mathrm{calcCost}(\mathbf{s},\mathbf{p}_{i},d_{i}(\cdot),i)\\!=\\!\gamma^{i}(c_{\mathrm{c}}(\mathbf{s},d_{i}(\cdot))+c_{\mathrm{n}}(\mathbf{s},\mathbf{p}_{i})),$
(3) $\displaystyle
c_{\mathrm{c}}(\mathbf{s},d_{i}(\cdot)))=w_{\mathrm{c}}d_{i}(x_{\mathbf{s}},y_{\mathbf{s}}),$
$\displaystyle
c_{\mathrm{n}}(\mathbf{s},\\!\mathbf{p}_{i})\\!=\\!w_{\mathrm{no}}c_{\mathrm{no}}(\mathbf{s},\\!\mathbf{p}_{i})\\!\\!+\\!\\!w_{\mathrm{na}}c_{\mathrm{na}}(\mathbf{s},\\!\mathbf{p}_{i})\\!\\!+\\!\\!w_{\mathrm{nt}}c_{\mathrm{nt}}(\mathbf{s},\\!\mathbf{p}_{i}),$

where,

$\displaystyle
c_{\mathrm{no}}(\mathbf{s},\mathbf{p}_{i})\\!=\\!((x_{\mathbf{s}}\\!-\\!x_{\mathbf{p}_{i}})\cos{\theta_{\mathbf{p}_{i}}}\\!+\\!(y_{\mathbf{s}}\\!-\\!y_{\mathbf{p}_{i}})\sin{\theta_{\mathbf{p}_{i}}})^{2},$
(4) $\displaystyle
c_{\mathrm{na}}(\mathbf{s},\mathbf{p}_{i})\\!=\\!(\\!-(x_{\mathbf{s}}\\!-\\!x_{\mathbf{p}_{i}})\sin{\theta_{\mathbf{p}_{i}}}\\!+\\!(y_{\mathbf{s}}\\!-\\!y_{\mathbf{p}_{i}})\cos{\theta_{\mathbf{p}_{i}}})^{2},$
$\displaystyle
c_{\mathrm{nt}}(\mathbf{s},\mathbf{p}_{i})=(1-\cos(\theta_{\mathbf{s}}-\theta_{\mathbf{p}_{i}}))^{2},$

$\gamma$ is decline rate, $w_{\mathrm{c}}$, $w_{\mathrm{no}}$,
$w_{\mathrm{na}}$, and $w_{\mathrm{nt}}$ are preset weights. The reason for
setting the decline rate is uncertainty in the movement of agents in the crowd
environment, and the cost in the long-term future has low reliability. The
cost $c_{\mathrm{no}}(\cdot)$ is to penalize the longitudinal distance between
$\mathbf{s}$ and $\mathbf{p}_{i}$ and the cost $c_{\mathrm{na}}(\cdot)$ is to
penalize the lateral distance between $\mathbf{s}$ and $\mathbf{p}_{i}$. The
combination of the two costs can be used to evaluate the distance between
$\mathbf{s}$ and the reference navigation path in the Cartesian plane, which
is more flexible than directly calculating the distance between $\mathbf{s}$
and the reference navigation path [25]. The cost $c_{\mathrm{nt}}(\cdot)$ is
to penalize the orientation gap between $\mathbf{s}$ and $\mathbf{p}_{i}$.

After $\mathcal{T}$ is built, the nodes in the $N_{\mathrm{th}}$ layer
$\mathcal{T}_{N}$ of the tree are traversed, and the node with the minimum
cost is selected. We iteratively backtrack the node’s parent until the tree’s
root node is reached. Finally, the initial state sequence
$\mathcal{S}_{\mathrm{init}}$ can be obtained. In the complex environment,
when the $N^{{}^{\prime}}_{\mathrm{th}}$ layer of the tree is built, it may be
empty sometimes. In this case, building a complete $N$ layer tree is given up
and a $N^{{}^{\prime}}\\!-\\!1$ layer tree is obtained. Then,
$\mathcal{S}_{\mathrm{init}}$ also degenerates from $N$ frames to
$N^{{}^{\prime}}\\!-\\!1$ frames. In the worst case, this method will
degenerate into the DWA.

###  IV-D Elastic-Band Model Predictive Control

After obtaining the initial state sequence $\mathcal{S}_{\mathrm{init}}$, the
next step is to optimize it using the EB-MPC method. We define the state
sequence optimization problem in the MPC form [10] and solve the problem using
the EB method [11].

The optimization model used has been given in Eq. 2, which is an MPC-formed
model. The expression of the objective function
$c(\mathcal{S},\mathcal{P},\mathcal{O},\mathcal{B})$ is as follows.

$\displaystyle
c(\mathcal{S},\\!\mathcal{P},\\!\mathcal{O},\\!\mathcal{B})\\!\\!=\\!\\!\sum_{i=1}^{N}\gamma^{i}($
$\displaystyle
c_{\mathrm{c}}(\mathbf{s}_{i},d_{i}(\cdot))\\!\\!+\\!\\!c_{\mathrm{n}}(\mathbf{s}_{i},\\!\mathbf{p}_{i})\\!\\!+\\!\\!c_{\mathrm{j}}(\mathbf{s}_{i},\\!\mathbf{s}_{i-1})),$

where, $\gamma$ and $c_{\mathrm{c}}(\cdot)$ is the same as Eq. 3.
$c_{\mathrm{n}}(\cdot)$ adds an additional cost $c_{\mathrm{nv}}(\cdot)$ on
the basis of Eq. 3, whose expression is as follows.

$c_{\mathrm{nv}}(\mathbf{s},\mathbf{p}_{i})=w_{\mathrm{nv}}(v_{\mathbf{s}}-\min(v_{\mathrm{max}},\sqrt{2a_{\mathrm{max}}\epsilon_{\mathbf{p}_{i}}}))^{2},$

where, $w_{\mathrm{nv}}$ is preset weight, $v_{\mathbf{s}}$ is $\mathbf{s}$’s
linear velocity, $\epsilon_{\mathbf{p}_{i}}$ is arc length between
$\mathbf{p}_{i}$ and the navigation path’s endpoint. This expression aims to
make the robot’s speed tend to the maximum value when it is far away from the
goal point. Otherwise, the speed tends to zero. $c_{\mathrm{j}}(\cdot)$ is the
jitter cost, whose expression is as follows.

$\displaystyle c_{\mathrm{j}}(\mathbf{s}_{i},\\!\mathbf{s}_{i-1})\\!=$
$\displaystyle
w_{\omega}\omega_{i}^{2}\\!\\!+\\!\\!w_{a_{v}}\Big{(}\frac{v_{i}\\!\\!-\\!\\!v_{i-1}}{\Delta
T}\Big{)}^{2}\\!\\!+\\!\\!w_{a_{\omega}}\Big{(}\frac{w_{i}\\!\\!-\\!\\!w_{i-1}}{\Delta
T}\Big{)}^{2},$

where, $w_{\omega}$, $w_{a_{v}}$, and $w_{a_{\omega}}$ are preset weights,
$\mathbf{s}_{i}=(x_{i},y_{i},\theta_{i},v_{i},\omega_{i})^{T}$. There are
three items in the jitter cost. The first item is to penalize high angular
velocities for reducing the shaking of the robot’s orientation. The second
item is to penalize high linear accelerations for reducing the jitter of the
robot’s speed. The third item is to penalize high angular accelerations for
reducing the vibration of the robot’s angular velocity. The above three items
work together to reduce the jitter of the robot.

After obtaining the complete definition of the optimization model, it can be
seen that the optimization model Eq. 2 is sparse for the optimization variable
$\mathcal{S}$, so the EB method can be used to solve it [26], whose process is
as follows. Each state $\mathbf{s}_{i}$ in the optimization variable
$\mathcal{S}$ can be regarded as a node, and the objective function and
constraints in the optimization model Eq. 2 can be regarded as edges. In this
way, a graph can be constructed, as shown on the right side of Fig. 1.
According to Eq. 2, there are only unary edges and binary edges in the
constructed graph. Subsequently, $\mathcal{S}_{\mathrm{init}}$ is applied to
initialize the graph, and the g2o framework [27] is used to perform graph
optimization. The algorithm used for graph optimization is the Levenberg-
Marquardt. At last, the optimized state sequence $\mathcal{S}_{\mathrm{opt}}$
can be obtained. Compared with $\mathcal{S}_{\mathrm{init}}$,
$\mathcal{S}_{\mathrm{opt}}$ can make the robot have less jitter.

Figure 5: This figure shows four examples of the robot navigating in different
crowds using the proposed method. The dark green circles indicate the robot,
the colorful hollow circles indicate other agents, the curves of different
colors indicate the moving trajectories of the corresponding agents, and the
values near the circles indicate the corresponding time. The more transparent
the circle, the earlier the time.

##  V EXPERIMENTAL RESULTS

The experiments are conducted in crowd, static, and hybrid environments to
verify our method in different scenarios. In addition, we design an ablation
study to verify the effectiveness of submodules. The testing robot is designed
as a differential wheel robot. The robot’s shape is set as a 0.3 m radius
circle, and its sensing range is limited to 3.5 m. The robot’s linear velocity
is set from 0 to 1 m/s, its angular velocity is set from -1 rad/s to 1 rad/s,
its linear acceleration is set from -1 m/$\mathrm{s}^{2}$ to 1
m/$\mathrm{s}^{2}$, and its angular acceleration is set from -1
rad/$\mathrm{s}^{2}$ to 1 rad/ $\mathrm{s}^{2}$.

###  V-A Crowd Environment Tests

In this experiment, the ORCA simulated scenarios as the ESA [4] and the
pedestrian trajectory dataset [28] are both used for testing. The testing
scenarios are updated at a frequency of 5 Hz.

For the ORCA simulated scenarios, 10, 15, and 20 agents are set in the
environment, respectively. Each agent is a circle with a radius of 0.3 m, its
moving policy is the ORCA, and its maximum speed is 1 m/s. The robot is
invisible to all the agents [14, 15, 4]. In each test, the agents are on a
circle with a radius of 5 m and moves to the targets, which are their opposite
position of the circle with disturbance. Meanwhile, the robot is also on the
circle and regards the opposite position as the goal point, as shown in Fig.
5.

For the trajectory dataset, the range of the pedestrian trajectories in the
dataset is recorded, and each pedestrian is also regarded as a circle with a
radius of 0.3 m. In each test, the center points of the trajectories range’s
upper and lower boundaries are used as the starting and goal points. A
starting time is randomly selected to broadcast pedestrian trajectories based
on the dataset and ensure that the pedestrians do not collide with the
starting point at the starting time.

In the above scenarios, the robot uses the proposed method, ORCA, LSTM-RL
[29], SARL, and ESA methods for planning, respectively. LSTM-RL, SARL, and ESA
are all reinforcement-learning-based methods and focus on considering the
interaction of the crowd. For each scenario, 300 tests are conducted and the
robot’s success rate is counted. The test succeeds if the robot reaches the
destination, and fails if it collides with the agents, moves out of bounds, or
fails to reach the end within the specified time. The LSTM-RL, SARL, and ESA
methods are retrained in the same environment as the ESA, and the difference
is that the robot is changed from the holonomic robot to the differential
robot during the training. When using the proposed method for planning, the
navigation path is the connection line between the robot’s current and the
goal points. The testing results are shown in Tab. II.

TABLE II: Comparison of different methods in crowd environment tests. Method |  | 10 agents
---
ORCA
| 15 agents
---
ORCA
| 20 agents
---
ORCA
Zara01 | Zara02 | Stu03
[4pt] DWA | 0.3$\%$ | 0.3$\%$ | 0$\%$ | 62.3$\%$ | 55$\%$ | 14$\%$
[4pt] ORCA | 25.6$\%$ | 11$\%$ | 8.3$\%$ | 76.6$\%$ | 72.6$\%$ | 22.3$\%$
[4pt] LSTM-RL | 51.3$\%$ | 34.6$\%$ | 20$\%$ | 60$\%$ | 49.3$\%$ | 1.6$\%$
[4pt] SARL | 84.6$\%$ | 61.6$\%$ | 41$\%$ | 69.6$\%$ | 15.3$\%$ | 0$\%$
[4pt] ESA | 75.3$\%$ | 56.3$\%$ | 35$\%$ | 67$\%$ | 60$\%$ | 13$\%$
[4pt] Ours | 89.3$\%$ | 81$\%$ | 76.6$\%$ | 92$\%$ | 93$\%$ | 68$\%$

According to the results in Tab. II, it can be seen that the proposed method
improves the success rate for all the testing scenarios, compared with the
current methods. In particular, the proposed method’s success rate does not
significantly decrease as the environment becomes more complex, which
indicates that the proposed method has higher reliability in complex
environments. In addition, it can be found that learning-based methods, such
as SARL, perform well in the ORCA environment while testing poorly on the
pedestrian trajectories dataset. The reason is that these learning-based
methods are trained in the ORCA environment, and the data distribution of the
pedestrian trajectories dataset and the ORCA environment is quite different,
so these methods cannot fit the pedestrian trajectories dataset environment.
In contrast, the proposed method does not have the above problems and has
better generalization ability.

Furthermore, the examples that use the proposed method to achieve crowd
navigation in different environments are shown in Fig. 5. This figure
describes the movement process of the robot in crowds using the proposed
method. According to Fig. 5(c), the robot first moves to the right to avoid
agents and then moves forward for a while. At about 13 seconds, the robot
slows down and turns towards the goal point, and it finally arrives at the
goal point at 23 seconds.

###  V-B Static Environment Tests

In the static environment tests, the testing scenario is shown in Fig. 6. The
start and goal points are randomly selected within the scenario, and the start
and goal points are ensured to keep a certain distance from the occupied grid
points. The A* algorithm with the Douglas-Peucker algorithm [30] generates the
navigation path. Then, our method and the TEB method are applied to carry out
local planning along the navigation path, respectively. Each method conducts
300 tests. Success rate, safety, jitter, time consumption for a single plan,
and navigation time from the start point to the goal point are used as
comparison metrics. The success rate is measured by the success times that the
robot reaches its destination divided by the total testing times. The safety
is measured by the minimum distance between the robot and occupied grid
points. The jitter is measured by the robot’s angular velocity, linear and
angular accelerations. Finally, the testing results are shown in Tab. III.

TABLE III: Comparison of different methods in static environment tests. Method |  | Succ.
---
Rate
| Safety
---
(m)
| Nav.
---
Time
(s)
| Mean
---
Ang. vel.
(rad/s)
| Mean
---
Lin. acc.
(m/$\mathrm{s}^{2}$)
| Mean
---
Ang. acc.
(rad/$\mathrm{s}^{2}$)
| Time
---
Consuming
(ms)
[6pt] TEB | 89.6 $\%$ | 0.18 | 16.14 | 0.46 | 0.44 | 0.65 |  77.3 $\pm$ 43.8
[6pt] Ours |  97.3 $\%$ | 0.27 | 18.43 | 0.44 | 0.43 | 0.58 | 94.5 $\pm$ 11.6

It can be seen from Tab. III that the proposed method has a 7.7 $\%$
improvement in the success rate, a 50 $\%$ improvement in the safety and a
10.7 $\%$ decline in terms of angular acceleration. This result shows that our
method has advantages in safety and jitter, compared with the TEB method.
Regarding navigation time, the proposed method spends 14.1 $\%$ more time than
the TEB method on average, and part of the reason is that the proposed method
tends to choose longer trajectories to keep the distance to the occupied grid
points in order to ensure safety. Regarding time consumption for a single
plan, the TEB method consumes 18.2 $\%$ time less than the proposed method on
average. However, the TEB method’s time consumption is unstable. When the
number of occupied grid points is large, its time consumption will increase
significantly. The TEB method’s maximum time consumption can reach 630.1 ms in
the experiment, which cannot meet the real-time requirement. In contrast, the
proposed method’s time consumption is relatively stable, and its maximum time
consumption is 146 ms in the experiment, which can fully meet the real-time
requirement.

Figure 6: This figure shows an example of the static environment test. The
green curve is the robot’s moving trajectory using the proposed method and the
red curve is the robot’s moving trajectory using the TEB method.

In Fig. 6, a static environment testing example is shown. It can be clearly
seen from the figure that compared with the red curve, the green curve is
farther away from the occupied grid points and smoother. This figure
demonstrates that compared with the TEB method, the proposed method can
effectively improve safety and reduce jitter.

###  V-C Hybrid Environment Demonstration

The proposed method is also tested in an environment with both static and
dynamic constraints, as shown in Fig. 7. The agents in the environment follow
the ORCA policy as Liu _et al._ ’s method [5]. In the figure, the black areas
indicate the obstacles, the colored hollow circles indicate the other agents,
and the green circles indicate the robot. It can be seen that the robot
successfully avoided the obstacles and agents, and reached the goal point
using the proposed method. It can also be seen from the right side of the Fig.
7 that during this process, the robot’s linear velocity, angular velocity,
linear acceleration and angular acceleration did not exceed the limitation.

Figure 7: This figure shows a demonstration in the environment with both
static and dynamic constraints. The left picture shows the robot’s traveling
process. The right pictures show the robot’s linear velocity, angular
velocity, linear acceleration, and angular acceleration during the process.

###  V-D Ablation Study

The scenario used in the ablation study is basically the same as the ORCA
environment tests, but the difference is that 25 agents were set up. In this
experiment, the difficulty of the test environment is increased to make the
comparison of results more distinguishable.

In the ablation study, the robot uses the proposed method without EB-MPC
optimization (No Opt.) to plan in the testing scenario. Then, the voxel
sampling is further replaced by random sampling (Rand.) for planning. Finally,
the traditional distance field similar to [31] is used for planning. Each one
is tested 300 times, and success rate, jitter, and time consumption for a
single plan are recorded. The testing results are shown in Tab. IV.

TABLE IV: Comparison of different methods in crowd environment tests. Method |  | Succ.
---
Rate
| Time
---
Consuming
(ms)
| Mean
---
Ang. vel.
(rad/s)
| Mean
---
Lin. acc.
(m/$\mathrm{s}^{2}$)
| Mean
---
Ang. acc.
(rad/$\mathrm{s}^{2}$)
[4pt] Trad. | 50.6$\%$ | 77.13 | 0.36 | 0.51 | 0.57
[4pt] Rand. | 57$\%$ | 76.20 | 0.37 | 0.51 | 0.57
[4pt] No Opt. | 67.6$\%$ | 73.03 | 0.41 | 0.57 | 0.70
[4pt] Complete | 67.3$\%$ | 103.53 | 0.35 | 0.29 | 0.38

According to the testing results, the success rate is increased by $6.4\%$,
when the proposed time-varying distance fields are used instead of the
traditional distance field. The voxel sampling also improves $10.6\%$ in the
success rate than the random sampling. This result can prove the effectiveness
of both in complex crowd environments. Furthermore, the robot’s jitter can be
significantly reduced almost without reducing the success rate when adding the
optimization method. Especially in linear and angular accelerations, there are
reductions of $49.1\%$ and $45.7\%$, respectively. In terms of time
consumption, replacing the distance fields and using voxel sampling hardly
caused an increase in time consumption. Although the optimization method
increases the time consumption by $41.7\%$, it can still guarantee the real-
time performance of the method. In conclusion, all parts of the proposed
method are proved effective as expectation.

##  VI CONCLUSIONS

This paper proposes a long-term dynamic window approach local planning method
for differential wheeled robots. This method can be applied to both crowd and
static environments, and its planned state sequence in real time can ensure
the safety and reduce jitter of the robot while satisfying the kinodynamic
constraints. The limitation of the method is that it does not consider the
interaction between the crowd or the interaction between the robot and the
crowd, which is the limitation of our method. In future work, we will consider
integrating the prediction of other agents[32, 33] the interaction between the
crowd in the planning to further improve the performance.

## References

  * [1] W. Chen, T. Zhang, and Y. Zou, “Mobile robot path planning based on social interaction space in social environment,” _International Journal of Advanced Robotic Systems_ , vol. 15, no. 3, p. 1729881418776183, 2018.
  * [2] D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance,” _IEEE Robotics & Automation Magazine_, vol. 4, no. 1, pp. 23–33, 1997.
  * [3] F. Leiva and J. Ruiz-del Solar, “Robust rl-based map-less local planning: Using 2d point clouds as observations,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 4, pp. 5787–5794, 2020.
  * [4] W. Shi, Y. Zhou, X. Zeng, S. Li, and M. Bennewitz, “Enhanced spatial attention graph for motion planning in crowded, partially observable environments,” in _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, 2022, pp. 4750–4756.
  * [5] L. Liu, D. Dugas, G. Cesari, R. Siegwart, and R. Dubé, “Robot navigation in crowded environments using deep reinforcement learning,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 5671–5677.
  * [6] C. Rösmann, F. Hoffmann, and T. Bertram, “Kinodynamic trajectory optimization and control for car-like robots,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2017, pp. 5681–5686.
  * [7] H. Shin, D. Kim, and S.-E. Yoon, “Kinodynamic comfort trajectory planning for car-like robots,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 6532–6539.
  * [8] C. Cao, P. Trautman, and S. Iba, “Dynamic channel: A planning framework for crowd navigation,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 5551–5557.
  * [9] D. Zhu, T. Zhou, J. Lin, Y. Fang, and M. Q.-H. Meng, “Online state-time trajectory planning using timed-esdf in highly dynamic environments,” in _2022 International Conference on Robotics and Automation (ICRA)_. IEEE, 2022, pp. 3949–3955.
  * [10] B. Brito, B. Floor, L. Ferranti, and J. Alonso-Mora, “Model predictive contouring control for collision avoidance in unstructured dynamic environments,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 4, pp. 4459–4466, 2019.
  * [11] S. Quinlan and O. Khatib, “Elastic bands: connecting path planning and control,” in _[1993] Proceedings IEEE International Conference on Robotics and Automation_ , 1993, pp. 802–807 vol.2.
  * [12] J. Van den Berg, M. Lin, and D. Manocha, “Reciprocal velocity obstacles for real-time multi-agent navigation,” in _2008 IEEE international conference on robotics and automation_. Ieee, 2008, pp. 1928–1935.
  * [13] Z.-H. Yin, L. Sun, L. Sun, M. Tomizuka, and W. Zhan, “Diverse critical interaction generation for planning and planner evaluation,” in _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2021, pp. 7036–7043.
  * [14] C. Chen, Y. Liu, S. Kreiss, and A. Alahi, “Crowd-robot interaction: Crowd-aware robot navigation with attention-based deep reinforcement learning,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 6015–6022.
  * [15] C. Chen, S. Hu, P. Nikdel, G. Mori, and M. Savva, “Relational graph learning for crowd navigation,” in _2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 10 007–10 013.
  * [16] S. Liu, P. Chang, W. Liang, N. Chakraborty, and K. Driggs-Campbell, “Decentralized structural-rnn for robot crowd navigation with deep reinforcement learning,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 3517–3524.
  * [17] A. Alahi, K. Goel, V. Ramanathan, A. Robicquet, L. Fei-Fei, and S. Savarese, “Social lstm: Human trajectory prediction in crowded spaces,” in _Proceedings of the IEEE conference on computer vision and pattern recognition_ , 2016, pp. 961–971.
  * [18] L. Sun, P.-Y. Hung, C. Wang, M. Tomizuka, and Z. Xu, “Distributed multi-agent interaction generation with imagined potential games,” 2023.
  * [19] D. Lam, C. Manzie, and M. Good, “Model predictive contouring control,” in _49th IEEE Conference on Decision and Control (CDC)_. IEEE, 2010, pp. 6137–6142.
  * [20] J.-H. Pauls, M. Boxheimer, and C. Stiller, “Real-time cooperative motion planning using efficient model predictive contouring control,” in _2022 IEEE Intelligent Vehicles Symposium (IV)_ , 2022, pp. 1495–1503.
  * [21] O. Brock and O. Khatib, “High-speed navigation using the global dynamic window approach,” in _Proceedings 1999 ieee international conference on robotics and automation (Cat. No. 99CH36288C)_ , vol. 1. IEEE, 1999, pp. 341–346.
  * [22] P. Ogren and N. E. Leonard, “A tractable convergent dynamic window approach to obstacle avoidance,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , vol. 1. IEEE, 2002, pp. 595–600.
  * [23] H. Oleynikova, A. Millane, Z. Taylor, E. Galceran, J. Nieto, and R. Siegwart, “Signed distance fields: A natural representation for both mapping and planning,” in _RSS 2016 Workshop: Geometry and Beyond-Representations, Physics, and Scene Understanding for Robotics_. University of Michigan, 2016.
  * [24] H. Q. T. Ngo, V. N. Le, V. D. N. Thien, T. P. Nguyen, and H. Nguyen, “Develop the socially human-aware navigation system using dynamic window approach and optimize cost function for autonomous medical robot,” _Advances in Mechanical Engineering_ , vol. 12, no. 12, p. 1687814020979430, 2020.
  * [25] M. A. Abbas, R. Milman, and J. M. Eklund, “Obstacle avoidance in real time with nonlinear model predictive control of autonomous vehicles,” _Canadian journal of electrical and computer engineering_ , vol. 40, no. 1, pp. 12–22, 2017.
  * [26] C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann, and T. Bertram, “Efficient trajectory optimization using a sparse model,” in _2013 European Conference on Mobile Robots_. IEEE, 2013, pp. 138–143.
  * [27] G. Grisetti, R. Kümmerle, H. Strasdat, and K. Konolige, “g2o: A general framework for (hyper) graph optimization,” in _Proceedings of the IEEE international conference on robotics and automation (ICRA), Shanghai, China_ , 2011, pp. 9–13.
  * [28] A. Lerner, Y. Chrysanthou, and D. Lischinski, “Crowds by example,” in _Computer graphics forum_ , vol. 26, no. 3. Wiley Online Library, 2007, pp. 655–664.
  * [29] M. Everett, Y. F. Chen, and J. P. How, “Motion planning among dynamic, decision-making agents with deep reinforcement learning,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 3052–3059.
  * [30] Z. Jian, S. Zhang, S. Chen, Z. Nan, and N. Zheng, “A global-local coupling two-stage path planning method for mobile robots,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 3, pp. 5349–5356, 2021.
  * [31] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in _2009 IEEE International Conference on Robotics and Automation_. IEEE, 2009, pp. 489–494.
  * [32] L. Sun, W. Zhan, D. Wang, and M. Tomizuka, “Interactive prediction for multiple, heterogeneous traffic participants with multi-agent hybrid dynamic bayesian network,” in _2019 IEEE Intelligent Transportation Systems Conference (ITSC)_ , 2019, pp. 1025–1031.
  * [33] L. Sun, C. Tang, Y. Niu, E. Sachdeva, C. Choi, T. Misu, M. Tomizuka, and W. Zhan, “Domain knowledge driven pseudo labels for interpretable goal-conditioned interactive trajectory prediction,” in _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2022, pp. 13 034–13 041.