Speed Planning Using Bezier Polynomials with Trapezoidal Corridors

Jialun Li1, Xiaojia Xie2, Hengbo Ma3, Xiao Liu2 and Jianping He1

Abstract—To generate safe and real-time trajectories for anautonomous vehicle in dynamic environments, path and speeddecoupled planning methods are often considered. This paperstudies speed planning, which mainly deals with dynamic obstacleavoidance given the planning path. The main challenges lie in thedecisions in non-convex space and the trade-off between safety,comfort and efficiency performances. This work uses dynamicprogramming to search heuristic waypoints on the S-T graph andto construct convex feasible spaces. Further, a piecewise Bezierpolynomials optimization approach with trapezoidal corridors ispresented, which theoretically guarantees the safety and optimal-ity of the trajectory. The simulations verify the effectiveness ofthe proposed approach.

I. INTRODUCTION

Autonomous vehicles are promising to revolutionize trans-portation systems and change the ways how people travel [1],[2]. To interact with other agents on road, the self-drivingcar adjusts its path and speed over time constantly basedon perception information. This task can be formulated as aproblem to optimizes a high-dimensional trajectory in termsof comfort and energy saving while satisfying safety anddynamic feasibility constraints. However, solving the originaloptimization problem is intractable with limited computationtime and infeasible for online running. Therefore, it is chal-lenging for the ego vehicle to generate collision-free, real-timeand comfortable trajectories to react to other road participants.

To deal with this issue, there are two major trajectorygeneration frameworks, spatio-temporal planning [3]–[5] andpath/speed decoupled planning in Frenet frame [6]–[8]. Thesetwo approaches share the same hierarchical ideas to achievereal-time planning by searching heuristic solutions first andoptimizing the preliminary results in convex subspaces later.

The spatio-temporal planning considers the spatial andtemporal maneuvers simultaneously. The search and optimiza-tion processes are completed in three-dimensional spaces.Correspondingly, the decoupled method decomposes the 3Dplanning into two stages as path planning and speed planning.In each planning cycle, path planning is executed to generate apath to avoid static, oncoming and low-speed vehicles. Then,speed planning adjusts the vehicle’s speed to keep a safedistance from dynamic obstacles which block the formed path.Although, the layered approach is prone to be suboptimalwith the appearance of dynamic obstacles compared to the3D optimization, it is more flexible for the separated designprocess. Besides, the computational complexity can be reduced

The authors1 are with the Department of Automation, Shanghai JiaoTong University, and Key Laboratory of System Control and InformationProcessing, Ministry of Education of China, Shanghai 200240, China. E-mail: jialunli, jphe@sjtu.edu.cn. The authors2 are with Megvii (Face++)Technology Inc., Beijing, China. E-mail: xiexiaojia, liuxiao@megvii.com.The author3 is with University of California, Berkeley, CA 94720, USA. E-mail: hengbo ma@berkeley.edu.

(a) (b)

Fig. 1: A merge scenario and its S-T graph.

by this approach as discussed in [6]–[8]. Hence, the decou-pled planning framework is widely adopted by academia andindustry for its flexibility and efficiency.

In the hierarchical planning framework, speed planningplays an important role in avoiding other dynamic vehicles.For example, when another vehicle merges into the samelane, the ego vehicle may follow the lane-changing vehiclewhile maintaining a distance from the car behind, as shownin Fig.1(a). During this process, the speed of the ego vehicleis expected to slow down first and then accelerate smoothlyin a short response time. The speed profiles determine thedistances from obstacles, forward and lateral accelerations,which influence the safety and comfort of passengers.

For the speed planning, [9] and [10] present a convexoptimization method with friction circle as the constraint. Liu[11] presents a novel slack convex feasible set algorithm tooptimize the time stamp of each waypoint with fixed stations.These approaches ensure the safety and comfort. For bettercomputational efficiency, the first search for heuristic profilesand post optimization paradigm is mostly taken. The searchmethods include A* [12], RRT [13] and dynamic programming[14]. As for the optimization, there are two main approaches.One is to optimize the stations at discretized time instantsdirectly [15], [16]. Another is to parameterize the speed profilewith polynomials and convert the speed optimization probleminto finding optimal polynomial coefficients which satisfycertain constraints [14].

In all the above speed optimization methods, the safetyconstraints are imposed at discretized time instant. The prob-lem lies in that the safety limitations should hold for thewhole planning horizon and the safety between two adjacentsampling time stamps cannot be guaranteed. These workstackle this problem by shortening the time interval to imposethe constraints. However, this will lead to more decisionvariables and much more computation time.

Motivated by the above observations, in this paper, wefocus on safety enforcement for the whole speed planning

arX

iv:2

104.

1165

5v1

[cs

.RO

] 2

3 A

pr 2

021

period in dynamic environments with feasibility, efficiency andcomfort considerations. We use piecewise Bezier polynomialsto optimize the speed profile on the S-T graph. Specially,we provide the sufficient condition on coefficients of theBezier polynomial to enforce the station curve in a serial oftrapezoidal corridors1 for the first time, by the convex hullproperty of the Bezier polynomial. During the search phase,dynamic programming is adopted. The main contributions ofour work are summarized as follows.

