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

Yuchen Sun, Dongchun Ren, Shiqi Lian, Mingyu Fan, and Xiangyi TengSection of Self-driving Vehicle

Meituan Group, Beijing, China 100102Email: {sunyuchen03, lianshiqi, rendongchun, fanmingyu, tengxiangyi}@meituan.com

Abstract— Trajectory planning is a fundamental task onvarious autonomous driving platforms, such as social roboticsand self-driving cars. Many trajectory planning algorithms usea reference curve based Frenet frame with time to reducethe planning dimension. However, there is a common implicitassumption in classic trajectory planning approaches, whichis that the generated trajectory should follow the referencecurve continuously. This assumption is not always true inreal applications and it might cause some undesired issuesin planning. One issue is that the projection of the plannedtrajectory onto the reference curve maybe discontinuous. Then,some segments on the reference curve are not the imageof any part of the planned path. Another issue is that theplanned path might self-intersect when following a simplereference curve continuously. The generated trajectories areunnatural and suboptimal ones when these issues happen.In this paper, we firstly demonstrate these issues and thenintroduce an efficient trajectory generation method which usesa new transformation from the Cartesian frame to Frenetframes. Experimental results on a simulated street scenariodemonstrated the effectiveness of the proposed method.

I. INTRODUCTION

Trajectory planning is used to create a safe and smoothtrajectory for an agent (e.g., a robot or a self-driving car)from one state (including the location, the velocity, theacceleration, the orientation and so on) to another state undercertain constraints or cost functions [1]. There are many waysto plan a trajectory for an intelligent agent and it is a commonpractice to use a reference curve, which is generated off-line,as the target to find the output trajectory. In this way, thetrajectory planning algorithm only need to follow the givenreference curve and does not require the information of maplike signs, turns and so on. To achieve optimality and timeefficiency, many trajectory planning algorithms [2]–[4] aredeveloped in Frenet frames with time to reduce the planningdimension with the help of a reference curve. In a Frenetframe, finding the optimal trajectory is essentially a threedimensional constrained optimization problem.

There are roughly two types of trajectory planning al-gorithms: the direct methods [5], [6] that use trajectorysampling or lattice search to find the optimal trajectory andthe path-speed decoupled methods [7], [8] that optimize pathand speed separately. In both of the methods, all obstaclesand environment information are projected onto the lanecenter based Frenet frames for each candidate lane, and thedrivable area is bounded by lane boundaries. When using

the Frenet frame, a projection operator which identifies thecorresponding projection point on the reference curve isalways required and used as the transformation from theCartesian frame to Frenet frames. The most widely usedprojection operator is defined as the nearest point on thereference curve, which is also the orthogonal projectionpoint. Given a reference curve, there are two sub-tasks intrajectory planning that a projection operator is necessary,which are:

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

• Trajectories generation: One need to generate as manyas possible candidate trajectories that follows the refer-ence curve for subsequent planning and optimization.

However, under certain circumstances, the projection of acandidate trajectory onto the reference curve maybe dis-continuous [3], [4] , which is commonly ignored in realapplications. Some continuous segments on the referencecurve could not be mapped back onto the generated trajectoryand make the first sub-task difficult. Moreover, trajectoriesgenerated by previous methods might self-intersect whencontinuously following a simple reference curve. The gener-ated trajectories might be unnatural and suboptimal trajecto-ries for the second sub-task. These discussed issues motivateus to develop novel a trajectory generation method that couldproduce reasonable candidate trajectories under most of thecircumstances. In this paper, based on our theoretical resulton trajectory following, the derived method could generatetrajectories that guarantee smooth, anomaly free, and followthe reference curve always.

II. RELATED WORK

The past decades have witnessed a great progress intrajectory planning. Many researches have been proposedand studied [3], [9]–[11] that they can generate globaltrajectories connecting a start and a possibly distant end state.Some approaches for trajectory planning follow a discretesampling or a road map-based scheme [12] [13], which firstlysample multiple rows of points and then generate a finiteset of trajectories with cost values, typically by differentialfunctions that describe vehicle dynamics and kinematics. A

