Real-Time Trajectory Planning for Autonomous Driving with Gaussian Process and Incremental Refinement

Real-time kinodynamic trajectory planning in dynamic environments is critical yet challenging for autonomous driving. In this letter, we propose an efficient trajectory planning system for autonomous driving in complex dynamic scenarios through iterative and incremental path-speed optimization. Exploiting the decoupled structure of the planning problem, a path planner based on Gaussian process first generates a continuous arc-length parameterized path in the Frenét frame, considering static obstacle avoidance and curvature constraints. We theoretically prove that it is a good generalization of the well-known jerk optimal solution. An efficient s-t graph search method is introduced to find a speed profile along the generated path to deal with dynamic environments. Finally, the path and speed are optimized incrementally and iteratively to ensure kinodynamic feasibility. Various simulated scenarios with both static obstacles and dynamic agents verify the effectiveness and robustness of our proposed method. Experimental results show that our method can run at 20 Hz. The source code is released as an open-source package.


DL-IAPS and PJSO: A Path/Speed Decoupled Trajectory Optimization and its Application in Autonomous Driving

This paper presents a free space trajectory optimization algorithm of au...

An Efficient Generation Method based on Dynamic Curvature of the Reference Curve for Robust Trajectory Planning

Trajectory planning is a fundamental task on various autonomous driving ...

Tunable Trajectory Planner Using G3 Curves

Trajectory planning is commonly used as part of a local planner in auton...

Baidu Apollo EM Motion Planner

In this manuscript, we introduce a real-time motion planning system base...

Hierarchical Trajectory Planning for Autonomous Driving in Low-speed Driving Scenarios Based on RRT and Optimization

Though great effort has been put into the study of path planning on urba...

Differential Flatness-Based Trajectory Planning for Autonomous Vehicles

As a core part of autonomous driving systems, motion planning has receiv...

Speed Planning Using Bezier Polynomials with Trapezoidal Corridors

To generate safe and real-time trajectories for an autonomous vehicle in...

Code Repositories

I Introduction

I-a Motivation

The trajectory planning system is crucial for autonomous driving tasks [12]. It needs to be robust and efficient, as well as able to handle static obstacles, dynamic traffic participants and various constraints (e.g., speed limits, kinodynamic feasibility) in real-time, which is challenging.

Existing works can be categorized into two main classes: spatio-temporal planning [1, 2, 4, 8] and path-speed decoupled planning [7, 6, 22]. Spatio-temporal (s-t) approaches mostly adopt a hierarchy scheme. They first find an initial solution in s-t space by searching or sampling, and some may further construct convex subspaces based on the rough solution. Then the preliminary result is improved by solving a receding-horizon optimization problem or fitting it with a parameterized curve. The main advantage of these approaches is that they can consider spatial and temporal constraints simultaneously. However, finding an initial guess in the spatio-temporal space is non-trivial, sometimes even harder than solving the optimization problem. Approximations (e.g.,

increasing grid size or sample resolution, heuristics) have to be used to make this problem tractable in limited time. As a result, these approaches are prone to local optimum. Path-speed decoupled approaches break the high dimensional problem into two easier subproblems: first, planing a path; then, generating a speed profile along that path. The benefits are two folds. Firstly, they do not require a preliminary result in the s-t domain. Secondly, they usually enjoy higher efficiency and flexibility.

Nevertheless, they are generally much harder to guarantee kinodynamic feasibility due to the decomposition structure.

Fig. 1: Illustration of trajectory planning with both dynamical agents and a static obstacle. The AV can either avoid the obstacle (gray trajectory) or merge into the right lane (red trajectory). More examples can be found at

Apart from the issues mentioned above, most existing works have difficulty handling the curvature constraint. Some ignore the constraint by assuming the resulting trajectory is smooth, which is not reasonable in some scenarios (e.g., sharp turns). Others have to approximate or relax the constraint using a heuristic limit, leading to an overly conservative or even infeasible solution.

In this paper, we propose an efficient trajectory planning framework to overcome the problems of existing methods. Our approach can be categorized as path-speed decoupled planning in general. Firstly, this path planning problem is converted to a probabilistic inference problem with Gaussian process (GP). This is motivated by recent progress on motion planning via probabilistic inference approach [15, 14], which shows significant increase in speed over other trajectory optimization solvers.

