# Urban Motion Planning Framework Based on N-Bézier Curves Considering Comfort and Safety

Authors: Ray Lattarulo; Leonardo González; Enrique Martí; José Matute; Mauricio Marcano; Joshue Pérez
Journal: Journal of Advanced Transportation
 (2018)

---

## Abstract

In last decades, great technology advances have been done related to the automotive sector, especially in Advanced Driver Assistance Systems (ADAS) developed to improve mobility in terms of comfort and safety during driving process; hence, automated driving is presented as an evolution of those systems in the present and upcoming years. The aim of this work is to present a complete framework of motion planning for automated vehicles, considering different constraints with parametric curves for lateral and longitudinal planners. Parametric Bézier curves are used as the core approach for trajectory design in intersections, roundabouts, and lane change maneuvers. Additionally, a speed planner algorithm is presented using the same parametric curve approach, considering comfort and safety. A simulation environment is used for testing the planning method in urban conditions. Finally, tests with the real platform in automated mode have been performed showing goods results.


---

## Body

## 1. Introduction

Every year 1.2 million people die in road related accidents. The NHTSA conducted a study concluding that 94% of accidents are due to human errors, where a great amount of these crashes (33%) is related to wrong decisions during driving process [1]. In 2009, 2.4% of fatalities in the United States were related to a common problem for drivers, i.e., drowsiness [2]. Considering these points, automated driving technology promises a great reduction in crash incidents, adding efficiency in fuel consumption, reduction of parking spaces (ride-sharing), and inclusion of elderly and people with disabilities [3].In last decades, a considerable amount of institutions, research centers, and companies is seeking to improve this technology faster [4], to make automated driving a reality in public roads as soon as possible [5]. Lateral and longitudinal controllers [6–8], as well as perception (sensors) [9, 10] and communications [11], are some of the topics mostly studied.There are some other topics; those have received less attention than the ones named before, i.e., vehicle decision algorithm. This area is one of the most challenging topics in automated driving because it must deal with a great number of task, as generation of smooth trajectories [12], speed profiles based on fuel consumption and comfort [13], obstacles avoidance [14], and even doing coordination among other participants to execute a maneuver [15].Some methods explored in automated vehicle decision are in [16]; the authors have studied a novel method for autonomous vehicles decision task. They considered dynamic maneuvers satisfying traffic norms and road limitations based on a data-driven vehicle dynamic model and an optimization process using curve arcs. Other authors have explored methods based on generation of multiple curves, i.e., clothoids, setting a possible group of tentacles (possible trajectories) to be tracked by the vehicle, and the optimal solution is obtained using a Markov Decision Process (MDP) [17]. Other approaches are related to Partially Observable Markov Decision Process (POMDP) for generation of high level decision policies (following the lane, lane change, parking, etc.) [18]. Rapidly exploring Random Tree (RRT) is another method currently used for trajectory planning; some authors as [19] are testing the integration of RRT and visual driver behavior in driving to improve the decision process. The problem is that those methods have been developed to support specific scenarios considering a bunch of specific conditions; sometimes they did not even consider online deployment [20].Other authors, as [21], are tackling the problem considering the importance of dealing with the general case of driving (especially in urban scenarios). The idea is handling situations that goes from, for example, cruise control to being able of respecting traffic lights or signs, among other things. The problem with this approach was that the authors only considered the longitudinal part of the vehicle's decision. On the other hand, [22, 23] have presented works about general methods for trajectory generation considering intersections and roundabouts, but the speed profile generation was considered later on [24].In [25] a method combining parametric curves of Bézier is presented; the goal is to deal with the general case of trajectory planning (in urban environments), combining this with a speed profile generation based on Model Predictive Control. The problem with the approach is, basically, considerably more difficult for using two different techniques to resolve the planning problem (they are not unified).In such a way, this work presents a unified motion planning framework (trajectory planning and speed profile) based on 4th and 5th degree parametric Bézier curves. It specially focuses on urban scenarios and their geometrical design. It leans on the construction of two-dimensional curves for both (lateral and longitudinal) planning methods, simplifying the possible designing parameters to the most considerable ones without degrading the curve capacities. The work specially focuses on giving a tool-set for trajectory designing under typical scenarios as intersections and lane changes and to a more specific and complex scenario as roundabouts. This, last scenario is especially interesting for its set of possible parameters and conditions; it is relevant to consider that the roundabout complexity has not been sufficiently studied for other authors in the past [26].Finally, the contribution will be organize as follows: Section2 has the detailed explanation of three urban scenarios considered: intersections, roundabout, and lane changes (the rest can be modeled as straight line or arcs). A definition of relevant Bézier curve properties is carried on, then a global planning approach is developed to satisfy some requirements of the local planning approach (reducing the total amount of map points); these concepts will be followed by the trajectory and speed planning approach. Section 3 presents the general components used in the software architecture (all modules considered for simulation and real vehicle tests). In the following, it presented an urban test case based on simulation (Section 4) and a real case of study (Section 5); both specially focus on roundabout considerations. Finally, Section 6 contains the conclusions of this article summarizing the contributions and future works.


## 2. Motion Planning Framework

This section explains all the information related to the trajectory planning approach based on real-time parametric 4th and 5th degree Bézier curves. The approach’s core will be focused on using these curves in path planning generation for urban scenarios (intersections, roundabouts, and lane changing) and speed planning for comfortable and safe behaviors. This approach is not limited to urban scenarios, although it is the main target of this work.The planning framework explanation will be divided into three parts: (i) global planning using a global map (based on a point to define the structure of intersections, lane change or roundabouts) describing the route with reduced amount of points, (ii) local planning approach based on Bézier and specially focus on intersections, lane changes, and roundabouts (the straight and arc segment can be resolved as sequences of points), and (iii) the speed planning approach based on the same type of curves.


### 2.1. N-Bézier Curve Basis