arX

iv:2

012.

1461

7v1

[cs

.RO

] 2

9 D

ec 2

020

Fig. 1. An illustration of the transformation from Cartesian frame to Frenetframes

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

candidate trajectory is selected if and only if it correspondsto the minimal cost value.

Kelly and Nagy [14] proposed an efficient path planningalgorithm which employs polynomials to ensure the smoothvariation of the curvature. Based on this method, a newapproach proposed in [6] firstly samples points along thereference lane and then connects the sampled points usingpolynomials. In this way, all generated candidate trajectoriesconform to the lane shape. However, these methods useCartesian coordinate system rather than the Frenet frames.Therefore, the planning dimension is relatively high and theoptimization is complicated.

To address the issue, Werling et al. [3] used the lanecenter line based Frenet frames to decouple the lateral andlongitudinal motion. This strategy makes the reference curvecoincide with the lane shape. Based on Frenet frame, thegeneration of the lateral and longitudinal motions can beachieved separately by using quintic polynomials versustime, which ensures continuous acceleration. Given the ad-vantages of the Frenet frames, many trajectory planningmethods [4], [15] are proposed based on it and achieve aroaring success in addressing various planning problems inreal applications. However, most of the methods ignore anintrinsic problem in the transformation from Cartesian frameto Frenet frames, which may cause unnatural and suboptimaltrajectories for the downstream tasks. In this paper, based onour theoretical result on trajectory following, the proposedmethod could generate candidate trajectories following thereference curve and guarantee smoothness and anomaly free.

III. PRELIMINARIES AND OUR THEORETICAL RESULTS

A. The Frenet Frame

A reference curve is always defined by parametric curves[16] [17] such as the spline curve. When s(t) (or s forshort) is the modular length of time t, we assume the vectorfunction ~r(s) denotes a moving point on the reference curveL in Euclidean space. Let ~tr(s) and ~nr(s) are the unimodulartangent vector and normal vector at ~r(s), respectively. Then,{~tr(s), ~nr(s)

}formulates a dynamic orthogonal coordinate

system. For any Cartesian coordinates ~x, assuming ~r(s) isthe root point of ~x, we have

~x(s, kx, dx) = ~r(s) + kx~tr(s) + dx~nr(s), (1)

where kx = kx(t) and dx = dx(t) are the horizontal and per-pendicular offsets, respectively. And the frenet coordinatesof ~x can be denoted by (s, kx, dx). Figure 1 is an exampleof a Frenet frame. To simplify the expression, one commonpractice is to use the nearest point to ~x on L as the rootpoint, such that kx = 0. Then the Frenet coordinate of ~x canbe denoted by (s(t), dx(t)) and the corresponding projectionoperator p of ~x onto L is s(t). That is to say, the projectorp satisfies:

p(~x) = s(t). (2)

B. The Projection Operator

Considering L2 be the reference curve and the L1 isa candidate trajectory of a moving agent, there are twoimplicitly ideal properties/desirable assumptions of the pro-jection operator p for trajectory planning tasks [3], whichare summarized as below.• p is a continuous and smooth projector such that the

image (math) of a continuous segment on L1 shouldbe a continuous interval on L2.

• The moving agent on the candidate trajectory L1 shouldbe actually following the reference curve L2, which canbe mathematically represented as 〈p(~x),~tr(s)〉 > 0.

However, the definition of the projection operator p givenin (2) is incomplete and the neither of the two desiredproperties is guaranteed in most of the previous motionplanning studies. That means that it may have several outputsat some point ~x when following the given reference curveL2. Fig. 3(a) presents a reference curve with a 90 degreesturn. When a virtual agent on L1 moves into the turn witha reasonable large distance to the reference curve L2, thetrajectory L1 should be contained in the available set foroptimization. If the projection operator p is continuous, theimage of p should contain all segment between A2 and B2.As can be seen, the point X on L2 is not the projection ofany point from L1 and this violates the assumption that p iscontinuous. Fig. 3(b) presents an example in path following.The reference curve has a rotation arc whose radius is R. Thevirtual agent is following L2 continuously with the constraintthat the lateral distance is constant dx(t) = 2R. When theagent moves from B1 to A1, the direction of its movementon the candidate trajectory is opposite with the direction ofprojection point on the reference curve.