• We use dynamic programming (DP) to search feasiblespace and construct convex regions from the non-convexones for online feasibility. The piecewise heuristic poly-nomials of stations with time stamps are achieved atthe same time to avoid minimizing the distance betweenstation curve and discretized heuristic points for theconcise objective function and efficiency.

• We provide the sufficient condition on coefficients ofthe Bezier polynomial to guarantee the station curve intrapezoidal corridors theoretically. Compared with exist-ing condition of enforcement in the piecewise rectangularcorridors [3], the condition is relaxed and solution spaceis demonstrably enlarged, which leads to a lower costfunction and better comfort.

• We conduct simulations to verify the effectiveness of theproposed method. Our method is better than the classicalBezier polynomial method in terms of the comfort withlower failure rates in highly dynamic environments. Theproposed method also outperforms other works on effi-ciency, e.g., speed planning modules of the public roadplanner [15] and the EM motion planner [14]. We releaseour module as an open-source package.

II. RELATED WORKS

Speed Planning Speed profiles can be generated by meansof (i) searching and optimizing. (ii) sampling lattices andselecting by cost. (iii) approximated optimization. The firstmethod searches for the best candidate speed profile andoptimizes the curve for smoothness. This method is commonlyadopted for optimality and efficiency. Xu first presents amethod of selecting the best lattice and conducting postoptimization [7]. The Baidu EM motion planner uses dynamicprogramming for search and piecewise monomial polynomialsfor optimization [14]. In [17], a trajectory planning algorithmfor open spaces is presented. The heuristic speed pointsare calculated based on the vehicle dynamic and directlyoptimized with Piecewise-Jerk Speed Optimization. As forthe approach (ii), different speed lattices are sampled andcombined with path lattices. The generated local spatial-temporal trajectories are evaluated and the best one is selected.The core problem is how to construct the lattices to makeup the suboptimality of discretization. Related works see [6],[8]. Besides, there are some works directly optimizing the

1Corridors imply subspaces of safe regions where the Bezier curves alwayslie in (Refer to Section III.C for details). This term is firstly presented in thesafe planning of unmanned aerial vehicles (UAVs).

speed profile. Liu presents a novel slack convex feasible setalgorithm [11] and Qian proposes a MPC method [18].

Bezier Polynomials Based Planning Bezier polynomial isthe linear combination of Bernstein basis and can be obtainedby linear transformation from polynomial with monomialbasis. Bezier polynomial has useful properties for the pathgeneration and speed planning problem. In [19], a smoothand continuous speed profile is computed by proper curveconcatenation without optimization and considering dynamicobstacles. In the trajectory generation of UAVs, the Bezierpolynomial is widely taken with rectangular corridors in 2Dspace to describe safe areas from obstacles [20], [21]. Asfor autonomous driving, Ding borrows this idea and proposesa way of generating sequential rectangular corridors whereBezier polynomials for the speed curve are optimized [3].

Corridors based Bezier polynomial optimization is prospec-tive to enhance safety, comfort and efficiency in speed plan-ning. However, the shapes of safe regions on S-T graph arequite different from those for UAVs to generate trajectories.Then, the shapes and generations of corridors of two scenesare distinct from each other. This will lead to different lim-itations on coefficients and optimization performances. Forthe S-T graph, the boundaries of obstacles are straight linesor parabolic curves, since the accelerations of obstacles areusually assumed to be constant in the planning horizon. In theUAV navigation scenarios, the obstacles are circle, rectanglesand polygons where rectangular corridors are easy to begenerated by cube inflations. Hence, the following parts aimto solve the gap between profile generation on S-T graph andexisting Bezier polynomial optimization methods.

III. S-T GRAPH AND TRAJECTORY FORMULATION

A. S-T Graph and Non-convex Optimizations

As mentioned above, speed planning is to append a speedcomponent to the path. In this process, the longitudinal andlateral comfort, physical and traffic constraints and safety con-straints should be considered. The S-T graph is an approachto analyze this issue intuitively (Fig.1(b)).

Before executing the motion planning module, the egovehicle predicts trajectories of surrounding vehicles. If itspath is blocked by other vehicles in the planning horizon,the stations of these obstacles over time will be mapped tothe S-T graph. We take the common case that they movewith constant speeds for simplicity. Then, the area denotingeach obstacle on the S-T graph is a parallelogram. The sidelength of parallelogram along s axis equals to the length ofthe vehicle plus half length of ego vehicle for convenience ofsafety regions representation.

A speed profile of the ego vehicle on the S-T graph reflectsits distances from others with time and its decisions on how toavoid these obstacles, such as yielding, overtaking, followingand so on. The curves below blocked areas mean to yieldthe car in front, while those above mean to overtake orkeep distance from the car behind. To keep safe, the feasiblespace of the speed curve should not overlap with the regionsprojected by obstacles.

As shown in Fig.2(a), the constraints on stations are im-posed at discrete time stamps for safety. In some time slots,the allowed stations are split into two disconnected intervalsby obstacles. When the station-time curve is optimized withthese segmented safety constraints, the problem is non-convexand hard to be solved in milliseconds for online running.

B. Bezier Polynomial and Its Properties

The Bezier polynomial is the polynomial function repre-sented by linear combinations of Bernstein basis. The n-thordered Bezier polynomial is written as

B(t) = c0b0n(t) + c1b

1n(t) + · · ·+ cnb

nn(t) =

n∑i=0