Bézier is a type of parametric curve, which has been commonly used for computer graphics, animations, and path generation in robotics [27]. In general, they are good for real-time implementation, and the computational cost of designing them is lower than clothoids and splines curves [28]. Bézier curves are described by (1)Bt∣n,P0,…,Pn=∑i=0nbiPi,bi=niti1-tn-iwhere {bi∈R} is the Bernstein polynomial, {Pi∈R2} are the control points used to generate the curve, {n∈N+} is the Bézier order, and {t∈R,t=[0,1]} is the parameter for curve construction.These curves have several properties relevant for the purpose of the current work:(i)
The starting point of the Bézier curve corresponds with control pointP0, and the ending point corresponds with Pn.(ii)
The first point tangent vector (att=0) will be given by P0P1→ and the last point tangent vector (at t = 1) will be given by Pn-1Pn→.(iii)
The curve will lie into the convex hull formed by the control points.(iv)
Bézier curves are continuous geometrically and in curvature{Cn&Gn,∀n∈N+}, and this continuity can be preserved in joints of two different curves.(v)
Bézier curves are symmetric, and the generated curve fort:0→1 is equal to the one for t:1→0.This work uses 4th and 5th grade Bézier curves (some considerations with 3rd degree will be done). Higher order curves are not considered because they do not have additional benefits within current approach but increase the complexity [29].Rewriting the general Bézier equation (see (1)) a more compact representation is obtained:(2)Bt=K0+K1t+K2t2+⋯+Kntnwhere each Ki will be a function of the control points (fixed position) and curve order. The values of Ki coefficients are given in Table 1. K1 and K2 determine curvature at starting and ending points of the curve, this property is going to be used repeatedly on explanations. Using the information from 3rd to 5th degree Bézier, K1 and K2 are defined as follows:(3)K1=nv1→K2=nn-12v2→-v1→where v1→=P1-P0 and v2→=P2-P1 are introduced and n is the degree of the curve.Table 1
Compact Bézier polynomial Coefficients “C”.
C Bézier curve order 3rd Order 4th Order 5th Order K0 P 0 P 0 P 0 K1 - 3 P 0 + 3 P 1 - 4 P 0 + 4 P 1 - 5 P 0 + 5 P 1 K2 3 P 0 - 6 P 1 + 3 P 2 6 P 0 - 12 P 1 + 6 P 2 10 P 0 - 20 P 1 + 10 P 2 K3 - P 0 + 3 P 1 - 3 P 2 + P 3 - 4 P 0 + 12 P 1 - 12 P 2 + 4 P 3 - 10 P 0 + 30 P 1 - 30 P 2 + 10 P 3 K4 − P 0 - 4 P 1 + 6 P 2 - 4 P 3 + P 4 5 P 0 - 20 P 1 + 30 P 2 - 20 P 3 + 5 P 4 K5 − − - P 0 + 5 P 1 - 10 P 2 + 10 P 3 - 5 P 4 + P 5In general terms the curvature inR2 is defined as [30](4)kt=B˙xtB¨yt-B¨xtB˙ytB˙xt2+B˙yt23=B˙t×B¨tB˙t3and in this case, function B is the Bézier equation. Evaluating at t=0, it is obtained:(5)kt=0=2K1×K2K13=n-1nv1→×v2→v1→3The previous equation explicitly shows that if the three starting points in a curve are colinear, generated Bézier curve will have zero curvature at its starting point (k(t=0)=0); this can be extended to the three ending points due to symmetry. This property is useful for designing the intersections and lane changes, and for the entrance and exit part of the roundabout.


### 2.2. Global Planning Environment Used for the Approach

In this work, global planning is used as base route to apply the local planning approach using Bézier. A simple map containing a point to define intersections, roundabouts, or lane changes (point with “x” in Figures1, 2 and 3) is the input of the global method, decreasing the amount of points and keeping geometric qualities of road.Figure 1
Trajectory planning on intersection.Figure 2
Trajectory planning on lane change.Figure 3
Trajectory planning on roundabouts.


#### 2.2.1. Intersections

Figure1 depicts the description of an intersection using a single point (junction of 2 or more straight paths). The unitary vectors defining the road (in intersection case) are given by(6)ub→=Pb-PiPb-Piua→=Pa-PiPa-Piwhere ua→ is an unitary vector that goes from intersection Pi to past global point Pb and ub→ is other unitary vector formed with the next global points Pa and Pi. It is relevant to see that the angle α (Figure 1) can be different from 90∘, modeling a great variety of intersections. The parameter D will be used later in local planning and for the definition of Pi.


#### 2.2.2. Lane Changes

In this case, the approach considers straight segments during the maneuver (Figure2). The vectors for path description defined in (6) can be used for lane change and they have the following relation:(7)ub→=-ua→Additionally, in this type of curve a new pointPLC′ is introduced to handle the maneuver in local planning. This point is defined by (8)PLC′=PLC+wcos⁡tan-1uayuax±π2sin⁡tan-1uayuax±π2where w is the road width and ua→ is the vector presented in (7). The symbol ± is negative when the lane change is done to the right side and positive to the left side.


#### 2.2.3. Roundabouts

Figure3 shows the definition of a roundabout using a simple point. It uses roundabout center point Pr, its radius R, entrance angle ai, and exit angle ao. Using these parameters, the roundabouts can be modeled with an approximated circular shape.Roundabout entrance and exit points are defined by(9)Pe=Pr+Rcos⁡tan-1ubyubx±aisin⁡tan-1ubyubx±aiPex=Pr+Rcos⁡tan-1uayuax∓aosin⁡tan-1uayuax∓aowhere Pe is roundabout entrance coordinates, Pex is roundabout exit coordinates, values of ua→ and ub→ are defined by (6), but the intersection point Pi is substituted by roundabout points Pex and Pe, respectively. In these equations, the upper sign is used when traffic is defined counter clockwise and lower one for clockwise traffic [31].In addition to these two points,nr points will be included (points with square shape in Figure 3) defining the route inside the roundabout. Following this procedure, the total amount of points substituting basic center point Pr will be given by nr+2.


### 2.3. Local Planning Method Based on Curvature

The main contribution of the current paper is in the generation of smooth and continued curvature trajectories by definition in automatic vehicles. The principal goal is urban scenarios, but it is not limited to them; hence the section will be divided into three scenarios: intersections, roundabout, and lane changes.


#### 2.3.1. Intersections

Figure1 shows an intersection defined using a single point. This configuration reduces the amount of point in map but demands a trajectory method to connect the two straight segments smoothly. Bézier curves of 5th order will be used to solved this problem.As it is shown in (5), three Bézier control points (from P0 to P2) set over the same line will ensure curvature zero in the starting point of the curve {K(t=0)=0:Py0,1,2=mPx0,1,2+b}; this means that the segment starts as a straight path. The same equation can be used to define curvature 0 in the ending part of the curve (t=1) with control points Pn to Pn-2 (symmetry criteria).Using this definition is inferred that the minimum Bézier order that can be used in an intersection is 4th (5 control points), but in this case control pointP2 will be set in the position of the intersection point Pi. Hence, the 5th order is picked as a solution without the addition of many control points and less restrictive than 4th order. The position of the control points is shown in Table 2 where the vectors ub→ and ua→ are given by (6).Table 2
Bézier control points for lateral and longitudinal approach for intersections “Int”, roundabout entrance “RE”, roundabout exit “Rex”, lane change maneuver “LC”, and speed planning.
Planning Control points n-order Bézier P 0 P 1 P 2 P 3 P 4 P 5 Lateral Int 4 D u b → + P i 2 D u b → + P i D u b → + P i D u a → + P i 2 D u a → + P i 4 D u a → + P i RE 3 D 2 u e → + P e D 2 u e → + P e P e D P 3 u P 4 → + P 4 P r + R u θ e →   ∗ - REx P r + R u θ e x →   ∗ ∗ D P 1 u P 0 → + P 0 P e x D 2 u e x → + P e x 3 D 2 u e x → + P e x - LC 5 u b → D 2 + P L C 3 u b → D 2 + P L C u b → D 2 + P L C u a → D 2 + P L C ′ 3 u a → D 2 + P L C ′ 5 u a → D 2 + P L C ′  Longitudinal Speed s 0 v 0 s 0 + D v 0 s 0 + 2 D v 0 s 0 + 3 D v 0 + W s 0 + 4 D v 0 + W s 0 + 5 D v 0 + W ∗   θ e = t a n - 1 P e y - P r y / P e x - P r x ,   u θ e → = cos ⁡ θ e + D / R s i n ⁡ ( θ e + D / R ) ∗ ∗   θ e x = t a n - 1 P e x y - P r y / P e x x - P r x ,   u θ e x → = cos ⁡ θ e x - D / R sin ⁡ θ e x - D / RThe distance of control points has been set to ensure that maximum curvature of Bézier curve is reached in middle point of the trajectory, in order to know if peak curvature is under the vehicle's mechanical limits. Figure4 shows the normalized curvature K/Kmax (curvature of the generated segment divided by the maximum curvature value) with respect to normalized trajectory distance d/dmax (total distance of the generated segment divided by the maximum one), considering angles between straight segments α from 20 degrees to 145 degrees.Figure 4
Curvature analysis for different intersection angles.With the purpose of finding the position of maximum curvature, it is applied derivative to (4):(10)ddtKt=B˙×B⃛B˙·B˙-3B˙×B¨B˙·B¨B˙5evaluating the derivative of Bézier curve in t=0.5:(11)B˙t=0.5=154Du2→-u1→B¨t=0.5=10Du2→+u1→B⃛t=0.5=0where D is the design parameter for trajectory and u1,2→ are unitary vectors defined by (3). From this set of equations it is obtained that B˙×B⃛=B˙·B¨=0 resulting in curvature equal to 0 in t=0.5 (maximum value).


