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.
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: Section 2 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 where is the Bernstein polynomial, are the control points used to generate the curve, is the Bézier order, and 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 point , and the ending point corresponds with .(ii)The first point tangent vector (at ) will be given by and the last point tangent vector (at t = 1) will be given by .(iii)The curve will lie into the convex hull formed by the control points.(iv)Bézier curves are continuous geometrically and in curvature , and this continuity can be preserved in joints of two different curves.(v)Bézier curves are symmetric, and the generated curve for is equal to the one for .
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:where each will be a function of the control points (fixed position) and curve order. The values of coefficients are given in Table 1. and 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, and are defined as follows:where and are introduced and is the degree of the curve.
In general terms the curvature in is defined as [30]and in this case, function is the Bézier equation. Evaluating at , it is obtained:
The 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 (); 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 Figures 1, 2 and 3) is the input of the global method, decreasing the amount of points and keeping geometric qualities of road.
2.2.1. Intersections
Figure 1 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 bywhere is an unitary vector that goes from intersection to past global point and is other unitary vector formed with the next global points and . It is relevant to see that the angle (Figure 1) can be different from , modeling a great variety of intersections. The parameter will be used later in local planning and for the definition of .
2.2.2. Lane Changes
In this case, the approach considers straight segments during the maneuver (Figure 2). The vectors for path description defined in (6) can be used for lane change and they have the following relation:
Additionally, in this type of curve a new point is introduced to handle the maneuver in local planning. This point is defined by where is the road width and 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
Figure 3 shows the definition of a roundabout using a simple point. It uses roundabout center point , its radius , entrance angle , and exit angle . Using these parameters, the roundabouts can be modeled with an approximated circular shape.
Roundabout entrance and exit points are defined bywhere is roundabout entrance coordinates, is roundabout exit coordinates, values of and are defined by (6), but the intersection point is substituted by roundabout points and , 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, 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 will be given by .
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
Figure 1 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 to ) set over the same line will ensure curvature zero in the starting point of the curve ; 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 () with control points to (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 point will be set in the position of the intersection point . 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 and are given by (6).
The 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. Figure 4 shows the normalized curvature (curvature of the generated segment divided by the maximum curvature value) with respect to normalized trajectory distance (total distance of the generated segment divided by the maximum one), considering angles between straight segments from 20 degrees to 145 degrees.
With the purpose of finding the position of maximum curvature, it is applied derivative to (4):evaluating the derivative of Bézier curve in :where is the design parameter for trajectory and are unitary vectors defined by (3). From this set of equations it is obtained that resulting in curvature equal to 0 in (maximum value).
2.3.2. Roundabouts
Figure 3 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 in or at exit in points must be (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 not ). 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).
Table 2 shows the control points used for trajectory generation at entrance (RE) and exit (REx) of roundabouts. Control points from to in the case of entrance ( to for exit) are designed using similar criteria as in intersections, with as a designing parameter. The points at entrance or at exit are selected using the distance in the arc of the circle from entrance or exit depending on the case.
The point , in entrance trajectory, is separated of point , a distance given bywhere is the tangent vector (with the circle) in the point and is the roundabout curvature. In the case of exit, the point is substitute by and by .
Figure 5 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 and Figure 5(b) shows the trajectories curvature fitting perfectly the curvature radius .
(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 distance (Figure 2). Where the unitary vectors and are explained in (7), is the width of the road and is the separation in axis between each pair of control points .
The location of control points is given in Table 2 (LC). The minimum value of is set to in our design; these criteria yield to a maximum curvature in and 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 .
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. Figure 7 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 points and as . These conditions generate Bézier curve x-coordinates proportional to the parameter given by the equationwith this equation is seeing that the distance in x-axis and the parameter have a proportional relationship. It is introduced 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 change , the resulting equation will bewhere the variable is longitudinal acceleration, is longitudinal speed, is time, and position. Doing a nomenclature change from and will result in the equation
Applying derivative to (15) maximum points of acceleration can be found. The resulting equation is
From (16), introducing the 5th order Bézier equation and doing some approximations, the following roots can be obtained:providing 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 equationwhere is the total acceleration felt by passengers (comfort criteria given by ISO2631-1); longitudinal acceleration and vertical acceleration contributions are approximated to zero (small changes ), is longitudinal speed, and is curvature in each point of the path.
Figure 6(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.
(a) Speed planning with overlapped intersections
(b) Speed planning without overlapped parts
Finally, Figure 6(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].
Figure 8 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.
The decision module is the main target of the current work (grey coloured module in Figure 8) 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.
(a) Trajectory generated with the approach
(b) Comparison of the longitudinal acceleration
(c) Comparison of the path curvature and simulated vehicle one
The 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. Table 3 shows the x-y coordinates in meters (simulator absolute coordinate system), the reference speed in , 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.
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 Figure 9(a).
The soft and continuous black line represents the trajectory generated using the Bézier approach. The values of (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 . This point is an entrance of a roundabout and it uses the definition of the entrance angle for its description (see (9)):The 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 Figure 9(b). The continuous line is the reference acceleration given by the speed profile of the urban scenario and the application of a comfort parameter ; based on [32] this amount of acceleration 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)).
Figure 9(c) shows a surface plot representing the different values of acceleration with respect to the speed profile generated and the curvature of the path. Five different values of were used: , , , , and 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 (Figure 10(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.
(a) Comparison in trajectory definition
(b) Comparison in trajectory definition
(c) Comparison in curvature for the trajectory definition case 2
The first case is shown in the left side of Figure 10(a), where the trajectory described by the manual driving is modeled using intersection points in the 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. Figure 10(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:where is the curvature, is the rate of change of yaw, is the roll angle of the vehicle, and 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).
Figure 10(c) shows vehicle lateral acceleration (as function of the vehicle speed and the curvature of the circuit) using different speed profiles, generated with equal to , , , , and . Values of total acceleration slightly overpass the limit of at some points because the actuators delay is not considered for longitudinal control and the consideration of 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 (the approach considers ). 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 (designing).
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 parameter , road geometry, and vehicle dynamics and in such way, substituting the definition by hand of parameter during trajectory planning.
Data Availability
The datasets generated during and/or analyzed during the current study are available from the corresponding author on reasonable request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was partly supported by ECSEL Project ENABLE-S3 with Grant Agreement no. 692455-2 and the AutoDrive ECSEL Project with Grant Agreement no. 737469.