cibin(t),

(1)where the Bernstein basis satisfies bin(t) = Ci

n · ti · (1 −t)n−i, t ∈ [0, 1]. The coefficients of the polynomial ci (i =0, 1, . . . , n) are also called control points. Compared to themonomial polynomial, Bezier curve has following properties.i) The time interval is defined on t ∈ [0, 1].ii) The Bezier polynomial starts at control point B(0) = c0and ends at B(1) = cn.iii) The Bezier curve B(t) is confined within the convex hullof control points. It can be described by the proposition 1.

Proposition 1. For Bezier polynomial B(t) =∑n

i=0 cibin(t),

if the coefficients satisfy p0 ≤ ci ≤ p0, i = 0, 1, . . . , n, wehave p0 ≤ B(t) ≤ p0, t ∈ [0, 1].

This is widely used in the Bezier curve optimization withrectangular corridor. We give a brief proof in the appendix.iv) The derivative of B(t), B(t), can also be written as aBezier polynomial with control points c1i = n ·(ci+1−ci), i =0, 1, . . . , n − 1. By this way, we are also able to calculatearbitrary derivatives of B(t). Similarly, control points ofdl+1B(t)

dtl+1 and dlB(t)dtl

satisfy cl+1i = (n− l)(cli+1 − cli).

C. Speed Curve Formulation by Piecewise Bezier Polynomials

Since the station-time points obtained by heuristic searchingare lack of comfort and smoothness considerations, it requiresa polynomial approach for optimization subsequently. Insteadof using traditional polynomials with monomial basis, wechoose Bezier polynomials to ensure the speed profile in thesafe region theoretically. The safety property guaranteed byBezier function is that constraints on control points can limitthe value of polynomials during the whole time period. Thisuses the convex hull property (iii) above and proposition 1.

The piecewise polynomials are adopted to replace higherorder polynomial. This will guarantee fitting performancewhile avoiding numerical instability. Since B(t) is defined ona fixed time interval [0, 1], for a piece of trajectory during[Tk, Tk+1], we use translation and scaling methods same as

[20] to transform the time domain into correct time unit. Thenthe station with time can be represented as

s(t) =

h0B0

(t−T0

h0

), t ∈ [0, T1]

h1B1

(t−T1

h1

), t ∈ [T1, T2]

...

hmBm

(t−Tm

hm

), t ∈ [Tm, Tm+1].

(2)

IV. HEURISTIC SEARCHING AND CORRIDOR GENERATION

A. Dynamic Programming

Since the speed optimization problem is non-convex, it isinfeasible to be solved directly during online planning. Hence,we take the search method for heuristic profiles and postconvex optimization to generate a smooth speed curve. We firstdiscretize the S-T graph into grids. The grids have the sametime interval as ∆t1. Then we use dynamic programming tosearch on the nodes, vertices of grids, to obtain the heuristicstations with discrete time.

In dynamic programming algorithm, the states are the nodesof S-T graph as (ti, sj). Let the cost function cost(ti, sj)denote the total cost from start state (0, s0) to (ti, sj).This cost can be achieved by the sum of cost at consec-utive single node selected and the cost of state transitionscostedge((ti−1, sk), (ti, sj)). The iterative equation betweenparent node (ti−1, sk) and child node (ti, sj) is given by

cost(ti, sj) = minti,sjcost(ti, sj), cost(ti−1, sk)

+ costnode(ti, sj) + costedge((ti−1, sk), (ti, sj)).(3)

The term costnode(ti, sj) is the cost at single node (ti, sj),determined by distances from obstacles and expected terminalstation at T . The transitions between states (ti−1, sk) and(ti, sj) satisfy limited velocity and acceleration constraints.After calculations of costs by iterations, the optimal end pointis chosen as (ti, sj) with ti = T and lowest cost. Then, theheuristic points (trj , s

rj), j = 0, 1, . . . , jm+1 are obtained by

visiting the parent of each node backward from the end to thestart. The piecewise heuristic station curves are represented bylinear interpolation as

srj(t) =srj+1 − srj

∆t1(t− tj) + srj , j = 0, 1, . . . , jm+1. (4)

These heuristic stations by DP not only provide a referenceand warm start for the next optimization process, but alsoimply decisions concerning the obstacles. Further, we canfollow their physical meanings and discard another regioninduced by the obstacle as shown in Fig.2(b). Then, theconstraint on station at each time instant is a continuousinterval. As a result, the non-convex space can be convertedinto piecewise convex ones for next optimization.

B. Piecewise Convex Safe Regions Representations

Suppose the whole safe region induced by DP are divideinto m + 1 pieces with time intervals [T0, T1], . . . , [Tm, T ]

(a) Nonconvex safe regions. (b) DP search and piecewise convexsafe regions.

(c) Representations of safe regions. (d) Generations of safe regions.

Fig. 2: Safety Regions and their Generations on S-T graph.

and T = Tm+1. As shown in Fig.2(c), the k-th convex saferegions can be represented as

Sk = (ti, si)|pk0 + hkpk1

ti − Tkhk

≤ si ≤ pk0 + hkpk1ti − Tkhk

,

ti ∈ [Tk, Tk+1],(5)

where pk0 , pk1 are bias and skew of the lower bound and pk0 , p

k1

