# Jointly Optimize Coding and Node Selection for Distributed Computing over
Wireless Edge Networks

Cong T. Nguyen, Diep N. Nguyen, Dinh Thai Hoang, Hoang-Anh Pham and Eryk
Dutkiewicz Cong T. Nguyen, Diep N. Nguyen, Dinh Thai Hoang, and Eryk
Dutkiewicz are with University of Technology Sydney, Australia (email:
<EMAIL_ADDRESS>Hoang.Dinh, Diep.Nguyen, and
Eryk.Dutkiewicz@uts.edu.au). Cong T. Nguyen and Hoang-Anh Pham is with Ho Chi
Minh City University of Technology, Vietnam (email<EMAIL_ADDRESS>and anhpham@hcmut.edu.vn).

###### Abstract

This work aims to jointly optimize the coding and node selection to minimize
the processing time for distributed computing tasks over wireless edge
networks. Since the joint optimization problem formulation is NP-hard and
nonlinear, we leverage the discrete characteristic of its decision variables
to transform the problem into an equivalent linear formulation. This
linearization can guarantee to find the optimal solutions and significantly
reduce the problem’s complexity. Simulations based on real-world datasets show
that the proposed approach can reduce the total processing time up to 2.3
times compared with that of state-of-the-art approach.

###### Index Terms:

Coded distributed computing, Maximum Distance Separable code, MINLP, and
straggling effects.

##  I Introduction

With the ability to utilize multiple wireless devices (e.g., edge and mobile
devices) to simultaneously execute intensive computing tasks, distributed
computing has recently become a highly-effective approach for large-scale
computations in wireless edge networks. Compared to centralized computing
methods, distributed computing is more fault-tolerant, i.e., it can still
function even when several nodes fail. Moreover, distributed computing systems
are more scalable, i.e., more devices can be easily added [1]. With these
outstanding advantages, distributed computing has been widely applied in areas
such as Internet-of-Things [2], edge intelligent [3], and especially with
recent advances in distributed learning [4].

However, the uncertainties of computing processes (e.g., inconsistent
computation time and/or failures) and wireless connections in wireless edge
networks lead to less predictable latency and cause serious straggling
problems. Specifically, straggling edge nodes with unstable wireless
connections and/or computing resources can dramatically slow down computing
processes for the whole network. To address this issue, coded distributed
computing [1] (CDC) has recently emerged to be a promising solution. The core
idea of CDC is to use advanced coding theoretic techniques to assign redundant
workload to edge nodes to compensate for the stochastic computation and
transmissions, thereby improving the latency and stability of conventional
distributed computing [1]. Among these techniques, the maximum distance
separable (MDS) code has been widely adopted in the literature [1]. By using
the MDS code, a task can be encoded into sub-tasks and distributed to $n$
nodes, and the final result can be obtained by decoding _any_ $k$ first
results from the $n$ sub-tasks ($k\leq n$).

In the MDS, choosing values of the $(n,k)$ code has a significant impact on
the total processing time of the CDC process. Particularly, since the size of
each sub-task is inversely proportional to $k$ [1], choosing a high value of
$k$ means that each node needs to solve a smaller sub-task, thereby reducing
the computation time at each node. However, in this case, the master node
needs to wait for more nodes to send back their computing results. This can
potentially lead to a longer delay if there are many slow computing nodes and
unreliable communication links in the system. Moreover, most of existing
approaches, e.g., [5, 6, 7], only focus on optimizing the $(n,k)$ code without
considering node selection, i.e., they assume that the nodes are identical in
terms of computing resource and communication links. However, in practice,
edge nodes have dissimilar hardware configurations and communication links.
Consequently, this significantly hinders the applicability of CDC in
heterogeneous environments such as wireless edge networks. Therefore, node
selection and the $(n,k)$ coding need to be jointly considered and optimized,
which is a very challenging task. To the best of our knowledge, our paper is
the first work in the literature aiming to optimize both the coding and the
node selection for CDC-based wireless edge networks.

In this letter, we develop a highly-effective framework to jointly optimize
the $(n,k)$ code and node selection for a CDC-based wireless edge network. In
particular, we first formulate the joint code and node selection optimization
problem as a Mixed Integer Non-linear Programming (MINLP). Since this problem
is NP-hard and nonlinear, we leverage the discrete characteristic of its
decision variables to develop a linearization approach, thereby transforming
the MINLP into an equivalent Mixed-Integer Linear Programming (MILP) problem.
This can significantly reduce the complexity of the original problem, and it
can be efficiently solved by commercial MILP solvers to obtain the optimal
solutions. Simulations based on real-world datasets are also conducted to
evaluate and compare the performance of our proposed approach with other
existing approaches. The results show that our proposed approach can
outperform state-of-the-art approach by up to 2.3 times.

Figure 1: Illustration of a coded distributed computing system.

##  II System Model

###  II-A System Overview

We consider a CDC-based system over a wireless edge network consisting of a
server and a set $\mathcal{N}$ of $N$ edge nodes. Upon receiving a computing
task $\mathcal{D}$, the server first divides it into $k$ sub-tasks using the
$(n,k)$ MDS code. Then, these $k$ sub-tasks are encoded into $n$ identical
coded sub-tasks and sent to $n$ nodes ($n\leq N$). With the MDS code, the size
of the sub-task sent to node $i$ is $d=D/k$, where $D$ is the size of task
$\mathcal{D}$. The total time $t_{i}$ needed for node $i$ to complete a sub-
task is $t_{i}=t^{s}_{i}+t^{c}_{i}$, where $t^{s}_{i}$ is the total
communication time it takes for the server to send the sub-task to node $i$
and for node $i$ to send the result back to the server. Moreover, $t^{c}_{i}$
is the computation time for node $i$ to obtain the result for its assigned
sub-task. After receiving the assigned sub-tasks, the nodes then perform the
computations locally and send the results to the server once they finish. The
server only needs the first $k$ ($k\leq n$) results among those of the $n$
sub-tasks to be able to derive the final result of $\mathcal{D}$. Let
$\mathcal{K}$ denote the set of the first $k$ nodes that successfully send the
result back to the server. Then, the total processing time $T$ can be defined
by $T=\max\\{t_{k}:\forall k\in\mathcal{K}\\}$. For example, if we have
$(n,k)=(4,2)$ and $\\{t_{i}\\}=\\{1,5,9,3\\}$, then $T=t_{4}=3$.

###  II-B Computation Model

The computation time of a node consists of the deterministic and stochastic
computation time, i.e., $t^{c}_{i}=t^{d}_{i}+t^{r}_{i}$. The deterministic
computation time is given by $t^{d}_{i}=d/\eta_{i}=D/k\eta_{i}$, where
$\eta_{i}$ is the number of computations that node $i$ can perform per second,
and $D$ is the size of task $\mathcal{D}$. The stochastic computation time is
assumed to follow an exponential distribution, i.e.,
$p_{t^{r}_{i}}=\lambda_{i}e^{-\lambda_{i}t}$, where
$\lambda_{i}=\alpha_{i}\eta_{i}/d$. $\alpha_{i}$ represents the stochastic
component of computation time coming from random memory access [8]. Thus, the
expected value of $t^{r}_{i}$ is
$\mathrm{E}[t^{r}_{i}]=1/\lambda_{i}=D/k\eta_{i}\alpha_{i}$.

###  II-C Communication Model

The communication time between a node and the server depends on the
communication link between them, which might be different for each node.
Typically, the communication time can be expressed as
$t^{s}_{i}=\tau_{i}(N^{d}_{i}+N^{u}_{i})$, where $\tau_{i}$ is the
deterministic time to upload/download a sub-task. $N^{d}_{i}$ and $N^{u}_{i}$
are the numbers of attempts required for a successful transmission. We assume
that $N^{d}_{i}$ and $N^{u}_{i}$ follow a geometric distribution with
probability $p_{i}$, and $N^{d}_{i}=N^{u}_{i}$ [8]. Then, the expected value
of $t^{s}_{i}$ is $\mathrm{E}[t^{s}_{i}]=2\tau_{i}/p_{i}$.

##  III Problem Formulation and Solutions

###  III-A Problem Formulation

In this work, we aim to minimize the total processing time $T$ of a task by
jointly optimizing the $(n,k)$ code and the node selection for the CDC-based
wireless edge network where the edge nodes have different computing power
$\eta_{i}$ and transmission time $\tau_{i}$. To this end, we formulate the
considered problem as model $P_{1}$ as follows:

$\displaystyle\min_{n,k,\mathbf{c},\mathbf{x}}$ $\displaystyle T,$ (1) s.t.
$\displaystyle\sum_{i\in\mathcal{N}}c_{i}=n,$ (2) $\displaystyle x_{i}\leq
c_{i},$ $\displaystyle\forall i\in\mathcal{N},$ (3)
$\displaystyle\sum_{i\in\mathcal{N}}x_{i}=k,$ (4) $\displaystyle
t_{i}+(1-x_{i})M\geq\dfrac{2\tau_{i}}{p_{i}}+\dfrac{D}{k\eta_{i}(1+\alpha_{i})},$
$\displaystyle\forall i\in\mathcal{N},$ (5) $\displaystyle
T\geq\sum_{i\in\mathcal{N}}t_{i},$ (6) $\displaystyle k\leq n,$ (7)
$\displaystyle n\leq N,$ (8) $\displaystyle c_{i},x_{i}\in\\{0,1\\},$
$\displaystyle\forall i\in\mathcal{N},$ (9) $\displaystyle n,k\in\mathbb{N}.$
(10)