Analytic formulation of curvature constraint is used and integrated into the path planning process. We also theoretically prove that this path generation method is a good generalization of the well-known jerk optimal solution [19]. To avoid local optimum, a novel and efficient s-t graph search method is introduced. Finally, an incremental path-speed adjustment method is adopted to ensure kinodynamic feasibility.

I-B Contribution

Compared with existing methods, our proposed approach is able to directly consider the curvature constraint without approximations, and efficiently generate high-quality kinodynamically feasible trajectories. The main contributions are summarized as follows:

  • A path planner incorporating GP that generates a collision-free path under strict curvature constraints.

  • A complete and efficient trajectory planning framework for complex dynamic environments with a safety and kinodynamic feasibility guarantee.

  • Comprehensive validation with various numerical experiments and open-source real-time implementation for the reference of the community.

Ii Related Work

Ii-a Iterative Path-speed Refinement

The online iterative path-speed refinement technique is widely adopted in the unmanned aerial vehicle (UAV) community. For example, Zhou et al. [21] iteratively adjust the infeasible velocity and acceleration control points of the B-spline trajectory to achieve dynamic feasibility for a quadcopter, while [18] and [16] use a similar bilevel optimization approach to get spatio-temporal optimal trajectories in real-time. However, this technique is less commonly used for AVs. The main reason is that various non-linearities (e.g., collision constraints on a rectangular vehicle body, nonlinear vehicle dynamics) make the optimization problem for AVs expensive to solve iteratively. Adopting this approach for AVs, Kapania et al. [11] use a two-step algorithm to generate the trajectory for car racing with near real-time performance. Similarly, Tang et al. [17] apply bilevel optimization for time optimal car racing. However, there are only applicable for static, obstacle-free racetracks.

Xu et al. [20] use recurrent spline optimization to ensure curvature constraints, but do not consider dynamic feasibility when speed changes. Recent work [22] takes both dynamic feasibility and obstacle avoidance into consideration. However, the performance is not satisfactory (around 5Hz with the presence of obstacles).

By formulating the path planning as a probabilistic inference problem, we are able to partially improve the invalid path with efficient incremental inference tool [10] from the SLAM community, instead of generating a new path from scratch.

Ii-B Motion Planning via Probabilistic Inference

Recent progress on motion planning via probabilistic inference has opened a new window onto the motion planning problem. Mukadam et al. [15] propose Gaussian process motion planning (GPMP), where they show GP driven by linear, time-varying stochastic differential equations (LTV-SDEs) is the appropriate tool connecting motion planning and probabilistic inference.

The follow-up GPMP2 [14] uses factor graphs to model the planning problem, and supports fast replanning by making use of an incremental Bayes tree solver [10].

Following them, we use Gaussian process for path planning, and we theoretically investigate the connection between this planning method and the well-known jerk optimal solution [19].

Iii System Overview

Fig. 2: Overview of the proposed trajectory planning framework and its relationship with other system components.

Fig. 1 shows a typical scenario this paper tries to deal with. Fig. 2 outlines the proposed framework and its relationship with other system components. It takes the trajectory prediction results, high-level behavior command and lane information as input, then generates collision free, executable trajectories.

The behavior means semantic level commands, such as “lane change”.

The trajectory planning process contains three major steps as depicted in Fig. 2 (blue part). First, A GP-based path planner generates a path in the Frenét frame. Second, the speed planning module finds an optimal speed-profile along that path with the efficient s-t graph search. Finally, when the result trajectory fails the feasibility check, we adjust the current path with the incremental refinement module. These three steps will be explained in Sect. IV, V and VI respectively.

Iv Gaussian Process Path Planning

Iv-a GP Prior of the Frenét Path

We consider a path in the Frenét frame , where is the lateral offset and is the arc-length along the reference line. We define a prior distribution , where is the lateral state, denotes the derivative w.r.t. arc-length,

is the vector-valued mean function, and

is a matrix-valued covariance function. Given a set of longitudinal locations ,

has a joint Gaussian distribution:


where is the mean vector, and is the covariance matrix , which are defined as


Following [15] and [14], and are generated by LTV-SDEs (arc-length as the parameter here):


where and are the arc-length varying matrices for the system, is the system input. is the white process noise, is the power spectral matrix. The mean and covariance function are generated by the solution of (4):


where and are the initial mean and covariance of , and is the state transition matrix. The prior distribution of the lateral states represented in terms of and is


This particular group of GPs generated by LTV-SDE has a special and helpful property: only depends on its adjacent values and . can be efficiently computed by