are those of the upper bound. hk denotes the length of k-thtime interval and satisfies hk = Tk+1 − Tk, k = 0, 1, . . . ,m.

Then, the whole safe region is

S = S0 ∪ · · · ∪ Sm.

The speed planning is safe if ∀t0 ∈ [0, T ], s(t0) ∈ S, which isequivalent to for t0 ∈ [Tk, Tk+1], s(t0) ∈ Sk, k = 0, 1, . . . ,m,i.e.

pk0 + hkpk1

t0 − Tkhk

≤ s(t0) ≤ pk0 + hkpk1t0 − Tkhk

. (6)

C. Piecewise Convex Safe Regions Generations

As discussed above, the constraint (6) is expected to befollowed for safety in whole planning horizon. However, thebounds on stations are stored as lower bounds lb and upperbounds ub at discrete time stamps after DP processing. Thevalues of pk0 , pk1 , pk0 , pk1 and hk in (6) cannot be achieveddirectly. Hence, the algorithm 1 is presented to obtain thesevalues and to generate sequential convex safe regions.

Fig.2(d) illustrates the generation of piecewise safe convexregions by algorithm 1. In the algorithm, the heuristic positionsare first sampled with the equal time interval ∆t2 as a serialof boundary points with ∆t2 = ∆t1

N , N ∈ N+. Then, threeadjacent points in upper and lower boundaries are detectedrespectively to see whether they are on the same line. Ifthey are not collinear, a new region will be created withdifferent skews. After the generations of regions, the functionRegionSplit will check the length of each corridor. If there issome region’s length above 1s, the region will be split intomultiple regions with time intervals not exceeding 1s. This

Algorithm 1: Piecewise Convex Regions GenerationInput: lb, ub, nums,∆t2Output: regions1: Initialize: regions[0], i = 0, j = 12: ConstructRegion(reg, 0, lb[0], ub[0], lb[1], ub[1])3: Insert(regions, reg)4: for i← 2 to nums− 2 do5: lskew = (lb[i]− lb[i− 1])/∆t26: uskew = (ub[i]− ub[i− 1])/∆t27: if ‖lskew − regions[j − 1].lskew‖ > ε or

‖uskew − regions[j − 1].uskew‖ > ε then8: regions[j − 1].tend = i9: regions[j − 1].t =

(regions[j − 1].tend − regions[j − 1].tbeg) ∗∆t210: ConstructRegion(reg, j, lb[i], ub[i], lb[i + 1], ub[i + 1])11: Insert(regions, reg)12: j ← j + 113: end if14: end for15: regions[j − 1].tend = n− 116: regions[j − 1].t =

(regions[j − 1].tend − regions[j − 1].tbeg) ∗∆t217: RegionSplit(regions)18: RegionMerge(regions)19: Return regions

Algorithm 2: ConstructRegion(reg, j, lb[i], ub[i], lb[i+1], ub[i+ 1])

Input: reg, j, lb[i], ub[i]Output: corridors1: reg.tbeg = i2: reg.lskew = (lb[i + 1]− lb[i])/∆t23: reg.lbias = lb[i]4: reg.uskew = (ub[i + 1]− ub[i])/∆t25: reg.ubias = ub[i]6: Insert(regions, reg)

operation aims to avoid underfitting of polynomial optimiza-tion and guarantee optimization performance. Accordingly, thefunction RegionMerge aims to merge the regions with smalltime intervals into adjacent corridors to avoid overfitting andspeed up the optimization process.

It should be noted that the creations of regions are dependenton the descriptions of predicted interacting vehicles. For sim-plicity, we assume that the obstacles are traveling at constantvelocities. Hence, if the boundaries of obstacles are not linear,they should be processed to linear bounds first.

V. PIECEWISE BEZIER POLYNOMIAL OPTIMIZATION

A. Problem of Safety Enforcement in Rectangular Corridors

To enforce the station curve in the safe region S , the convexhull property of the Bezier function is used and the definitionof corridor is given first.

Definition 1. Let coefficients of the Bezier Polynomial satisfyci ∈ Ω, i = 0, 1, . . . , n and then the Bezier curve B(t) residesin a set Scor for the convex hull property. If Scor is the subsetof safe set S, i.e. Scor ⊆ S, Scor is called a corridor.

(a) Optimization in piecewise rect-angular corridors (failure).

(b) Optimization in piecewise rect-angular corridors (success).

(c) Suboptimal of optimization inrectangular corridors.

(d) Optimization in piecewise trape-zoidal corridors.

Fig. 3: Optimization in trapezoidal and rectangular corridors.

With the concept of corridor, the safety can be checkedby calculation of Scor by ci ∈ Ω and contrast with S. Theshape of corridor is defined as that of region covered by Scor.Usually, a serial of rectangular corridors are chosen to guranteethe safety of trajectory parameterized by coefficients of Bezierpolynomials. Constraints on the coefficients are described bythe following proposition.

Proposition 2. For arbitrary pieces of the trajectory, if thereexists control points cki ∈ Ωk

1 = ck|pk0 +hkpk1 ≤ ck ≤ pk0 , i =

0, 1, . . . , n, k = 0, 1, . . . ,m, s(t) is safe on the S-T graph andScor is a rectangular corridor Srec.

Proof. With conditions cki ∈ Ωk1 and property of Bezier

