energy constrained trajectory generation for adas

6
Energy Constrained Trajectory Generation for ADAS er´ emie Daniel, Abderazik Birouche, Jean-Philippe Lauffenburger and Michel Basset Abstract— This paper presents a new constrained trajectory generation method dedicated to Advanced Driver Assistance Systems (ADAS). Based on the information provided by the digital map database of a navigation system, and considering different constraints related to the road profile, the vehicle and the driver, a convex optimization algorithm generates specific Spline-based trajectories. Characteristically, these trajectories are safe, they stay within the traffic lane borders, at the same time minimize an energy criterion along the path, and finally, they are curvature continuous. The present solution has been tested on several roads and the results show the efficiency of the energy constrained trajectory generation method. I. I NTRODUCTION Trajectory generation has been widely used in different domains as it can provide a set of continuous information which is helpful for control-oriented applications. In addi- tion, most of the path generation solutions include constraints which help to define the optimal or suboptimal trajectory [1]. There are numerous trajectory generation solutions, based on the use of several mathematical models. Parametric Cubic Splines [2] are used here, because they represent a straightforward interpolation, they have a curvature continu- ity property, and they are adapted to all road contexts (bend, straight line, roundabout, etc.), contrary to the other models. This paper presents a new constrained trajectory gen- eration approach to control-oriented ADAS, based on the information provided by a digital map database. Considering the digital map limitations [3], the proposed solution is a map-based continuous trajectory generation formulated as an optimization problem. Contrary to recent research works such as [4] which focuses on road curve reconstruction and curvature estimation, the presented study is dedicated to the definition of a curvature-continuous trajectory for ADAS control applications, such as Lane Departure Avoidance, Longitudinal Control, etc. Taking account of the geometric constraints related to the road profile which keep the vehicle in a specified driving area, the constrained trajectories gen- erated guarantee robustness regarding to the inaccuracies of the data provided by the digital map [3]. The main contribution of this paper lies in the integration of different constraints related to the road, to the vehicle and to the driver. This constrained generation is formulated as a convex optimization problem tending to minimize a given cost criterion. If most optimizations minimize the time required to cover the trajectory or the trajectory distance, here the strain energy of the trajectory is minimized. As this J. Daniel, A. Birouche, J.P. Lauffenburger and M. Basset are with the Mod´ elisation Intelligence Processus Syst` emes (MIPS) labora- tory, 12 rue des fr` eres Lumi` ere, 68093 Mulhouse Cedex, France. (E- mail:[email protected]). criterion is directly linked to the curvature, its minimization leads to the generation of smooth trajectories which can be of great help for trajectory tracking. Indeed, the proposed trajectories can be used as references for such systems thus replacing the usual trajectory represented by the road lane centreline. To check the validity of the present solution, tests were carried out using real road data. The results have shown that the new method leads to the generation of safe and smooth curvature continuous lane trajectories. After the description of the work context in Section.II, Section.III describes the trajectory generation approach. Section.IV then presents the results obtained with the trajec- tory generation process, finally followed by the conclusion in Section.V. II. WORK CONTEXT A. Problem Statement The solution proposed here is devoted to a trajectory gen- eration approach for control-oriented ADAS taking account of constraints limiting the behaviour of a Wheeled Rolling System ( WRS) and thus the control inputs to be generated. The objective is to provide a reference trajectory which can be followed by a WRS satisfying several constraints linked to the vehicle, the road to be followed and finally the driver. Giving a starting configuration q 0 =(x 0 , y 0 , θ 0 , κ 0 , ˙ κ 0 ) D 0 R 5 defined by the WRS’s Centre of Gravity ( CoG) position (x 0 , y 0 ), the WRS’s orientation (θ 0 ), an initial curvature κ 0 of the path to be followed by the CoG, its respective deriva- tive ˙ κ 0 and a final configuration q n =(x n , y n , θ n , κ n , ˙ κ n ), an optimization technique defines the reachable path regarding the optimization criterion considered. A trajectory is a con- tinuous sequence of achievable configurations (x, y , θ , κ , ˙ κ ) defined with respect to the constraints to be verified. In the approach described, the road is considered to be flat and only 2-dimensional parametric paths (x(t ), y(t )) defined in R 2 are computed. The sequence of reachable configurations is defined, based on constraints of a different nature (geometric, dynamic and kinematic). B. Multiple Constrained Path Generation WRS-like cars are known to be non-holonomic systems. These systems are subject to limitations in the way they move: not all the solutions of the configuration space are possible and limitations in the directions of motion have to be processed [5]. This study considers three types of constraints. The first of these limitations are geometric constraints linked, on the one hand, to the configuration space in which the WRS moves (to keep the vehicle in a prescribed driving area) and on

Upload: independent

Post on 12-Nov-2023

0 views

Category:

Documents


0 download

TRANSCRIPT

Energy Constrained Trajectory Generation for ADAS

Jeremie Daniel, Abderazik Birouche, Jean-Philippe Lauffenburger and Michel Basset

Abstract— This paper presents a new constrained trajectorygeneration method dedicated to Advanced Driver AssistanceSystems (ADAS). Based on the information provided by thedigital map database of a navigation system, and consideringdifferent constraints related to the road profile, the vehicle andthe driver, a convex optimization algorithm generates specificSpline-based trajectories. Characteristically, these trajectoriesare safe, they stay within the traffic lane borders, at the sametime minimize an energy criterion along the path, and finally,they are curvature continuous. The present solution has beentested on several roads and the results show the efficiency ofthe energy constrained trajectory generation method.

I. INTRODUCTION

Trajectory generation has been widely used in differentdomains as it can provide a set of continuous informationwhich is helpful for control-oriented applications. In addi-tion, most of the path generation solutions include constraintswhich help to define the optimal or suboptimal trajectory[1]. There are numerous trajectory generation solutions,based on the use of several mathematical models. ParametricCubic Splines [2] are used here, because they represent astraightforward interpolation, they have a curvature continu-ity property, and they are adapted to all road contexts (bend,straight line, roundabout, etc.), contrary to the other models.

This paper presents a new constrained trajectory gen-eration approach to control-oriented ADAS, based on theinformation provided by a digital map database. Consideringthe digital map limitations [3], the proposed solution is amap-based continuous trajectory generation formulated asan optimization problem. Contrary to recent research workssuch as [4] which focuses on road curve reconstruction andcurvature estimation, the presented study is dedicated to thedefinition of a curvature-continuous trajectory for ADAScontrol applications, such as Lane Departure Avoidance,Longitudinal Control, etc. Taking account of the geometricconstraints related to the road profile which keep the vehiclein a specified driving area, the constrained trajectories gen-erated guarantee robustness regarding to the inaccuracies ofthe data provided by the digital map [3].

The main contribution of this paper lies in the integrationof different constraints related to the road, to the vehicleand to the driver. This constrained generation is formulatedas a convex optimization problem tending to minimize agiven cost criterion. If most optimizations minimize the timerequired to cover the trajectory or the trajectory distance,here the strain energy of the trajectory is minimized. As this

J. Daniel, A. Birouche, J.P. Lauffenburger and M. Basset arewith the Modelisation Intelligence Processus Systemes (MIPS) labora-tory, 12 rue des freres Lumiere, 68093 Mulhouse Cedex, France. (E-mail:[email protected]).

criterion is directly linked to the curvature, its minimizationleads to the generation of smooth trajectories which can beof great help for trajectory tracking. Indeed, the proposedtrajectories can be used as references for such systems thusreplacing the usual trajectory represented by the road lanecentreline. To check the validity of the present solution, testswere carried out using real road data. The results have shownthat the new method leads to the generation of safe andsmooth curvature continuous lane trajectories.

After the description of the work context in Section.II,Section.III describes the trajectory generation approach.Section.IV then presents the results obtained with the trajec-tory generation process, finally followed by the conclusionin Section.V.

II. WORK CONTEXT

A. Problem Statement

The solution proposed here is devoted to a trajectory gen-eration approach for control-oriented ADAS taking accountof constraints limiting the behaviour of a Wheeled RollingSystem (WRS) and thus the control inputs to be generated.The objective is to provide a reference trajectory which canbe followed by a WRS satisfying several constraints linkedto the vehicle, the road to be followed and finally the driver.Giving a starting configuration q0 = (x0,y0,θ0,κ0, κ0)∈D0⊂R5 defined by the WRS’s Centre of Gravity (CoG) position(x0, y0), the WRS’s orientation (θ0), an initial curvature κ0of the path to be followed by the CoG, its respective deriva-tive κ0 and a final configuration qn = (xn,yn,θn,κn, κn), anoptimization technique defines the reachable path regardingthe optimization criterion considered. A trajectory is a con-tinuous sequence of achievable configurations (x,y,θ ,κ, κ)defined with respect to the constraints to be verified. In theapproach described, the road is considered to be flat and only2-dimensional parametric paths (x(t), y(t)) defined in R2

are computed. The sequence of reachable configurations isdefined, based on constraints of a different nature (geometric,dynamic and kinematic).

B. Multiple Constrained Path Generation

WRS-like cars are known to be non-holonomic systems.These systems are subject to limitations in the way theymove: not all the solutions of the configuration space arepossible and limitations in the directions of motion have tobe processed [5].

This study considers three types of constraints. The first ofthese limitations are geometric constraints linked, on the onehand, to the configuration space in which the WRS moves(to keep the vehicle in a prescribed driving area) and on

Fig. 1. Vehicle in the Lane

the other hand, to the mechanical limits (bounded steeringangle) of the WRS. Due to the non-holonomy property ofWRS, these vehicles are subject to kinematic constraints. Thissecond type of constraints involves (linear and rotational)velocity and acceleration limits [6] of the kinematic variablesdescribing the WRS. Finally, the third category of constraintsare dynamic constraints which characterize the performanceof the vehicle (braking/accelerating capabilities, maximumlateral acceleration, tyre/ground interaction etc.) and theactuators performance. They restrict the overall performanceof the WRS.

C. Definition of the Constraints

1) Geometric Constraints: The geometric constraints areof two types: limitations related to the configuration spaceand limitations related to the mechanical conception of theWRS. The first constraints define the area prescribed in theconfiguration space, i.e. the set of possible configurations,in which the WRS is allowed to move. Considering car-likevehicles, this area is defined by the road on which the vehicleis located. Normal driving conditions obviously imply thatthe WRS stays in its current road lane. Consider the situationdescribed in Fig.1 representing the road driving lane: the totallane width is L and the reachable driving area, consideringhalf of the vehicle width (W + ε)/2 (with ε due to thepositioning error), is represented by a strip with a width ofVAW = L− (W + ε).

Concerning the mechanical limitations of WRS, the lim-itations of the steering system are considered, which implya minimum break radius of the vehicle Rmin. This turningradius is lower-bounded and consequently, the trajectoryinstantaneous curve radius must be constantly greater thanthe lower bound vehicle turning radius:

κtra jectory =1

Rtra jectory< κmax1 =

1Rmin

(1)

Finally, in order to avoid any steering function disconti-nuities and since the steering angle is directly dependent onthe instantaneous curvature of the trajectory to be followed,a continuous-curvature trajectory is necessary. So, the gen-erated trajectories must be C2 continuous.

2) Kinematic Constraints: The kinematic constraints de-pend on the speed profile along the trajectory. The mainkinematic constraint to be considered for WRS is the lim-itation of the steering velocity. It is well known that thedynamic behaviour of the steering system is upper bounded.

Since the steering velocity is related to the derivative ofthe curvature, this implies that the trajectory instantaneouscurvature is upper-bounded:

κtra jectory < κmax (2)

3) Dynamic Constraints: These constraints are due tothe limited and often nonlinear dynamic behaviour of theWRS and its subsystems (bounded acceleration capabilities,variable ground/wheel interaction, etc.). They mainly influ-ence the longitudinal and lateral accelerations and thus thevelocities of the WRS. In order to provide safe trajectories,the maximum centrifugal acceleration allowed for curvenegotiation is considered here. In the literature, differentmodels linking the centrifugal acceleration to the trajectorycurvature are available. This paper focuses on a simplifiedmodel given by:

κmax2 =Γmax

v2 (3)

with Γmax the maximum allowed lateral acceleration andv the vehicle speed. Note that in the automotive domain,the driver is sensitive to the accelerations and mainly to thelateral acceleration. Considering a maximum value of accel-eration Γmax, driver-dependent factors are taken in account.This relation implies that the path curvature must be upperbounded in order to ensure a limited centrifugal acceleration:

κtra jectory < κmax2 (4)

D. Energy Cost CriterionAn interesting solution to generate constrained trajectories

is to formalize the problem as an optimization problem.Indeed, optimizers are powerful tools into which constraintscan be easily integrated and generate optimal trajectories, re-garding these constraints. Furthermore, the different types ofoptimization algorithms [7], mostly allow the minimizationof a cost criterion which can be defined, regarding variouselements or parameters. Common criteria are no criterion,Trajectory length, Trajectory cover time, etc. In the proposedconstrained trajectory generation, the strain energy criterionhas been chosen. The latter is expressed according to thetrajectory curvature κ as follows [8]:

E =∫

κ2ds (5)

This criterion is in accordance with the aforementionedconstraints, especially with the constraints of a curvature-dependent expression. In addition, if this energy is onlylinked to the trajectory geometry, its minimization impliessmoothing the trajectory curvature which is directly linkedto the energy consumption of the vehicle.

III. NAVIGATION-BASED CONSTRAINED TRAJECTORYGENERATION

A. StrategyFig.2 shows how the trajectory generation interacts with

the other components of the present ADAS control applica-tion structure. The different elements of a navigation system

Fig. 2. ADAS Control Application Architecture

can be found: a GPS Receiver, a Map-Matching Algorithm, aDigital Map Database and an Electronic Horizon Provider.The Electronic Horizon (EH) is used as the source of infor-mation for the three main components of this structure: theSituation Classification, the Road Model Estimation (whichis also used for the constraints definition) and the TrajectoryGeneration. Note that this paper only describe the trajectorygeneration process.

The strategy adopted for constrained trajectory generation,presented in Fig.3, is divided in two parts: the Road ModelEstimation and the Trajectory Generation. The Road ModelEstimation uses the road centerline shape points which areextracted from the EH. Based on these EH points, the firststep is to use a transformation algorithm which generatesthe road boundary points. These points are then used by aSpline algorithm which provides continuous boundaries andgives the required information to the Trajectory Generationprocess. The latter uses the road model and the vehicle widthto define the trajectory validity area (cf. Fig.1). This validityarea is then used as the template for the optimization processwhich also includes the other aforementioned constraints (cf.Section.II-C) and which minimizes the trajectory energy.

B. Mathematical model

Among all the different mathematical models, the Para-metric Cubic Splines model [2] has been chosen. Indeed,this mathematical model, mostly used in computer graphics,provides smooth curvature-continuous trajectories contraryto other solutions such as arc-circle methods for instance. Inaddition, parametrization allows the computation of nearly alltwo-dimensional trajectories. A Cubic Spline is a piecewisepolynomial interpolation. Contrary to basic interpolationmethods, it avoids the use of large degree polynomials,which leads to trajectory oscillations. The two-dimensionalParametric Cubic Spline used here, with t the parameter, isof the following form:

fi (t){

fxi (t) = a fxit3 +b fxi

t2 + c fxit +d fxi

fyi (t) = a fyit3 +b fyi

t2 + c fyit +d fyi

t ∈ [ti, ti+1] , i = 1,2, ...,(n−1) with: t1 < t2 < ... < tn(6)

Fig. 3. Trajectory Generation Strategy

n being the number of interpolated points.To provide the curvature continuity along the trajectory,

first and second derivative continuity at each interpolatedpoint is necessary. This is obtained by solving a linearsystem for each Cartesian coordinate. However, for one set ofinterpolated points, an infinite number of Splines is possible.These Splines are defined depending on two major elements:boundary conditions and parameter values. As they can havestrong effects on the Spline shape, they must be correctly pre-defined. Information about the conditions and the parametervalues selection can be found in [9].

C. Optimization Problem Formulation

This section describes the integration of the differentconstraints (cf. Section.II-C) into the selected mathematicalmodel. Considering that Splines have polynomial expres-sions, the goal of the present trajectory generation process isto find the optimal coefficients a fSi

, b fSi, c fSi

and d fSiwith:

Si = [xi,yi]T , i = 1,2, ...,n− 1 for the constrained trajectoryfi(t) (cf. 6) with t defined in [0,1].

1) Inequality Constraints: Let gi(t) and ei(t) be thetwo boundary curves of the validity area. The optimizationalgorithm must find a Spline fi(t) such that:

ei (t)≤ fi (t)≤ gi (t) with:gi (t) = gSi (t) = agSi

t3 +bgSit2 + cgSi

t +dgSiei (t) = eSi (t) = aeSi

t3 +beSit2 + ceSi

t +deSi

(7)

To generate a Spline which is constricted by the validityarea boundaries, the difference between the desired Splineand the upper and lower bounds must be negative or positiverespectively:

fSi (t)−gSi (t)≤ 0 and fSi (t)− eSi (t)≥ 0 (8)

Remember that the optimization goal is to find the optimalset of Spline coefficients. The current requirement is also to

find Spline positivity conditions regarding the coefficients.In the literature, several studies have been carried out onthe positivity of cubic polynomials. It has been shown thatpositivity conditions for a cubic polynomial f (t) = at3 +bt2 + ct + d are described with t ∈ [0,1] by two sets ofinequalities [10]. The present study focuses on the followingset of inequalities:

f (t)≥ 0⇒ (a,b,c,d) ∈ A, with:A = {a+b+ c+d ≥ 0,b+2c+3d ≥ 0,c+3d ≥ 0,d ≥ 0}

(9)The adaptation of (9) to the present context gives the set

of linear inequalities presented here with Γ = {eSi , fSi ,gSi}:αeSi≤ α fSi

≤ αgSiβeSi≤ β fSi

≤ βgSiγeSi≤ γ fSi

≤ γgSiδeSi≤ δ fSi

≤ δgSi

with:

αΓ = aΓ +bΓ + cΓ +dΓ

βΓ = bΓ +2cΓ +3dΓ

γΓ = cΓ +3dΓ

δΓ = dΓ

(10)2) Equality Constraints: In addition to the inequalities,

the optimization process must fulfil the C0, C1 and C2

continuity requirements. For Parametric Cubic Splines, thisis expressed by:

fSi (ti+1) = fSi+1 (ti+1)fSi (ti+1) = fSi+1 (ti+1)fSi (ti+1) = fSi+1 (ti+1)

a fSi

+b fSi+ c fSi

+d fSi= d fSi+1

3a fSi+2b fSi

+ c fSi= c fSi+1

6a fSi+2b fSi

= 2b fSi+1(11)

It can be noted that geometric constraints (limitations ofthe configuration space and continuity of the trajectory) areexplicitly formulated in the optimization through (10) and(11) and that the kinematics and dynamics constraints areimplicitly described by the criterion to be minimized (theminimization of the strain energy should grant low curvaturevariations and values). However, a post-checking of thesehypotheses is performed after the optimization. Note that thechecking of the curvature derivative κ is based on [5] and isexpressed in a conservative way by:

κ =φ

vb(12)

with v a constant velocity, b the vehicle wheelbase and φ

the wheel angle.

D. Cost Criterion

As mentioned previously, the selected cost criterion isthe trajectory strain energy which is directly linked to thecurvature κ:

E =∫

κ2ds (13)

with :κ (x(t),y(t), t) =

y(t) x(t)− x(t) y(t)

(x2 (t)+ y2 (t))32

(14)

A curve minimizing this bending energy is known as aminimal energy Spline [11]. Nevertheless, replacing x(t) and

y(t) functions into their formal expressions, involve (13) tobecome non-linear. The optimal solution minimizing (13) isalso hard to formulate and to calculate. To overcome thisproblem, a suboptimal solution is determined. Consideringthe expressions of x = fx(t) and y = fy(t), the objective isto simultaneously minimize the curvature of each parametriccurve κx and κy given by:

κx (x(t) , t) =x(t)

(1+ x2 (t))32

and κy (y(t) , t) =y(t)

(1+ y2 (t))32

(15)Considering that y(t)2 and x(t)2 are small compared with

1 as in [12] and [13], the energy of the suboptimal solution,corresponding to the cubic interpolation Spline with C2

continuity, is assumed to be:

E =tn∫

t0

(x2 + y2)dt (16)

Considering the Spline expression and its continuity prop-erties, the continuous expression of the suboptimal energyE can be discretized using the classic Euler method withhi = ti+1− ti such that [13] :

E =n−2

∑i=1

4hi

3

(b2

fxi+b fxi

b fxi+1+b2

fxi+1

)

+n−2

∑i=1

4hi

3

(b2

fyi+b fyi

b fyi+1+b2

fyi+1

)(17)

E. Optimization

The trajectory generation process presented in this paperand formulated as an optimization problem has to fulfil theaforementioned conditions: linear inequality constraints (cf.(10)), linear equality constraints (cf. (11)) and a discretizedquadratic cost criterion (cf. (17)). There are several optimiza-tion techniques which help to solve such systems (cf. [7] foradditional information). However, considering the differenttypes of constraints and the fact that they all have a linearor quadratic expression, the convex quadratic programmingoptimization approach has been chosen:

Θ∗ = minx

12 ΘT HΘ+ f T Θ such that:

{AΘ = BCΘ≤ D (18)

with Θ ∈ R2×4×(n−1) the optimal coefficients of fi(t)interpolating n points such that:

Θ = [a fS1,b fS1

,c fS1,d fS1

, ...,a fSn−1,b fSn−1

,c fSn−1,d fSn−1

]T

(19)It is clear that this algorithm is well suited for the present

problem since:• The equality and inequality constraints can respectively

be written in the AΘ = B and CΘ≤ D matrix form,• The energy criterion, due to its discrete formulation, is

only expressed using the quadratic terms of the different

b fxiand b fyi

coefficients. It can also be easily expressedin the matricial expression ΘT HΘ but, as there are onlyquadratic elements, f T Θ = 0,

• This optimization is convex quadratic, so there is onlyone global minimum,

• This algorithm does not need long calculation timewhich may coincide with real-time constraints.

IV. TESTING RESULTS

A. Testing Conditions

Validation tests of the trajectory generation process werecarried out using data collected from previous tests. Thesedata correspond to the EH shape points of classic countryroads. Stored shape points are used as the basic informationfor the road estimation and for the Trajectory Generationprocess (cf. Fig.3). The results presented in this Section wereobtained using two different trajectory generation processes.The first process only includes the geometric constraintswhile the second one presents the results obtained with theaddition of the Spline strain energy minimization. The aimis to clearly show the impact of the energy minimization onthe generated trajectories. Results are described in Fig.5 andfor clarity reasons, the road boundaries have been removedfrom the figure. Only the validity area boundaries and thetwo trajectories are presented. Fig.6 presents the associatedcurvatures: the road centerline curvature (which is not rep-resented in Fig.5) and the curvatures of both constrainedtrajectories. A 7m width road and a 2m width vehicle havebeen taken for this test.

B. Results

Fig.5 presents the trajectory generation results for a leftbend which is not a constant-curvature bend. Globally thedifferences between the trajectories cannot be distinguishedeasily, due to the narrow validity area. However, it can benoticed that the trajectory which minimizes the energy (dash-dotted line) uses the lateral space available in the validityarea more efficiently: it successively uses the external andthe internal parts of this area, contrary to the other trajectory(dashed line) which stays close to the validity area centre.It can therefore be guessed that the curvature shape for thetrajectory which does not minimize the energy will be veryclose to the road centreline curvature.

This guess is confirmed in Fig.6. Indeed, the trajectorywhich does not minimize the energy presents a shape similarto that of the road centreline. In addition, it is clear that thetrajectory minimizing the energy smoothes the curvature.

A consequence of this smoothing is that the trajectory isgranting the strict constraint on the curvature presented in (1).Indeed, the maximal curvature value passes from 0.11m−1 to0.07m−1 which is lower than the car maximum curvature of0.09m−1 (taking an average turn radius of 11m). Consideringthe constraint described by (3) and (4), the maximum curva-ture of the best trajectory which is 0.07m−1 and a legal speedlimit of 30km.h−1 (given by the map for this situation whenfollowing the trajectory), a lateral acceleration of 4.9m.s−2 isobtained. This is larger than the common lateral acceleration

0 20 40 60 80 100 120 140-0.04

-0.02

0

0.02

0.04

0.06

0.08

0.1

0.12

0.14Curvature Comparison

Distance from start point (m)

Cu

rvat

ure

(m

-1)

Road middle line curvatureSpline without energy minimization curvatureSpline with energy minimization curvature

Fig. 6. Test Curvatures

TABLE IENERGY COMPARISON

Test 1 Test 2 Test 3 Test 4 Test 5 Test 6ENME 0.0119 0.0364 0.0536 0.0222 0.0387 0.0554EWME 0.0106 0.0361 0.0517 0.0151 0.0364 0.0493Efficiency 10.5% 0.8% 3.7% 31.9% 6.1% 11.1%

value used for comfortable driving (3m.s−2). A solution tothis problem could be to reduce the vehicle speed. Finally theconstraints put on the curvature derivative (2) is granted asthe generated trajectory has a maximum curvature derivativevalue of 0.029m−1.s−1 which is lower than the threshold of0.038m−1.s−1 (obtained by using (12) with a wheelbase of2.5m, a speed of 30km.h−1, and a maximum steering anglespeed of 15.7rad.s−1 achievable with an electrically drivensteering wheel).

C. Energy Comparison

Table.I presents the different energies computed respec-tively without (ENME ) and with (EWME ) the energy mini-mization in the trajectory generation process. In addition, thereduction efficiency is described (E f f iciency = ENME−EWME

ENME).

Note that the first test of this table corresponds to the testpresented in the previous section. Five additional tests werealso carried out using other real data measurements. It mustfirst be noted that the integration of the energy criterion hasa positive impact on the smoothness of the trajectories (theenergy is reduced in all cases). Then, the energy reductionis variable as it passes from 0.8% to 31.9%. The variationof the minimization efficiency is mainly due to the initialsmoothness of the road geometry.

V. CONCLUSION

Considering the different requirements for safe, comfort-able driving, this paper has proposed a new solution for thegeneration of constrained trajectory through the integrationof constraints into a Parametric Cubic Spline model andthrough the formulation of the problem as an optimiza-tion problem. The information provided by the digital map

Fig. 4. Test Trajectories Zoom

-60 -50 -40 -30 -20 -10 0-50

-45

-40

-35

-30

-25

-20

-15

-10

-5

0

Constrained trajectory generation

X coordinate (m)

Y c

oo

rdin

ate

(m

)

Validity area boundariesConstrained trajectory without energy minimizationConstrained trajectory with energy minimization

Fig. 5. Test Trajectories

database of a navigation system helps to generate the tra-jectory validity area (corresponding to the limit boundariesof the reachable trajectories) which is useful in the formal-ization of the constraints as a convex optimization problem.In addition, the minimization of the trajectory strain energyhas been implemented to generate suboptimal trajectories.The test results have shown the efficiency of the solution asthe trajectories were always located in the validity area andminimized the energy. This prepares the way for numerousapplications in the automotive domain, but also in others ason-ground aeronautics or wheeled robotics.

In the future, the proposed solution will be improved withthe explicit integration of the dynamic and kinematic con-straints presented in (2) and (4) directly in the optimizationformulation, or considering additional constraints related tothe vehicle (wheel/road interaction, etc.), to the road profile(elevation), and to the driver. Another research line can berelated to the modification of the cost criterion (distancedependent, etc.).

REFERENCES

[1] F. Gomez-Bravo, F. Cuesta, A. Ollero, and A. Viguria. Continuouscurvature path generation based on ß-spline curves for parking ma-noeuvres. Robotic Autonomous Systems, 56(4):360–372, 2008.

[2] C. De Boor. A practical guide to spline. New York: Springer-Verlag,1978.

[3] CAMP. Enhanced digital mapping project final report. United StatesDepartment of Transportation, Washington D.C., 2004.

[4] K. Li, H.S. Tan, J. Misener, and K.J. Hedrick. Digital map as a virtualsensor - dynamic road curve reconstruction for a curve speed assistant.Vehicle System Dynamics, 46(12):1141–1158, December 2008.

[5] T. Fraichard and A. Scheuer. From reeds and shepp’s to continuous-curvature paths. IEEE Transactions on Robotics and Automation,20:1025–1035, 2004.

[6] T.J. Graettinger and B. H. Krogh. Evaluation and time-scaling oftrajectories for wheeled mobile robots. Journal of dynamic systems,measurement, and control, vol. 111:222–231, 1989.

[7] S. Boyd and L. Vandenberghe. Convex Optimization. CambridgeUniversity Press, March 2004.

[8] H. Delingette, M. Hebert, and K. Ikeuchi. Trajectory generation withcurvature constraint based on energy minimization. In InternationalWorkshop on Intelligent Robots and Systems (IROS), Osaka, Japan,1991.

[9] J. Daniel, C. Truong, J-P. Lauffenburger, and M. Basset. Real-timetrajectory generation for advanced driver assistance systems applica-tions. In International Forum On Strategic Technologies (IFOST09),Ho Chi Minh, Vietnam, 2009.

[10] J.W. Schmidt and W. Heß. Positivity of cubic polynomials on inter-vals and positive spline interpolation. BIT Numerical Mathematics,28(2):340–352, June 1988.

[11] B.K.P. Horn. The curve of least energy. ACM Transaction onMathematical Software, 9:441–460, 1983.

[12] J. Ye and R. Qu. Fairing of parametric cubic splines. MathematicalAnd Computer Modelling, 30:121–131, 1999.

[13] D.S.G. Pollock. Smoothing with cubic splines. In Handbook of TimeSeries Analysis, Signal Processing, and Dynamics, pages 293 – 322.Academic Press, London, 1999.