ERK'2020, Portorož, 144-147 144
Trajectory Planning By Applying Optimal Velocity Proﬁle
Algorithm on Bernstein-B´ ezier Motion Primitives
Martina Loknar, Gregor Klanˇ car, Saˇ so Blaˇ ziˇ c
Faculty of Electrical Engineering, Trˇ zaˇ ska cesta 25, SI-1000 Ljubljana, Slovenia
E-mail: martina.loknar@fe.uni-lj.si
Abstract
This paper deals with minimal-time smooth trajectory plan-
ning for autonomous mobile systems. The resulting path
is deﬁned piecewise by multiple Bernstein-B´ ezier motion
primitives that enable continuous velocity and curvature
at the junctions. The solution is found by calculating
travel time along Bernstein-B´ ezier segments with some
free parameters by applying the optimal velocity proﬁle
algorithm that considers velocity, acceleration and jerk
constraints. The proposed optimization approach was
validated in a simulation environment.
1 Introduction
Path planning algorithms generate a geometric path from
an initial to a ﬁnal point through pre-deﬁned via-points,
either in the joint space or in the operating space of the
robot. On the other hand, trajectory planning algorithms
assign a time law to the geometric path. There is an in-
creasing demand for robots and automatic machines to
operate while some function is optimized and quite of-
ten the research is focused in time optimization. Path
planning and trajectory planning algorithms are therefore
attracting considerable interest and remain the crucial is-
sues in the ﬁeld of automation and robotics [1].
Deﬁning the time of via-points progression inﬂuences
the kinematic and the dynamic properties of the motion:
the inertial forces and torques depend on the accelerations
along the trajectory, while the vibrations of its mechani-
cal structure are mostly determined by the values of the
jerk. In order to satisfy kinematic feasibility of the vehi-
cle the resulting path should be smooth; it should be real-
izable at high speed and at the same time be harmless for
the robot in terms of avoiding excessive accelerations of
the actuators and vibrations of its mechanical system [1].
Path smoothing is often not integrated in path planning
but is usually done after the optimal path is found, which
requires additional collision checks and can inﬂuence the
path optimality. One promising approach would therefore
be ﬁnding a smooth path to combine motion primitives in
a path planning phase [2]. The smoothing techniques,
not only limited to sample based planners, rely on using
curves to interpolate or ﬁt the given waypoints.
The ﬁrst studies to obtain the shortest curvature con-
strained smooth paths consisting of straight lines and cir-
cular arcs were performed by [3], but the resulting tracks
had a discontinuous curvature. This is why the authors in
[4] used Dubins paths and smoothed them with clothoid
arcs. The main advantage of clothoids is the linear change
of their curvature. Unfortunately clothoids are deﬁned in
terms of Fresnel integrals; transcendental functions that
cannot be solved analytically. This fact makes clothoids
difﬁcult to use in real-time applications so that for real-
time motion planning many authors resort to curves with
nonlinear curvature that are easier to compute, for exam-
ple B´ ezier curves [2]. Other smoothing methods also in-
clude application of cubic, quintic or higher order poly-
nomials [5] and B-splines [6].
This paper presents the results of a study where the
optimal velocity algorithm was validated on ﬁfth-order
Bernstein-B´ ezier curve segments with continuous veloc-
ity and curvature transitions. The optimal velocity al-
gorithm considers restrictions of speed, radial and tan-
gential acceleration and radial and tangential jerk. The
purpose of the analysis was to determine the travel time
along Bernstein-B´ ezier motion primitives in order to ﬁnd
a time optimal corridor-restricted path with the minimum
travel time and to provide a better insight into possible
trajectory planning strategies.
2 Problem statement
Consider the problem of a ground vehicle with a mis-
sion deﬁned by corridor and dynamical constraints in a
two-dimensional free space. Our goal is to develop and
implement an algorithm for generating a trajectory that
satisﬁes these restrictions.
Let the motion of a particle along a three times con-
tinuously differentiable plane curveC be described as a
function of time t2 [0;t
f
] by the position vector r(t)
measured from a given ﬁxed origin. Velocityv(t), accel-
erationa(t) and jerkj(t) vectors can be expressed in the
tangential-normal form as:
v(t) =v(t)  ^
T (1a)
a(t) =a
T
(t)  ^
T+a
R
(t)  ^
N (1b)
j(t) =j
T
(t)  ^
T+j
R
(t)  ^
N; (1c)
where
^
T and
^
N are the unit tangent vector and the unit
normal vector, respectively:
^
T(t) =
v(t)
kv(t)k
;
^
N(t) =
_
^
T(t)
k
_
^
T(t)k
: (2)
145
Given a feasible segment of the path, which in our case
is a Bernstein-B´ ezier curve, the optimization problem is
to ﬁnd the velocity proﬁlev(t) that reaches the end of the
curve in minimum time in a way that none of the velocity,
acceleration or jerk constraints from Eqs. (3a;3b;3c) are
violated:
0   k v(t)k   v
MAX
; 8t2 [0;t
f
]; (3a)
a
2
T
(t)
a
2
TMAX
+
a
2
R
(t)
a
2
RMAX
  1; 8t2 [0;t
f
]; (3b)
j
2
T
(t)
j
2
TMAX
+
j
2
R
(t)
j
2
RMAX
  1; 8t2 [0;t
f
]: (3c)
The acceleration and jerk constraints are deﬁned in a sim-
ilar way as in [7].
3 Bernstein-B´ ezier motion primitives
AnN-dimensional,n-th order Bernstein polynomialr
n
(  ) :
[0;1]!R
N
can be deﬁned as:
r
n
(  ) =
n
X
i=0
P
i;n
B
i;n
(  );   2 [0;1]; (4)
where  is normalized time (0      1),P
i;n
2R
N
is
thei-th control point andB
i;n
(  ) is the Bernstein poly-
nomial basis deﬁned as:
B
i;n
(  ) =
  n