function, for ∀t0 ∈ [Tk, Tk+1], it follows that

s(t0) ∈ Sreck = (ti, si)|pk0 + hkpk1 ≤ si ≤ pk0 ,

ti ∈ [Tk, Tk+1] ⊆ Sk.(7)

Further, we have Srec ⊆ S.

The problem of using rectangular corridors on S-T graph isthat when the bounds of corridors meet pk0 +hkp

k1 > pk0 , there

is no feasible solution of the optimization problem and theplanner will fail. In order to avoid this case, the time intervals

of k-th corridors should satisfy, hk ≤pk0−pk

0

pk1

.

Accordingly, [3] proposes a seed generation and cubeinflation method to adjust time intervals with proper valuescleverly. However, this will generate too many corridors andoptimized variables in complex environments, which leads toa loss of computation efficiency. Besides, if ∃ pk1 > 0 orpk1 > 0, we have Srec $ S. The safe regions are not fullyutilized for optimization of the station curve. This reflects thatthe constraints on control points are too tight to reduce thesolution space and degrade the optimality of designed stations.

Next, we will explore safety constraints on ci in trapezoidalcorridors to fully used safe regions without loss of optimality.

B. Safety Enforcement in Trapezoidal Corridors

Before moving to the sufficient conditions on control pointsci, we first give the following lemma.

Lemma 1. Let M ∈ R(n+1)×(n+1) denote the transitionmatrix from the Bernstein basis b0n(t), b1n(t), . . . , bnn(t) tothe monomial basis 1, t, t2, . . . , tn. We have Mi,0 = 1, 0 ≤Mi,j ≤ 1, i = 0, 1, . . . , n, j = 0, 1, . . . , n.

Proof. It follows that

ti = ti(t+ 1− t)n−i =

n−i∑j=0

Cjn−it

n−j(1− t)j

=

n−i∑j=0

Cjn−i

Cn−jn

Cn−jn tn−j(1− t)j .

Hence, the elements of matrix M satisfy

Mn−j,i =

Cj

n−i

Cjn, i+ j ≤ n

0, i+ j > n.(8)

We have Mi,0 = 1, 0 ≤Mi,j ≤ 1, i, j = 0, 1, . . . , n.

Theorem 1. For arbitrary piece of the trajectory,∀pk0 , pk0 , pk1 , pk1 ∈ R, there exists control points cki ∈ Ωk

2 =

ck|pk0 + hkpk1Mi,1 ≤ cki ≤ pk0 + hkpk1Mi,1, i = 0, 1, . . . , n,

s.t. s(t) is safe and Scor is a trapezoidal corridor.

Proof. On the S-T graph for t ∈ [Tk, Tk+1], it holds that

pk0 + hkpk1

t− Tkhk

< pk0 + hkpk1t− Tkhk

. (9)

According to lemma 1, Mi,1 satisfies 0 ≤ Mi,1 ≤ 1. Thus,we have Tk ≤ Tk +hkMi,1 ≤ Tk+1 and let t = Tk +hkMi,1.Then we obtain

pk0 + hkpk1Mi,1 < pk0 + hkpk1Mi,1,

and ∃cki , s.t. pk0 + hkpk1Mi,1 ≤ cki ≤ pk0 + hkpk1Mi,1. As for

the safety of s(t), ∀t0 ∈ [Tk, Tk+1],

s(t0) ≤n∑

i=0

(pk0 + hkpk1Mi,1)bin

(t− Tkhk

)≤ pk0

n∑i=0

bin

(t− Tkhk

)+ hkpk1

n∑i=0

Mi,1bin

(t− Tkhk

)= pk0 + hkp1

t− Tkhk

.

Similarly, we can achieve s(t0) ≥ pk0 + hkpk1t−Tk

hk. Therefore,

we obtain s(t0) ∈ Strak = Sk and s(t) ∈ Stra = S. Hences(t) is safe and the corridor is a trapezoid.

The proof of Theorem 1 utilizes the idea of transformingterms in boundaries into the Bernstein basis. The process ofproof also shows that s(t) is exactly constrained in the safeset S according to the convex property of Bezier function, notits subset, e.g. Srec.

In Theorem 1, conditions on ci is pk0 + hkpk1Mi,1 ≤ cki ≤

pk0 +hkpk1Mi,1. Compared to safety enforcement in rectangular

corridors in proposition 2, we have pk0+hkpk1Mi,1 ≤ pk0+hkp

k1

and pk0 + hkpk1Mi,1 ≥ pk0 . The advantage of this is two folds:i) By the proof of pk0 +hkp

k1Mi,1 < pk0 +hkpk1Mi,1, the lower

boundaries are smaller than the higher boundaries all the time.Therefore, the planner will not be insoluble and fail with safetyenforcement. ii) The constraints are relaxed and the solutionstend to be better than the ones by rectangular corridors.

Remark 1. Considering the corridors with nth-ordered poly-nomials as boundaries, if there exists control points cki ∈Ωk

3 = ck|∑n

j=0 hjkp

kjMi,j ≤ ck ≤

∑nj=0 h

jkp

kjMi,j , i =

0, 1, . . . , n, it follows that∑n

j=0 hjkp

kj

(t−Tk

hk

)j≤ s(t) ≤∑n

j=0 hjkp

kj

(t−Tk

hk

)jand s(t) is safe. However, the lower

