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

12/29/2020 ∙ by Yuchen Sun, et al. ∙ 0

Trajectory planning is a fundamental task on various autonomous driving platforms, such as social robotics and self-driving cars. Many trajectory planning algorithms use a reference curve based Frenet frame with time to reduce the planning dimension. However, there is a common implicit assumption in classic trajectory planning approaches, which is that the generated trajectory should follow the reference curve continuously. This assumption is not always true in real applications and it might cause some undesired issues in planning. One issue is that the projection of the planned trajectory onto the reference curve maybe discontinuous. Then, some segments on the reference curve are not the image of any part of the planned path. Another issue is that the planned path might self-intersect when following a simple reference curve continuously. The generated trajectories are unnatural and suboptimal ones when these issues happen. In this paper, we firstly demonstrate these issues and then introduce an efficient trajectory generation method which uses a new transformation from the Cartesian frame to Frenet frames. Experimental results on a simulated street scenario demonstrated the effectiveness of the proposed method.



There are no comments yet.


page 1

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Trajectory planning is used to create a safe and smooth trajectory for an agent (e.g., a robot or a self-driving car) from one state (including the location, the velocity, the acceleration, the orientation and so on) to another state under certain constraints or cost functions [18]. There are many ways to plan a trajectory for an intelligent agent and it is a common practice to use a reference curve, which is generated off-line, as the target to find the output trajectory. In this way, the trajectory planning algorithm only need to follow the given reference curve and does not require the information of map like signs, turns and so on. To achieve optimality and time efficiency, many trajectory planning algorithms [5, 16, 13] are developed in Frenet frames with time to reduce the planning dimension with the help of a reference curve. In a Frenet frame, finding the optimal trajectory is essentially a three dimensional constrained optimization problem.

There are roughly two types of trajectory planning algorithms: the direct methods [19, 12] that use trajectory sampling or lattice search to find the optimal trajectory and the path-speed decoupled methods [15, 4] that optimize path and speed separately. In both of the methods, all obstacles and environment information are projected onto the lane center based Frenet frames for each candidate lane, and the drivable area is bounded by lane boundaries. When using the Frenet frame, a projection operator which identifies the corresponding projection point on the reference curve is always required and used as the transformation from the Cartesian frame to Frenet frames. The most widely used projection operator is defined as the nearest point on the reference curve, which is also the orthogonal projection point. Given a reference curve, there are two sub-tasks in trajectory planning that a projection operator is necessary, which are:

  • Reference curve following: One has to determine whether a candidate trajectory is actually following the lane by checking its projection on the reference curve.

  • Trajectories generation: One need to generate as many as possible candidate trajectories that follows the reference curve for subsequent planning and optimization.

However, under certain circumstances, the projection of a candidate trajectory onto the reference curve maybe discontinuous [16, 13] , which is commonly ignored in real applications. Some continuous segments on the reference curve could not be mapped back onto the generated trajectory and make the first sub-task difficult. Moreover, trajectories generated by previous methods might self-intersect when continuously following a simple reference curve. The generated trajectories might be unnatural and suboptimal trajectories for the second sub-task. These discussed issues motivate us to develop novel a trajectory generation method that could produce reasonable candidate trajectories under most of the circumstances. In this paper, based on our theoretical result on trajectory following, the derived method could generate trajectories that guarantee smooth, anomaly free, and follow the reference curve always.

Fig. 1: An illustration of the transformation from Cartesian frame to Frenet frames

Ii Related Work

The past decades have witnessed a great progress in trajectory planning. Many researches have been proposed and studied [16], [11, 17, 7] that they can generate global trajectories connecting a start and a possibly distant end state. Some approaches for trajectory planning follow a discrete sampling or a road map-based scheme [14] [9], which firstly sample multiple rows of points and then generate a finite set of trajectories with cost values, typically by differential functions that describe vehicle dynamics and kinematics. A candidate trajectory is selected if and only if it corresponds to the minimal cost value.