(a) discontinuous (b) self-interact

Fig. 3. The issues of discontinuous projection operator p, (a) multiple outputs for a single ~x, and the projection of L1 onto L2 is discontinuous, (b) asuboptimal trajectory when following the reference curve faithfully. L1 is a candidate trajectory and L2 is the reference curve

C. Our Theoretical Results

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

Theorem 3.1: Let L2 be the reference curve and ~x be anypoint (an agent) on the candidate trajectory L1. ~r(t) is thenearest point on L2 to ~x and κdx(t) < 1 , then ~r − ~x isperpendicular to the tangent ~tr(t) and the agent is actuallyfollowing L2 at the same direction.

The theorem 3.1 indicates that the projection point ~r(t)follows reference curve L2 at the same direction if and only ifκdx(t) < 1. Moreover, ~r(t) will jump and be discontinuouson L2 at these points where the curvature κ is large. If amethod can handle the discontinuous points properly andguarantee κdx(t) < 1, it can generate the smoothness of thegenerated trajectories which follow the reference curve at thesame direction.

For the convenience the downstream optimizers, it is acommon practice to discretize the reference curve in theform of vectors. The discretization is realized through pointsampling on the reference curve. The sampling points on thecurve is connected into a polygonal line, where the samplingpoints are sequenced in the order of their distance to one endpoint of the curve. The obtained polygonal line is used torepresent the original curve. To avoid the intractable casessuch as sharp angles and fractals, the following empiricalconditions should be satisfied in the sampling process:• The distance between two adjacent samples should be

extremely smaller than 1/κ.• 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, theobtained polygonal line that is built by line segments is not

enough to ensure that the normal line at each point on thereference curve go through of it. To deal with this, the linearextension strategy [18] is adopted to extend the polygonalline. Specifically, the first and last segments on the polygonalline are extended to rays with infinite length.

Algorithm 1 The Projection Operator: TransF-κInput: The polygonal line in the form of vectors: U ={Lc : (xc, yc)|c = 1, 2, ...,M} , which is formulated bysampling points from the reference curve L (M denotesthe sampling size); the Cartesian coordinates of a pointA : (xA, yA).

Output: The Frenet coordinate of A : (sA, dA)1: Compute the distances between A and all points in U .2: Identify all the points in U that have the identical and

the nearest distance to A, which form a subset N .3: Choose the point Lm such that m = argmax c : Lc ∈N , i.e., the biggest index from subset N .

4: if Lm is not an end point then5: v+ = 〈

−−−→LmA,

−−−−−−→LmLm+1〉 # where

−−−→X1X2 = X2 − X1

is the difference of the coordinates of two points6: v− = 〈

−−−→LmA,

−−−−−−→LmLm−1〉 # where 〈·, ·〉 is the inner

product of two vectors7: if v− < v+ then8: m = m+ 19: end if

10: (sA, dA)=AffineTrans(A,m,U) # Algo 211: else12: (sA, dA)=ParallelTrans(A,m,U) # Algo 313: end if14: return A : (sA, dA)

IV. THE PROPOSED METHOD

In this section, we propose a new efficient trajectory gen-eration method such that the generated candidate trajectoriesare smooth and guarantees to follow the reference curve atthe same direction. The method is developed based on a new

Algorithm 2 Affine TransformInput: The polygonal line in the form of vectors: U ={Lc : (xc, yc)|c = 1, 2, ...,M} (M denotes the samplingsize); the index m of the reference piece; The Cartesiancoordinates of a point A : (xA, yA)

Output: The Frenet coordinate of A : (sA, dA)1: Get the heading vector ~b1 of the angular bisector at the