bound may be larger than the upper bound in this inequalityand there is no solution for the optimization problem.

C. Trajectory Optimization FormulationThere are several objectives for the Bezier function to

minimize and the cost function is designed as

J = w1

∫ T

0

(s(t)− sr(t))2 dt + w2

∫ T

0

(s(t)− vr)2 dt

+ w3

∫ T

0

s(t)2dt + w4

∫ T

0

...s (t)2dt + w5 (s(T )− sr(T ))2 ,

(10)where sr(t) is the reference stations by DP and vr is thecruise speed. The cruise speed varies with scenarios, but isconsidered as a constant in one planning period. The firstterm is to minimize the distance between the Bezier curveand the heuristic s− t profile. The second one optimizes thebias between the actual speed and reference speed. This aimsto let the vehicle keep a high velocity. The third and fourthterms are to smooth the heuristic s− t curve by penalizing theacceleration and jerk respectively. Besides, the end station isexpected to reach the certain value sr(T ) by the last term.

The constraints include boundary constraints, continuityconstraints, safety constraints and physical constraints.i) Boundary Constraints: The piecewise curve starts at fixedposition, speed and acceleration and it follows that

c0,li h(1−l)k =

dls(t)

dtl

∣∣∣∣t=0

, l = 0, 1, 2, (11)

where ck,li is the control point for the l-th order derivative ofthe k-th Bezier curve.ii) Continuity Constraints: The piecewise curve is continuousat the connecting time points in terms of position, speed andacceleration. According to these conditions, we come to

ck,ln h(1−l)k = ck+1,l

0 h(1−l)k+1 , l = 0, 1, 2, k = 0, 1, . . . ,m− 1.

(12)iii) Safety Constraints: With trapezoidal corridors as discussedin the last part, safety constraints can be represented as

pk0 + hkpk1Mi,1 ≤ ck,0i ≤ pk0 + hkpk1Mi,1, k = 0, 1, . . . ,m.

(13)iv) Physical Constraints: These constraints consider real physi-cal conditions of the vehicle and limit the velocity, acceleration

and jerk. We first use the hodograph property (iv) to calculatevelocity, acceleration and jerk as Bezier polynomials. Then theconstraints can be given by

βk,1 ≤ ck,li ≤ βk,1,

βl ≤ ck,li ≤ βl, l = 2, 3,(14)

where k = 0, 1, . . . ,m and it follows that ck,l+1i = (n −

l)(ck,li+1−ck,li ). The upper bounds βk,1 are determined by speed

limits on road and centripetal acceleration constraints. Let acmbe the maximum acceleration permitted and κk the maximumcurvature of the path for t ∈ [Tk, Tk+1] (see [15] for details).The lateral acceleration constraints are given by

ck,li ≤ βk,1 =

√acmκk

. (15)

The bounds on longitudinal accelerations and jerks are con-stant for different pieces of speed profiles.

Then, the trajectory optimization process can be formulatedas a quadratic programming (QP) problem as

P : minc

cTQcc + qcT c + const

s.t. Aeqc = beq

Aiec ≤ bie.

(16)

We refer readers to the appendix for the detailed formulationprocess. This problem can be solved by solvers like OSQP.

VI. EXPERIMENTS AND RESULTS ANALYSIS

The planning approach proposed is implemented in C++11with the OSQP solver [22]. All experiments are carried out ona dual-core 2.90GHz Intel i5-4210H processor. The planninghorizon is 7s. In the dynamic programming procedure, theresolution of time is 1s. Hence, in each corridor, the heuristicstation sr(t) is a first-order polynomial.

A. Optimality and Low Failure Rate Verification

We conduct experiments on the S-T graph first to verifythe optimality and low failure rate of the proposed approachcompared to Bezier polynomial with rectangular corridors.Consider the scenario in Fig.1(a). We project different stationsof the vehicles onto the S-T graph. Suppose the ego vehicleis currently traveling at a speed of v(0) = 10.0m/s andan acceleration of a(0) = 0. The parameters are set to bew1 = 0.1, w2 = 0.1, w3 = 10.0, w4 = 5.0 and w5 = 3.0.

As shown in Fig.4(a), Bezier curves are calculated withinrectangular and trapezoidal corridors with same parameters.Fig.4(c) illustrates corresponding speed profiles and 4(d)shows acceleration profiles. We use longitudinal accelerationsto quantify the comfort. As shown in the figure, the maximumacceleration of the curve generated by trapezoidal corridors issmaller than that by rectangular corridors. Table I gives maxi-

mal accelerations and average accelerations√

1T

∫ T

0(s(t))2dt

in planning horizon [0, T ] and T = 7s. This proves that TChas better comfort and smoothness performance than RC.

In the case shown in Fig.4(b), the TC method works and theRC approach does not find a feasible solution. This is because

(a) Bezier curves with obstacles. (b) Bezier curves with obstacles.

(c) Speed profiles. (d) Acceleration profiles.

Fig. 4: Bezier curves within trapezoidal and rectangular corridors.

the solution space of TC is larger than that of RC. As a result,the TC method has a lower failure rate in complex situations.

TABLE I: Comfort of planners by different corridors.1 2

Max. Acc. Ave. Acc. Max. Acc. Ave. Acc.RC 0.95 0.62 - -TC 0.78 0.54 0.96 0.59