In the rest of this paper, we will use the white noise on jerk model (WNOJM) for (

4), whose and have a relatively simple form. See Appendix -A for details.

Iv-B Path Planning via Probabilistic Inference

To illustrate how to convert the path planning into a probabilistic inference problem, we consider a simple case first. Assume we have a linear “observer” that observes the lateral states:


where is a positive definite diagonal matrix. Together with (1), we have the joint Gaussian distribution:


where is a matrix that makes the matrix size meet; is a vector that contains observations,


and the posterior is


Suppose we want to plan a path from the initial state to end state . Let , and be a sufficiently small value. The mean of the posterior (16

) is the desired lateral state, and middle points are interpolated with (

9). Next, we will prove that this path is actually the jerk optimal path given that .

Definition IV.1

A jerk optimal path is a connection between the start state and an end state within the interval that minimizes the cost function:

Theorem IV.1

Let and . Then the result path given by (9) and (16) is the jerk optimal path connecting and , provided that .

See Appendix -B. Theorem IV.1 implies that the general path generated by (16) is actually piecewise quintic polynomials, and thus this approach is a good generalization of the known jerk optimal solution [19].

Iv-C Differentiable Path Constraints

Fig. 3: An example of collision constraint with two bounding circles. is the distance from to , is the radius of the circles and is the angle between and .

Two major constraints need to be considered in path planning: collision avoidance and curvature constraints.

1) Collision Constraint. As shown in Fig. 3, is a path point of the desired path , and it normally represents the center of the rear axle or the mass of the AV. To enforce the differentiable collision constraint, we cover the AV with several bounding circles . We approximated the Frenét coordinates of with


where and is obtained by solving


where is the curvature of the reference point. Then the representation of collision constraint is


where are static obstacles or road boundaries, is the safety margin, and is a function that returns the nearest distance to . We exploit the Euclidean Signed Distance Field (ESDF) for the distance query. As depicted by Fig. 4, the lane is discretized by a longitudinal resolution and a lateral resolution . Static obstacles are projected w.r.t. the reference line.

Fig. 4: Illustration of discretization of the lane w.r.t. the reference line (red line in the figure) and projection of the static obstacles.

2) Curvature Constraint. Constraining the maximum curvature of the path is crucial for large turning maneuver such as collision avoidance and sharp turning. It would be risky if the planned path cannot be followed by the vehicle. However, this constraint is seldom addressed in existing literatures. Firstly, it is difficult to obtain explicit representation of the curvature using time-parametrized continuous curve [4]. Besides, when describe the path with a set of discrete points [22], one can only approximate the curvature using finite difference, which requires fine discretization.

The main advantage of the proposed continuous arc-length parametrized path is that we have the closed-form representation of the curvature of any path point in the path, which is a function of the lateral states. From [19], we have


where and are the curvature and derivative of the curvature of the reference point. By rearranging terms, we have the explicit formulation of the curvature:


Finally, the curvature constraint is represented as


Iv-D Maximum a Posteriori Solution

Path constraints are modeled as the likelihood function

, which specifies the probability of being collision free

or within the curvature limit , given the current lateral state . We use a distribution of the exponential family to represent the likelihood:


where is a vector-valued cost function and

is a hyperparameter. For the likelihood function of the collision constraint

, is defined as


where is a second-order continuous penalty function (the continuity can be checked by directly taking derivatives) given by


where , is a parameter that specifies the boundary, and is a normalization factor. For the likelihood function of curvature constraint , is defined similarly:


where is basically the same as , except that it is two-side bounded since the curvature is allowed to be in the range .

With path constraints, it is unlikely to obtain a full posterior as Sect. IV-B does. Fortunately, for path planning, we only care about the maximum a posteriori (MAP) solution:


where .

V Efficient Speed-Profile Generation

(a) Expansion and local trauncation
(b) Overtaking scenario
(c) s-t graph of the overtaking scenario
Fig. 5: Illustration of the efficient s-t graph search algorithm. (a) Results of forward expansion for two rounds. Gray lines represent the discarded states during local truncation. (b) An illustration scenario where an agent is trying to overtake the AV. (c) The corresponding s-t search results. The blue area in the figure represents the blocked region, the gray lines are the intermediate results of the s-t graph search, and the red line is the optimal speed-profile.

