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.READ FULL TEXT VIEW PDF
The trajectory planning system is crucial for autonomous driving tasks . 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.
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 . 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.
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.
The online iterative path-speed refinement technique is widely adopted in the unmanned aerial vehicle (UAV) community. For example, Zhou et al.  iteratively adjust the infeasible velocity and acceleration control points of the B-spline trajectory to achieve dynamic feasibility for a quadcopter, while  and  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.  use a two-step algorithm to generate the trajectory for car racing with near real-time performance. Similarly, Tang et al.  apply bilevel optimization for time optimal car racing. However, there are only applicable for static, obstacle-free racetracks.
Xu et al.  use recurrent spline optimization to ensure curvature constraints, but do not consider dynamic feasibility when speed changes. Recent work  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  from the SLAM community, instead of generating a new path from scratch.
Recent progress on motion planning via probabilistic inference has opened a new window onto the motion planning problem. Mukadam et al.  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.
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 .
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.
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, andis 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
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
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 .
A jerk optimal path is a connection between the start state and an end state within the interval that minimizes the cost function:
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.
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 . Besides, when describe the path with a set of discrete points , 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 , 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
Path constraints are modeled as the likelihood function
, which specifies the probability of being collision freeor 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:
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.
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.
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  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 estimateusing the initial speed-profile. This approximation is sometimes problematic, and we will discuss it further in Sect. VI.
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 , we propose to iteratively refine the trajectory in an incremental manner.
A factor graph  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 , 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  for more details.
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.
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 https://github.com/jchengai/gpir is implemented in C++, and we use GTSAM  to implement Algorithm 2. All the experiments are conducted on a desktop computer with an Intel I7-8700K CPU (3.7 GHz).
We validate our method in the CARLA  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 https://youtu.be/NHEZDrAzghI.
|Time||GP Path Planning||Speed Profile Generation||Iterative Refinement||Total|
|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.
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 , an approach based on iterative path-speed refinement; (d) TDR-OBCA , 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.
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 , as mentioned in IV-B. After four iterations, the lateral acceleration is limited to the acceptable range.
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.
As already proved by , 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
Using , we have , which gives