#### 2.3.2. Roundabouts

Figure3 shows the roundabout using simple point description. In such a way, roundabouts are modeled using two Bézier curves, each one is used to connect the straight road with the inner part, and the roundabout middle part is modeled as a circle with constant radius. The criteria used for design roundabouts trajectory have three considerations:(i)
The curvature at entrance int=0 or at exit in t=1 points must be 0 (straight paths).(ii)
Generated segments at the entrance and exit of roundabout must fit its inner part with a curvature equal to the inverse of the radius.(iii)
Bézier joining point angle must be the same of the circle arc angle (continuous trajectory).In order to follow the aforementioned criteria, 3 colinear control points are designed to ensure the first consideration; another 2 points are selected to form a tangent segment to the circle, which assures curvature and direction continuity (second and third criteria) along the roundabout (three colineal points will generate curvature 0 and notR-1). Hence, a Bézier curve 4th order is used to ensure curvature zero in straight path side and different of 0 in the inner roundabout side for entrance and exit segments (Figure 3).Table2 shows the control points used for trajectory generation at entrance (RE) and exit (REx) of roundabouts. Control points from P0 to P2 in the case of entrance (P2 to P4 for exit) are designed using similar criteria as in intersections, with D as a designing parameter. The points P4 at entrance or P0 at exit are selected using the distance D in the arc of the circle from entrance or exit depending on the case.The pointP3, in entrance trajectory, is separated of point P4, a distance given by(12)DP3=34uP4→×P2-P4Krwhere uP4→ is the tangent vector (with the circle) in the point P4 and Kr is the roundabout curvature. In the case of exit, the point P4 is substitute by P0 and P3 by P1.Figure5 shows the behavior of trajectory generation and its curvature using the specifications given in the planning method. Figure 5(a) depicts 3 different trajectories generated for different distances D and Figure 5(b) shows the trajectories curvature fitting perfectly the curvature radius 0.1[m-1].Figure 5
Examples of roundabout trajectories for different D values.
(a)
Generated trajectories (b)
Curvature of the different trajectories


#### 2.3.3. Lane Change

The approach is done using similar criteria as intersections, but control points are aligned with lane axis and equidistant by a distanceD (Figure 2). Where the unitary vectors ub and ua are explained in (7), w is the width of the road and D is the separation in ua axis between each pair of control points {D∈R,D=Pn-1-Pnua}.The location of control points is given in Table2 (LC). The minimum value of D is set to w in our design; these criteria yield to a maximum curvature in t≈0.20 and t≈0.80 in the curve definition. If a vehicle is capable of dealing with this curvature considering dynamic limitations in steering wheel angle and lateral acceleration, it will be also capable of handling any trajectory with D>W.The overtaking is considered as a special case composed by two lane changes; a first lane change is using the proposed method and when the first lane change is finished, this lane will be kept until the overtaking process is done. The returning will be done applying symmetry criteria and same propositions used in first lane change.


### 2.4. Speed Planning Based on Comfort and Vehicle Constraints

The speed control is directly affected by sudden changes in reference speed, and if these changes are smooth and continuous the reference speed tracking will be done with a better performance. In such a way a speed planner is proposed based on Bézier curves that permits:(i)
Anticipate future conditions in the speed of the road.(ii)
Applying physical constraints in vehicle acceleration and deceleration.(iii)
Keeping safely the vehicle velocity under speed limits.(iv)
Incorporating comfort of passengers.The speed planner approach uses 5th order Bézier curve to keep advantages of a higher degree and symmetry. Figure7 depicts the location of control points for a speed profile curve that is function of distance in the path. The formula related to each control point is presented in Table 2.Define the separation between consecutive control pointsPi-1 and Pi as {D∈R,D=distancex(Pi-1,Pi)∀i=1,2,3,4,5}. These conditions generate Bézier curve x-coordinates proportional to the parameter t given by the equation(13)Bxt=5Dt=swith this equation is seeing that the distance in x-axis and the parameter t∈[0,1] have a proportional relationship. It is introduced {s∈[s0,s0+5D]} to make the calculation easier (total distance of the speed curve).The physical constraints of the vehicle (acceleration and deceleration process) will have a direct relation with the generated curve. In this case, it is considered the general equation that relates speed to acceleration and doing a variable changevl=ds/dt, the resulting equation will be(14)alt=dvltdt⇒als=vlsdvlsdswhere the variable al is longitudinal acceleration, vl is longitudinal speed, t is time, and s position. Doing a nomenclature change from vl=By and ds=dBx=5Ddt will result in the equation(15)als=BydBydBx=BydByds,s∈s0,s0+5DApplying derivative to (15) maximum points of acceleration can be found. The resulting equation is(16)dalds=dByds2+Byd2Byds2From (16), introducing the 5th order Bézier equation and doing some approximations, the following roots can be obtained:(17)samax=5D0.49vo/W+0.88vo/W+1.41,ifv0W>00.52vo/W-0.03vo/W-0.46,ifv0W<-1providing a direct solution for the maximum acceleration in a speed curve.The first step in the speed planning method is applying the comfort speed limitation (reference speed) presented in [32] with the equation(18)aω=1.4ax2+1.4vl2K2+az2where aω is the total acceleration felt by passengers (comfort criteria given by ISO2631-1); longitudinal acceleration ax and vertical acceleration az contributions are approximated to zero (small changes ≈0), vl is longitudinal speed, and K is curvature in each point of the path.Figure6(a) depicts speed planning approach with some segments overlapped one with the other. In this stage Bézier curves will be generated using the maximum acceleration criteria for each upward and downward (ensured to be under the speed limit) reference speed step. The intersections between curves will be calculated. In the example, these intersections appeared three times. Calculating the Bézier curve for each speed step (current segment is independent of the segment before) will ensure that numerical instabilities (accumulation of numerical errors by concatenated curves) will not be present a long time.Figure 6
Stages for speed planning approach.
(a)
Speed planning with overlapped intersections (b)
Speed planning without overlapped partsFigure 7
Speed planning designing method.Finally, Figure6(b) depicts the last stage in the speed planning calculation that will regenerate all the Bézier curves considering the intersections in speed profile, generating a smoother profile. This behavior can be easily understood looking at Figure 7. The profile will be constrained to the acceleration limits of the vehicle, considering comfort during driving process and keeping speed safely under road limits.