B. Efficiency Verification

We test our speed planning method in ROS and compare itwith the start-of-art approaches to illustrate its efficiency. Theperception and prediction modules are omitted by the simula-tion scenario and the localization of ego vehicle are obtainedby the simulator. The low-level controller is implemented byPure Pursuit algorithm. The motion planning part includes pathplanner implemented by the same method as [17] and speedplanners implemented by different approaches for tests.

In Fig.5(a), the ego vehicle (blue) begins to move forward.The global path is planned and shown as the long green line,covered by a red line representing local path planning forobstacle avoidance. The green line is also known as the guidedline to generate the Frenet frame. Fig.5(b) - Fig.5(d) showsthat the vehicle perceives the passing vehicle (orange) at theintersection and slows down to avoid the obstacle successfully.Fig.5(e) illustrates that the car passes the turn. Fig.5(f) showsthat the vehicle reaches the destination.

We conduct the experiment twice to test the efficiencies ofdifferent speed planners and the Table II contrasts the results.Our method outperforms EM planner [14] and Public Road(PR) Planner [15] with 5.70 ms and 5.42 ms. The standarddeviations and worst time values are also better than others.

TABLE II: Time consumptions of different velocity planners.

Velocity Planner Ave./ms Std./ms Worst/ms Planning times

1EM 16.81 7.77 39.57 379PR 7.91 4.04 28.71 429

Ours 5.70 2.72 16.46 403

2EM 15.52 6.71 38.37 421PR 7.89 3.69 35.47 548

Ours 5.42 2.45 13.61 470

(a) Beginning to move. (b) Avoiding the dynamic obstacle.

(c) Avoiding the dynamic obstacle. (d) Avoiding the dynamic obstacle.

(e) Passing the turn successfully. (f) Reaching the destination.

Fig. 5: Autonomous navigation of the ego vehicle.

VII. CONCLUSION

In this paper, we investigate speed planning for autonomousvehicles. We use dynamic programming to search coarsestations first. Then an approach to generating trapezoidal cor-ridors is presented. We also propose the sufficient conditionson control points of Bezier polynomials to guarantee the safetytheoretically. Compared to previous safety enforcement inrectangular corridors, our method is proved to be more relaxedand solvable when incorporated into the Bezier polynomialsoptimization. After that, we formulate the velocity optimiza-tion as a QP problem. Simulations show that our approach isbetter than the safety enforcement in rectangular corridors interms of optimality and low failure rates. The proposed wayis also faster than the start-of-art methods, e.g. EM Plannerand Public Road Planner.

APPENDIX

A. Proof of Proposition 1

Proof. Since ci ≤ p0, it follows that

B(t) ≤ p0

n∑i=0

bin(t) = p0(t+ 1− t)n = p0.

Similarly, we have B(t) ≥ p0. Hence, it holds that p0 ≤B(t) ≤ p0, t ∈ [0, 1].

B. QP FormulationThis part illustrates how to formulate the Bezier polynomial

optimization as a QP problem. First, we express the Beziercurve as a polynomial

sk(t) =hk

n∑i=0

cki bin

(t− Tkhk

)

=hk

n∑i=0

pki

(t− Tkhk

)i

= hkfk

(t− Tkhk

),

(17)

where fk(t) =∑n

i=0 pki t

i, k = 0, 1, . . . ,m is a polynomialcurve. Let M ∈ R(n+1)×(n+1) denote the transition ma-trix from the Bernstein basis b0n(t), b1n(t), . . . , bnn(t) to themonomial basis 1, t, t2, . . . , tn. Then, we have ck = Mpk

with ck = [ck0 , . . . , ckn]T and pk = [pk0 , . . . , p

kn]T . According

to lemma 1, it holds that |M | > 0 and M is invertible. Hence,if the objective function can be written as

J =m∑

k=0

[(pk)TQkpk + qkpk

]+ const ≥ 0, (18)

where Qk is positive definite and known, then we have

J =

c0

...cm

T

(M−1)TQ0M−1 0

. . .0 (M−1)

TQmM−1

c0

...cm

+

q0

...qm

T M−1 0

. . .0 M−1

c0

...cm

+ const

=cTQcc+ qcT c+ const ≥ 0.

(19)Qc is also a positive-definite matrix. Since the constraints areall linear with c, the optimization problem is a QP problem.

Next we will illustrate that equation (18) holds and how tocalculate Qk and qk. We first calculate some terms to achievethe cost function J . To begin with, it holds that∫ Tk+1

Tk

(dls(τ)

dτ l

)2

dτ =

∫ hk

0

(dls(τ + Tk)

dτ l

)2

dτ

=

∫ hk

0

(dls(τ + Tk)

dtl

(dt

dτ

)l)2

dτ =1

h2l−3k

∫ 1

0

(dlfk(t)

dtl

)2

dt.

(20)

As for∫ 1

0

(dlfk(t)

dtl

)2

dt, it follows that∫ 1

0

(dlfk(t)

dtl

)2

dt =

∫ 1

0

∑i≥l,j≥l

pki pkj t

i+j−2ldt

=∑

i≥l,j≥l

i(i− 1) · · · (i− l)j(j − 1) · · · (j − l)i+ j + 1− 2l

pki pkj .

(21)

We also have∫ 1

0tfk(t)dt =

