gpir
None
view repo
Realtime 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 pathspeed optimization. Exploiting the decoupled structure of the planning problem, a path planner based on Gaussian process first generates a continuous arclength 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 wellknown jerk optimal solution. An efficient st 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 opensource package.
READ FULL TEXT VIEW PDFNone
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 realtime, which is challenging.
Existing works can be categorized into two main classes: spatiotemporal planning [1, 2, 4, 8] and pathspeed decoupled planning [7, 6, 22]. Spatiotemporal (st) approaches mostly adopt a hierarchy scheme. They first find an initial solution in st 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 recedinghorizon 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 spatiotemporal space is nontrivial, 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. Pathspeed 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 st 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 pathspeed 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 wellknown jerk optimal solution [19]. To avoid local optimum, a novel and efficient st graph search method is introduced. Finally, an incremental pathspeed 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 highquality kinodynamically feasible trajectories. The main contributions are summarized as follows:
A path planner incorporating GP that generates a collisionfree 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 opensource realtime implementation for the reference of the community.
The online iterative pathspeed 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 Bspline trajectory to achieve dynamic feasibility for a quadcopter, while [18] and [16] use a similar bilevel optimization approach to get spatiotemporal optimal trajectories in realtime. However, this technique is less commonly used for AVs. The main reason is that various nonlinearities (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 twostep algorithm to generate the trajectory for car racing with near realtime performance. Similarly, Tang et al. [17] apply bilevel optimization for time optimal car racing. However, there are only applicable for static, obstaclefree 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.
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, timevarying stochastic differential equations (LTVSDEs) is the appropriate tool connecting motion planning and probabilistic inference.
The followup 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 wellknown jerk optimal solution [19].
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, highlevel 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 GPbased path planner generates a path in the Frenét frame. Second, the speed planning module finds an optimal speedprofile along that path with the efficient st 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 arclength along the reference line. We define a prior distribution , where is the lateral state, denotes the derivative w.r.t. arclength,
is the vectorvalued mean function, and
is a matrixvalued covariance function. Given a set of longitudinal locations ,has a joint Gaussian distribution:
(1) 
where is the mean vector, and is the covariance matrix , which are defined as
(2)  
(3) 
Following [15] and [14], and are generated by LTVSDEs (arclength as the parameter here):
(4)  
(5) 
where and are the arclength 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):
(6)  
(7) 
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
(8) 
This particular group of GPs generated by LTVSDE has a special and helpful property: only depends on its adjacent values and . can be efficiently computed by
(9)  
(10)  
(11)  
(12) 
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.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:
(13) 
where is a positive definite diagonal matrix. Together with (1), we have the joint Gaussian distribution:
(14) 
where is a matrix that makes the matrix size meet; is a vector that contains observations,
(15) 
and the posterior is
(16)  
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
(17) 
where and is obtained by solving
(18) 
where is the curvature of the reference point. Then the representation of collision constraint is
(19) 
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 timeparametrized 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 arclength parametrized path is that we have the closedform representation of the curvature of any path point in the path, which is a function of the lateral states. From [19], we have
(20) 
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:
(21) 
Finally, the curvature constraint is represented as
(22) 
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:(23) 
where is a vectorvalued cost function and
is a hyperparameter. For the likelihood function of the collision constraint
, is defined as(24) 
where is a secondorder continuous penalty function (the continuity can be checked by directly taking derivatives) given by
(25) 
where , is a parameter that specifies the boundary, and is a normalization factor. For the likelihood function of curvature constraint , is defined similarly:
(26) 
where is basically the same as , except that it is twoside bounded since the curvature is allowed to be in the range .
With path constraints, it is unlikely to obtain a full posterior as Sect. IVB does. Fortunately, for path planning, we only care about the maximum a posteriori (MAP) solution:
(27)  
where .
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 arclength 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 st 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 st 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:
(28) 
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
(29) 
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 realtime 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
(30) 
where is the control input in the th round. (30) indicates that child states with similar are near to each other in the st 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 speedprofile 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 speedprofile 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:
(31) 
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 speedprofile. This approximation is sometimes problematic, and we will discuss it further in Sect. VI.As aforementioned in Sect. VB, 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.
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.
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
(32) 
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 st the graph search. The proposed method^{1}^{1}1Source code will be released at https://github.com/jchengai/gpir 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 I78700K CPU (3.7 GHz).





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 st 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.
Planning Steps  

Time  GP Path Planning  Speed Profile Generation  Iterative Refinement  Total  
Min(ms)  2.36  1.10  2.03^{a}  5.56^{b}  6.06^{a}  10.09^{b} 
Avg.(ms)  6.09  3.77  6.41^{a}  16.47^{b}  17.22^{a}  27.28^{b} 
Max.(ms)  22.78  6.99  29.96^{a}  71.89^{b}  57.73^{a}  101.66^{b} 
Incremental version
Nonincremental version
G3PCur (ours)  DLIAPS[22]  TDROBCA[8]  

Success^{a} Rate (%)  98.90  55.40  97.40 
Avg. Time (ms)  43.78^{b}  154.15  1466.32 
Max. Time (ms)  55.16  179.70  2288.5 
means collisionfree 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) Realtime 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 nonincremental 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 nonincremental 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) G3PCur: with curvature constraints. Two opensource version of the SOTA planners are used for comparison: (c) DLIAPS [22], an approach based on iterative pathspeed refinement; (d) TDROBCA [8], an approach based on optimal control. We feed DLIAPS and TDROBCA with the result of G3P since they require a collisionfree initial path as the warm start. We adopt the curvature limit used in DLIAPS. 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 [19], as mentioned in IVB. After four iterations, the lateral acceleration is limited to the acceptable range.
In this paper, we presented a realtime, kinodynamic trajectory planning framework in dynamic environments. We introduced a GP path planner that generates a collisionfree path under curvature constraints, and outperforms the SOTA method. An efficient st graph search method with a local truncation mechanism was also proposed to enable fast speedprofile 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 [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
(36) 
Since ,
becomes the identity matrix and (
36) simplifies to(37) 
Using , we have , which gives
(38) 
Next, we will prove that (9) is quintic Hermite interpolation. By using the property of the state transition matrix and (33), (9) simplifies to
(39) 
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.