Kelly and Nagy [10] proposed an efficient path planning algorithm which employs polynomials to ensure the smooth variation of the curvature. Based on this method, a new approach proposed in [12]

firstly samples points along the reference lane and then connects the sampled points using polynomials. In this way, all generated candidate trajectories conform to the lane shape. However, these methods use Cartesian coordinate system rather than the Frenet frames. Therefore, the planning dimension is relatively high and the optimization is complicated.

To address the issue, Werling et al. [16] used the lane center line based Frenet frames to decouple the lateral and longitudinal motion. This strategy makes the reference curve coincide with the lane shape. Based on Frenet frame, the generation of the lateral and longitudinal motions can be achieved separately by using quintic polynomials versus time, which ensures continuous acceleration. Given the advantages of the Frenet frames, many trajectory planning methods [13, 6] are proposed based on it and achieve a roaring success in addressing various planning problems in real applications. However, most of the methods ignore an intrinsic problem in the transformation from Cartesian frame to Frenet frames, which may cause unnatural and suboptimal trajectories for the downstream tasks. In this paper, based on our theoretical result on trajectory following, the proposed method could generate candidate trajectories following the reference curve and guarantee smoothness and anomaly free.

Fig. 2: The representation of a candidate trajectory in a Frenet frame

Iii Preliminaries and Our Theoretical Results

(a) discontinuous
(b) self-interact
Fig. 3: The issues of discontinuous projection operator p, (a) multiple outputs for a single , and the projection of onto is discontinuous, (b) a suboptimal trajectory when following the reference curve faithfully. is a candidate trajectory and is the reference curve

Iii-a The Frenet Frame

A reference curve is always defined by parametric curves [1][2] such as the spline curve. When (or for short) is the modular length of time

, we assume the vector function

denotes a moving point on the reference curve in Euclidean space. Let and are the unimodular tangent vector and normal vector at , respectively. Then, formulates a dynamic orthogonal coordinate system. For any Cartesian coordinates , assuming is the root point of , we have


where and are the horizontal and perpendicular offsets, respectively. And the frenet coordinates of can be denoted by . Figure 1 is an example of a Frenet frame. To simplify the expression, one common practice is to use the nearest point to on as the root point, such that . Then the Frenet coordinate of can be denoted by and the corresponding projection operator p of onto is . That is to say, the projector satisfies:


Iii-B The Projection Operator

Considering be the reference curve and the is a candidate trajectory of a moving agent, there are two implicitly ideal properties/desirable assumptions of the projection operator p for trajectory planning tasks [16], which are summarized as below.

  • p is a continuous and smooth projector such that the image (math) of a continuous segment on should be a continuous interval on .

  • The moving agent on the candidate trajectory should be actually following the reference curve , which can be mathematically represented as .

However, the definition of the projection operator p given in (2) is incomplete and the neither of the two desired properties is guaranteed in most of the previous motion planning studies. That means that it may have several outputs at some point when following the given reference curve . Fig. 3(a) presents a reference curve with a 90 degrees turn. When a virtual agent on moves into the turn with a reasonable large distance to the reference curve , the trajectory should be contained in the available set for optimization. If the projection operator p is continuous, the image of p should contain all segment between and . As can be seen, the point on is not the projection of any point from and this violates the assumption that p is continuous. Fig. 3(b) presents an example in path following. The reference curve has a rotation arc whose radius is . The virtual agent is following continuously with the constraint that the lateral distance is constant . When the agent moves from to , the direction of its movement on the candidate trajectory is opposite with the direction of projection point on the reference curve.

Iii-C Our Theoretical Results

To address the intrinsic issues of the definition of the projection operator p, the curvature is required to develop a novel transformation, to replace the original projection operator , from Cartesian coordinates to Frenet coordinates. Our method is based on the following theoretical result.

Theorem III.1

Let be the reference curve and be any point (an agent) on the candidate trajectory . is the nearest point on to and , then is perpendicular to the tangent and the agent is actually following at the same direction.