junction Lm−1.2: Get the heading vector ~b2 of the angular bisector at the

junction Lm.3: if ~b1 is parallel to ~b2 then4: (sA, dA)=ParallelT rans(A,m,U)5: else6: sm−1 =

∑m−1i=1 ‖

−−−−→Li−1Li‖

7: Calculate the intersection point O of the two angularbisectors, which go through points Lm−1 and Lm withheadings ~b1 and ~b2, respectively.

8: Calculate the intersection point P of line−→OA and−−−−−−→

Lm−1Lm.9: sA = sm−1 + ‖

−−−−−→Lm−1P‖

10: dA = distance from A to the line Lm−1Lm.11: end if12: return (sA, dA)

projection operator, TransF-κ, from the Cartesian frame toFrenet frames.

A. Our Projection Operator: TransF-κ

Based on the theorem 3.1 and sampling requirements,it is intuitive and natural to project a moving point ontothe nearest point on the reference curve L. However, theremay be several nearest projection points with the identicaldistance. In our projection operator: TransF-κ, we choosethe projection point with the biggest s to ensure that themoving point is actually following at the same direction withL, where s denotes the distance on curve from the projectionpoint to the start point on L. Algorithm 1 shows the detailsof TransF-κ, the transformation from a Cartesian coordinatesto the Frenet coordinate of an arbitrary point.

In Algorithm 1 lines 1-2, we first pick out all points on Uwhich are the nearest to A and form a set containing all suchpoints. Then we choose the last one, i.e. one whose index isthe biggest. At line 3, This point with the biggest s is usedand the output trajectory is shorten. With the point Lm, lines5-9 decide A should be projected to the former or latter adja-cent piece to Lm. The condition at line 7 identifies which sideA should be projected on. The inverse image of two piecesare separated by the angular bisector at Lm. Finally, lines10-12 are used to calculate the projection point. The AffineTransform given in Algorithm 2 is applied on the piecesof U without linear extension while the Parallel Transformgiven in Algorithm 3 is employed on the extended piecesof U . One significant difference between these two operatorsis that the inverse image of common pieces is bounded bytwo angular bisectors while the extended pieces is boundedby only one angular bisector. The calculation approaches of

Algorithm 3 Parallel TransformInput: The polygonal line in the form of vectors: U ={Lc : (xc, yc)|c = 1, 2, ...,M} (M denotes the samplingsize); the index m of the reference piece; The Cartesiancoordinates of A : (xA, yA)

Output: The Frenet coordinate of A : (sA, dA)1: if m = 0 then2: Denote

−−−→L0L1 as ~E.

3: Set sA = 0.4: Denote

−−→L0A as ~V

5: else6: Denote

−−−−−−→Lm−1Lm as ~E

7: Set sA =∑m−1

i=1 ‖−−−−→Li−1Li‖

8: Denote−−−−→Lm−1A as ~V

9: end if10: Let ~E⊥ be a nonzero orthogonal vector to ~E such that〈 ~E⊥, ~E〉 = 0.

11: sA = sA + ~V · ~E/‖ ~E‖.12: dA = |〈~V , ~E⊥〉/‖ ~E⊥‖|13: return (sA, dA)

the projection results are based on these two situations. Thedetails of Affine Transform operator and Parallel Transformoperator are given in Algorithm 2 and 3, respectively.

Further discussions about Algorithm 2 and 3 shouldbe made for clarification. As can be seen, both Affineand Parallel Transform are approximation to the transformfunction Eq. (1) in Section III. The reason why we needthe approximation methods are that the polygonal line Uas a sampled approximation for reference curve L is nolonger smooth and thus there is no curvature, tangent vectorand so on. Therefore, two approximation operators are usedto make the lateral distance d vary continuously when thepoint A moves cross an angular bisector. In other words,they guarantee that point A on an angular bisector has thesame projection point on U no matter it is projected to theformer or latter piece. However, since they are approximationmethods, some error may occur. An apparent error is thatthe created coordinate system may not be orthogonal to thereference curve. That is why we introduce some samplingconditions which should be satisfied in Section III.C. Thoseconditions ensure that the approximation errors are smallenough to be ignored and make them work well in real-worldapplications.