i
    i
(1    )
n  i
; (5)
for alli2f0;:::;ng. Binomial coefﬁcient is deﬁned as:
  n
i
  =
n!
i!(n  i)!
: (6)
LettingP
n
= [P
0;n
;:::;P
n;n
]2R
N  (n+1)
be the vec-
tor of control points ofr
n
(  ), the Bernstein polynomial
in Eq. (4) can be rewritten in matrix form as:
r
n
(  ) =P
n
2
6
6
4
B
0;n
(  )
B
1;n
(  )
:::
B
n;n
(  )
3
7
7
5
: (7)
In the case when two or three-dimensional Bernstein poly-
nomials are used to describe planar and spatial curves,
Bernstein polynomials are often referred to as B´ ezier curves.
These curves have useful path planning properties. The
ﬁrst and the last points of the Bernstein polynomial intro-
duced in Eq. (4) are its endpoints:
r
n
(0) =P
0;n
and r
n
(1) =P
n;n
: (8)
TheN-dimensional,n-th order Bernstein polynomial also
lies within the convex hull deﬁned by its control points.
Furthermore, the start and the end of the curve is tangent
to the ﬁrst and the last section of the convex polygon, re-
spectively (Fig. 1).
dr
n
d            =0
=n(P
1;n
  P
0;n
); (9)
dr
n
d            =1
=n(P
n;n
  P
n  1;n
): (10)
Other properties of Bernstein polynomials (derivatives,
calculating deﬁnite integrals, the de Casteljau’s algorithm,
degree elevation etc.) do not fall within the scope of this
article; more details on this topic can be found in [8].
P
0
P
1
P
2
P
3
P
4
P
5
Figure 1: Fifth order Bernstein-B´ ezier curve with its convex
hull (dotted lines). The curve is tangent to the sides of the con-
vex hull, line segmentsP0P1 andP4P5.
3.1 Merging of motion primitives
For the sake of generality, we consider a merging of two
B´ ezier curves of different orders,r
j
nj
(  ) of ordern
j
and
r
j+1
nj+1
(  ) of order n
j+1
, into a spline. The three condi-
tions for continuous ﬁrst and second derivatives (C
2
con-
tinuity) in the junction are:
lim
  !1
r
j
nj
(  ) = lim
  !0
r
j+1
nj+1
(  ); (11)
lim
  !1
dr
j
nj
(  )
d 
= lim
  !0
dr
j+1
nj+1
(  )
d 
; (12)
lim
  !1