∑i

1i+2p

ki ,∫ 1

0fk(t)dt =∑

i1

i+1pki . Suppose J =

∑5i=1 wiJi, the terms of J satify

J1 =

m∑k=0

∫ Tk+1

Tk

(sk(t)− ak(t− Tk)− bk)2 dt

=

m∑k=0

∫ Tk+1

Tk

[sk(t)

2 − 2 (ak(t− Tk) + bk) sk(t)]dt+ const

=

m∑k=0

h3k

∫ 1

0fk(t)

2dt− 2h3kak

∫ 1

0tfk(t)dt− 2h2kbk

∫ 1

0fk(t)dt

+ const(22)

J2 =

m∑k=0

∫ Tk+1

Tk

sk(t)2dt− 2vr

∫ T

0s(t)dt+ const

=m∑

k=0

hk

∫ 1

0fk(t)

2dt− 2vrs(T ) + const

(23)

J3 =m∑

k=0

1

hk

∫ 1

0fk(t)

2dt, J4 =

m∑k=0

1

h3k

∫ 1

0

...f k(t)

2dt (24)

J5 =(s(T )− sr(T ))2 = s(T )2 − 2sr(T )s(T ) + const. (25)

Then we can come to the equation (18) by replacing integralterms and using J =

∑5i=1 wiJi.REFERENCES

[1] W. Schwarting, J. Alonso-Mora, and D. Rus, “Planning and decision-making for autonomous vehicles,” Annual Review of Control, Robotics,and Autonomous Systems, 2018.

[2] B. Paden, M. Cap, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey ofmotion planning and control techniques for self-driving urban vehicles,”IEEE Trans. on Intelligent Vehicles, 2016.

[3] W. Ding, L. Zhang, J. Chen, and S. Shen, “Safe trajectory generation forcomplex urban environments using spatio-temporal semantic corridor,”IEEE Robotics and Automation Letters, 2019.

[4] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning forbertha—a local, continuous method,” in 2014 IEEE IV.

[5] T. Mercy, W. Van Loock, and G. Pipeleers, “Real-time motion planningin the presence of moving obstacles,” in 2016 ECC.

[6] T. Gu, J. M. Dolan, and J.-W. Lee, “Runtime-bounded tunable motionplanning for autonomous driving,” in 2016 IEEE IV.

[7] W. Xu, J. Wei, J. M. Dolan, H. Zhao, and H. Zha, “A real-time motionplanner with trajectory optimization for autonomous vehicles,” in 2012IEEE ICRA.

[8] X. Li, Z. Sun, D. Cao, Z. He, and Q. Zhu, “Real-time trajectoryplanning for autonomous urban driving: Framework, algorithms, andverifications,” IEEE/ASME Trans. on Mechatronics, 2015.

[9] T. Lipp and S. Boyd, “Minimum-time speed optimisation over a fixedpath,” International Journal of Control, 2014.

[10] Y. Zhang, H. Chen, S. L. Waslander, T. Yang, S. Zhang, G. Xiong, andK. Liu, “Speed planning for autonomous driving via convex optimiza-tion,” in 2018 IEEE ITSC.

[11] C. Liu, W. Zhan, and M. Tomizuka, “Speed profile planning in dynamicenvironments via temporal optimization,” in 2017 IEEE IV.

[12] C. Hubmann, M. Aeberhard, and C. Stiller, “A generic driving strategyfor urban environments,” in 2016 IEEE ITSC.

[13] Z. Du, D. Li, K. Zheng, and S. Liu, “Speed profile optimisation forintelligent vehicles in dynamic traffic scenarios,” International Journalof Systems Science, 2020.

[14] 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,” arXiv preprint,2018.

[15] Y. Zhang, H. Sun, J. Zhou, J. Hu, and J. Miao, “Optimal trajectory gener-ation for autonomous vehicles under centripetal acceleration constraintsfor in-lane driving scenarios,” in 2019 IEEE ITSC).

[16] Y. Meng, Y. Wu, Q. Gu, and L. Liu, “A decoupled trajectory planningframework based on the integration of lattice searching and convexoptimization,” IEEE Access, 2019.

[17] J. Zhou, R. He, Y. Wang, S. Jiang, Z. Zhu, J. Hu, J. Miao, and Q. Luo,“Autonomous driving trajectory optimization with dual-loop iterativeanchoring path smoothing and piecewise-jerk speed optimization,” IEEERobotics and Automation Letters, 2020.

[18] X. Qian, I. Navarro, A. de La Fortelle, and F. Moutarde, “Motionplanning for urban autonomous driving using bezier curves and mpc,”in 2016 IEEE ITSC, 2016.

[19] D. Gonzalez, V. Milanes, J. Perez, and F. Nashashibi, “Speed profilegeneration based on quintic bezier curves for enhanced passengercomfort,” in 2016 IEEE ITSC.

[20] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generationfor quadrotors using fast marching method and bernstein basis polyno-mial,” in 2018 IEEE ICRA.

[21] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and effi-cient quadrotor trajectory generation for fast autonomous flight,” IEEERobotics and Automation Letters, 2019.

[22] B. Stellato, G. Banjac, P. Goulart, A. Bemporad, and S. Boyd, “Osqp:An operator splitting solver for quadratic programs,” MathematicalProgramming Computation, 2020.