B. Our Trajectory Generation Algorithm

Trajectory generation is a key step in trajectory planningin dynamic and unknown environments. The downstreamtrajectory optimizer [19] may fail to create an acceptabletrajectory plan that obeys traffic rules and without misleadingdirections (such as self-interactions) when the input can-didate trajectories are sloppy. Algorithm 4 is a commonframework for trajectory generation. Many sophisticated gen-eration methods can be extended from Algorithm 4. Withoutloss of generality, we choose Algorithm 4 as the basis ofour generation method.

Algorithm 4 A Commonly-used Continuous Candidate Tra-jectory Generation Method (May cause self-interaction!)Input: The polygonal line in the form of vectors: U = {Lc :

(sc, dc = 0)|c = 1, 2, ...,M} (M denotes the samplingsize); Vectors of the lateral boundaries at the samplepoints: Bc = [ac, bc], c = 1, 2, ...,M .

Output: A candidate trajectory T consisted of vectors of Mpoints: T = {Tc : (s′c, d′c)|c = 1, 2, ...,M}

1: i = 02: T = ∅3: while i < M do4: i = i+ 15: d′i= randomly pick a number between ai and bi6: s′i = si7: T = T ∪ (s′i, d

′i)

8: end while9: return A candidate trajectory T

As discussed in Section III, Algorithm 4 and the relatedsophisticated generation methods will produce candidatetrajectories with misleading directions and self-interactionwhen the curvature is large and the gap of the corridorboundaries is wide. Typical examples can be found in Fig.3.

To ensure the candidate trajectory is actually followingat the same direction as the reference curve, our methodintroduce the curvature κ that κdx(t) < 1 as an additionalrestriction condition to repair the generated trajectories byAlgorithm 4. Algorithm 5 is our proposed trajectory gen-eration method which uses the outputs of Algorithm 4.

The inputs of Algorithm 5 include the n-order differen-tiable reference curve L. The curvature κ at any points onL can be calculated by κ = 〈tr, nr〉, where tr = ~tr(s)and nr = ~nr(s) are the uni-modular tangent and normalvectors at ~rs on L, respectively (the same notations as inSection III.A). The input of Algorithm 5 T is an output ofAlgorithm 4, which might consist of misleading directionsand self-interactions. The output of Algorithm 5 is a renewedcandidate trajectory T ′, which is repaired from T . At theline 5 of algorithm 5, the curvature at the sampling pointLi is calculated. If κd′i < 1, it is safe to include thepoint Ti : (s′i, d

′i) into the candidate trajectory. Otherwise,

at lines 9-12, the point have to be transformed into theCartesian frame and then transformed back into the Frenetframe. Because several nearest points to (xi, yi) might beidentified in U , (s′′i , d

′′i ) is different with (s′i, d

′i). In this

way, the segment on reference curve L with high curvatureκ is jumped over and the obtained new point is safe to beincluded into T ′′.

V. SIMULATION STUDY AND DISCUSSION

In this section, we create an artificial environment fortrajectory planning. The generation algorithms 4 and 5 areused and compared in trajectory planning. High resolutionvideo clips of these experimental results have been uploadedas supplement materials.

Algorithm 5 The Proposed Trajectory Generation MethodInput: An n-order differentiable reference curve L; The

polygonal line in the form of vectors: U = {Lc :(sc, dc = 0)|c = 1, 2, ...,M}; The candidate trajectoryof M points: T = {Tc : (s′c, d′c)|c = 1, 2, ...,M} whichis generated by Algo 4.

Output: A renewed candidate trajectory T ′ from T : T ′′={T ′′c (s′′c , d′′c )|c = 1, 2, ...,M}

1: i = 02: T ′′ = ∅3: while i < M do4: i = i+ 15: Calculate κ = curvature of L at Li(si).6: if κd′i < 1 then7: T ′′ = T ′′ ∪ (s′i, d

′i)