d
2
r
j
nj
(  )
d 
2
= lim
  !0
d
2
r
j+1
nj+1
(  )
d 
2
; (13)
This yields the following relations between the control
points of both B´ ezier curves:
P
j+1
0;nj+1
=P
j
nj;nj
; (14)
P
j+1
1;nj+1
= (1+
n
j
n
j+1
)P
j
nj;nj
  n
j
n
j+1
P
j
nj  1;nj
; (15)
P
j+1
2;nj+1
=
  1+
n
j
n
j+1
  2+
1+n
j
1+n
j+1
   P
j
nj;nj
  2
  1+
n
j
(1+n
j
)
n
j+1
(1+n
j+1
)
  P
j
nj  1;nj
+
n
j
(1+n
j
)
n
j+1
(1+n
j+1
)
P
j
nj  2;nj
:
(16)
3.2 Path generation
B´ ezier curves constructed by large numbers of control
points are numerically unstable. For this reason, in path
planning, it is desirable to construct a smooth way by
joining together low degree B´ ezier curves.
We employed the curves of the ﬁfth order, as this is
the least degree of B´ ezier curves that can satisfy the re-
quirement for curvature (C
2
) continuity. The ﬁfth-order
Bernstein -B´ ezier curver
5
(  ) = [x(  );y(  )]
T
is deﬁned
by six control pointsP
i;5
= [x
i
;y
i
],i2f0;1;:::5g.
r
5
(  ) = (1    )
5
P
0;5
+5  (1    )
4
P
1;5
+10  2
(1    )
3
P
2;5
+10  3
(1    )
2
P
3;5
+5  4
(1    )P
4;5
+  5
P
5;5
: (17)
146
The complete path through the corridor consists of sev-
eral Bernstein-B´ ezier curve sections. According to Eqs.
(14-16) forn
j
= 5 andn
j
=n
j+1
the ﬁrst three control
points of the(j +1)-th Bernstein-B´ ezier curver
j+1
5
are:
P
j+1
0;5
=P
j
5;5
; (18)
P
j+1
1;5
= 2P
j
5;5
  P
j
4;5
; (19)
P
j+1
2;5
= 4P
j
5;5
  4P
j
4;5
+P
j
3;5
: (20)
whereP
j
i;n
is thei-th control point ofj-th curver
j
5
. The
last control pointP
j+1
5;5
is deﬁned by the ﬁnal position:
P
j+1
5;5
=
  x
j+1
5
y
j+1
5
  : (21)
The expressions for the control points P
j+1
4;5
and P
j+1
3;5
follow from evaluating the ﬁrst and the second derivative
of r
j+1
5
in   = 1 and setting its values to v
spline
and
a
spline
, respectively:
dr
j+1
5
d              =1
=v
spline
; (22)
d
2
r
j+1
5
d  2
            =1
=a
spline
: (23)
P
j+1
4;5
andP
j+1
3;5
are deﬁned by both the ﬁnal position and
the ﬁnal orientation’
j+1
5
:
P
j+1
4;5
=P
j
5;5
  1
5
v
spline
    cos(’
j+1
5
)
sin(’
j+1
5
)
  ; (24)
P
j+1
3;5
=  P
j
5;5
+2P
j
4;5
+
+
1
20
a
spline
    cos(’
j+1
5
)
sin(’
j+1
5
)
  ;