The theorem III.1 indicates that the projection point follows reference curve at the same direction if and only if . Moreover, will jump and be discontinuous on at these points where the curvature is large. If a method can handle the discontinuous points properly and guarantee , it can generate the smoothness of the generated trajectories which follow the reference curve at the same direction.

For the convenience the downstream optimizers, it is a common practice to discretize the reference curve in the form of vectors. The discretization is realized through point sampling on the reference curve. The sampling points on the curve is connected into a polygonal line, where the sampling points are sequenced in the order of their distance to one end point of the curve. The obtained polygonal line is used to represent the original curve. To avoid the intractable cases such as sharp angles and fractals, the following empirical conditions should be satisfied in the sampling process:

  • The distance between two adjacent samples should be extremely smaller than .

  • The angle between two adjacent sample points should not exceed 10.

  • The distances between every pair of adjacent samples are identical.

It should be noted that the polygonal line has finite length due to the limitations of sampling size. As a result, the obtained polygonal line that is built by line segments is not enough to ensure that the normal line at each point on the reference curve go through of it. To deal with this, the linear extension strategy [3] is adopted to extend the polygonal line. Specifically, the first and last segments on the polygonal line are extended to rays with infinite length.

0:  The polygonal line in the form of vectors: , which is formulated by sampling points from the reference curve ( denotes the sampling size); the Cartesian coordinates of a point .
0:  The Frenet coordinate of
1:  Compute the distances between and all points in .
2:  Identify all the points in that have the identical and the nearest distance to , which form a subset .
3:  Choose the point such that , i.e., the biggest index from subset .
4:  if  is not an end point then
5:      # where is the difference of the coordinates of two points
6:      # where is the inner product of two vectors
7:     if  then
9:     end if
10:     ()=AffineTrans # Algo 2
11:  else
12:     ()=ParallelTrans # Algo 3
13:  end if
14:  return  
Algorithm 1 The Projection Operator: TransF-
0:  The polygonal line in the form of vectors: ( denotes the sampling size); the index of the reference piece; The Cartesian coordinates of a point )
0:  The Frenet coordinate of
1:  Get the heading vector of the angular bisector at the junction .
2:  Get the heading vector of the angular bisector at the junction .
3:  if  is parallel to  then
4:     ()=
5:  else
7:     Calculate the intersection point of the two angular bisectors, which go through points and with headings and , respectively.
8:     Calculate the intersection point of line and .
10:      distance from to the line .
11:  end if
12:  return  ()
Algorithm 2 Affine Transform
0:  The polygonal line in the form of vectors: ( denotes the sampling size); the index of the reference piece; The Cartesian coordinates of )
0:  The Frenet coordinate of
1:  if then
2:     Denote as .
3:     Set .
4:     Denote as
5:  else
6:     Denote as
7:     Set
8:     Denote as
9:  end if
10:  Let be a nonzero orthogonal vector to such that .
11:  .
13:  return  ()
Algorithm 3 Parallel Transform

Iv The Proposed Method

In this section, we propose a new efficient trajectory generation method such that the generated candidate trajectories are smooth and guarantees to follow the reference curve at the same direction. The method is developed based on a new projection operator, TransF-, from the Cartesian frame to Frenet frames.

Iv-a Our Projection Operator: TransF-

Based on the theorem III.1 and sampling requirements, it is intuitive and natural to project a moving point onto the nearest point on the reference curve . However, there may be several nearest projection points with the identical distance. In our projection operator: TransF-, we choose the projection point with the biggest to ensure that the moving point is actually following at the same direction with , where denotes the distance on curve from the projection point to the start point on . Algorithm 1 shows the details of TransF-, the transformation from a Cartesian coordinates to the Frenet coordinate of an arbitrary point.