In $P_{1}$, objective (1) aims to minimize the total processing time $T$ of
task $\mathcal{D}$. Constraints (2) to (4) set the values for $\mathbf{c}$ and
$\mathbf{x}$ which represent the nodes selection decisions. Particularly,
$c_{i}=1$ if node $i$ is among the $n$ nodes selected, and $x_{i}=1$ if node
$i\in\mathcal{K}$. In this way, these constraints ensure that (i) there are
$n$ nodes selected, (ii) if node $i$ is selected, then $c_{i}=1$, (iii) $k$
nodes are selected from $n$ nodes, and (iv) there are exactly $k$ nodes with
$x_{i}=1$.

Then, constraint (5) determines the processing time $t_{i}$ of the selected
$k$ nodes. $M$ is a large number to ensure that $t_{i}$ is bounded for only
the nodes with $x_{i}=1$. After the $t_{i},\forall i\in\mathcal{N}$, are
bounded, constraint (6) ensures that $T$ is bounded by the highest $t_{i}$.
Constraints (7) and (8) set the conditions for $n$ and $k$, i.e., $k\leq n\leq
N$. Finally, constraints (9) and (10) set the value domains for $n$, $k$,
$c_{i}$, and $x_{i}$.

###  III-B Complexity of the MINLP Formulation

From the above formulation, we can observe two characteristics regarding its
complexity. First, the considered optimization problem is NP-hard as proven in
Proposition 1.

######  Proposition 1.

The optimization problem $P_{1}$ is NP-hard.

###### Proof:

The considered problem can be decomposed into two sub-problems, namely MDS
code optimization and node selection. The node selection sub-problem, defined
by (3)-(10) is equivalent to the 0-1 knapsack problem [11]. Therefore, the
node selection sub-problem is NP-hard. Consequently, the considered joint
optimization problem is NP-hard. ∎

Moreover, constraint (5) is nonlinear due to the appearance of $k$ at the
denominator, and consequently it makes $P_{1}$ to be an MINLP problem [12].
Thus, in the following section, we first propose an effective method to
convert $P_{1}$ into an equivalent MILP problem, namely $P_{2}$, by using a
linearization technique. After that, $P_{2}$ can be effectively solved by
using commercial solvers. Note that many commercial solvers can find the
optimal solution for MILP problems. Moreover, MILP solvers usually can obtain
the optimal solution much faster than those of the MINLP solvers [12]. As a
result, it is very effective to deploy at the server to quickly find the
optimal decisions for CDC processes.

###  III-C Proposed Linearization Approach

In order to transform $P_{1}$ to an equivalent MILP problem, we first exploit
the discrete nature of variable $k$ by introducing the following constraints:

$\displaystyle\sum_{j\in\mathcal{N}}jy_{j}=k,$ (11)
$\displaystyle\sum_{j\in\mathcal{N}}y_{j}=1,$ (12) $\displaystyle
y_{j}\in\\{0,1\\},$ $\displaystyle\forall j\in\mathcal{N}.$ (13)

Constraints (11) and (12) ensure that the newly introduced binary variables
$y_{j}$ equal 1 only when $j=k$. For example, if $k=2$ then $y_{2}=1$, while
$y_{j}=0,\forall j\neq k$. Then, constraint (5) can be transformed to a linear
format, i.e.,

$t_{i}+(1-x_{i})M\geq\dfrac{2\tau_{i}}{p_{i}}+\sum_{j=1}^{N}y_{j}(\dfrac{D}{j\eta_{i}}+\dfrac{D}{j\eta_{i}\alpha_{i}}).$
(14)

$P_{2}$ can now be defined by objective function (1) and constraints (1)-(4)
and (6)-(14). Then, we prove in Proposition 2 that $P_{2}$ is equivalent
$P_{1}$.

######  Proposition 2.

$P_{2}$ is equivalent $P_{1}$.

###### Proof:

Without loss of generality, assume $k^{\prime}$ is the value of $k$ in a
feasible solution of the considered MINLP model. Then, (11) becomes
$\sum_{j\in\mathcal{N}}jy_{j}=k^{\prime}$. Moreover, since
$\sum_{j=1}^{N}y_{j}=1$ (from (12)), we have $y_{k^{\prime}}=1$ and
$y_{j}=0,\forall j\neq k^{*}$. Thus, (14) becomes:

$\begin{split}t_{i}+(1-x_{i})M\geq\dfrac{2\tau_{i}}{p_{i}}+\sum_{j\in\mathcal{N}}y_{j}\bigg{(}\dfrac{D}{j\eta_{i}}+\dfrac{D}{j\eta_{i}\alpha_{i}}\bigg{)}\\\
=\dfrac{2\tau_{i}}{p_{i}}+\bigg{(}\dfrac{D}{k^{\prime}\eta_{i}}+\dfrac{D}{k^{\prime}\eta_{i}\alpha_{i}}\bigg{)},\end{split}$
(15)

which is equal to (5). As a result, constraints (11)-(14) are equivalent to
constraint (5), and the proof is now completed. ∎

As a result of the proposed linearization, $P_{2}$ can now be effectively
solved by commercially available MILP solvers, e.g., CPLEX [12], which can
guarantee to find the optimal solution and require much lower solving time
compared to that of MINLP solvers.

##  IV Performance Evaluation

###  IV-A Experimental Setup

We evaluate the performance of the proposed approach in a CDC-based wireless
edge network consisting of $N=50$ nodes and 10 tasks with different sizes.
Since there is no publicly available dataset for CDC, we adopt two datasets
from closely related fields. Particularly, we adopt the task sizes $D$ from
the Google Cloud Jobs dataset [9]. Moreover, we arrange the tasks in ascending
order of $D$ to clearly show the effects of task sizes on the optimal
solutions. Furthermore, we adopt the node capabilities metrics, including
$\tau_{i}$ and $\eta_{i}$, from the GWA-T traces dataset [10] that contains
the performance metrics of virtual machines. For the remaining parameters, we
set $p_{i}=0.9$ and $\alpha_{i}=2,\forall i\in\mathcal{N}$ [8]. Additionally,
we compare the proposed approach with the following baseline methods:

  * •

Myopic: $k$ is set to the maximum possible value, i.e., $k=N$. This is the
optimal approach if there is no heterogeneity in the system.

  * •

OneNode: The server selects only the fastest node. This is equivalent to
uncoded and non-distributed computing.

  * •

Static optimal code [5]: $k$ is determined by:

$k=\bigg{(}1+\dfrac{1}{W_{-1}(-e^{-\overline{\lambda}-1})}\bigg{)},$ (16)

where $W_{-1}(.)$ is the lower branch of the Lambert $W$ function and
$\overline{\lambda}$ is the average straggling parameter. Since [5] does not
optimize node selection, we use our node selection optimization to find the
optimal results.

###  IV-B Simulation Results

Fig. 2 shows the total processing time of each task when using different
approaches. As illustrated in the figure, our proposed approach outperforms
all other approaches for all tasks. Particularly, our proposed approach can
achieve $T$ that is up to 90 and 10 times lower than those of the Myopic and
OneNode approaches, respectively. For the static optimal code approach,
although it applies the optimal code in [5] and our proposed optimization to
find the best nodes to perform, its performance is still not as good as that
of our proposed framework. Specifically, for the largest-sized task, our
proposed solution can reduce the total processing time up to 2.3 times
compared with that of the static optimal code solution. The main reason is
that, compared to our proposed approach, the static optimal code chooses a
higher $k$. Although this reduces the sub-task size, the server has to wait
for more nodes to send their results, and thus it may suffer more from the
straggling nodes and communication links.

Figure 2: Total processing time of the system. Figure 3: The values of $k$.

Fig. 3 shows the values of $k$ in the solutions obtained by the MILP and the
static optimal code. Although their values are different, we can observe that
as the task size increases, $k$ also increases. The reason is that, for tasks
with smaller sizes, the nodes’ communication time has more impacts on the
total processing time. In this case, if $k$ is high, the master node has to
wait for more nodes. In contrast, when the task size is larger, the nodes’
communication time becomes insignificant compared to the computation time. As
a result, a higher $k$ is more desirable to reduce the workload at each node,
thereby improving the total computation time. However, if $k$ is too high, the
delay will be increased due to impacts of the straggling nodes and slow
communication links, as observed from the total processing time achieved by
our proposed approach and the static optimal code approach in Fig. 2.

Furthermore, to clearly show the relation between the task size and the
communication time, we examine the processing time of the nodes in the
smallest-size task (task 1) and the largest-sized task (task 10). Note that
the optimal $k$ for task 1 is 5, whereas the optimal $k$ for task 10 is 18. As
shown in Fig. 4, the communication time is a significant factor in executing
task 1. For example, it constitutes up to 19.5% of the total processing time
of node 3 at task 1. In contrast, for task 10, the communication time occupies
only 1.1% of the processing time of the same node. This is also the reason why
the optimal values of $k$ for higher in large-sized tasks. Thus, for tasks
with large sizes, a high $k$ can significantly reduce the computation time
which has much higher impacts on the total processing time.