In this section, we assume that the AV’s acceleration limit are and , the initial longitudinal state is , predicted future trajectories of dynamical agents are available, and a desired path generated by Sect. IV is already given. With a little abuse of notation, in this section represents the arc-length along the desired path.

V-a Efficient S-t Graph Search

In speed planning, local minimums inevitably exist. For example, in a merging scenario, the AV can choose to either decelerate to yield or accelerate to overtake the human driver. To escape from the local minimum and ensure safety, a searching, or sampling step is necessary. As outlined in Algorithm 1, our proposed s-t graph search algorithm consists of three main steps: trajectory projection, forward expansion, and local truncation.

1) Trajectory Projection. A dynamical agent is critical if its future trajectory has possible intersections with the AV’s desired path. For each critical agent, a sequence of trajectory points are sampled within time interval . Then we check at every trajectory point whether the bounding box of the agent at that position has an overlap with the desired path. If so, a blocked region is marked in the s-t graph, by finding its projection w.r.t. the desired path.

2) Forward Expansion. Starting with the initial state , we perform forward simulation with a set of discretized control inputs for time interval using the constant acceleration model:


During the expansion, any child state, which falls into the blocked regions, will be abandoned. After expansion, each survived child state will repeat that process until reaching the maximum planning duration or path length. Each state has a corresponding cost, which is defined as


where is the cost of the parent state, is the control cost, is the deviation from the reference velocity, is the cost of being close to blocked regions, and are weights.

3) Local Truncation. As shown in Fig. ((a)a), the number of states will grow exponentially during the expansion, which would be problematic for real-time implementation. To reduce the computation costs, we propose a local truncation mechanism. In the -th round of expansion, the longitudinal position of an arbitrary child state is


where is the control input in the -th round. (30) indicates that child states with similar are near to each other in the s-t graph, thus contributing little to the exploration of the free space. Based on this observation, in each round, the child states within radius will be clustered into groups. In each group, only the state with the minimum cost will be used for the next round. This can be viewed as the balance between exploration and exploitation. As Fig. ((a)a) depicted, the number of states is largely reduced after the local truncation.

Fig. ((b)b) shows a scenario where an agent is trying to overtake the AV, and Fig. ((c)c) shows the corresponding s-t search results. The search result indicates that the AV decides to yield to the human driver.