## 2.1. N-Bézier Curve Basis

Bézier is a type of parametric curve, which has been commonly used for computer graphics, animations, and path generation in robotics [27]. In general, they are good for real-time implementation, and the computational cost of designing them is lower than clothoids and splines curves [28]. Bézier curves are described by (1)Bt∣n,P0,…,Pn=∑i=0nbiPi,bi=niti1-tn-iwhere {bi∈R} is the Bernstein polynomial, {Pi∈R2} are the control points used to generate the curve, {n∈N+} is the Bézier order, and {t∈R,t=[0,1]} is the parameter for curve construction.These curves have several properties relevant for the purpose of the current work:(i)
The starting point of the Bézier curve corresponds with control pointP0, and the ending point corresponds with Pn.(ii)
The first point tangent vector (att=0) will be given by P0P1→ and the last point tangent vector (at t = 1) will be given by Pn-1Pn→.(iii)
The curve will lie into the convex hull formed by the control points.(iv)
Bézier curves are continuous geometrically and in curvature{Cn&Gn,∀n∈N+}, and this continuity can be preserved in joints of two different curves.(v)
Bézier curves are symmetric, and the generated curve fort:0→1 is equal to the one for t:1→0.This work uses 4th and 5th grade Bézier curves (some considerations with 3rd degree will be done). Higher order curves are not considered because they do not have additional benefits within current approach but increase the complexity [29].Rewriting the general Bézier equation (see (1)) a more compact representation is obtained:(2)Bt=K0+K1t+K2t2+⋯+Kntnwhere each Ki will be a function of the control points (fixed position) and curve order. The values of Ki coefficients are given in Table 1. K1 and K2 determine curvature at starting and ending points of the curve, this property is going to be used repeatedly on explanations. Using the information from 3rd to 5th degree Bézier, K1 and K2 are defined as follows:(3)K1=nv1→K2=nn-12v2→-v1→where v1→=P1-P0 and v2→=P2-P1 are introduced and n is the degree of the curve.Table 1
Compact Bézier polynomial Coefficients “C”.
C Bézier curve order 3rd Order 4th Order 5th Order K0 P 0 P 0 P 0 K1 - 3 P 0 + 3 P 1 - 4 P 0 + 4 P 1 - 5 P 0 + 5 P 1 K2 3 P 0 - 6 P 1 + 3 P 2 6 P 0 - 12 P 1 + 6 P 2 10 P 0 - 20 P 1 + 10 P 2 K3 - P 0 + 3 P 1 - 3 P 2 + P 3 - 4 P 0 + 12 P 1 - 12 P 2 + 4 P 3 - 10 P 0 + 30 P 1 - 30 P 2 + 10 P 3 K4 − P 0 - 4 P 1 + 6 P 2 - 4 P 3 + P 4 5 P 0 - 20 P 1 + 30 P 2 - 20 P 3 + 5 P 4 K5 − − - P 0 + 5 P 1 - 10 P 2 + 10 P 3 - 5 P 4 + P 5In general terms the curvature inR2 is defined as [30](4)kt=B˙xtB¨yt-B¨xtB˙ytB˙xt2+B˙yt23=B˙t×B¨tB˙t3and in this case, function B is the Bézier equation. Evaluating at t=0, it is obtained:(5)kt=0=2K1×K2K13=n-1nv1→×v2→v1→3The previous equation explicitly shows that if the three starting points in a curve are colinear, generated Bézier curve will have zero curvature at its starting point (k(t=0)=0); this can be extended to the three ending points due to symmetry. This property is useful for designing the intersections and lane changes, and for the entrance and exit part of the roundabout.


## 2.2. Global Planning Environment Used for the Approach

In this work, global planning is used as base route to apply the local planning approach using Bézier. A simple map containing a point to define intersections, roundabouts, or lane changes (point with “x” in Figures1, 2 and 3) is the input of the global method, decreasing the amount of points and keeping geometric qualities of road.Figure 1
Trajectory planning on intersection.Figure 2
Trajectory planning on lane change.Figure 3
Trajectory planning on roundabouts.


### 2.2.1. Intersections

Figure1 depicts the description of an intersection using a single point (junction of 2 or more straight paths). The unitary vectors defining the road (in intersection case) are given by(6)ub→=Pb-PiPb-Piua→=Pa-PiPa-Piwhere ua→ is an unitary vector that goes from intersection Pi to past global point Pb and ub→ is other unitary vector formed with the next global points Pa and Pi. It is relevant to see that the angle α (Figure 1) can be different from 90∘, modeling a great variety of intersections. The parameter D will be used later in local planning and for the definition of Pi.


### 2.2.2. Lane Changes

In this case, the approach considers straight segments during the maneuver (Figure2). The vectors for path description defined in (6) can be used for lane change and they have the following relation:(7)ub→=-ua→Additionally, in this type of curve a new pointPLC′ is introduced to handle the maneuver in local planning. This point is defined by (8)PLC′=PLC+wcos⁡tan-1uayuax±π2sin⁡tan-1uayuax±π2where w is the road width and ua→ is the vector presented in (7). The symbol ± is negative when the lane change is done to the right side and positive to the left side.


### 2.2.3. Roundabouts

Figure3 shows the definition of a roundabout using a simple point. It uses roundabout center point Pr, its radius R, entrance angle ai, and exit angle ao. Using these parameters, the roundabouts can be modeled with an approximated circular shape.Roundabout entrance and exit points are defined by(9)Pe=Pr+Rcos⁡tan-1ubyubx±aisin⁡tan-1ubyubx±aiPex=Pr+Rcos⁡tan-1uayuax∓aosin⁡tan-1uayuax∓aowhere Pe is roundabout entrance coordinates, Pex is roundabout exit coordinates, values of ua→ and ub→ are defined by (6), but the intersection point Pi is substituted by roundabout points Pex and Pe, respectively. In these equations, the upper sign is used when traffic is defined counter clockwise and lower one for clockwise traffic [31].In addition to these two points,nr points will be included (points with square shape in Figure 3) defining the route inside the roundabout. Following this procedure, the total amount of points substituting basic center point Pr will be given by nr+2.


## 2.2.1. Intersections