In Algorithm 1 lines 1-2, we first pick out all points on which are the nearest to and form a set containing all such points. Then we choose the last one, i.e. one whose index is the biggest. At line 3, This point with the biggest is used and the output trajectory is shorten. With the point , lines 5-9 decide A should be projected to the former or latter adjacent piece to . The condition at line 7 identifies which side should be projected on. The inverse image of two pieces are separated by the angular bisector at . Finally, lines 10-12 are used to calculate the projection point. The Affine Transform given in Algorithm 2 is applied on the pieces of without linear extension while the Parallel Transform given in Algorithm 3 is employed on the extended pieces of . One significant difference between these two operators is that the inverse image of common pieces is bounded by two angular bisectors while the extended pieces is bounded by only one angular bisector. The calculation approaches of the projection results are based on these two situations. The details of Affine Transform operator and Parallel Transform operator are given in Algorithm 2 and 3, respectively.

Further discussions about Algorithm 2 and 3 should be made for clarification. As can be seen, both Affine and Parallel Transform are approximation to the transform function Eq. (1) in Section III. The reason why we need the approximation methods are that the polygonal line as a sampled approximation for reference curve is no longer smooth and thus there is no curvature, tangent vector and so on. Therefore, two approximation operators are used to make the lateral distance vary continuously when the point moves cross an angular bisector. In other words, they guarantee that point on an angular bisector has the same projection point on no matter it is projected to the former or latter piece. However, since they are approximation methods, some error may occur. An apparent error is that the created coordinate system may not be orthogonal to the reference curve. That is why we introduce some sampling conditions which should be satisfied in Section III.C. Those conditions ensure that the approximation errors are small enough to be ignored and make them work well in real-world applications.

0:  The polygonal line in the form of vectors: ( denotes the sampling size); Vectors of the lateral boundaries at the sample points: , .
0:  A candidate trajectory consisted of vectors of points:
3:  while   do
5:     = randomly pick a number between and
8:  end while
9:  return  A candidate trajectory
Algorithm 4 A Commonly-used Continuous Candidate Trajectory Generation Method (May cause self-interaction!)
0:  An -order differentiable reference curve ; The polygonal line in the form of vectors: ; The candidate trajectory of points: which is generated by Algo 4.
0:  A renewed candidate trajectory from : =
3:  while   do
5:     Calculate = curvature of at ().
6:     if  then
8:     else
9:        Calculate the Euclidean coordinates of Frenet coordinates , which is denoted as .
10:        Calculate the Frenet coordinates of referring to , = TransF- (U,). # Algo1
13:     end if
14:  end while
15:  return  A renewed trajectory from
Algorithm 5 The Proposed Trajectory Generation Method

Iv-B Our Trajectory Generation Algorithm

Trajectory generation is a key step in trajectory planning in dynamic and unknown environments. The downstream trajectory optimizer [8] may fail to create an acceptable trajectory plan that obeys traffic rules and without misleading directions (such as self-interactions) when the input candidate trajectories are sloppy. Algorithm 4 is a common framework for trajectory generation. Many sophisticated generation methods can be extended from Algorithm 4. Without loss of generality, we choose Algorithm 4 as the basis of our generation method.

As discussed in Section III, Algorithm 4 and the related sophisticated generation methods will produce candidate trajectories with misleading directions and self-interaction when the curvature is large and the gap of the corridor boundaries is wide. Typical examples can be found in Fig. 3.

To ensure the candidate trajectory is actually following at the same direction as the reference curve, our method introduce the curvature that as an additional restriction condition to repair the generated trajectories by Algorithm 4. Algorithm 5 is our proposed trajectory generation method which uses the outputs of Algorithm 4.

The inputs of Algorithm 5 include the -order differentiable reference curve . The curvature at any points on can be calculated by , where and are the uni-modular tangent and normal vectors at on , respectively (the same notations as in Section III.A). The input of Algorithm 5 is an output of Algorithm 4, which might consist of misleading directions and self-interactions. The output of Algorithm 5 is a renewed candidate trajectory , which is repaired from . At the line 5 of algorithm 5, the curvature at the sampling point is calculated. If , it is safe to include the point into the candidate trajectory. Otherwise, at lines 9-12, the point have to be transformed into the Cartesian frame and then transformed back into the Frenet frame. Because several nearest points to might be identified in , is different with . In this way, the segment on reference curve with high curvature is jumped over and the obtained new point is safe to be included into .