1:Notation: a given path , predicted trajectories , s-t graph , states , forward steps
2:Initialize: ,
3: ProjectTrajectory(, ), Node()
4:for  do
5:     for  do
7:         for  do
8:               ForwardExpansion()
9:              if NotBlocked(then
10:                  CalculateCost()
12:              end if
13:         end for
14:     end for
15:      LocalTruncation()
16:end for
Algorithm 1 Efficient s-t graph search

V-B S-t Curve Smoothing

Although we efficiently find a speed-profile through Algorithm 1, we implicitly assume that the acceleration changes instantaneously during the forward expansion. To improve the driving comfort, a smoothing step is desired. We fit the coarse speed-profile with a piecewise Bézier curve and construct the overall problem as a quadratic programming (QP). This approach has been extensively studied,

and we refer interested readers to [4] for details.

Particular attention should be paid to the speed constraint. The maximum speed is determined by two elements. One is the speed limit of the road . The other is derived from the lateral acceleration constraint:


where is the maximum lateral acceleration, and is the curvature of the path. However, since we do not know

in advance, we can only estimate

using the initial speed-profile. This approximation is sometimes problematic, and we will discuss it further in Sect. VI.

Vi Incremental Refinement

As aforementioned in Sect. V-B, the final trajectory may violate the lateral acceleration constraint if we overestimate (31). Existing solutions mostly adopt an iterative optimization approach, which is computationally expensive. Actually, since violations are usually sparse and local, one would naturally ask whether it is necessary to resolve the whole problem in each iteration as the majority of the trajectory is valid and unchanged. Motivated by [14], we propose to iteratively refine the trajectory in an incremental manner.

Vi-a Factor Graph and Bayes Tree

A factor graph [13] is a bipartite graph that represents the factorization of a function. It is known that performing inference on a factor graph is equivalent to solving the MAP problem (27). Fig. 6a shows an example factor graph of the MAP problem (27), where factors represent priors and likelihood functions. Through variable elimination, a factor graph is converted to a Bayes tree [9], which is an efficient tool for incremental inference. Fig. 6b shows the corresponding Bayes tree of Fig. 6a with an elimination order from the first state to the last state. One important property is that changes in the factor graph only affect parts of the Bayes tree. See [10] for more details.

Vi-B Incremental Trajectory Refinement

As outlined in Algorithm 2, in the initialization step, the path and speed profile are generated using the methods described in Sect. IV and Sect. V respectively. In each iteration, trajectory points are sampled at time stamp . If the lateral acceleration of the trajectory point exceeds the allowable value , a new factor is added to the factor graph, which constrains the maximum lateral acceleration of the path at . The lateral acceleration is estimated using the speed profile of the last iteration with


where is interpolated using (9) with adjacent states.

When the Bayes tree updates, only the parts of path associated with the newly added factors is updated. Thereafter, we regenerate the speed profile for the new path. This process is repeated until all constraints are satisfied or the maximum number of iterations is reached.

Fig. 6: An example factor graph and Bayes tree. When a lateral acceleration factor is added to the graph, only part of the Bayes tree (dashed orange box in (b)) will be affected.
4:for k in  do
6:     for  in  do
8:         if  then
10:         end if
11:     end for
12:     if  then break
13:     end if
16:end for
Algorithm 2 Incremental trajectory refinement

Vii Experimental Results

Vii-a Implementation Details

We initialize the GP path planner by solving a simplified problem where constraints are ignored. This step is necessary for almost all path planners, since we have to decide on which side (left or right) to pass by the obstacles. The total length of the planning path is and we uniformly sample longitudinal locations along the reference line. Between and , we add 10 more interpolated lateral states using (9) for collision and curvature constraints. The ESDF () is built with a resolution in each planning cycle. For speed planning, the control limits of the AV are set to and . We use , and in s-t the graph search. The proposed method111Source code will be released at is implemented in C++, and we use GTSAM [3] to implement Algorithm 2. All the experiments are conducted on a desktop computer with an Intel I7-8700K CPU (3.7 GHz).

Vii-B Qualitative Results

(a) t=11.60s, adding obstacle to lane
(b) t=13.86s, decelerating to yield
(c) t=17.04s, accelerating to pass
(d) t=11.60s
(e) t=13.86s
Fig. 7: Illustration of obstacle avoidance in dynamic environments. (d) and (e) show the corresponding s-t graphs of (a) and (b).

We validate our method in the CARLA [5] simulation environment. The predicted trajectories are generated by the constant velocity model for simplicity, but our method does not depend on any particular prediction module.

Fig. 7 depicts a scenario where the AV has to avoid an obstacle in front of it while there are other dynamical agents around. The AV decelerates and yields to other agents to avoid a potential risky interaction near the obstacle. This case shows our proposed path planner’s capability of obstacle avoidance and effectiveness of the s-t graph search algorithm. More illustrative scenarios, including consecutive obstacle avoidance, navigating in dense traffic, lane changing at high speed and sharp turning manuever can be found in the supplementary video at

Planning Steps
Time GP Path Planning Speed Profile Generation Iterative Refinement Total
Min(ms) 2.36 1.10 2.03a 5.56b 6.06a 10.09b
Avg.(ms) 6.09 3.77 6.41a 16.47b 17.22a 27.28b
Max.(ms) 22.78 6.99 29.96a 71.89b 57.73a 101.66b
  • Incremental version

  • Non-incremental version

TABLE I: Time Consumption of Different Planning Steps in One Cycle
G3P-Cur (ours) DL-IAPS[22] TDR-OBCA[8]
Successa Rate (%) 98.90 55.40 97.40
Avg. Time (ms) 43.78b 154.15 1466.32
Max. Time (ms) 55.16 179.70 2288.5
  • means collision-free and the violation of the curvature constraint is within 5%.

  • Avg. time is larger than Table I since tasks here are much more challenging.

TABLE II: Results For Random Planning Tasks

Vii-C Quantitative Results

1) Real-time Performance: To show the computational efficiency of our proposed method, we let the AV randomly navigate in Town03 of CARLA with dynamical agents and record the computation time for 20 minutes. Static obstacles are randomly added to the AV’s driving lane every . We also implement a non-incremental version of Algorithm 2 for comparison. As shown in Table I, our method runs above for most of the time. The maximum value only occurs in some extreme circumstances, for example, avoiding an obstacle in a sharp turn. The incremental version is nearly 2.5 times faster than the non-incremental version in the iterative refinement step.