Figure1 depicts the description of an intersection using a single point (junction of 2 or more straight paths). The unitary vectors defining the road (in intersection case) are given by(6)ub→=Pb-PiPb-Piua→=Pa-PiPa-Piwhere ua→ is an unitary vector that goes from intersection Pi to past global point Pb and ub→ is other unitary vector formed with the next global points Pa and Pi. It is relevant to see that the angle α (Figure 1) can be different from 90∘, modeling a great variety of intersections. The parameter D will be used later in local planning and for the definition of Pi.


## 2.2.2. Lane Changes

In this case, the approach considers straight segments during the maneuver (Figure2). The vectors for path description defined in (6) can be used for lane change and they have the following relation:(7)ub→=-ua→Additionally, in this type of curve a new pointPLC′ is introduced to handle the maneuver in local planning. This point is defined by (8)PLC′=PLC+wcos⁡tan-1uayuax±π2sin⁡tan-1uayuax±π2where w is the road width and ua→ is the vector presented in (7). The symbol ± is negative when the lane change is done to the right side and positive to the left side.


## 2.2.3. Roundabouts

Figure3 shows the definition of a roundabout using a simple point. It uses roundabout center point Pr, its radius R, entrance angle ai, and exit angle ao. Using these parameters, the roundabouts can be modeled with an approximated circular shape.Roundabout entrance and exit points are defined by(9)Pe=Pr+Rcos⁡tan-1ubyubx±aisin⁡tan-1ubyubx±aiPex=Pr+Rcos⁡tan-1uayuax∓aosin⁡tan-1uayuax∓aowhere Pe is roundabout entrance coordinates, Pex is roundabout exit coordinates, values of ua→ and ub→ are defined by (6), but the intersection point Pi is substituted by roundabout points Pex and Pe, respectively. In these equations, the upper sign is used when traffic is defined counter clockwise and lower one for clockwise traffic [31].In addition to these two points,nr points will be included (points with square shape in Figure 3) defining the route inside the roundabout. Following this procedure, the total amount of points substituting basic center point Pr will be given by nr+2.


## 2.3. Local Planning Method Based on Curvature

The main contribution of the current paper is in the generation of smooth and continued curvature trajectories by definition in automatic vehicles. The principal goal is urban scenarios, but it is not limited to them; hence the section will be divided into three scenarios: intersections, roundabout, and lane changes.


### 2.3.1. Intersections

Figure1 shows an intersection defined using a single point. This configuration reduces the amount of point in map but demands a trajectory method to connect the two straight segments smoothly. Bézier curves of 5th order will be used to solved this problem.As it is shown in (5), three Bézier control points (from P0 to P2) set over the same line will ensure curvature zero in the starting point of the curve {K(t=0)=0:Py0,1,2=mPx0,1,2+b}; this means that the segment starts as a straight path. The same equation can be used to define curvature 0 in the ending part of the curve (t=1) with control points Pn to Pn-2 (symmetry criteria).Using this definition is inferred that the minimum Bézier order that can be used in an intersection is 4th (5 control points), but in this case control pointP2 will be set in the position of the intersection point Pi. Hence, the 5th order is picked as a solution without the addition of many control points and less restrictive than 4th order. The position of the control points is shown in Table 2 where the vectors ub→ and ua→ are given by (6).Table 2
Bézier control points for lateral and longitudinal approach for intersections “Int”, roundabout entrance “RE”, roundabout exit “Rex”, lane change maneuver “LC”, and speed planning.
Planning Control points n-order Bézier P 0 P 1 P 2 P 3 P 4 P 5 Lateral Int 4 D u b → + P i 2 D u b → + P i D u b → + P i D u a → + P i 2 D u a → + P i 4 D u a → + P i RE 3 D 2 u e → + P e D 2 u e → + P e P e D P 3 u P 4 → + P 4 P r + R u θ e →   ∗ - REx P r + R u θ e x →   ∗ ∗ D P 1 u P 0 → + P 0 P e x D 2 u e x → + P e x 3 D 2 u e x → + P e x - LC 5 u b → D 2 + P L C 3 u b → D 2 + P L C u b → D 2 + P L C u a → D 2 + P L C ′ 3 u a → D 2 + P L C ′ 5 u a → D 2 + P L C ′  Longitudinal Speed s 0 v 0 s 0 + D v 0 s 0 + 2 D v 0 s 0 + 3 D v 0 + W s 0 + 4 D v 0 + W s 0 + 5 D v 0 + W ∗   θ e = t a n - 1 P e y - P r y / P e x - P r x ,   u θ e → = cos ⁡ θ e + D / R s i n ⁡ ( θ e + D / R ) ∗ ∗   θ e x = t a n - 1 P e x y - P r y / P e x x - P r x ,   u θ e x → = cos ⁡ θ e x - D / R sin ⁡ θ e x - D / RThe distance of control points has been set to ensure that maximum curvature of Bézier curve is reached in middle point of the trajectory, in order to know if peak curvature is under the vehicle's mechanical limits. Figure4 shows the normalized curvature K/Kmax (curvature of the generated segment divided by the maximum curvature value) with respect to normalized trajectory distance d/dmax (total distance of the generated segment divided by the maximum one), considering angles between straight segments α from 20 degrees to 145 degrees.Figure 4
Curvature analysis for different intersection angles.With the purpose of finding the position of maximum curvature, it is applied derivative to (4):(10)ddtKt=B˙×B⃛B˙·B˙-3B˙×B¨B˙·B¨B˙5evaluating the derivative of Bézier curve in t=0.5:(11)B˙t=0.5=154Du2→-u1→B¨t=0.5=10Du2→+u1→B⃛t=0.5=0where D is the design parameter for trajectory and u1,2→ are unitary vectors defined by (3). From this set of equations it is obtained that B˙×B⃛=B˙·B¨=0 resulting in curvature equal to 0 in t=0.5 (maximum value).


### 2.3.2. Roundabouts

Figure3 shows the roundabout using simple point description. In such a way, roundabouts are modeled using two Bézier curves, each one is used to connect the straight road with the inner part, and the roundabout middle part is modeled as a circle with constant radius. The criteria used for design roundabouts trajectory have three considerations:(i)
The curvature at entrance int=0 or at exit in t=1 points must be 0 (straight paths).(ii)
Generated segments at the entrance and exit of roundabout must fit its inner part with a curvature equal to the inverse of the radius.(iii)
Bézier joining point angle must be the same of the circle arc angle (continuous trajectory).In order to follow the aforementioned criteria, 3 colinear control points are designed to ensure the first consideration; another 2 points are selected to form a tangent segment to the circle, which assures curvature and direction continuity (second and third criteria) along the roundabout (three colineal points will generate curvature 0 and notR-1). Hence, a Bézier curve 4th order is used to ensure curvature zero in straight path side and different of 0 in the inner roundabout side for entrance and exit segments (Figure 3).Table2 shows the control points used for trajectory generation at entrance (RE) and exit (REx) of roundabouts. Control points from P0 to P2 in the case of entrance (P2 to P4 for exit) are designed using similar criteria as in intersections, with D as a designing parameter. The points P4 at entrance or P0 at exit are selected using the distance D in the arc of the circle from entrance or exit depending on the case.The pointP3, in entrance trajectory, is separated of point P4, a distance given by(12)DP3=34uP4→×P2-P4Krwhere uP4→ is the tangent vector (with the circle) in the point P4 and Kr is the roundabout curvature. In the case of exit, the point P4 is substitute by P0 and P3 by P1.Figure5 shows the behavior of trajectory generation and its curvature using the specifications given in the planning method. Figure 5(a) depicts 3 different trajectories generated for different distances D and Figure 5(b) shows the trajectories curvature fitting perfectly the curvature radius 0.1[m-1].Figure 5
Examples of roundabout trajectories for different D values.
(a)
Generated trajectories (b)
Curvature of the different trajectories