Figure 4: Processing time components of (a) task 1 and (b) task 10.

##  V Conclusion

This letter aims to develop a highly-effective approach to jointly optimize
the MDS code and node selection, thereby significantly enhancing the
efficiency of CDC in distributed computing over wireless edge networks.
Particularly, we have first modeled a joint coding and node selection
optimization problem to minimize the processing time for CDC. We have then
developed a linearization approach to quickly find the optimal solutions.
Simulations based on real-world datasets have been then conducted. The results
have shown that our proposed approach can outperform state-of-the-art approach
by up to 2.3 times.

## References

  * [1] J. S. Ng et al., “A Survey of Coded Distributed Computing,” _arXiv preprint arXiv:2008.09048_. [Online]. Available: https://arxiv.org/abs/2008.09048
  * [2] H. El-Sayed et al., “Edge of Things: The Big Picture on the Integration of Edge, IoT and the Cloud in a Distributed Computing Environment,” _IEEE Access_ , vol. 6, pp. 1706-1717, Dec. 2018.
  * [3] T. Qiu et al, “Edge Computing in Industrial Internet of Things: Architecture, Advances and Challenges,” _IEEE Commun. Surv. Tutor._ , vol. 22, no. 4, pp. 2462-2488, July 2020.
  * [4] W. Y. B. Lim et al., “Federated Learning in Mobile Edge Networks: A Comprehensive Survey,” _IEEE Commun. Surv. Tutor._ , vol. 22, no. 3, pp. 2031-2063, Apr. 2020.
  * [5] K. Lee, M. Lam, R. Pedarsani, D. Papailiopoulos and K. Ramchandran, “Speeding Up Distributed Machine Learning Using Codes,” _IEEE Trans. Inf. Theory_ , vol. 64, no. 3, pp. 1514-1529, Mar. 2018.
  * [6] R. Tandon, Q. Lei, A. G. Dimakis and N. Karampatziakis, “Gradient coding: Avoiding stragglers in distributed learning,” _Int. Conf. Mach. Learn. (ICML)_ , Sydney, Australia, Aug. 06-11, 2017, pp. 3368-3376.
  * [7] C. Karakus, Y. Sun, S. N. Diggavi and W. Yin, “Redundancy Techniques for Straggler Mitigation in Distributed Optimization and Learning,” _J. Mach. Learn. Res._ , vol. 20, no. 72, pp. 1-47, Apr. 2019.
  * [8] S. Prakash et al., “Coded Computing for Low-Latency Federated Learning Over Wireless Edge Networks,” _IEEE J. Sel. Areas Commun._ , vol. 39, no. 1, pp. 233-250, Jan. 2021.
  * [9] A. Hussain and M. Aleem, “GoCJ: Google cloud jobs dataset for distributed and cloud computing infrastructures,” _Data_ , vol. 3, no. 4, pp. 38-50, Dec. 2018.
  * [10] S. Shen, V. Van Beek and A. Iosup, “Statistical Characterization of Business-Critical Workloads Hosted in Cloud Datacenters,” _15th IEEE/ACM Int. Symp. Cluster, Cloud and Grid Comput._ , Shenzhen, China, 4-7 May 2015, pp. 465-474.
  * [11] M. X. Goemans, _Advanced algorithms_.Cambridge, MA: MIT Laboratory for Computer Science, 1994.
  * [12] J. Kronqvist, D. E. Bernal, A. Lundell and I. E. Grossmann, “A review and comparison of solvers for convex MINLP” _Optim. Eng._. vol. 20, no. 2, 20(2):pp. 397-455, June, 2019.


# Adaptive Spatio-Temporal Voxels Based Trajectory Planning for Autonomous
Driving in Highway Traffic Flow

Zhiqiang Jian1, Songyi Zhang1, Lingfeng Sun2, Wei Zhan2, Masayoshi Tomizuka2,
and Nanning Zheng1† *This work was supported by the National Natural Science
Foundation of China (No.62088102, 61790563).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

Trajectory planning is crucial for the safe driving of autonomous vehicles in
highway traffic flow. Currently, some advanced trajectory planning methods
utilize spatio-temporal voxels to construct feasible regions and then convert
trajectory planning into optimization problem solving based on the feasible
regions. However, these feasible region construction methods cannot adapt to
the changes in dynamic environments, making them difficult to apply in complex
traffic flow. In this paper, we propose a trajectory planning method based on
adaptive spatio-temporal voxels which improves the construction of feasible
regions and trajectory optimization while maintaining the quadratic
programming form. The method can adjust feasible regions and trajectory
planning according to real-time traffic flow and environmental changes,
realizing vehicles to drive safely in complex traffic flow. The proposed
method has been tested in both open-loop and closed-loop environments, and the
test results show that our method outperforms the current planning methods.

##  I INTRODUCTION

Highways are suitable application scenarios for autonomous driving [1]. In
these scenarios, the key issue is how to deal with the interaction between the
ego vehicle and the other agents in the traffic flow. A trajectory planner
aims to solve the problem. The trajectory planner can generate a trajectory
that meets the kinodynamic constraints of the vehicle in real-time according
to the current state of the vehicle and the environment, and ensure the
safety, efficiency, and comfort of the vehicle when traveling along the
planned trajectory.

Currently, a kind of the state-of-the-art trajectory planning methods searches
for a feasible region in the spatio-temporal domain, and all the trajectories
belonging to it can guarantee the safety of the vehicle. Afterward, an
optimization problem is defined according to the feasible region, and its
solution uniquely corresponds to the planned trajectory. For example, Ding _et
al._ [2] propose the Spatio-Temporal Semantic Corridor (SSC) to build a
feasible region based on the results of behavior planning, and then define an
optimization problem and solve it to plan a trajectory. Similarly, Zhang _et
al._ [3] propose the Spatio-Temporal Voxels (STV) for feasible region
construction and also define an optimization problem under the constraints of
the feasible region to obtain a trajectory. These methods have achieved good
performance.

However, these methods still have some shortcomings. Ding _et al._ ’s method
relies on the behavior planner, and unqualified behavior planning results will
lead to unqualified planned trajectories. Zhang _et al._ ’s method does not
have the above problem, but its feasible region construction and trajectory
optimization are not flexible enough. Zhang _et al._ ’s method use voxels to
describe the feasible region, but the number and size of the voxels are fixed
and the lateral kinodynamic constraints of the vehicle are ignored. Besides,
its objective function only considers jerks, which is insufficient. These
problems make the method perform poorly in more dense traffic flows.

Figure 1: This figure shows one frame of the traffic flow from the CitySim
dataset. The green boxes indicate the annotated vehicles.

In order to solve the problems, this paper proposes an Adaptive Spatio-
Temporal Voxels Based Trajectory Planning (ASVP) method. The ASVP method
improves the method proposed by Zhang _et al._ : In terms of the feasible
region construction, the ASVP method uses multi-resolution voxels to construct
the feasible region, and the number of the voxels in the feasible region can
also change according to the complexity of the environment. In terms of the
optimization problem definition, the objective function and the constraints
are both modified, and more factors are considered in the optimization. Based
on the improvements, the proposed method can ensure the safe, efficient, and
comfortable traveling of the vehicle in complex traffic flows.

Figure 2: This figure shows the framework of the proposed method, which
includes four modules. In the first three modules, the blue vehicle is the ego
vehicle. The colored circles indicate the voxels and different colors
represent different lanes. The colored arrows indicate the edges of the voxel
graph. In the last module, the colored boxes are the voxels in the spatio-
temporal domain. The black region indicates the marked obstacle vehicle in the
spatio-temporal domain. The blue curve indicates the planned trajectory.

The proposed method is tested in both open-loop and closed-loop environments.
The open-loop tests are conducted by replaying the CitySim dataset [4], as
shown in Fig. 1. The closed-loop environment uses the gym simulator [5]. In
both tests, the proposed method achieves significant performance in the safety
and efficiency of the vehicle. Besides, an ablation study is designed, which
verifies the effectiveness of the innovations proposed in the paper.

In conclusion, the contributions of the paper are as follows.

  * •

A novel feasible region construction method is proposed, which can be adjusted
according to the environment and adapted to scenarios of different complexity.

  * •

The defined optimization problem considers the safety, efficiency, and comfort
of the vehicle while maintaining the quadratic programming form.

  * •

Combining the above innovations, an effective highway traffic flow trajectory
planning method is implemented, which is open source.
111https://github.com/flztiii/traffic_flow_trajectory_planning

##  II RELATED WORKS

One of the main methods of trajectory planning is to convert the planning
problem into an optimization problem [6]. Some methods try to directly solve
the optimization problem, such as the Sequential Quadratic Programming (SQP)
method applied by Ziegler _et al._ [7]. However, since the optimization
problem is non-convex and has high complexity, more methods simplify the
problem before solving it, including sampling and evaluation methods [8, 9,
10, 11], spatial and temporal domain separation methods [12, 13, 14, 15], and
direct spatio-temporal domain simplification methods [16, 2, 17, 3].

The sampling and evaluation methods sample some trajectories, and then
evaluate these trajectories through the objective function. The trajectory
with the lowest cost is selected as the planning trajectory. For example,
Werling _et al._ [8] use polynomial curves to generate sampling trajectories
in the Frenet coordinate system, and then evaluate them through the objective
function to achieve trajectory planning. Zhang _et al._ [11] also apply
polynomial curves to generate paths and velocity profiles to obtain sampling
trajectories, and finally complete trajectory planning through evaluation.