2) Curvature Constraint: To examine our proposed curvature constraint (22), we randomly generate 1000 challenging planning tasks. We implement two versions of the GP path planner – (a) G3P: without curvature constraints, and (b) G3P-Cur: with curvature constraints. Two open-source version of the SOTA planners are used for comparison: (c) DL-IAPS [22], an approach based on iterative path-speed refinement; (d) TDR-OBCA [8], an approach based on optimal control. We feed DL-IAPS and TDR-OBCA with the result of G3P since they require a collision-free initial path as the warm start. We adopt the curvature limit used in DL-IAPS. The benchmark results are summarized in Table II and Fig. (a)a shows an example. From the results, we see that our method outperforms other SOTA planners both in success rate and efficiency.

(a) Curvature constraint
(b) Lateral acceleration constraint
Fig. 8: (a) The AV has to avoid three random placed obstacles consecutively in a narrow space and should not violate the curvature constraint. (b) Resulting path and lateral acceleration in a lane changing scenario at a speed of 17.5 m/ after four iterations.

3) Lateral Acceleration Constraint: Fig. (b)b shows the results of Algorithm 2 in a lane change scenario at a speed of 17.5 with a lane width. The maximum lateral acceleration and deceleration are and respectively. The trajectory of iteration is not feasible, and it can be viewed equivalently to the result of the seminal work [19], as mentioned in IV-B. After four iterations, the lateral acceleration is limited to the acceptable range.

Viii Conclusions

In this paper, we presented a real-time, kinodynamic trajectory planning framework in dynamic environments. We introduced a GP path planner that generates a collision-free path under curvature constraints, and outperforms the SOTA method. An efficient s-t graph search method with a local truncation mechanism was also proposed to enable fast speed-profile generation. Kinodynamic feasibility is guaranteed by the novel incremental refinement scheme. Our method was examined with various numerical experiments.

The main limitation of this work is that the uncertainties are not well modeled (e.g., prediction uncertainty). However, we believe that planning as probabilistic inference is a promising framework that can systematically deal with uncertainties, and this remains for future work.

-a White-noise on jerk model

For (4) with WNOJM, we have and


Let , and the state transition matrix is computed by


From (12), we have


-B Proof of Theorem iv.1

As already proved by [19], we know that a quintic polynomial is the jerk optimal connection between two states and . Then we only have to prove that the result path given by (9) and (16) does connect and , and it is indeed a quintic polynomial. By rearranging the posterior mean of (16), we have


Since ,

becomes the identity matrix and (

36) simplifies to


Using , we have , which gives


Next, we will prove that (9) is quintic Hermite interpolation. By using the property of the state transition matrix and (33), (9) simplifies to