V Simulation Study and Discussion

Fig. 4: The sub-figures shown in (a)-(d) illustrate a result of trajectory planning by Algorithm 4 . The sub-figures shown in (e)-(h) present a result of trajectory planning by Algorithm 5.

In this section, we create an artificial environment for trajectory planning. The generation algorithms 4 and 5 are used and compared in trajectory planning. High resolution video clips of these experimental results have been uploaded as supplement materials.

V-a Experimental settings

In our planning environment, an U shaped curved road is assumed and the lane-center curve is provided. Additionally, a static obstacle is placed near the corner of the road to mimic parking cars or road blockers. A moving agent is required to move from one end point of the road center to the other end point. The moving agent should follow the lane-center curve smoothly and avoid the obstacle.

The detailed trajectory planning consists of the following steps:

  • Implement candidate trajectory generation method multiple times to create many candidate trajectories. (Algorithm 4 or Algorithm 5)

  • Use the constrained planning optimizer to generate the final planed trajectory.

V-B Experimental results and discussions

We recorded the whole simulation process of the planning and movement of the moving agent in the form of videos. Screenshots of several representative time stamps from the videos are from collected and shown in Fig. 4. The blue curve denotes the lane center line and also the reference curve. The moving agent is represented as a triangle and its planned trajectory is shown as the red line. The obstacle is denoted as black cross marks.

Figs. 4(a)-(e) are the screenshots from the experimental results based on Algorithm 4 for trajectory generation. Fig. 4(a) is the initial state, where the planned trajectory works well. However, in Fig. 4

(b), the planned trajectory is heading the opposite with the reference curve. The reason why such a situation occurs is that at this moment there exists multiple points on reference curve (lane center line) which have the same nearest distance to the last point of trajectory. Since

Algorithm 4 randomly samples the points from the reference curve, it may choose a wrong projection point and thus follow the opposite direction to the reference curve. Figs. 4(c) and (d) are the consequences of this issue when the moving agent continues to move forward. Apparently, the generated trajectories is unnatural and unacceptable for a car to execute. Actually, the issue occurs frequently and may cause serious accidents when an autonomous vehicle passing a U-shaped curved road.

For comparisons, the results of trajectory planning based on Algorithm5 have been shown in Fig. 4(f)-(g). Fig. 4(f) is the initial state of the moving agent and the planned trajectory is more reasonable than the one shown in Fig. 4(a). Then, instead of randomly picking a number, Algorithm 5 decides whether the requirement of product of and lateral distance is smaller than 1 is successfully satisfied. If not, it uses Algorithm 1 to calculate the Frenet coordinate. Subsequently, Algorithm 5 manages to address the problem caused by Algorithm 4 by choosing the biggest index among those nearest points. As a result, the Fig. 4(f) shows a more reasonable planned trajectory. Fig. 4(g) and (h) are the following steps of the moving agent which successfully passes the obstacle and follows the U-shaped curved road.

Vi Conclusion

Classic trajectory planning algorithms with Frenet frame make a common implicit assumption that any segment on the reference curve is the image (math) of some part of the planned trajectory, which means the agent should follow the reference curve continuously. However, this assumption is not always true in practice and it may cause some serious issues.