The spatial and temporal domain separation methods plan separately in the
spatial and temporal domains, and then combine the two to obtain the planned
trajectory. For example, Xu _et al._ [12] use the sampling-evaluation method
to obtain the separate initial solutions from the spatial and temporal
domains, and then use the numerical optimization method to obtain the planned
trajectory. Fan _et al._ [14] propose a method similar to Expectation-
Maximization (EM) to iteratively optimize the trajectory in the separated
spatial and temporal domains, thereby obtaining the planned trajectory.

The direct spatio-temporal domain simplification methods construct a
simplified feasible region in the spatio-temporal domain according to the
environment and the vehicle’s kinodynamic constraints and then solve the
optimization problem in the feasible region. For example, Ziegler et al. [16]
apply the state lattice method to discretize the spatio-temporal domain, thus
realizing the simplification. The SSC proposed by Ding et al. [2] and the STV
proposed by Zhang et al. [3] also obtain a simplified feasible region, so as
to optimize the trajectory accordingly. Currently, this kind of method
achieves the best performance in highway trajectory planning. The method
proposed in this paper is also carried out following the idea.

##  III METHOD

###  III-A Framework

The proposed method’s framework is shown in Fig. 2. The method’s framework is
inspired by Zhang _et al._ ’s method [3], but we have modified the framework’s
modules. The planner updates at 5 $\mathrm{Hz}$ frequency and outputs the
trajectory in the future time period $T$ to the control module in real-time.
In each planning episode, the planner conducts feasible region construction
and trajectory optimization. These two processes are carried out in the Frenet
coordinate system and the ego vehicle and other agent information are all
converted to the Frenet coordinate system [17]. In this case, the spatio-
temporal domain can be described by the $s$-$d$-$t$ coordinate system. The $s$
axis is the lane’s direction, and the $d$ axis is perpendicular to the lane’s
direction.

In the feasible region construction, spatio-temporal voxels are generated
according to the environment. Each voxel corresponds to a lane $l$, where
$l\in\\{\mathrm{L,C,R}\\}$. Lane $\mathrm{L}$ indicates the left lane,
$\mathrm{R}$ indicates the right lane, and $\mathrm{C}$ indicates the current
lane. Afterward, a voxel graph is built and each voxel’s cost is calculated.
For each behavior, a voxel sequence with the least cost is searched in the
voxel graph. Behaviors include left lane change, right lane change, and lane
keeping.

In the trajectory optimization, each voxel sequence is modified according to
its corresponding behavior, and a feasible region can be obtained accordingly.
According to the feasible region, a quadratic programming problem can be
defined to describe the trajectory planning problem. The quadratic programming
problem’s solution uniquely corresponds to the planned trajectory. Then, the
planned trajectory is verified. If the verification fails, the voxel sequence
will be changed and the quadratic programming will be performed again. If the
verification succeeds, the planned trajectory’s cost is evaluated. Finally,
for each behavior, a trajectory with the cost is planned. The trajectory with
the lowest cost is output to the control module.

###  III-B Feasible Region Construction

We discretize the future time period $T$ into $n$ segments $\\{\Delta
T_{i}\\}_{i=0}^{n-1}$. for $\forall i$, $\Delta T_{i+1}\geq\Delta T_{i}$. For
each lane $l$ in each time segment $\Delta T_{i}$, a voxel set
$\\{\mathbf{v}_{l,i,j}\\}_{j=0}^{m-1}$ will be generated. Each voxel
$\mathbf{v}$ represents a box region in the spatio-temporal domain and is
defined as $\mathbf{v}=(ls,us,ld,ud,lt,ut)^{\mathrm{T}}$. $us$ and $ls$ are
the voxel $\mathbf{v}$’s upper and lower bounds on the $s$ axis. $ud$ and $ld$
are the voxel $\mathbf{v}$’s upper and lower bounds on the $d$ axis. $ut$ and
$lt$ are the voxel $\mathbf{v}$’s upper and lower bounds on the $t$ axis. For
$\forall i$, there exists $ut_{i}-lt_{i}=\Delta T_{i}$ and $ut_{i}=lt_{i+1}$.

Voxels generation. The generated voxel set is defined as
$\\{\mathbf{v}_{l,i,j}\\}_{j=0}^{m-1}$. For $\forall j$, the voxel
$\mathbf{v}_{l,i,j}$’s the upper and lower bounds on the $t$ axis remain
unchanged and are denote as $ut_{l,i}$ and $lt_{l,i}$, which can be calculated
according to the time segments $\\{\Delta T_{i}\\}_{i=0}^{n-1}$.

Then, the voxel $\mathbf{v}_{l,i,j}$’s upper and lower bounds $us_{l,i,j}$ and
$ls_{l,i,j}$ on the $s$ axis are calculated. According to the vehicle’s
initial longitudinal position $s_{0}$ and velocity $v_{0}^{(s)}$, the
reachable lower bound $s_{l,i}^{\mathrm{(min)}}$ of the $s$ axis when the
vehicle decelerates at the maximum deceleration within the time $lt_{l,i}$ and
the reachable upper bound $s_{l,i}^{\mathrm{(max)}}$ of the $s$ axis when the
vehicle accelerates at the maximum acceleration within the time $ut_{l,i}$ can
be obtained [3]. For each lane $l$, its related agent set is defined as
$\mathcal{O}_{l}$. (For example, if $l=\mathrm{L}$, $\mathcal{O}_{l}$ will be
the agents on the left lane.) For $\forall o\in\mathcal{O}_{l}$, its occupied
range of the $s$ axis within time $lt_{l,i}$ to $ut_{l,i}$ can be obtained,
which is defined as $[s_{o}^{\mathrm{(min)}},s_{o}^{\mathrm{(max)}}]$. In this
case, the free range set $\mathcal{S}_{l,i}$ can be calculated as follows.

$\mathcal{S}_{l,i}=[s_{l,i}^{\mathrm{(min)}},s_{l,i}^{\mathrm{(max)}}]\setminus\bigcup_{o\in\mathcal{O}_{l}}[s_{o}^{\mathrm{(min)}},s_{o}^{\mathrm{(max)}}].$
(1)

$\mathcal{S}_{l,i}$ contains multiple free ranges of the $s$ axis. Each free
range corresponds to a voxel and the free range’s number in
$\mathcal{S}_{l,i}$ is $m$. The $j_{\mathrm{th}}$ free range’s upper and lower
bounds are $us_{l,i,j}$ and $ls_{l,i,j}$.

At last, the voxel $\mathbf{v}_{l,i,j}$’s upper and lower bounds $ud_{l,i,j}$
and $ld_{l,i,j}$ on the $d$ axis are calculated. The $\mathbf{v}_{l,i,j}$’s
upper and lower bounds on the $d$ axis are related to its lane $l$. If
$l=\mathrm{L}$ or $l=\mathrm{R}$, $ud_{l,i,j}$ and $ld_{l,i,j}$ will be the
corresponding lane boundary’s position shrunk by the vehicle’s width. If
$l=\mathrm{C}$, according to the vehicle’s initial lateral position $d_{0}$
and velocity $v_{0}^{(d)}$, its reachable lateral range will be calculated,
which determines $ud_{l,i,j}$ and $ld_{l,i,j}$. Through the above process, all
the voxels in the time period $T$ can be generated, defined as
$\mathcal{V}=\\{\mathbf{v}_{l,i,j}|l\in\\{\mathrm{L},\mathrm{C},\mathrm{R}\\},i\in[0,n-1],j\in[0,m-1]\\}$.

Algorithm 1 Voxel Graph Building

1:Voxel set $\mathcal{V}$

2:Voxel graph $\mathcal{G}$

3:for $i=0,\cdots,n-1$ do

4: $\mathcal{G}_{i}$ $\leftarrow$ $\emptyset$

5: for $\mathbf{v}$ $\in$
$\\{\mathbf{v}_{l,i,j}|l\in\\{\mathrm{L},\mathrm{C},\mathrm{R}\\},j\in[0,m-1]\\}$
do

6: Initialize a node $\eta$ with the voxel $\mathbf{v}$

7: if not $i=0$ then

8: for $\eta_{\mathrm{f}}$ $\in$ $\mathcal{G}_{i-1}$ do

9: if not LaneCheck($\eta$, $\eta_{\mathrm{f}}$) then

10: continue

11: if not IntersectionCheck($\eta$, $\eta_{\mathrm{f}}$) then

12: continue

13: Push $\eta_{\mathrm{f}}$ into $\eta$’s father set
$\mathcal{G}_{\mathrm{f}}$

14: Push CalcCost($\eta$, $\eta_{\mathrm{f}}$) into $\eta$’s cost set
$\mathcal{C}$

15: Push $\eta$ into $\mathcal{G}_{i}$

16: Push $\mathcal{G}_{i}$ into $\mathcal{G}$