Expanding it with (34) and (35), it is easy to verify that (39) is precisely the quintic Hermite interpolation; i.e., the result path is a quintic polynomial.


  • [1] Z. Ajanovic, B. Lacevic, B. Shyrokau, M. Stolz, and M. Horn (2018) Search-based optimal motion planning for automated driving. In 2018 IEEE/RSJ Int. Conf. Intell. Rob. Sys. (IROS), pp. 4523–4530. Cited by: §I-A.
  • [2] B. Brito, B. Floor, L. Ferranti, and J. Alonso-Mora (2019) Model predictive contouring control for collision avoidance in unstructured dynamic environments. ieee_j_ral 4 (4), pp. 4459–4466. Cited by: §I-A.
  • [3] F. Dellaert (2012) Factor graphs and GTSAM: a hands-on introduction. Technical report Georgia Institute of Technology. Cited by: §VII-A.
  • [4] W. Ding, L. Zhang, J. Chen, and S. Shen (2019) Safe trajectory generation for complex urban environments using spatio-temporal semantic corridor. ieee_j_ral 4 (3), pp. 2997–3004. External Links: Document Cited by: §I-A, §IV-C, §V-B.
  • [5] A. Dosovitskiy, G. Ros, F. Codevilla, A. Lopez, and V. Koltun (2017) CARLA: An open urban driving simulator. In Proc. 1st Annu. Conf. Rob. Learn. (CoRL), pp. 1–16. Cited by: §VII-B.
  • [6] H. Fan, F. Zhu, C. Liu, L. Zhang, L. Zhuang, D. Li, W. Zhu, J. Hu, H. Li, and Q. Kong (2018) Baidu apollo EM motion planner. arXiv preprint arXiv:1807.08048. Cited by: §I-A.
  • [7] T. Gu, J. Atwood, C. Dong, J. M. Dolan, and J. Lee (2015) Tunable and stable real-time trajectory planning for urban autonomous driving. In 2015 IEEE/RSJ Int. Conf. Intell. Rob. Sys. (IROS), pp. 250–256. Cited by: §I-A.
  • [8] R. He, J. Zhou, S. Jiang, Y. Wang, J. Tao, S. Song, J. Hu, J. Miao, and Q. Luo (2021) TDR-OBCA: a reliable planner for autonomous driving in free-space environment. In 2021 Ame. Control Conf. (ACC), pp. 2927–2934. Cited by: §I-A, §VII-C, TABLE II.
  • [9] M. Kaess, V. Ila, R. Roberts, and F. Dellaert (2010) The Bayes tree: an algorithmic foundation for probabilistic robot mapping. In Algoritm. Found. Rob. IX, pp. 157–173. Cited by: §VI-A.
  • [10] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert (2012) ISAM2: incremental smoothing and mapping using the Bayes tree. Int. J. Rob. Res. 31 (2), pp. 216–235. Cited by: §II-A, §II-B, §VI-A.
  • [11] N. R. Kapania, J. Subosits, and J. Christian Gerdes (2016) A sequential two-step algorithm for fast generation of vehicle racing trajectories. J. Dyn. Sys., Meas., Control 138 (9). Cited by: §II-A.
  • [12] C. Katrakazas, M. Quddus, W. Chen, and L. Deka (2015) Real-time motion planning methods for autonomous on-road driving: state-of-the-art and future research directions. Transp. Res. Part C Emerg. Technol. 60, pp. 416–442. Cited by: §I-A.
  • [13] F.R. Kschischang, B.J. Frey, and H.-A. Loeliger (2001) Factor graphs and the sum-product algorithm. ieee_j_it 47 (2), pp. 498–519. External Links: Document Cited by: §VI-A.
  • [14] M. Mukadam, J. Dong, X. Yan, F. Dellaert, and B. Boots (2018) Continuous-time Gaussian process motion planning via probabilistic inference. Int. J. Rob. Res 37 (11), pp. 1319–1340. Cited by: §I-A, §II-B, §IV-A, §VI.
  • [15] M. Mukadam, X. Yan, and B. Boots (2016) Gaussian process motion planning. In 2016 IEEE Int. Conf. Rob. Autom. (ICRA), pp. 9–15. Cited by: §I-A, §II-B, §IV-A.
  • [16] W. Sun, G. Tang, and K. Hauser (2021) Fast uav trajectory optimization using bilevel optimization with analytical gradients. ieee_j_ro (), pp. 1–15. External Links: Document Cited by: §II-A.
  • [17] G. Tang, W. Sun, and K. Hauser (2019) Time-optimal trajectory generation for dynamic vehicles: a bilevel optimization approach. In 2019 IEEE/RSJ Int. Conf. Intell. Rob. Sys. (IROS), pp. 7644–7650. Cited by: §II-A.
  • [18] Z. Wang, X. Zhou, C. Xu, J. Chu, and F. Gao (2020) Alternating minimization based trajectory generation for quadrotor aggressive flight. ieee_j_ral 5 (3), pp. 4836–4843. Cited by: §II-A.
  • [19] M. Werling, S. Kammel, J. Ziegler, and L. Gröll (2012) Optimal trajectories for time-critical street scenarios using discretized terminal manifolds. Int. J. Rob. Res. 31 (3), pp. 346–359. Cited by: §-B, §I-A, §II-B, §IV-B, §IV-C, §VII-C.
  • [20] W. Xu, Q. Wang, and J. M. Dolan (2021) Autonomous vehicle motion planning via recurrent spline optimization. In 2021 IEEE Int. Conf. Rob. Autom. (ICRA), pp. 7730–7736. Cited by: §II-A.
  • [21] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen (2019) Robust and efficient quadrotor trajectory generation for fast autonomous flight. ieee_j_ral 4 (4), pp. 3529–3536. External Links: Document Cited by: §II-A.
  • [22] J. Zhou, R. He, Y. Wang, S. Jiang, Z. Zhu, J. Hu, J. Miao, and Q. Luo (2020) Autonomous driving trajectory optimization with dual-loop iterative anchoring path smoothing and piecewise-jerk speed optimization. ieee_j_ral 6 (2), pp. 439–446. Cited by: §I-A, §II-A, §IV-C, §VII-C, TABLE II.