### 2.3.3. Lane Change

The approach is done using similar criteria as intersections, but control points are aligned with lane axis and equidistant by a distanceD (Figure 2). Where the unitary vectors ub and ua are explained in (7), w is the width of the road and D is the separation in ua axis between each pair of control points {D∈R,D=Pn-1-Pnua}.The location of control points is given in Table2 (LC). The minimum value of D is set to w in our design; these criteria yield to a maximum curvature in t≈0.20 and t≈0.80 in the curve definition. If a vehicle is capable of dealing with this curvature considering dynamic limitations in steering wheel angle and lateral acceleration, it will be also capable of handling any trajectory with D>W.The overtaking is considered as a special case composed by two lane changes; a first lane change is using the proposed method and when the first lane change is finished, this lane will be kept until the overtaking process is done. The returning will be done applying symmetry criteria and same propositions used in first lane change.


## 2.3.1. Intersections

Figure1 shows an intersection defined using a single point. This configuration reduces the amount of point in map but demands a trajectory method to connect the two straight segments smoothly. Bézier curves of 5th order will be used to solved this problem.As it is shown in (5), three Bézier control points (from P0 to P2) set over the same line will ensure curvature zero in the starting point of the curve {K(t=0)=0:Py0,1,2=mPx0,1,2+b}; this means that the segment starts as a straight path. The same equation can be used to define curvature 0 in the ending part of the curve (t=1) with control points Pn to Pn-2 (symmetry criteria).Using this definition is inferred that the minimum Bézier order that can be used in an intersection is 4th (5 control points), but in this case control pointP2 will be set in the position of the intersection point Pi. Hence, the 5th order is picked as a solution without the addition of many control points and less restrictive than 4th order. The position of the control points is shown in Table 2 where the vectors ub→ and ua→ are given by (6).Table 2
Bézier control points for lateral and longitudinal approach for intersections “Int”, roundabout entrance “RE”, roundabout exit “Rex”, lane change maneuver “LC”, and speed planning.
Planning Control points n-order Bézier P 0 P 1 P 2 P 3 P 4 P 5 Lateral Int 4 D u b → + P i 2 D u b → + P i D u b → + P i D u a → + P i 2 D u a → + P i 4 D u a → + P i RE 3 D 2 u e → + P e D 2 u e → + P e P e D P 3 u P 4 → + P 4 P r + R u θ e →   ∗ - REx P r + R u θ e x →   ∗ ∗ D P 1 u P 0 → + P 0 P e x D 2 u e x → + P e x 3 D 2 u e x → + P e x - LC 5 u b → D 2 + P L C 3 u b → D 2 + P L C u b → D 2 + P L C u a → D 2 + P L C ′ 3 u a → D 2 + P L C ′ 5 u a → D 2 + P L C ′  Longitudinal Speed s 0 v 0 s 0 + D v 0 s 0 + 2 D v 0 s 0 + 3 D v 0 + W s 0 + 4 D v 0 + W s 0 + 5 D v 0 + W ∗   θ e = t a n - 1 P e y - P r y / P e x - P r x ,   u θ e → = cos ⁡ θ e + D / R s i n ⁡ ( θ e + D / R ) ∗ ∗   θ e x = t a n - 1 P e x y - P r y / P e x x - P r x ,   u θ e x → = cos ⁡ θ e x - D / R sin ⁡ θ e x - D / RThe distance of control points has been set to ensure that maximum curvature of Bézier curve is reached in middle point of the trajectory, in order to know if peak curvature is under the vehicle's mechanical limits. Figure4 shows the normalized curvature K/Kmax (curvature of the generated segment divided by the maximum curvature value) with respect to normalized trajectory distance d/dmax (total distance of the generated segment divided by the maximum one), considering angles between straight segments α from 20 degrees to 145 degrees.Figure 4
Curvature analysis for different intersection angles.With the purpose of finding the position of maximum curvature, it is applied derivative to (4):(10)ddtKt=B˙×B⃛B˙·B˙-3B˙×B¨B˙·B¨B˙5evaluating the derivative of Bézier curve in t=0.5:(11)B˙t=0.5=154Du2→-u1→B¨t=0.5=10Du2→+u1→B⃛t=0.5=0where D is the design parameter for trajectory and u1,2→ are unitary vectors defined by (3). From this set of equations it is obtained that B˙×B⃛=B˙·B¨=0 resulting in curvature equal to 0 in t=0.5 (maximum value).


## 2.3.2. Roundabouts

Figure3 shows the roundabout using simple point description. In such a way, roundabouts are modeled using two Bézier curves, each one is used to connect the straight road with the inner part, and the roundabout middle part is modeled as a circle with constant radius. The criteria used for design roundabouts trajectory have three considerations:(i)
The curvature at entrance int=0 or at exit in t=1 points must be 0 (straight paths).(ii)
Generated segments at the entrance and exit of roundabout must fit its inner part with a curvature equal to the inverse of the radius.(iii)
Bézier joining point angle must be the same of the circle arc angle (continuous trajectory).In order to follow the aforementioned criteria, 3 colinear control points are designed to ensure the first consideration; another 2 points are selected to form a tangent segment to the circle, which assures curvature and direction continuity (second and third criteria) along the roundabout (three colineal points will generate curvature 0 and notR-1). Hence, a Bézier curve 4th order is used to ensure curvature zero in straight path side and different of 0 in the inner roundabout side for entrance and exit segments (Figure 3).Table2 shows the control points used for trajectory generation at entrance (RE) and exit (REx) of roundabouts. Control points from P0 to P2 in the case of entrance (P2 to P4 for exit) are designed using similar criteria as in intersections, with D as a designing parameter. The points P4 at entrance or P0 at exit are selected using the distance D in the arc of the circle from entrance or exit depending on the case.The pointP3, in entrance trajectory, is separated of point P4, a distance given by(12)DP3=34uP4→×P2-P4Krwhere uP4→ is the tangent vector (with the circle) in the point P4 and Kr is the roundabout curvature. In the case of exit, the point P4 is substitute by P0 and P3 by P1.Figure5 shows the behavior of trajectory generation and its curvature using the specifications given in the planning method. Figure 5(a) depicts 3 different trajectories generated for different distances D and Figure 5(b) shows the trajectories curvature fitting perfectly the curvature radius 0.1[m-1].Figure 5
Examples of roundabout trajectories for different D values.
(a)
Generated trajectories (b)
Curvature of the different trajectories