Voxel graph building. The voxel graph $\mathcal{G}$ contains $n$ layers and
each layer $\mathcal{G}_{i}$ contains multiple nodes $\eta$. The node $\eta$
consists of the voxel $\mathbf{v}$, the cost set $\mathcal{C}$ and the parent
node set $\mathcal{G}_{\mathrm{f}}$. The voxel graph’s building is shown in
Alg. 1. For the nodes in the first layer $\mathcal{G}_{0}$, since there is no
need to consider its cost set and parent node set, the nodes can be directly
defined according to the voxels.

For the nodes in the $i_{\mathrm{th}}$ layers, their definition is as follows.
First, for each voxel $\mathbf{v}$ in the voxel set
$\\{\mathbf{v}_{l,i,j}|l\in\\{\mathrm{L},\mathrm{C},\mathrm{R}\\},j\in[0,m-1]\\}$,
a node $\eta$ is defined accordingly. Each node $\eta_{\mathrm{f}}$ in the
previous layer $\mathcal{G}_{i-1}$ is judged whether it is the parent node of
$\eta$. Judgment conditions include lane checking and intersection checking.
In the lane checking, it is judged whether the lane of the node $\eta$’s voxel
is the current lane. If so, the lane of the node $\eta_{\mathrm{f}}$’s voxel
must also be the current lane. If not, the lane of the node
$\eta_{\mathrm{f}}$’s voxel must be the same as $\eta$, or be the current
lane. The lane checking aims to prevent multiple lane changes during one
planning episode. In the intersection checking, it is judged whether the
overlapping of the nodes $\eta$ and $\eta_{\mathrm{f}}$’s voxels on the $s$
axis and $d$ axis is larger than the threshold. If the lane checking and
intersection checking are satisfied, the node $\eta_{\mathrm{f}}$ will be
added into the node $\eta$’s parent node set $\mathcal{G}_{\mathrm{f}}$.
Finally, the cost $c$ is calculated according to the nodes $\eta$ and
$\eta_{\mathrm{f}}$’s voxels and added to the node $\eta$’s cost set
$\mathcal{C}$. The calculation is as follows.

$c=1-\frac{2s_{\mathrm{inter}}}{\Delta
T_{i}^{2}(a_{\mathrm{max}}^{(s)}-a_{\mathrm{min}}^{(s)})},$ (2)

where $s_{\mathrm{inter}}$ is the overlapping length of the nodes $\eta$ and
$\eta_{\mathrm{f}}$’s voxels on the $s$ axis, $a_{\mathrm{max}}^{(s)}$ is the
vehicle’s longitudinal maximum acceleration, $a_{\mathrm{min}}^{(s)}$ is the
vehicle’s longitudinal maximum deceleration. The cost reflects how much other
agents limit the vehicle’s motion. When $c=0$, other agents have no
restrictions on the ego vehicle’s motion, and when $c=1$, other agents
completely restrict the ego vehicle’s motion.

Voxel graph search. After building the voxel graph $\mathcal{G}$, the final
step is to generate the voxel sequence
$\mathcal{V}_{b}=\\{\mathbf{v}_{l,i}\\}_{i=0}^{n-1}$ for each behavior $b$.
Nodes that correspond to the behavior $b$ are selected in the voxel map
$\mathcal{G}$’s last layer $\mathcal{G}_{n-1}$. For example, if the behavior
$b$ is left lane change, the nodes whose voxels’ corresponding lane is the
left lane will be selected. For each selected node, the depth-first search is
applied to get the paths connecting it and the nodes in the first layer
$\mathcal{G}_{0}$. The path with the least cost will be chosen as
$\mathcal{V}_{b}$. Through the above process, the voxel sequences
corresponding to all three behaviors can be obtained.

###  III-C Trajectory Optimization

Voxel sequence change. For each behavior $b$, a trajectory $\Xi_{b}$ is
planned according to the voxel sequence
$\mathcal{V}_{b}=\\{\mathbf{v}_{l,i}\\}_{i=0}^{n-1}$. If the behavior $b$ is
not lane keeping, the voxel sequence $\mathcal{V}_{b}$ needs to be changed and
the process is as follows.

The index $i$ in the voxel sequence $\mathcal{V}_{b}$ is found, so that voxel
$\mathbf{v}_{l,i}$ and $\mathbf{v}_{l,i+1}$ have different lanes. In the voxel
graph $\mathcal{G}$’s $i+1_{\mathrm{th}}$ layer $\mathcal{G}_{i+1}$, the
voxels that hold the same lane as the voxel $\mathbf{v}_{l,i}$ are selected.
These voxels are intersected with the voxel $\mathbf{v}_{l,i}$ on the $s$ axis
separately, and the maximum range is regarded as the voxel
$\mathbf{v}_{l,i}$’s new upper and lower bounds on the $s$ axis. Similarly, in
the voxel graph $\mathcal{G}$’s $i_{\mathrm{th}}$ layer $\mathcal{G}_{i}$, the
voxels that hold the same lane as the voxel $\mathbf{v}_{l,i+1}$ are selected.
These voxels are intersected with the voxel $\mathbf{v}_{l,i+1}$ on the $s$
axis separately, and the maximum range is regarded as the voxel
$\mathbf{v}_{l,i+1}$’s new upper and lower bounds on the $s$ axis. Afterward,
The voxel $\mathbf{v}_{l,i}$ and $\mathbf{v}_{l,i+1}$’s union on the $d$ axis
is regarded as the two voxels’ new upper and lower bounds on the $d$ axis. The
voxel sequence change aims to allow the planner to consider the agents on both
lanes at the same time in the process of changing lanes to ensure safety.

Optimization definition. an optimization problem is defined based on the voxel
sequence $\mathcal{V}_{b}$ to achieve trajectory planning. The key to
optimization problem definition includes optimization variables, constraints,
and an objective function. For the optimization variables, the planned
trajectory is described by a piece-wise quintic Bezier curve
$\Xi(t)=\\{\xi_{i}^{(\sigma)}(t)|i\in[0,n-1],\sigma\in\\{s,d\\}\\}$, whose
expression is as follows.

$\displaystyle\xi_{i}^{(\sigma)}(t)=\Delta
T_{i}B_{i}^{(\sigma)}(\frac{t-lt_{l,i}}{\Delta T_{i}}),\quad
t\in[lt_{l,i},ut_{l,i}],$ (3) $\displaystyle
B_{i}^{(\sigma)}(x)=\sum_{k=0}^{5}\begin{pmatrix}5\\\
k\end{pmatrix}p_{i,k}^{(\sigma)}(1-x)^{(5-k)}x^{k},$

where $lt_{l,i}$ and $ut_{l,i}$ are the voxel $\mathbf{v}_{l,i}$’s upper and
lower bounds on the $t$ axis and $\Delta T_{i}=ut_{l,i}-lt_{l,i}$.
$\mathcal{P}=\\{p_{i,k}^{(\sigma)}|\sigma\in\\{s,d\\},k\in[0,5],i\in[0,n-1]\\}$
are the piece-wise Bezier curve’s control points. As long as $\mathcal{P}$ is
determined, the piece-wise Bezier curve $\Xi(t)$ can be uniquely determined.
Therefore, $\mathcal{P}$ can be regarded as the optimization variables.

The constraints include equality constraints and inequality constraints. Among
the equality constraints, the first consideration is that the planned
trajectory’s initial configuration needs to be the same as the ego vehicle’s
current configuration, so the following equation needs to be satisfied.

$\displaystyle\xi_{0}^{(s)}(lt_{l,0})=s_{0},\quad\xi_{0}^{(d)}(lt_{l,0})=d_{0},$
(4) $\displaystyle\frac{\partial\xi_{0}^{(\sigma)}(lt_{l,0})}{\partial t}$
$\displaystyle=v_{0}^{(\sigma)},\frac{\partial^{2}\xi_{0}^{(\sigma)}(lt_{l,0})}{\partial
t^{2}}=a_{0}^{(\sigma)},\sigma\in\\{s,d\\},$

where $s_{0}$ and $d_{0}$ are the vehicle’s initial longitudinal and lateral
positions, $v_{0}^{(s)}$ and $v_{0}^{(d)}$ are the vehicle’s initial
longitudinal and lateral velocities, and $a_{0}^{(s)}$ and $a_{0}^{(d)}$ are
the vehicle’s initial longitudinal and lateral accelerations. Among the
equality constraints, the second consideration is that the planned trajectory
needs to meet the $\mathrm{C}^{2}$ continuity, so the following equation needs
to be satisfied.

$\displaystyle\frac{\partial^{\mu}\xi_{i}^{(\sigma)}(ut_{l,i})}{\partial
t^{\mu}}=\frac{\partial^{\mu}\xi_{i+1}^{(\sigma)}(lt_{l,i+1})}{\partial
t^{\mu}},$ (5) $\displaystyle\mu=$ $\displaystyle
0,1,2,\quad\sigma\in\\{s,d\\},\quad i\in[0,n-2],$

where $\frac{\partial^{\mu}\xi_{i}^{(\sigma)}(t)}{\partial t^{\mu}}$ is the
function $\xi_{i}^{(\sigma)}(t)$’s $\mu$ order derivative.

For the inequality constraints, safety and comfort are considered. The
constraint brought by safety is that the Bezier curve $\Xi(t)$ must be
contained in the voxel sequence. The constraints brought by comfort are that
the Bezier curve $\Xi(t)$’s first-order, second-order, and third-order
derivatives, which represent the limits on velocity, acceleration, and jerk
respectively, must be within the limit range. Due to the Bezier curve’s convex
hull property, as long as the Bezier curve’s control points are within the
limit range, all the points on the Bezier curves can be guaranteed to be
within the limit range [2]. Therefore, the safety inequality constraints are
as follows.