8: else9: Calculate the Euclidean coordinates of Frenet coor-

dinates (s′i, d′i), which is denoted as (xi, yi).

10: Calculate the Frenet coordinates of (xi, yi) referringto U , (s′′i , d

′′i ) = TransF-κ (U,(xi, yi)). # Algo1

11: T ′′ = T ′′ ∪ (s′′i , d′′i )

12: i = min{i|s′i > s′′i }13: end if14: end while15: return A renewed trajectory T ′′ from T ′

A. Experimental settings

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

The detailed trajectory planning consists of the followingsteps:• Implement candidate trajectory generation method mul-

tiple times to create many candidate trajectories. (Algo-rithm 4 or Algorithm 5)

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

B. Experimental results and discussions

We recorded the whole simulation process of the planningand movement of the moving agent in the form of videos.Screenshots of several representative time stamps from thevideos are from collected and shown in Fig. 4. The bluecurve denotes the lane center line and also the referencecurve. The moving agent is represented as a triangle and itsplanned trajectory is shown as the red line. The obstacle isdenoted as black cross marks.

Figs. 4(a)-(e) are the screenshots from the experimentalresults based on Algorithm 4 for trajectory generation. Fig.4(a) is the initial state, where the planned trajectory workswell. However, in Fig. 4(b), the planned trajectory is heading

(a) (b) (c) (d)

(e) (f) (g) (h)

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 oftrajectory planning by Algorithm 5.

the opposite with the reference curve. The reason why sucha situation occurs is that at this moment there exists multiplepoints on reference curve (lane center line) which have thesame nearest distance to the last point of trajectory. SinceAlgorithm 4 randomly samples the points from the referencecurve, it may choose a wrong projection point and thusfollow the opposite direction to the reference curve. Figs.4(c) and (d) are the consequences of this issue when themoving agent continues to move forward. Apparently, thegenerated trajectories is unnatural and unacceptable for acar to execute. Actually, the issue occurs frequently and maycause serious accidents when an autonomous vehicle passinga U-shaped curved road.

For comparisons, the results of trajectory planning basedon Algorithm5 have been shown in Fig. 4(f)-(g). Fig. 4(f)is the initial state of the moving agent and the plannedtrajectory is more reasonable than the one shown in Fig. 4(a).Then, instead of randomly picking a number, Algorithm 5decides whether the requirement of product ofκ and lateraldistance 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 problemcaused by Algorithm 4 by choosing the biggest index amongthose nearest points. As a result, the Fig. 4(f) shows amore reasonable planned trajectory. Fig. 4(g) and (h) arethe following steps of the moving agent which successfullypasses the obstacle and follows the U-shaped curved road.

VI. CONCLUSION

Classic trajectory planning algorithms with Frenet framemake a common implicit assumption that any segment onthe reference curve is the image (math) of some part of theplanned trajectory, which means the agent should follow thereference curve continuously. However, this assumption is

not always true in practice and it may cause some seriousissues.

In this paper, we study and analyze these issues anddiscussed their possible consequences in details. Then wepropose an efficient candidate trajectory generation algorithmwhere a new projection operator from the Cartesian frame toFrenet frame is used. The new trajectory generation methodis able to generate as many as possible candidate trajectoriesthat follows the reference curve at the same direction forsubsequent planning and optimization. As far as we know,it is the first time that a trajectory generation method isproposed with the constrain of κd < 1. The proposedtrajectory planning algorithm enjoys the same computationaltime complexity as the classic algorithms and has beensuccessfully applied in our real practice.

REFERENCES

[1] W. Zhan, J. Chen, C. Chan, C. Liu, and M. Tomizuka, “Spatially-partitioned environmental representation and planning architecturefor on-road autonomous driving,” in 2017 IEEE Intelligent VehiclesSymposium (IV), 2017, pp. 632–639.