## 2.3.3. Lane Change

The approach is done using similar criteria as intersections, but control points are aligned with lane axis and equidistant by a distanceD (Figure 2). Where the unitary vectors ub and ua are explained in (7), w is the width of the road and D is the separation in ua axis between each pair of control points {D∈R,D=Pn-1-Pnua}.The location of control points is given in Table2 (LC). The minimum value of D is set to w in our design; these criteria yield to a maximum curvature in t≈0.20 and t≈0.80 in the curve definition. If a vehicle is capable of dealing with this curvature considering dynamic limitations in steering wheel angle and lateral acceleration, it will be also capable of handling any trajectory with D>W.The overtaking is considered as a special case composed by two lane changes; a first lane change is using the proposed method and when the first lane change is finished, this lane will be kept until the overtaking process is done. The returning will be done applying symmetry criteria and same propositions used in first lane change.


## 2.4. Speed Planning Based on Comfort and Vehicle Constraints

The speed control is directly affected by sudden changes in reference speed, and if these changes are smooth and continuous the reference speed tracking will be done with a better performance. In such a way a speed planner is proposed based on Bézier curves that permits:(i)
Anticipate future conditions in the speed of the road.(ii)
Applying physical constraints in vehicle acceleration and deceleration.(iii)
Keeping safely the vehicle velocity under speed limits.(iv)
Incorporating comfort of passengers.The speed planner approach uses 5th order Bézier curve to keep advantages of a higher degree and symmetry. Figure7 depicts the location of control points for a speed profile curve that is function of distance in the path. The formula related to each control point is presented in Table 2.Define the separation between consecutive control pointsPi-1 and Pi as {D∈R,D=distancex(Pi-1,Pi)∀i=1,2,3,4,5}. These conditions generate Bézier curve x-coordinates proportional to the parameter t given by the equation(13)Bxt=5Dt=swith this equation is seeing that the distance in x-axis and the parameter t∈[0,1] have a proportional relationship. It is introduced {s∈[s0,s0+5D]} to make the calculation easier (total distance of the speed curve).The physical constraints of the vehicle (acceleration and deceleration process) will have a direct relation with the generated curve. In this case, it is considered the general equation that relates speed to acceleration and doing a variable changevl=ds/dt, the resulting equation will be(14)alt=dvltdt⇒als=vlsdvlsdswhere the variable al is longitudinal acceleration, vl is longitudinal speed, t is time, and s position. Doing a nomenclature change from vl=By and ds=dBx=5Ddt will result in the equation(15)als=BydBydBx=BydByds,s∈s0,s0+5DApplying derivative to (15) maximum points of acceleration can be found. The resulting equation is(16)dalds=dByds2+Byd2Byds2From (16), introducing the 5th order Bézier equation and doing some approximations, the following roots can be obtained:(17)samax=5D0.49vo/W+0.88vo/W+1.41,ifv0W>00.52vo/W-0.03vo/W-0.46,ifv0W<-1providing a direct solution for the maximum acceleration in a speed curve.The first step in the speed planning method is applying the comfort speed limitation (reference speed) presented in [32] with the equation(18)aω=1.4ax2+1.4vl2K2+az2where aω is the total acceleration felt by passengers (comfort criteria given by ISO2631-1); longitudinal acceleration ax and vertical acceleration az contributions are approximated to zero (small changes ≈0), vl is longitudinal speed, and K is curvature in each point of the path.Figure6(a) depicts speed planning approach with some segments overlapped one with the other. In this stage Bézier curves will be generated using the maximum acceleration criteria for each upward and downward (ensured to be under the speed limit) reference speed step. The intersections between curves will be calculated. In the example, these intersections appeared three times. Calculating the Bézier curve for each speed step (current segment is independent of the segment before) will ensure that numerical instabilities (accumulation of numerical errors by concatenated curves) will not be present a long time.Figure 6
Stages for speed planning approach.
(a)
Speed planning with overlapped intersections (b)
Speed planning without overlapped partsFigure 7
Speed planning designing method.Finally, Figure6(b) depicts the last stage in the speed planning calculation that will regenerate all the Bézier curves considering the intersections in speed profile, generating a smoother profile. This behavior can be easily understood looking at Figure 7. The profile will be constrained to the acceleration limits of the vehicle, considering comfort during driving process and keeping speed safely under road limits.


## 3. Integration on Automated Vehicle Architecture

The architecture presented in [22] is a versatile and modular framework composed by six abstraction blocks which defines the major areas of influence in automated driving applications: acquisition, perception, communication, decision, control, and actuation. This section gives an introduction to the aforementioned architecture combined with details of simulated and experimental platforms used in this approach. For further general details refer to [33].Figure8 shows a brief summary of each block that is contained within the architecture. Acquisition is in charge of gathering raw input data coming from real or simulated sensors in the vehicle and obtaining information such as position, velocity, acceleration, obstacles around, driver status, actuators position, and others. Perception has the goal of postprocessing the information coming from acquisition generating descriptions on physical unit of the environment and the ego-vehicle. Communication refers to the capacities of exchanging information with other participants during driving (infrastructure, pedestrians, or other vehicles) to cooperate or knowing current states of other participants. Control is constituted for the lateral (steering) and longitudinal (throttle and brake) tracking controllers of the vehicle, and actuation contains the low level controllers of each actuator.Figure 8
Control architecture of automated vehicles.The decision module is the main target of the current work (grey coloured module in Figure8) and it is divided into 3 main subtasks: those are as follows: (i) the global planning is in charge of doing a first approximation of the route; in the current work one general simple point for each roundabout, intersection, or lane change will be used; (ii) the local planning generates smooth and continues trajectories to be tracked by the vehicle; the major contributions of this work are done in this block with the Bézier trajectory designing conditions and the speed profile based on the same type of curves; finally, (iii) the behavioral planning deals with all the unexpected conditions occurring while driving, e.g., obstacle avoidance, overtaking, etc.


## 4. Simulation Test Case