$\displaystyle ls_{l,i}\leq p^{(s)}_{i,k}\leq$ $\displaystyle us_{l,i},\quad
ld_{l,i}\leq p^{(d)}_{i,k}\leq ud_{l,i},$ (6) $\displaystyle i\in[0,n-1],$

where $ls_{l,i}$ and $us_{l,i}$ are the voxel $\mathrm{v}_{l,i}$’s upper and
lower bounds on the $s$ axis, and $ld_{l,i}$ and $ud_{l,i}$ are the voxel
$\mathrm{v}_{l,i}$’s upper and lower bounds on the $d$ axis. The comfort
inequality constraints are as follows.

$\displaystyle v_{\mathrm{min}}^{(\sigma)}\leq
p^{(\sigma)}_{i,k}{}^{\prime}\leq v_{\mathrm{max}}^{(\sigma)},\quad
k\in[0,4],$ (7) $\displaystyle a_{\mathrm{min}}^{(\sigma)}\leq
p^{(\sigma)}_{i,k}{}^{\prime\prime}\leq a_{\mathrm{max}}^{(\sigma)},\quad
k\in[0,3],$ $\displaystyle j_{\mathrm{min}}^{(\sigma)}\leq
p^{(\sigma)}_{i,k}{}^{\prime\prime\prime}\leq
j_{\mathrm{max}}^{(\sigma)},\quad k\in[0,2],$ $\displaystyle
i\in[0,n-1],\quad\sigma\in\\{s,d\\},$

where $v_{\mathrm{min}}^{(s)}$ and $v_{\mathrm{max}}^{(s)}$ are the vehicle’s
longitudinal velocity upper and lower bounds, $v_{\mathrm{min}}^{(d)}$ and
$v_{\mathrm{max}}^{(d)}$ are the vehicle’s lateral velocity upper and lower
bounds, $a_{\mathrm{min}}^{(s)}$ and $a_{\mathrm{max}}^{(s)}$ are the
vehicle’s longitudinal acceleration upper and lower bounds,
$a_{\mathrm{min}}^{(d)}$ and $a_{\mathrm{max}}^{(d)}$ are the vehicle’s
lateral acceleration upper and lower bounds, $j_{\mathrm{min}}^{(s)}$ and
$j_{\mathrm{max}}^{(s)}$ are the vehicle’s longitudinal jerk upper and lower
bounds, and $j_{\mathrm{min}}^{(d)}$ and $j_{\mathrm{max}}^{(d)}$ are the
vehicle’s lateral jerk upper and lower bounds.
$p^{(\sigma)}_{i,k}{}^{\prime}$, $p^{(\sigma)}_{i,k}{}^{\prime\prime}$ and
$p^{(\sigma)}_{i,k}{}^{\prime\prime\prime}$ are the Bezier curve $\Xi(t)$’s
first-order, second order, and third-order derivatives’ control points (A
Bezier curve’s derivative is still a Bezier curve), which are the linear
combination of the Bezier curve $\Xi(t)$’s control points [18].

The objective function is defined as $\sum_{j=0}^{4}w_{j}\varepsilon_{j}$,
which is weighted sum of five terms. The term $\varepsilon_{0}$ is calculated
as follows.

$\varepsilon_{0}=\sum_{\sigma\in\\{s,d\\}}\sum_{i=0}^{n-1}\int_{lt_{l,i}}^{ut_{l,i}}\Big{(}\frac{\partial^{3}\xi_{i}^{(\sigma)}(t)}{\partial
t^{3}}\Big{)}^{2}\mathrm{d}t.$ (8)

The term $\varepsilon_{0}$’s purpose is to minimize the jerk. The term
$\varepsilon_{1}$ is calculated as follows.

$\varepsilon_{1}=\sum_{\sigma\in\\{s,d\\}}\sum_{i=0}^{n-1}\Big{(}\xi_{i}^{(\sigma)}(ut_{l,i})-\alpha_{i}^{(\sigma)}\Big{)}^{2},$
(9)

where $\alpha_{i}^{(s)}$ and $\alpha_{i}^{(d)}$ are the ideal longitudinal and
lateral end positions of the Bezier curve’s $i_{\mathrm{th}}$ segment. The
term $\varepsilon_{1}$ aims to make the end position of the Bezier curve’s
each segment close to the expectation. $\alpha_{i}^{(s)}$ and
$\alpha_{i}^{(d)}$’s calculation are as follows. The front and rear agents of
the bounding voxel $\mathbf{v}_{l,i}$ on the lane $l$ are found from time
$lt_{l,i}$ to $ut_{l,i}$. If the front agent does not exist,
$\alpha_{i}^{(s)}$ will be set to the voxel $\mathbf{v}_{l,i}$’s upper bound
on the $s$ axis. If the front and rear agents exist, the front agent’s
position $s_{\mathrm{f}}$ on the $s$ axis at time $lt_{l,i}$ and the rear
agent’s position $s_{\mathrm{r}}$ on the $s$ axis at time $ut_{l,i}$ will be
obtained. Then, $\alpha_{i}^{(s)}$ can be calculated as follows.