[2] J. Fickenscher, S. Schmidt, F. Hannig, B. Bouzouraa, and J. Teich,“Path planning for highly automated driving on embedded gpus,”Journal of Low Power Electronics and Applications, vol. 8, no. 35,2018.

[3] M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectorygeneration for dynamic street scenarios in a frenet frame,” in IEEEInternational Conference on Robotics and Automation, 2010.

[4] A. Oulmas, N. Andreff, and S. Regnier, “Chained formulation of 3dpath following for nonholonomic autonomous robots in a serret-frenetframe,” in American Control Conference, 2016.

[5] J. Ziegler and C. Stiller, “Spatiotemporal state lattices for fast tra-jectory planning in dynamic on-road driving scenarios,” in 2009IEEE/RSJ International Conference on Intelligent Robots and Systems,2009, pp. 1879–1884.

[6] M. Mcnaughton, C. Urmson, J. M. Dolan, and J. W. Lee, “Motionplanning for autonomous driving with a conformal spatiotemporal lat-tice,” in IEEE International Conference on Robotics and Automation,2011.

[7] Tianyu Gu, J. Atwood, Chiyu Dong, J. M. Dolan, and Jin-WooLee, “Tunable and stable real-time trajectory planning for urbanautonomous driving,” in 2015 IEEE/RSJ International Conference onIntelligent Robots and Systems (IROS), 2015, pp. 250–256.

[8] H. Fan, F. Zhu, C. Liu, L. Zhang, L. Zhuang, D. Li, W. Zhu, J. Hu,H. Li, and Q. Kong, “Baidu apollo EM motion planner,” CoRR, vol.abs/1807.08048, 2018. [Online]. Available: http://arxiv.org/abs/1807.08048

[9] O. Ljungqvist, N. Evestedt, D. Axehill, M. Cirillo, and H. Pettersson,“A path planning and path-following control framework for a general2-trailer with a car-like tractor,” Journal of Field Robotics, vol. 36,no. 8, 2019.

[10] C. Wriedt and C. Beierle, “Implementation of trajectory planning forautomated driving systems using constraint logic programming,” in11th International Conference on Agents and Artificial Intelligence,2019.

[11] T. Heil, A. Lange, and S. Cramer, “Adaptive and efficient lanechange path planning for automated vehicles,” in IEEE InternationalConference on Intelligent Transportation Systems, 2016.

[12] H. SChoset, K. Lynch, S. Hutchinson, G. Kantor, W. Burgard,L. Kavraki, and S. Thrun, Principles of Robot Motion: Theory,Algorithms, and Implementations. MIT Press: Cambridge, MA, USA,2005.

[13] L. Jaillet, J. Cortes, and T. Simeon, “Sampling-based path planningon configuration-space costmaps,” IEEE Transactions on Robotics,vol. 26, no. 4, pp. 635–646, 2010.

[14] A. Kelly and B. Nagy, “Reactive nonholonomic trajectory generationvia parametric optimal control,” The International journal of roboticsresearch, vol. 22, no. 7/8, pp. p.583–601, 2003.

[15] M. Fu, T. Wang, Y. Xu, and S. Gao, “Path-following control ofan underactuated hovercraft with dynamic uncertainties under serret-frenet frame,” in 36th CHinese Control Conference (CCC), 2017.

[16] E. Abbena, A. Gray, and S. Salamon, “Modern differential geometryof curves and surfaces with mathematica, fourth edition,” AmericanMathematical Monthly, vol. 26, no. 1, pp. 203–208, 2016.

[17] M. Berger and B. Gostiaux, Differential Geometry: Manifolds, Curves,and Surfaces. Springer Science and Business Media, 2012.

[18] Y. Brudnyi and P. Shvartsman, “The whitney problem of existence ofa linear extension operator,” Journal of Geometric Analysis, vol. 7,no. 4, pp. 515–574, 1997.

[19] L. Huiying, H. Shaoping, and G. Yuxian, “Robot continuous trajectoryplanning based on frenet-serret formulas,” in International Conferenceon Computer, Mechatronics, Control and Electronic Engineering,2010.