The current section explains the approach with a numerical example using the simulation tool Dynacar [34]. The test location corresponds with an urban scenario of the Basque Country in Spain. This is depicted in Figure 9(a) and it is constituted by 2 roundabouts, 3 right turns, and 6 left turns.Figure 9
Simulated results based on real urban scenario.
(a)
Trajectory generated with the approach (b)
Comparison of the longitudinal acceleration (c)
Comparison of the path curvature and simulated vehicle oneThe total amount of points used in the map definition will be divided among: 1 starting point, 1 ending point, 9 intersections, and 2 roundabouts, resulting in 13 points defining all the route. Table3 shows the x-y coordinates in meters (simulator absolute coordinate system), the reference speed in [m/s], and the type of point (1 for intersections and 2 for roundabouts). For roundabout global planning, additional information for its description is required: the radius (meters) and the angles of entrance and exit (radians). The previously described points are shown in Figure 9(a) with a cross marker.Table 3
Point description of the map used for global planning.
X Y V Type R a i a o [m] [ m ] [ m / s ] [ ] [ 1 / m ] [ r a d ] [ r a d ] 0.00 0.00 11.11 1 - - -  80.48 97.09 11.11 2 17.29 0.52 0.09  88.04 177.90 11.11 1 - - -  196.21 172.89 11.11 1 - - -  203.72 266.16 11.11 1 - - -  56.55 278.46 11.11 1 - - -  86.24 371.34 11.11 1 - - -  -49.30 397.61 11.11 2 17.76 0.00 0.00  5.08 281.16 11.11 1 - - -  -29.37 283.73 11.11 1 - - -  -39.17 187.34 11.11 1 - - -  50.16 180.36 11.11 1 - - -  53.19 227.94 11.11 1 - - -From this map a global plan is generated, which gives a better description of roundabouts in the bend segment (completion of the circle arc). In case of intersections, points in global planner coincide with those used to define the route in the scenario. All the global planner points are depicted as circles in Figure9(a).The soft and continuous black line represents the trajectory generated using the Bézier approach. The values ofD (designing parameter of the approach) are set by hand but they can be selected using optimization algorithm as in [31, 35].An interesting example of the entrance/exit angle in the roundabouts is the one located in the absolute coordinates[80.48;97.09]. This point is an entrance of a roundabout and it uses the definition of the entrance angle for its description (see (9)):(19)Pe=80.4897.09+17.29cos⁡-2.26sin⁡-2.26=69.483.8The entrance point to present a numerical example is shown, but the exit point can be calculated using the same data and (9).The speed planning defined for that trajectory is shown in Figure9(b). The continuous line is the reference acceleration given by the speed profile of the urban scenario and the application of a comfort parameter aω=0.5[m/s2]; based on [32] this amount of acceleration aw corresponds with a value between “not uncomfortable” and “a little uncomfortable” for vehicle passengers. Results show that vehicle succeed on tracking the generated speed profile (by tracking the acceleration profile shown in dashed line). One major cause was the consideration of the maximum acceleration and deceleration (red bounds of Figure 9(b)).Figure9(c) shows a surface plot representing the different values of acceleration aw with respect to the speed profile generated and the curvature of the path. Five different values of aω were used: 0.5, 1.0, 1.5, 2.0, and 2.5[m/s2] to generate the surface. Its objective is verifying the smooth and continued relationship with the Bézier speed profile and the level of comfort of the passenger.


## 5. Real Circuit Tests

The current section shows the trajectory planning approach used with the real vehicle. In this sense, two possible definitions of the route were implemented (Figure10(a)). For both the center of the lane is used (taken from a manual driving record using GPS) as guide for the implementation of trajectories.Figure 10
Real life validation of the Bézier planning approach.
(a)
Comparison in trajectory definition (b)
Comparison in trajectory definition (c)
Comparison in curvature for the trajectory definition case 2The first case is shown in the left side of Figure10(a), where the trajectory described by the manual driving is modeled using intersection points in the 180[∘] turn (roundabout). In this case, the error in the turning points (top and bottom of the circuit) is greater than using roundabout for the definition of the curve (right side Figure 10(a)).The trajectory in the right side was tested with 5 different speed profiles and the behavior of the vehicle was analyzed with the curvature. Figure10(b) depicts the results for these tests. Curvature of the vehicle in the trajectory is represented with a grey line and calculated according to the formula:(20)Kt=ωtcos⁡ΦtVltwhere K is the curvature, ω is the rate of change of yaw, Φ is the roll angle of the vehicle, and Vl is the longitudinal speed. Curvature of the path is depicted in black and the vehicle is able to replicate the behavior of the planned curvature (the overshoots in curvature are caused by controllers).Figure10(c) shows vehicle lateral acceleration (as function of the vehicle speed and the curvature of the circuit) using different speed profiles, generated with aw equal to 0.5, 1.0, 1.5, 2.0, and 2.5[m/s2]. Values of total acceleration slightly overpass the limit of 2.5 at some points because the actuators delay is not considered for longitudinal control and the consideration of ax≈0 in speed planning. Additionally, this figure shows the lateral acceleration in relationship with the curvature and the speed but without considering the total acceleration because in the speed planner those are assumed as zero. The lack of data taken, considerations over longitudinal acceleration, and controllers delay during tracking generate some surface holes without data. Table 4 shows the maximum acceleration obtained with the approach after filtering all the values ax>0.005[m/s2] (the approach considers ax≈0). It is relevant to understand that those values must be filtered because actions like control behavior, delay of the controllers, or even the longitudinal acceleration are not considered and the magnitudes can differ. In general, the results obtained are close to proposed values with parameter aω (designing).Table 4
Maximum lateral acceleration obtained in the real experiment using speed profile.
Uncomfortable levels Not A little Fairly Very Extreme Uncomfortable magnitudesaω[m/s2] 0.5 1.0 1.5 2.0 2.5 Filtered max. values[m/s2] 0.45 1.03 1.54 2.00 3.02


## 6. Conclusion and Future Works

This work presents a trajectory and speed planning approach based on Bézier curves used in the decision module of an automated vehicle control architecture. The approach considers urban scenarios with intersections, roundabouts, and lane changes but it is not limited to those; the approach could also be used in scenarios such as highways. In the case of the speed planner, a parametric curve approach to adapt future changes in the speed limit considering physical constraints of the vehicle and traffic rules is presented (i.e., considering the maximum speed limit of the road).Simplified mathematical models for trajectory planning using Bézier curves on intersections, roundabouts, and lane changes are presented reducing the time of calculation and allowing the use of these definitions for other applications, i.e., trajectory optimization process with computational efficiency. Additionally, the approach is evaluated using a simulated urban scenario based on a location of Basque Country showing that a great variety of urban scenario can be managed using this approach.The trajectory planner was tested for automated driving, using a comparison of the trajectory with the Bézier curves and the one generated using a manual GPS recording in manual driving. Additionally the generated curvature in the vehicle for different conditions (variations in the speed profile) was compared with the curvature of the trajectory planned showing that it is feasible to be tracked by the vehicle.The speed planning approach shows the capability of using parametric curves to generate a profile to be tracked correctly by the vehicle, giving a prediction of future behaviors. Consideration of longitudinal acceleration in curve segments is one of the future improvements related to the speed planner of this work.This approach is considered better than others due to the fact that it includes comfort and safety criteria during trajectory and speed planning. For comfort the ISO2631-1 standard to generate speed profiles based on geometric information given by the trajectory planning was considered. Comfortable speed profiles are related to trajectories and this can be associated with safety during driving (vehicle lateral forces are related to possible dangerous situations), and additionally a continuous curvature of the trajectory planned can reduce possible unexpected situations given by vehicle' automatic control (increasing safety).Some of the future works are related to using the presented method to generate possible optimal solutions in terms of the parameterD, road geometry, and vehicle dynamics and in such way, substituting the definition by hand of parameter D during trajectory planning.



---

*Source: 6060924-2018-07-15.xml*