$\displaystyle\gamma_{\mathrm{f}}=s_{\mathrm{f}}+\frac{(v^{(s)}_{0})^{2}-(v^{(s)}_{\mathrm{f}})^{2}}{2a^{(s)}_{\mathrm{max}}}-v^{(s)}_{0}T_{\mathrm{res}},$
(10)
$\displaystyle\gamma_{\mathrm{r}}=s_{\mathrm{r}}+\frac{(v^{(s)}_{\mathrm{r}})^{2}-(v^{(s)}_{0})^{2}}{2a^{(s)}_{\mathrm{max}}}+v^{(s)}_{\mathrm{r}}(T_{\mathrm{res}}+ut_{l,i}-lt_{l,i}),$
$\displaystyle\alpha_{i}^{(s)}=\left\\{\begin{array}[]{l}\gamma_{\mathrm{f}},\quad\gamma_{\mathrm{r}}\leq\gamma_{\mathrm{f}},\\\
\frac{w_{\mathrm{f}}}{w_{\mathrm{f}}+w_{\mathrm{r}}}\gamma_{\mathrm{f}}+\frac{w_{\mathrm{r}}}{w_{\mathrm{f}}+w_{\mathrm{r}}}\gamma_{\mathrm{r}},\quad\mathrm{else},\\\
\end{array}\right.$

where $v^{(s)}_{0}$ is the ego vehicle’s initial longitudinal velocity,
$v^{(s)}_{\mathrm{f}}$ is the front agent’s longitudinal velocity,
$v^{(s)}_{\mathrm{r}}$ is the rear agent’s longitudinal velocity,
$T_{\mathrm{res}}$ is the preset response time, $a^{(s)}_{\mathrm{max}}$ is
the vehicle’s maximum longitudinal deceleration, and $w_{\mathrm{f}}$ and
$w_{\mathrm{r}}$ are preset weights. If the rear agent does not exist,
$\alpha_{i}^{(s)}$ equals to $\gamma_{\mathrm{f}}$. $\alpha_{i}^{(d)}$ is
determined by the voxel $\mathbf{v}_{l,i}$’s lane. For example, if the voxel
$\mathbf{v}_{l,i}$’s lane is the left lane, $\alpha_{i}^{(d)}$ equals to the
lane width. The term $\varepsilon_{2}$ is calculated as follows.

$\varepsilon_{2}=\sum_{\sigma\in\\{s,d\\}}\sum_{i=0}^{n-1}\Big{(}\frac{\partial\xi_{i}^{(\sigma)}(ut_{l,i})}{\partial
t}-\beta_{i}^{(\sigma)}\Big{)}^{2},$ (11)

where $\beta_{i}^{(s)}$ and $\beta_{i}^{(d)}$ are the ideal end velocities of
the Bezier curve’s $i_{\mathrm{th}}$ segment. $\beta_{i}^{(s)}$ is also
determined by the voxel $\mathbf{v}_{l,i}$’s front agent on the lane $l$ from
time $lt_{l,i}$ to $ut_{l,i}$. If the front agent does not exist,
$\beta_{i}^{(s)}$ will be set as the upper limit of velocity allowed by
kinematics. If the front agent exists, $\beta_{i}^{(s)}$ will be set to the
front agent’s longitudinal velocity. $\beta_{i}^{(d)}$ is set to zero. The
term $\varepsilon_{3}$ is calculated as follows.

$\varepsilon_{3}=\sum_{i=0}^{n-1}\int_{lt_{l,i}}^{ut_{l,i}}\Big{(}\frac{\partial\xi_{i}^{(d)}(t)}{\partial
t}\Big{)}^{2}\mathrm{d}t.$ (12)

The purpose of the term $\varepsilon_{3}$ is to make the lateral velocity
close to constant and the lateral position change uniformly. The term
$\varepsilon_{4}$ is calculated as follows.

$\varepsilon_{4}=\sum_{i=0}^{n-1}\int_{lt_{l,i}}^{ut_{l,i}}\Big{(}\frac{\partial^{2}\xi_{i}^{(s)}(t)}{\partial^{2}t}\Big{)}^{2}\mathrm{d}t.$
(13)

The term $\varepsilon_{4}$ aims to make the longitudinal acceleration close to
constant and the longitudinal velocity change uniformly.

Optimization solver. It can be easily seen through the calculation that the
defined constraints are linear, the defined objective function is quadratic,
and its quadratic coefficient matrix is semi-positive. These properties show
that the defined optimization problem is quadratic programming. Therefore,
Object-Oriented Software for Quadratic Programming (OOQP) [19] can be applied
to solve the problem to obtain the planned trajectory. The planned trajectory
is verified whether it is acceptable. The verification conditions are whether
the trajectory meets all the constraints and whether the trajectory’s
curvatures are smaller than the threshold.

Verification and evaluation. If the optimization fails or the planned
trajectory fails to pass the verification, the voxel sequence
$\mathcal{V}_{b}$ will be changed again. We continuously remove the tail voxel
of the voxel sequence and repeat the optimization until the planned trajectory
passes the verification. Due to the existence of $\Delta T_{i+1}\geq\Delta
T_{i}$, the tail voxel of the voxel sequence has stronger constraints in the
optimization. Therefore, as the tail voxel of the voxel sequence is kept
removed, the optimization problem becomes increasingly easier to be solved. As
the above process continues, the planned trajectory will become more and more
aggressive. After the trajectories for each behavior are planned, the
trajectories are evaluated. The evaluation method uses Eq. 2 to calculate the
cost of the voxel sequence corresponding to the trajectory, and the trajectory
with the lowest cost is output to the controller.

##  IV EXPERIMENTAL RESULTS

The experiment includes the open-loop tests, closed-loop tests, and ablation
study. The purpose of the open-loop tests is to compare the proposed method,
the SOTA method, and human drivers through a large number of tests under
safety and efficiency and proves the proposed method’s effectiveness. The
purpose of the closed-loop experiment is to conduct a long-term test to
demonstrate the proposed method’s stability. In the process, we analyzed the
planning process using the proposed method, and further verified the
effectiveness of the method. The ablation study aims to verify whether the
proposed innovations can improve the planner. In the experiment, the proposed
method is implemented based on C++ and Robot Operating System (ROS) [20], and
runs on a computer equipped with a CPU i7-8750H. The vehicle’s maximum
longitudinal acceleration and deceleration are 2 $\mathrm{m/s}^{2}$, maximum
lateral acceleration and deceleration are 2 $\mathrm{m/s}^{2}$, and maximum
longitudinal and lateral jerks are 2 $\mathrm{m/s}^{3}$. The parameters used
by the method are detailed in the open source code.

###  IV-A Open-Loop Tests

In the open-loop tests, the CitySim dataset is applied [4]. Compared with the
NGSIM dataset, the CitySim dataset has higher vehicle density and complexity.

For the experiment design, Sun _et al._ ’s method [21] is referred to. 10
seconds of traffic flow is randomly selected from the CitySim dataset, from
which an agent is randomly selected as the ego vehicle. The agent’s state is
regarded as the ego vehicle’s initial state, and the lane the agent is in
after 10 seconds is regarded as the target lane. Afterward, the agent is
removed from the traffic flow and a trajectory planning method is used to
update the ego vehicle’s state in the traffic flow. The states of other agents
in the traffic flow are updated according to the CitySim dataset. The update
frequency is 5 Hz and the update duration is 10 seconds. During the planning
process, the planning method only considers other agents within 100 meters of
its own lane and adjacent lanes. For the case where the target lane is the
same as the current lane (lane keeping) and the case where the target lane is
different from the current lane (lane change), each one is carried out 100
times respectively, and the _success rate_ , _failure rate_ , _risk_ , and
_efficiency_ are recorded.

The _success rate_ is defined as the ratio of the number of times the ego
vehicle does not collide with other agents and is in the target lane at the
end of the test to the total number of tests. The _failure rate_ is defined as
the ratio of the number of times the ego vehicle collided with other agents or
failed to plan to the total number of tests. The _risk_ is defined as the
ratio of the time the vehicle is in danger to the total testing time. The
danger is defined as the situation in which the ego vehicle’s available
response time is less than 1 second. The response time is defined that when
the front agent brakes with the maximum deceleration, the ego vehicle can
brake with the maximum deceleration after the response time, so as to ensure
that the ego vehicle will not collide with the front agent. In common sense, a
response time of 1 second is enough to ensure the safety of the vehicle. The
_efficiency_ is defined as the average velocity of the ego vehicle.

The proposed method and the STV method [3] proposed by Zhang _et al._ are used
for testing. We implement the STV method and apply it for real-time trajectory
planning. At the same time, the ego vehicle’s corresponding agent’s states
from the dataset are also recorded as the human driver. The testing results
are shown in Tab. I. In the test, the average time consumption of the proposed
method’s one single planning episode is 58.3 ms, which fully meets the real-
time requirements.

TABLE I: Comparison of different methods in the open-loop test. Method | Lane Keeping | Lane Change
---|---|---
Succ. $\uparrow$ | Fail $\downarrow$ | Risk $\downarrow$ | Effi. $\uparrow$ | Succ. $\uparrow$ | Fail $\downarrow$ | Risk $\downarrow$ | Effi. $\uparrow$
STV | 56% | 44% | 45.9% | 11.76 | 0% | 83% | 50.9% | 15.30
Human | - | - | 25.8% | 12.41 | - | - | 52.4% | 16.29
Ours | 91% | 9% | 10.2% | 12.74 | 45% | 24% | 23.7% | 17.11
Figure 3: This figure shows four screenshots in the open-loop tests. The red
boxes indicate the agents. The blue box indicates the ego vehicle operated by
the human driver. The green box indicates the ego vehicle using the proposed
method. Fig. (a), (b), and (c) are the lane keeping and Fig. (d) is the lane
changing.

It can be seen that according to each measurement standard, the proposed
methods outperform the STV methods in all the metrics. The reasons for the STV
method’s poor performance in the test mainly include three points. The first
reason is that the traffic flow in the CitySim dataset is too dense and the
ego vehicle cannot maintain a sufficient distance from other agents, so the
STV method is difficult to generate the required voxels, causing planning
failure. The second reason is that the STV method does not consider the
lateral kinodynamic constraints in the voxels generation, so it cannot perform
real-time trajectory planning in lane changing. The third reason is that the
STV method’s objective function is too naive and constraints are too strict,
which makes it difficult to achieve better results in trajectory optimization.
The proposed method solves the above problems and makes the effect of the
planning method greatly improved.

Compared with the human driver, the proposed method also achieves lower risk
and higher efficiency. Fig. 3 shows four examples of the comparison between
the proposed method and the human driver. Fig. 3 (a), (b), and (c) are the
screenshots of the lane-keeping tests. Fig. 3 (a) shows a sparse environment,
where the proposed method makes the ego vehicle travel faster than the human
driver to achieve higher efficiency. Fig. 3 (b) shows a little bit dense
environment, where the proposed method prefers to keep a long distance to the
front agent to achieve lower risk. Fig. 3 (c) shows a very dense environment,
where the proposed method almost performs the same as the human driver. These
three cases can illustrate how the proposed method can achieve lower risk and
higher efficiency than the human driver. Fig. 3 (d) shows a case that a human
driver conducts a dangerous lane change in the dense traffic flow, where the
proposed method fails to conduct the same behavior. In the lane-changing test
scenario, the reason for the low success rate of the proposed method is that
the traffic flow in the CitySim dataset is dense, and lane-changing often
requires an aggressive strategy. However, the proposed method uses a more
cautious strategy due to the lack of accurate prediction of the other agents’
behaviors. As a result, in some cases, the proposed method is more likely to
let the vehicle maintain the lane.

###  IV-B Close-Loop Tests

In the closed-loop test, the gym simulation [5] is applied. A four-lane
highway scene is constructed through gym simulation, and other agents are
randomly placed in it. The ego vehicle’s maximum longitudinal velocity is set
to 20 m/s, and that of other agents is set to 15 m/s. The ego vehicle runs
continuously for 8 minutes using the proposed method, and the vehicle’s
longitudinal and lateral velocities during the process are recorded.

The vehicle finally passes the test successfully. Within 8 minutes, the
vehicle safely interacted with other agents on the scene and maintained a high
velocity. During the period, the vehicle successfully completed several times
of smooth following and changing lanes to overtake. The vehicle’s longitudinal
and lateral velocity during the whole test process is shown in Fig. 4.

Figure 4: This figure shows the longitudinal and lateral velocities of the ego
vehicle during the 8 minutes close-loop tests. The red lines indicate the
longitudinal and lateral velocity limitations. The shadowed region indicates
an example of vehicle following.

From Fig. 4, it can be seen that the ego vehicle’s longitudinal and lateral
velocities meet the constraints. During the test, the ego vehicle changes
lanes and overtakes 12 times, and its average longitudinal velocity reaches
16.86 m/s, which is higher than the other agents’ maximum velocity. It shows
that when using the proposed method, the ego vehicle will not be hindered by
the other low-velocity agents, and can travel with higher efficiency.
Especially, the gray area in Fig. 4 is the ego vehicle’s longitudinal velocity
profile during a follow-up process lasting about 1 minute. It can be seen that
when following a front agent using our planning method, the ego vehicle can
decelerate smoothly until it reaches a steady state at the same velocity as
the front agent. In contrast, the STV method is difficult to achieve stable
vehicle following, and there will be frequent acceleration and deceleration
during the vehicle following process. The reason is that the change of the
objective function and the constraints on the position and velocity of the
front agent are continuous in our defined optimization problem but
discontinuous in the STV method.

Figure 5: This figure shows an example of the planned trajectory during lane
changing. (a) The left picture shows the visualization of the simulator and
the right picture shows the visualization of the planning program. The yellow
box indicates the ego vehicle, the blue and red boxes indicate the other
agent. The red curve indicates the planned trajectory. (b) The blue profiles
are the attributes of the planned trajectory. The red lines indicate the
limitations.

In order to further demonstrate the proposed method, one single planning
episode’s result is in Fig. 5. Fig. 5 (a) shows the scenario of this planning
episode. Due to the obstruction of a low-speed agent in front and no other
agents on the right lane, the proposed method plans the right lane-changing
trajectory for overtaking. Fig. 5 (b) shows the properties of the planned
trajectory. It can be seen that the planned trajectory’s longitudinal and
lateral velocities, accelerations, and jerks are continuous, which proves that
the planned trajectory by our method satisfies the $\mathrm{C}^{2}$
continuity. In addition, the planned trajectory’s longitudinal and lateral
velocities, accelerations, and jerks all meet the constraints, which proves
that the trajectory planned by our method can meet the needs of comfort.

###  IV-C Ablation Study

The ablation study uses the same process as the open-loop tests. In the
ablation study, we fix the number of voxels in the voxel sequence as the first
comparison method. The voxels’ sizes in the time domain are fixed, that is,
$\Delta T_{i+1}=\Delta T_{i},\forall i$, as the second comparison method. Only
the jerks are considered in the objective function, that is,
$w_{j}=0,j=1,2,3,4$, as the third comparison method. Only the jerks and end
states are considered in the objective function, that is, $w_{j}=0,j=3,4$, as
the fourth comparison method. These four methods are used to test separately,
and compared with the results obtained by the original method, as shown in
Tab. II.

It can be seen from the table that when the number of voxels is fixed, the
method is worse than the original method under all metrics, which can prove
that the introduction of the changeable voxel number is effective. Then, when
the size of voxels in the time domain is fixed, other metrics except risk are
worse than the original method. The reason for this result is that the size of
the voxel in the time domain is positively correlated with the vehicle’s
response time. Fixing the size of voxels in the time domain will make the
vehicle’s response time as close as possible to this size, and when the
vehicle’s response time cannot approach this size, it will lead to planning
failure. This characteristic causes the vehicle to either be in safety or
planning fail. That’s why using fixed-size voxels leads to a high failure rate
and low risk. The original method solves this problem using multi-resolution
voxels. Besides, the results of the 3rd and 4th comparing methods prove that
the original method can improve the planner by introducing new items in the
objective function.

TABLE II: Comparison of different methods in the ablation study. Method | Lane Keeping | Lane Change
---|---|---
Succ. $\uparrow$ | Fail $\downarrow$ | Risk $\downarrow$ | Effi. $\uparrow$ | Succ. $\uparrow$ | Fail $\downarrow$ | Risk $\downarrow$ | Effi. $\uparrow$
Comp. I | 81% | 19% | 10.5% | 12.53 | 3% | 60% | 26.3% | 15.42
Comp. II | 82% | 18% | 4.9% | 12.41 | 20% | 62% | 19.2% | 16.26
Comp. III | 59% | 41% | 18.9% | 11.65 | 16% | 58% | 28.1% | 15.31
Comp. IV | 91% | 9% | 10.2% | 12.63 | 43% | 26% | 23.5% | 16.10
Origin | 91% | 9% | 10.2% | 12.74 | 45% | 24% | 23% | 17.11

##  V CONCLUSIONS

In this paper, a variable-length, multi-resolution voxel sequence is used to
represent highway scenes, and a new optimization problem is defined to
describe the trajectory planning problem. Combining the above two innovations,
this paper implements an environment-adaptive trajectory planning method,
which can enable autonomous vehicles to achieve efficient, safe and
comfortable traveling in highway scenarios. Experiments prove that the
proposed method can perform better than the current planning methods. In
future work, on the one hand, we will expand the applicable scenarios of this
method so that it can be applied to urban environments. On the other hand, we
plan to combine this method with the prediction method[22, 23], introduce the
motion uncertainty of other agents[24], and further improve the effect of the
method.

## References

  * [1] L. Claussmann, M. Revilloud, D. Gruyer, and S. Glaser, “A review of motion planning for highway autonomous driving,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 21, no. 5, pp. 1826–1848, 2019.
  * [2] W. Ding, L. Zhang, J. Chen, and S. Shen, “Safe trajectory generation for complex urban environments using spatio-temporal semantic corridor,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 3, pp. 2997–3004, 2019\.
  * [3] T. Zhang, W. Song, M. Fu, Y. Yang, X. Tian, and M. Wang, “A unified framework integrating decision making and trajectory planning based on spatio-temporal voxels for highway autonomous driving,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 8, pp. 10 365–10 379, 2021\.
  * [4] O. Zheng, M. Abdel-Aty, L. Yue, A. Abdelraouf, Z. Wang, and N. Mahmoud, “Citysim: A drone-based vehicle trajectory dataset for safety oriented research and digital twins,” 2022. [Online]. Available: https://arxiv.org/abs/2208.11036
  * [5] E. Leurent, “An environment for autonomous driving decision-making,” https://github.com/eleurent/highway-env, 2018.
  * [6] D. González, J. Pérez, V. Milanés, and F. Nashashibi, “A review of motion planning techniques for automated vehicles,” _IEEE Transactions on intelligent transportation systems_ , vol. 17, no. 4, pp. 1135–1145, 2015.
  * [7] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for bertha—a local, continuous method,” in _2014 IEEE intelligent vehicles symposium proceedings_. IEEE, 2014, pp. 450–457.
  * [8] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a frenet frame,” in _2010 IEEE International Conference on Robotics and Automation_. IEEE, 2010, pp. 987–993.
  * [9] K. Chu, M. Lee, and M. Sunwoo, “Local path planning for off-road autonomous driving with avoidance of static obstacles,” _IEEE transactions on intelligent transportation systems_ , vol. 13, no. 4, pp. 1599–1616, 2012.
  * [10] X. Li, Z. Sun, D. Cao, D. Liu, and H. He, “Development of a new integrated local trajectory planning and tracking control framework for autonomous ground vehicles,” _Mechanical Systems and Signal Processing_ , vol. 87, pp. 118–137, 2017.
  * [11] J. Zhang, Z. Jian, J. Fu, Z. Nan, J. Xin, and N. Zheng, “Trajectory planning with comfort and safety in dynamic traffic scenarios for autonomous driving,” in _2021 IEEE Intelligent Vehicles Symposium Workshops (IV Workshops)_. IEEE, 2021, pp. 342–349.
  * [12] W. Xu, J. Wei, J. M. Dolan, H. Zhao, and H. Zha, “A real-time motion planner with trajectory optimization for autonomous vehicles,” in _2012 IEEE International Conference on Robotics and Automation_. IEEE, 2012, pp. 2061–2067.
  * [13] J. Villagra, V. Milanés, J. Pérez, and J. Godoy, “Smooth path and speed planning for an automated public transport vehicle,” _Robotics and Autonomous Systems_ , vol. 60, no. 2, pp. 252–265, 2012.
  * [14] H. Fan, F. Zhu, C. Liu, L. Zhang, L. Zhuang, D. Li, W. Zhu, J. Hu, H. Li, and Q. Kong, “Baidu apollo em motion planner,” _arXiv preprint arXiv:1807.08048_ , 2018.
  * [15] W. Lim, S. Lee, M. Sunwoo, and K. Jo, “Hybrid trajectory planning for autonomous driving in on-road dynamic scenarios,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 22, no. 1, pp. 341–355, 2019.
  * [16] J. Ziegler and C. Stiller, “Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios,” in _2009 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2009, pp. 1879–1884.
  * [17] W. Ding, L. Zhang, J. Chen, and S. Shen, “Epsilon: An efficient planning system for automated vehicles in highly interactive environments,” _IEEE Transactions on Robotics_ , vol. 38, no. 2, pp. 1118–1138, 2021.
  * [18] G. Farin, _Curves and surfaces for computer-aided geometric design: a practical guide_. Elsevier, 2014.
  * [19] E. M. Gertz and S. J. Wright, “Object-oriented software for quadratic programming,” _ACM Transactions on Mathematical Software (TOMS)_ , vol. 29, no. 1, pp. 58–81, 2003.
  * [20] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, A. Y. Ng, _et al._ , “Ros: an open-source robot operating system,” in _ICRA workshop on open source software_ , vol. 3, no. 3.2. Kobe, Japan, 2009, p. 5.
  * [21] M. Sun, F. Baldini, P. Trautman, and T. Murphey, “Move beyond trajectories: Distribution space coupling for crowd navigation,” in _Robotics: Science and Systems_. RSS Foundation, 2021, pp. 1–12.
  * [22] 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.
  * [23] 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.
  * [24] 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.