(25)
It should be pointed out that the values of the ﬁrst and
the second curve derivative in the (j + 1)-th Bernstein-
B´ ezier curve end pointP
j+1
5;5
,v
spline
anda
spline
, are not
true speed and tangential acceleration. The notation that
we used simply reﬂects the analogy.
The ﬂowchart in Fig.(2) describes the general princi-
ple of operation of the proposed trajectory planning al-
gorithm. Corridor consists of m segments and j is the
number of the current segment on which the calculation
is performed: j2f1;2;:::mg. The given coordinates
and values of velocity, angle and tangential acceleration
inP
j
0;5
according to Eqs.(19-20) also determine the sec-
ond and the third control point of the B´ ezier curve,P
j
1;5
andP
j
2;5
. The problem is to calculate the three remaining
control points of the B´ ezier curve along which the travel
time is the shortest. To ﬁnd the solution, MATLAB’s in-
tegrated function fmincon was used. This is a solver-
based nonlinear optimization approach that ﬁnds a min-
imum of a constrained nonlinear multivariable function.
The starting point of the optimization is travel time along
a B´ ezier curve, given by setting the coordinates, veloc-
ity, angle and tangential acceleration in the ﬁnal control
pointP
j
5;5
. The control pointsP
j
3;5
andP
j
4;5
are deter-
mined according to the Eqs.(24-25). The travel time t
along this B´ ezier curve is calculated by applying the al-
gorithm that generates the optimal velocity proﬁle. The
functionfmincon then iteratively modiﬁes the free pa-
rameters ((x;y);v;’ and a
T
) in P
j
5;5
in a way that the
travel time along the curve decreases until the local min-
imum is found (t
MIN
in Fig.(2)). This is a basic concept
of the gradient method: from the starting point the op-
timization function at each iteration takes either a direct
(Newton) step or a conjugate gradient step to reach the
local minimum of the cost function. The calculation is
repeated for each segment to ﬁnally determine the com-
plete trajectoryG
1
along the corridor.
  
            
𝑗 = 1 
𝑗 = 𝑗 + 1 
NOS 
NO 
input data: corridor and 
dynamical restrictions 
Bézier curve with fixed (𝑥 , 𝑦 ), ϕ, 
𝑣 , 𝑎 𝑇 in 1
𝑠𝑡
 control point 
solution: fixed parameters for 
Bézier curve on 𝑗 -th segment 
optimal velocity profile algorithm 
on Bézier curve ⇒ 𝑡 
setting (𝑥 , 𝑦 ), ϕ, 𝑣 , 𝑎 𝑇 in 5
𝑡 ℎ
 
control point 
  
end 
start  
Type equation here. 
YES
S 
 𝑡 𝑀𝐼𝑁 found? 
