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 . 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.
Ii Related Work
The past decades have witnessed a great progress in trajectory planning. Many researches have been proposed and studied , [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  , 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.
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.  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.
Iii Preliminaries and Our Theoretical Results
Iii-a The Frenet Frame
, we assume the vector functiondenotes 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 , 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.
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  is adopted to extend the polygonal line. Specifically, the first and last segments on the polygonal line are extended to rays with infinite length.
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.
Iv-B Our Trajectory Generation Algorithm
Trajectory generation is a key step in trajectory planning in dynamic and unknown environments. The downstream trajectory optimizer  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
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.
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. SinceAlgorithm 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.
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.
-  (2016) Modern differential geometry of curves and surfaces with mathematica, fourth edition. American Mathematical Monthly 26 (1), pp. 203–208. Cited by: §III-A.
-  (2012) Differential geometry: manifolds, curves, and surfaces. Springer Science and Business Media. Cited by: §III-A.
-  (1997) The whitney problem of existence of a linear extension operator. Journal of Geometric Analysis 7 (4), pp. 515–574. Cited by: §III-C.
-  (2018) Baidu apollo EM motion planner. CoRR abs/1807.08048. External Links: Cited by: §I.
-  (2018) Path planning for highly automated driving on embedded gpus. Journal of Low Power Electronics and Applications 8 (35). Cited by: §I.
-  (2017) Path-following control of an underactuated hovercraft with dynamic uncertainties under serret-frenet frame. In 36th CHinese Control Conference (CCC), Cited by: §II.
-  (2016) Adaptive and efficient lane change path planning for automated vehicles. In IEEE International Conference on Intelligent Transportation Systems, Cited by: §II.
-  (2010) Robot continuous trajectory planning based on frenet-serret formulas. In International Conference on Computer, Mechatronics, Control and Electronic Engineering, Cited by: §IV-B.
-  (2010) Sampling-based path planning on configuration-space costmaps. IEEE Transactions on Robotics 26 (4), pp. 635–646. Cited by: §II.
-  (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.
-  (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.
-  (2011) Motion planning for autonomous driving with a conformal spatiotemporal lattice. In IEEE International Conference on Robotics and Automation, Cited by: §I, §II.
-  (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.
-  (2005) Principles of robot motion: theory, algorithms, and implementations. MIT Press: Cambridge, MA, USA. Cited by: §II.
-  (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.
-  (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.
Implementation of trajectory planning for automated driving systems using constraint logic programming. In
11th International Conference on Agents and Artificial Intelligence, Cited by: §II.
-  (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.
-  (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.