In this paper, we study and analyze these issues and discussed their possible consequences in details. Then we propose an efficient candidate trajectory generation algorithm where a new projection operator from the Cartesian frame to Frenet frame is used. The new trajectory generation method is able to generate as many as possible candidate trajectories that follows the reference curve at the same direction for subsequent planning and optimization. As far as we know, it is the first time that a trajectory generation method is proposed with the constrain of . The proposed trajectory planning algorithm enjoys the same computational time complexity as the classic algorithms and has been successfully applied in our real practice.


  • [1] E. Abbena, A. Gray, and S. Salamon (2016) Modern differential geometry of curves and surfaces with mathematica, fourth edition. American Mathematical Monthly 26 (1), pp. 203–208. Cited by: §III-A.
  • [2] M. Berger and B. Gostiaux (2012) Differential geometry: manifolds, curves, and surfaces. Springer Science and Business Media. Cited by: §III-A.
  • [3] Y. Brudnyi and P. Shvartsman (1997) The whitney problem of existence of a linear extension operator. Journal of Geometric Analysis 7 (4), pp. 515–574. Cited by: §III-C.
  • [4] 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. CoRR abs/1807.08048. External Links: Link, 1807.08048 Cited by: §I.
  • [5] J. Fickenscher, S. Schmidt, F. Hannig, B. Bouzouraa, and J. Teich (2018) Path planning for highly automated driving on embedded gpus. Journal of Low Power Electronics and Applications 8 (35). Cited by: §I.
  • [6] M. Fu, T. Wang, Y. Xu, and S. Gao (2017) Path-following control of an underactuated hovercraft with dynamic uncertainties under serret-frenet frame. In 36th CHinese Control Conference (CCC), Cited by: §II.
  • [7] T. Heil, A. Lange, and S. Cramer (2016) Adaptive and efficient lane change path planning for automated vehicles. In IEEE International Conference on Intelligent Transportation Systems, Cited by: §II.
  • [8] L. Huiying, H. Shaoping, and G. Yuxian (2010) Robot continuous trajectory planning based on frenet-serret formulas. In International Conference on Computer, Mechatronics, Control and Electronic Engineering, Cited by: §IV-B.
  • [9] L. Jaillet, J. Cortés, and T. Siméon (2010) Sampling-based path planning on configuration-space costmaps. IEEE Transactions on Robotics 26 (4), pp. 635–646. Cited by: §II.
  • [10] A. Kelly and B. Nagy (2003) Reactive nonholonomic trajectory generation via parametric optimal control. The International journal of robotics research 22 (7/8), pp. p.583–601. Cited by: §II.
  • [11] O. Ljungqvist, N. Evestedt, D. Axehill, M. Cirillo, and H. Pettersson (2019) A path planning and path-following control framework for a general 2-trailer with a car-like tractor. Journal of Field Robotics 36 (8). Cited by: §II.
  • [12] M. Mcnaughton, C. Urmson, J. M. Dolan, and J. W. Lee (2011) Motion planning for autonomous driving with a conformal spatiotemporal lattice. In IEEE International Conference on Robotics and Automation, Cited by: §I, §II.
  • [13] A. Oulmas, N. Andreff, and S. Regnier (2016) Chained formulation of 3d path following for nonholonomic autonomous robots in a serret-frenet frame. In American Control Conference, Cited by: §I, §I, §II.
  • [14] H. SChoset, K.M. Lynch, S. Hutchinson, G.A. Kantor, W. Burgard, L.E. Kavraki, and S. Thrun (2005) Principles of robot motion: theory, algorithms, and implementations. MIT Press: Cambridge, MA, USA. Cited by: §II.
  • [15] Tianyu Gu, J. Atwood, Chiyu Dong, J. M. Dolan, and Jin-Woo Lee (2015) Tunable and stable real-time trajectory planning for urban autonomous driving. In 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vol. , pp. 250–256. Cited by: §I.
  • [16] M. Werling, J. Ziegler, S. Kammel, and S. Thrun (2010) Optimal trajectory generation for dynamic street scenarios in a frenét frame. In IEEE International Conference on Robotics and Automation, Cited by: §I, §I, §II, §II, §III-B.
  • [17] C. Wriedt and C. Beierle (2019)

    Implementation of trajectory planning for automated driving systems using constraint logic programming


    11th International Conference on Agents and Artificial Intelligence

    Cited by: §II.
  • [18] W. Zhan, J. Chen, C. Chan, C. Liu, and M. Tomizuka (2017) Spatially-partitioned environmental representation and planning architecture for on-road autonomous driving. In 2017 IEEE Intelligent Vehicles Symposium (IV), Vol. , pp. 632–639. Cited by: §I.
  • [19] J. Ziegler and C. Stiller (2009) Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios. In 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, Vol. , pp. 1879–1884. Cited by: §I.