YES
S 
𝑗 = 𝑚 ? 
fmincon 
Figure 2: The basic principle of operation of the proposed tra-
jectory planning algorithm.
In order to study the effectiveness of the proposed
method, the time optimal trajectory planning algorithm
is executed in two other versions. In the second version,
minimal travel timet
MIN;2seg
is calculated along B´ ezier
curves for two successive corridor segments (for j and
j +1) and in the third versiont
MIN;3seg
it is calculated
for three successive corridor segments (forj, j +1 and
j+2). Then, only the control pointsP
j
i;5
(i2f0;:::;ng)
of thej-th segment are retained. This procedure is per-
formed on the next segments until the the complete tra-
jectory along the corridor is found:G
2
in the second and
G
3
in the third version of the algorithm.
4 Simulation results
To demonstrate our trajectory planning method, we de-
signed the problem as follows: compute the time opti-
mal trajectory through the given 6-segment corridor with
the following dynamical restrictions: v
MAX
= 1:5m=s,
a
TMAX
= 2m=s
2
, a
RMAX
= 4m=s
2
, j
TMAX
= 6m=s
3
147
andj
RMAX
= 8m=s
3
. On the starting line, the values of
velocity and tangential acceleration are v = 1m=s and
a
T
= 0:5m=s
2
. The initial position is [0, 0] and the ini-
tial orientation is set parallel to the corridor center line.
Figure (3) shows the given six segment corridor and
the optimal trajectories G
i
, depicted with a thick black
line. The sets of discarded Bernstein-B´ ezier curves are
depicted with thin grey lines. G
1
,G
2
,G
3
are calculated
based on knowledge of the shape of the current segment,
two succesive segments and three successive segments,
respectively.
Figure 3: Time optimal trajectoryG3 (left). For comparison:
iteration steps in the last three segments forG1 (top right) and
G2 (bottom right).
Table (1) lists the travel timest
i
at the end of the separate
corridor segments. The evidence from this study proves
that the time optimal trajectory can be determined by our
trajectory planning method. Furthermore, the more suc-
cessive segments are taken into account when performing
the calculation, the lower is the overall minimal travel
time. Table (2) presents some computational properties
of the proposed algorithm: number of iterations per seg-
mentN
it;seg
, number of points (per segment)F-count
where the function evaluations took place, and the to-
tal computation time t
COMP
. It should be pointed out
that the problem has many constraints and theF-count
can be signiﬁcantly less than the total number of function
evaluations.
[s] t
1
t
2
t
3
t
4
t
5
t
6
P
6
i=1
t
i
G
1
1.79 0.80 2.93 2.48 0.96 0.85 9,81
G
2
1,75 0,62 2,74 2,41 1,03 0,65 9,20
G
3
1,73 0,62 2,75 2,43 1,01 0,64 9,18
Table 1:ti and overall travel time
P
6
i=1
ti forGi.
N
it;seg
F-count t
COMP
[s]
G
1
25 250 246
G
2
13 190 306
G
3
19 305 738
Table 2: Computational properties of the proposed algorithm.
The current solutions to the time optimal trajectory
generation problem under kinematic constraints (that in-
clude jerk limitations) exist mainly for robot manipula-
tors, whereas the literature on the same topic in the case
of autonomous vehicles is still scarce. A key problem
with much of the literature is also that the approaches of-
ten do not support Cartesian constraints.
5 Conclusion
This paper presents a trajectory planning algorithm in a
free space that is based on Bernstein-B´ ezier curves and
considers corridor and dynamical (velocity, acceleration
and jerk) constraints. B´ ezier curves provide an efﬁcient
way to generate the optimized trajectory and satisfy the
constraints at the same time. The simulation results show
that the minimal travel time through the corridor is the
lowest in the case when three successive corridor seg-
ments are considered in the proposed method.
Despite some limitations, we believe that the results
of this study could be the basis for our future research
on similar receding horizon control methods to generate
real-time optimal trajectories with dynamical restrictions.
In the future, we will also consider a development of a
more sophisticated way to determine whether the calcu-
lated path is within the corridor boundaries. Using higher
order B´ ezier curves with more free parameters might also
prove to be beneﬁcial.
Acknowledgements
The authors acknowledge the ﬁnancial support from the
Slovenian Research Agency (research core funding No.
P2-0219).
References
[1] A. Gasparetta, P. Boscario, A. Lanzutti, and R. Vidoni, Mo-
tion and Operation Planning of Robot Systems, Chapter 1
- Path Planning and Trajectory Planning Algorithms: A
General Overview, vol. 29. 2015.
[2] G. Klanˇ car, M. Seder, S. Blaˇ ziˇ c, I.
ˇ
Skrjanc, and I. Petrovi´ c,
“Drivable Path Planning Using Hybrid Search Algorithm
Based on E* and Bernstein-B´ ezier Motion Primitives,”
IEEE Transactions on Systems, Man, and Cybernetics: Sys-
tems, no. 1957, pp. 1–15, 2019.
[3] L. E. Dubins, “On Curves of Minimal Length with a Con-
straint on Average Curvature, and with Prescribed Initial
and Terminal Positions and Tangents,” American Journal
of Mathematics, vol. 79, no. 3, p. 497, 1957.
[4] M. Shanmugavel, A. Tsourdos, B. White, and
R. Zbikowski, “Co-operative path planning of multi-
ple UA Vs using Dubins paths with clothoid arcs,” Control
Engineering Practice, vol. 18, no. 9, pp. 1084–1092, 2010.
[5] F. Ghilardelli, G. Lini, and A. Piazzi, “Path Genera-
tion Using   ˆ4-Splines for a Truck and Trailer Vehicle,”
IEEE Transactions on Automation Science and Engineer-
ing, vol. 11, pp. 187–203, jan 2014.
[6] T. Berglund, U. Erikson, H. Jonsson, K. Mrozek, and
I. S¨ oderkvist, “Automatic generation of smooth paths
bounded by polygonal chains,” no. August 2014, 2001.
[7] M. Lepetiˇ c, G. Klanˇ car, I.
ˇ
Skrjanc, D. Matko, and
B. Potoˇ cnik, “Time optimal path planning considering
acceleration limits,” Robotics and Autonomous Systems,
vol. 45, no. 3-4, pp. 199–210, 2003.
[8] C. Kielas-Jensen and V . Cichella, “BeBOT: Bernstein Poly-
nomial Toolkit for Trajectory Generation,” IEEE Interna-
tional Conference on Intelligent Robots and Systems, no. 3,
pp. 3288–3293, 2019.