Conference PaperPDF Available

Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

Authors:
Conference Paper

Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame

Abstract and Figures

Safe handling of dynamic highway and inner city scenarios with autonomous vehicles involves the problem of generating traffic-adapted trajectories. In order to account for the practical requirements of the holistic autonomous system, we propose a semi-reactive trajectory generation method, which can be tightly integrated into the behavioral layer. The method realizes long-term objectives such as velocity keeping, merging, following, stopping, in combination with a reactive collision avoidance by means of optimal-control strategies within the Frenét-Frame of the street. The capabilities of this approach are demonstrated in the simulation of a typical high-speed highway scenario.
Content may be subject to copyright.
OptimalTrajectoryGenerationforDynamicStreetScenariosina
Fren´
etFrame
MoritzWerling,JuliusZiegler,S¨
orenKammel,and SebastianThrun
AbstractSafehandling ofdynamic highway and inner city
scenarioswithautonomousvehiclesinvolvestheproblemof
generatingtrafc-adaptedtrajectories.Inorder to account for
thepracticalrequirementsof theholistic autonomous system,
weproposeasemi-reactivetrajectory generationmethod,which
can betightly integratedintothebehaviorallayer.Themethod
realizeslong-termobjectives suchasvelocitykeeping,merging,
following,stopping,in combinationwithareactive collision
avoidance bymeansofoptimal-controlstrategieswithin the
Fren´
et-Frame[12]of thestreet.The capabilitiesof this ap-
proachare demonstratedin thesimulationofatypicalhigh-
speed highway scenario.
I.INTRODUCTION
A.Motivation
Thepast three decadeshavewitnessedambitiousresearch
inthe area ofautomated driving.Asautonomousvehicles
advance toward handling realisticroadtrafc,theyface street
scenarioswheredynamicsofothertrafcparticipantsmust
be consideredexplicitly.Thisincludesevery day driving
maneuverslikemerging intotrafcow,passing with on-
coming trafc,changing lanes,oravoiding othervehicles.
Undersimpliedconditions,suchasduring the2007 DARPA
Urban Challenge1,thiscan betackledwithfairlysimple
heuristicsand conservative estimates[18].However,these
approachesquicklyreachtheirlimitsin nose-to-tail trafc
and athigh driving speedsresulting in poorperformance or
evenaccidents[5].Thisiswheretrajectoryconceptscome
into play,whichexplicitlyaccountforthetimeton the
planning and execution level.
Thepresentedmethod embarkson this strategy and sets
itselfapartfrompreviousworkinthat it isespecially
suitableforhighway driving,asit generatesvelocityinvari-
antmovement2and transfersvelocityand distance control
M.Werling iswiththeDepartmentofAppliedComputerScience and
Automation (AIA),University ofKarlsruhe,76131 Karlsruhe,Germany
moritz.werling@iai.fzk.de
J.ZiegleriswiththeDepartmentofMeasurementand Control(MRT),
University ofKarlsruheziegler@mrt.uka.de
S.Kammel iswithRobertBoschLLC Researchand Technology Center,
PaloAlto,California94304 soeren.kammel@us.bosch.com
S.Thrun iswithStanfordArticialIntelligence Laboratory,Stanford
University,Stanford,California94305 thrun@stanford.edu
The authorsgratefullyacknowledgethe cooperation betweenthe “Valley
rallyprojectofStanfordUniversityand theGermanTransregionalCol-
laborativeResearchCenter28 Cognitive Automobiles.Both projectscross-
fertilizedeach otherand revealedsignicantsynergy.
1TheDARPA UrbanChallengeisaresearch programconductedina
competitiveformat toaddress the challengesofautonomousdriving,see
http://www.darpa.mil/grandchallenge.
2It ishighly desirableto generatelane change and merging maneuvers,
whicharetimedcompletelyindependentlyfromthe absolutetravelling
speed.
totheplanning level.Additionally,the algorithmprovides
for reactiveobstacle avoidance by thecombinedusageof
steering and breaking/acceleration.
B.Relatedwork
Severalmethodsfortrajectory planning havebeen pro-
posed[11],[19],[2],[4]thatnd aglobal trajectorycon-
necting astartand a-possibly distant-goalstate.However,
thesemethodsfail tomodel theinherentunpredictability
ofothertrafc,and theresulting uncertainty,giventhat
theyrely on preciseprediction ofothertrafcparticipants
motionsoveralong timeperiod.Otherapproachestaken
towardstrajectory planning followadiscreteoptimization
scheme(e.g.[16],[1],[7]):Anitesetoftrajectoriesis
computed,typically by forwardintegration ofthedifferential
equationsthatdescribevehicledynamics.Fromthis set,the
trajectoryischosenthatminimizesagivencostfunctional.
Forgeneration ofthetrajectorysetaparametricmodel is
chosen,like curvaturepolynomialsofarbitrary order.While
thisreducesthesolution space and allowsfor fastplanning,it
mayintroduce suboptimality.Wewill showinSec.II that this
canleadto both overshootsand stationary offsetsincurves.
In[9],atree oftrajectoriesis sampled by simulating the
closedloop systemusing therapidlyexploring randomtree
algorithm[10].Thesystemincorporatesmany heuristicsin
theformofsampling biasestoassertwell behaved operation.
Anapproachthat isinasimilarspirit to ourmethod but
onlyconsidersthefree problemthat isnotconstrained by
obstacleshasbeentaken by [17].Here,theoptimalcontrol
trajectoryforanaero dynamicsystemisfound withina
function space that is spanned by aGalerkin base.
Forthe abovementionedreasonsand to,at leastpartly,
overcomethelimitationsofthe approachesdescribedinthe
literature,wepropose a localmethod,whichiscapableof
realizing high-leveldecisionsmadeby an upstream,behav-
ioral layer (long-termobjectives)and also performs(reactive)
emergency obstacle avoidance in unexpectedcriticalsitua-
tions.One aspect thatsetsourmethod especiallyapartfrom
otherschemesistheguaranteedstability(temporalconsi-
tency)ofthenon-reactivemaneuversthatfollowsdirectly
fromBellmansprincipleofoptimality.Withinthiswork
we adherewiththestrategy ofstrictly decoupling feedback
fromplanning.Wedemonstrated beforethat it isadvantagous
toseparatethenavigation taskintoreal timetrajectory
generation and subsequent localstabilization through trajec-
torytracking feedbackcontrol.Thisisincontrast tosome
otherapproachesthatclosethe control loop by feeding the
observedstateofthesystemdirectly backintotheplanning
2010 IEEE International Conference on Robotics and Automation
Anchorage Convention District
May 3-8, 2010, Anchorage, Alaska, USA
978-1-4244-5040-4/10/$26.00 ©2010 IEEE 987
stage.Thefocusofthisworkwill beon thetrajectory
generation phase,i.e.generating thenominal inputrequired
tosafely operatethevehicleinspecicmaneuvering modes.
II.OPTIMALCON TROLAPP ROACH
Applying optimalcontrol theorytotrajectory generation
isnotnew.Incontrast tothewell knownworks[13],[3],
ourmainfocusisnoton theoptimization ofa certaincost
functional.Weinsteadformulatetheproblemoftrajectory
tracking inan optimalsensetotake advantageofthetheory
asserting consistencyinthe choice ofthebestfeasibletra-
jectory overtime.Withthis,weseektomakesurethatonce
an optimalsolution isfound,it will beretained(Bellmans
PrincipleofOptimality).Forthe car,thiswouldmeanthat it
followstheremainderofthepreviouslycalculatedtrajectory
ineach planning stepand thereforetemporalconsistencyis
provided.
Thisisincontrast tomethods suchas[16],[1],[7],
wherethetrajectoriesarerepresented parametrically,e.g.by
assuming systeminputsorcurvatureto bepolynomials,and
theset isgenerated by sampling fromtheparameterspace [1]
orby optimizing on it in ordertomeetcertainend constraints
[7],[16].In general,theoptimal trajectory-intermsofthe
costfunctional-isnotpartofthefunction space spanned
by theparameters.Consequently,Bellmansprincipleof
optimality doesnothold,and on thenext iteration atrajectory
will be chosenthat is slightly different.Figure1illustrates
that thistemporalconsistencycanleadto overshootsoreven
instabilities.
Whileourmaincriterion inchoosing a costfunctional is
compliance withBellmansprincipleofoptimality,trajecto-
riesminimizing it muststill be closetothedesiredtrafc
behaviorofthe autonomouscar.Therefore,letusverbally
describethe “idealbehaviorofanautonomouscarmoving
along astreet: Supposethe carhasa certainlateraloffset to
thedesiredlane,say duetoarecentlyinitiatedlane changeor
an obstacle avoidance maneuver.The carshouldthenreturn
withinitsdriving physicstothedesiredlanemaking thebest
compromisebetweenthe ease and comfortperceivedinthe
carand thetimeit takesto get tothedesiredlaneposition.
T
a
T
b
n0
n0
n1
n2
n1n2
(...)
(...)
Fig.1.Two different transientbehaviorsofthesame planning strategy de-
pending on thereplanning frequency:(top)High replanning frequencywith
tolerabletransient; (bottom) lowreplanning frequencycausesovershoots.
T
aand T
baretheinverseplanning frequenciesand nithestarting
pointsofsubsequentplanning steps.
At thesametime,thebestcompromisehasto befound
inthelongitudinaldirection inananalog manner:Assuming
the cardrivestoo fastortoo closetothevehicleinfront,
it hastoslowdown noticeably butwithoutexcessiverush.
Mathematicallyspeaking,easeand comfortcan bebest
described by thejerk,whichisdened by the changeof
acceleration overtime,whereneededtimeis simplyT=
tendtstart ofthemaneuver.
Asthesolution tothegeneralrestricted optimization prob-
lemisnot limitedtoa certainfunction class3,theproblem
becomeshighlycomplicatedand can besolved numerically
atbest.Thisiswhy ourapproachsearches,asareasonable
approximation fortherestricted optimization problem,only
withinthesetofoptimalsolutionstotheunrestricted(free)
optimization problemand choosesthebestsolution,which
fulllstherestrictions.Thisinturnmeansthatas soon asthe
bestsolution isvalid(restrictionsarethen notactive)tem-
poralconsistency ofthenon-reactivetrajectoriesisassured.
Theverication ofthereactiveheuristicisyet to beshown
simulatively.
III.MOTION PLA NN ING INTHEFREN´
ET FRAME
A well knownapproachintracking control theoryis
theFren´
etFramemethod,whichassertsinvariant tracking
performance underthe action ofthespecialEuclidean group
SE(2):=SO(2)×R2.Here,wewill applythismethod in
orderto be abletocombinedifferent lateraland longitudinal
costfunctionalsfordifferent tasksaswell astomimic
human-likedriving behavioron thehighway.Asdepictedin
Fig.2,themoving reference frameisgiven by thetangential
and normalvector~
tr,~nrata certain pointofsome curve
referredtoasthecenterlineinthefollowing.Thiscenter
linerepresentseithertheidealpathalong thefree road,in
themostsimple casetheroadcenter,ortheresult ofa
path planning algorithmforunstructuredenvironments[20].
Ratherthanformulating thetrajectory generation problem
directlyinCartesianCoordinates~x,weswitchtothepro-
posed dynamicreference frame and seekto generate a one-
dimensional trajectoryforboththerootpoint~ralong the
centerline and theperpendicularoffsetdwiththerelation
~x(s(t),d(t)) =~r(s(t)) +d(t)~nr(s(t)),(1)
as showninFig.2,wheresdenotesthe coveredarclength
ofthe centerline,and ~
tx,~nxarethetangentialand normal
vectorsoftheresulting trajectory~x(s(t),d(t)).
Human perception obviouslyweightslateraland longitudi-
nalchangesofacceleration differently.Since thevectorpairs
~
tx,~nxand ~
ts,~nsalmostalign athigherspeeds,we consider
thepreviouslyintroducedjerkintheseFren´
etcoordinatesas
...
dand ...
s.From[15]we also knowthatquinticpolynomials
arethejerk-optimalconnection betweenastartstateP0=
[p0,˙p0,¨p0]and anend stateP1=[p1,˙p1,¨p1]withinthetime
intervalT:=t1t0inaone-dimensionalproblem.More
3Thisbecomesclearifyou imaginethe autonomouscarbeing trapped
betweenfourmoving cars,oneineach direction,forcing the car’smotion
intoasinglepossiblesolution,e.g.asinusoidal.
988
~r(s)
d(t)
~nr
~
tr
trajectorytrajectorycenterlinecenterline
~x(s,d)
~
tx
~nx
s(t)
Fig.2.Trajectory generation inaFren´
et-frame
precisely,theyminimize the costfunctionalgiven by thetime
integralofthesquareofjerk
Jt(p(t)) :=Zt1
t0
...
p2(τ)dτ.
Wewill usethisresult alsoforourapproach:
Proposition 1:GiventhestartstateP0=[p0,˙p0,¨p0]att0
and [ ˙p1,¨p1]ofthe end stateP1atsomet1=t0+T,the
solution totheminimization problemofthe costfunctional
C=kjJt+ktg(T)+kph(p1)
witharbitraryfunctionsgand hand kj,kt,kp>0isalsoa
quinticpolynomial.
Proof:4Assumetheoptimalsolution totheproposed
problemwasnotaquinticpolynomial.Itwouldconnect the
thetwo pointsP0and P1(p1,opt)withinthetimeinterval
Topt.Thenaquinticpolynomial through thesamepointsand
thesametimeintervalwill alwaysleadtoasmallercost
termRt1
t0
...
p2(τ)inaddition tothesametwo othercost terms.
Thisisincontradiction tothe assumption sothat theoptimal
solution hasto be a quinticpolynomial.
IV.GENERATION O FLATERALMOV EMENT
A.High SpeedTrajectories
Since weseektominimize thesquaredjerk oftheresulting
trajectory,we choosethestartstateofouroptimization
D0=[d0,˙
d0,¨
d0]according tothepreviouslycalculated
trajectory,s.Sec.VI,sothatno discontinuitiesoccur.Forthe
optimization itself,welet˙
d1=¨
d1=0(thetargetmanifold
intheoptimalcontrol lingo)aswewant tomoveparallel
tothe centerline.Inaddition,we chooseg(T)=Tand
h(d1)=d2
1sothatweget the costfunctional
Cd=kjJt(d(t)) +ktT+kdd2
1,(2)
since wewant to penalize solutionswithslowconvergence
and those,whichareoff fromthe centerd=0at the end.
Notice,that thiscostfunctionaland theonesusedinthe
sequeldo notdepend on thevelocity ofthevehicle(except
forSect.IV-B).AsweknowfromProp.1that theoptimal
solution isaquinticpolynomial,we couldcalculateitscoef-
cientsand Tminimizing (2) (ratherlengthy expressions)and
4Froman optimalcontrolsperspectivethisisdirectlyclear,asthe end
pointcostsg(T)and h(p1)do notchangetheEuler-Lagrange equation.
checkit (incombination withthebest longitudinal trajectory
s(t))againstcollision.If we arelucky,it isvalidand we
aredone.If it isnot,wewould havetond a collision-free
alternative,somekind ofsecond besttrajectory,by slightly
modifying Talong withthe coefcientsofd(t)(and s(t))
and checkforcollision again,and so on.
Instead ofcalculating thebest trajectoryexplicitlyand mod-
ifying the coefcientsto getavalidalternative,wegenerate
intherststep,suchasin[16],awholetrajectoryset: By
combining differentend conditionsdiand Tj
[d1,˙
d1,¨
d1,T]ij=[di,0,0,Tj]
forthepolynomials,as showninFig.3atsimulation time
t=0,all possiblemaneuversaresufcientlycovered.
Inthesecond stepwepickthevalidtrajectorywiththe
lowestcost.Notice that,aswe continueineachstepalong
theoptimal trajectory(non-reactive,long-termgoals),the
remaining trajectorywill be,incontrast toFig.1,theoptimal
solution inthenextstep.Thisiscontributed,on theonehand,
tothefact thatwe choosethediscretepointsinabsolute
time(inthesimulation ofFig.3everyfull second),sothat
ineachstepthepreviously optimal trajectoryisavailablein
thenextstep,on theother,thatwe areinthe correct(optimal)
function class fortheunrestricted problem.
B.LowSpeedTrajectories
Athigherspeeds,d(t)and s(t)can be chosenindepen-
dently5,asproposedinthelastsection.Atextremelow
speeds,however,this strategy disregardsthenon-holonomic
property ofthe car,sothat themajority ofthetrajectories
hasto berejected duetoinvalidcurvatures(s.Sec.VI).
Forthisreason thebehavioral layercanswitch belowa
certain velocitythresholdtoaslightly different trajectory
modegenerating thelateral trajectoryin dependence on the
longitudinalmovement,that is
~x(s(t),d(t)) =~r(s(t)) +d(s(t)) ~nr(s(t)).
Rememberthatour focusisnoton theminimization ofa
certaincostfunctional,butwetake advantageofoptimization
theoryin ordertoratethegeneratedtrajectoriesconsistently.
Asquinticpolynomialsford(s)(dened overthe centerline
arclengths)leadtoclothoid-spline-likeparallelmaneuvers
fororientation deviationsfromthe centerlinesmallerthanπ
2,
westicktothepolynomialsalsoforlowspeedsand modify
the costfunctional to
Cd=kjJs(d(s)) +ktS+kdd2
1,
withS=s1s0and with(·):=
s(·)
Js(d(s)) :=Zs1
s0
d′′′2(σ)dσ.
According toProp.1,thequinticpolynomialsoversbelong
totheoptimalfunction class.Thesetgeneration canthen
5excluding extreme maneuvers,wherethe combinedlateraland longitu-
dinalforceson the carplayanimportantroll
989
carriedanalogously out tod(t)withthestartpointD0=
[d0,d
0,d′′
0]and thevariousend points
[d1,d
1,d′′
1,T]ij=[di,0,0,Tj].
0 1 2 3 4 5 6 7 8 9 10
−2
−1
0
1
2
3
d/m
t/s
Fig.3.Optimal lateralmovementresulting fromcyclicreplanning with
green being theoptimal trajectory,blackthevalid,and graytheinvalid
alternatives
V.GENERATION O FLONGITUD INALMOVEMENT
Incontrast to previousworkswheretimeortravelled
distance wasthekeycriterion,wewill focushereon comfort
and contribute at thesametimetosafetyathigh speeds,as
smoothmovementsadaptmuch bettertothetrafcow.For
thatreason,we alsotakethelongitudinal jerkintoaccount
in ouroptimization problem.
A.Following,Merging,and Stopping
Since distance keeping,merging,and stopping atcertain
positionsrequiretrajectories,which describethetransfer
fromthe currentstatetoalongitudinal,possiblymoving,
targetposition starget(t),wegenerate a longitudinal trajectory
set,analogouslytothelateral trajectories,starting atS0=
[s0,˙s0,¨s0]and varythe end constraintsby differentsiand
Tjaccording to
[s1,˙s1,¨s1,T]ij=[[starget(Tj)+si],˙starget (Tj),¨starget(Tj),Tj]
asdepictedfort=0inFig.4,and nallyevaluateforeach
polynomial the costfunctional
Ct=kjJt+ktT+ks[s1sd]2.
Following
For following,themoving targetpointcan bederivedfrom
international trafcrules,e.g.[14],requiring a certaintem-
poralsafety distance tothevehicle ahead,knownasthe
constant timegap law,sothat thedesired position ofthe
following vehicle along thelaneisgiven by
starget(t):=slv(t)[D0+τ˙slv(t)],
withconstantsD0and τand theposition slvand velocity˙slv
oftheleading vehicle along thelane.Aswewouldliketo
derive alternativetrajectoriestothevicinity ofthispoint,
themovementoftheleading vehiclehasto bepredicted
and wereasonablyassume¨slv(t)=¨slv(t0)=const.Time
integration leadsusto
˙slv(t)=˙slv(t0)+¨slv(t0)[tt0]
slv(t)=slv(t0)+˙slv(t0)[tt0]+1
2¨slv(t0)[tt0]2,
whichweneedintherequiredtimederivatives
˙starget (t)=˙slv(t)τ¨slv(t),
¨starget (t)=¨slv(t1)τ...
slv(t)=¨slv(t1).
0 1 2 3 4 5 6 7 8 9 10
0
1
2
3
4
5
6
7
s/m
/s
t/s
Fig.4.Optimal longitudinal tracking ofatargetposition in bluewith green
being theoptimal trajectory,blackthevalid,and graytheinvalidalternatives
ineachreplanning step
Merging and Stopping
Inthesamefashion asabove,we can denethetargetpoint
starget(t)=1
2[sa(t)+sb(t)],(3)
whichenablesusto position the autonomouscarnext toa
pairofvehiclesatsa(t)and sb(t),beforesqueezing slowly
in between during atightmerging maneuver.
Forstopping at intersectionsduetoaredlightorastop sign,
wedenestarget =sstop,˙starget 0,and ¨starget 0.
B.VelocityKeeping
Inmany situations,suchasdriving with no vehicles
directlyahead,the autonomouscardoesnotnecessarily have
to be ata certain position butneedstoadapt toadesired
velocity˙sd=const.given by thebehavioral level.Analog
tothe calculusofvariationsin[15] (withthe additionalso-
calledtransversality condition fors1)and Prop.1,quartic
polynomialscan befound tominimize the costfunctional
Cv=kjJt(s(t)) +ktT+k˙s[ ˙s1˙sd]2
foragivenstartstateS0=[s0,˙s0,¨s0]att0and [ ˙s1,¨s1]of
the end stateS1atsomet1=t0+T.Thismeans,thatwe
can generate an optimal longitudinal trajectorysetofquartic
polynomialsby varying the end constraintsby ˙siand Tj
according to
[ ˙s1,¨s1,T]ij=[[ ˙sd+˙si],0,Tj],
asdepictedinFig.5.
990
0 1 2 3 4 5 6 7 8 9 10
0
1
2
3
4
5
6
7
8
˙s/m
/s
t/s
Fig.5.Optimalvelocityadaption to˙sd=5.0m
/swith green being the
optimal trajectory,blackthevalid,and graytheinvalidalternativesineach
replanning step
VI.COMBINING LATE RALAND LONGITUD INAL
TRAJECTORIES
Before combining thelateraland longitudinal trajectory
sets,denotedasTlat and Tlon inthesequel,each oneis
checkedagainstoutsizedacceleration valuesof¨sand ¨
dor
d′′ (graytrajectoriesintheguresoftheprevious section).
Aswedo notconcentrateinthiscontribution on maxing out
thevehiclesphysics,we choosethemfairlyconservative,
leaving enough safetymargintothefeedbackcontroller.The
remaindersineachsetarethen brought togetherinevery
combination Tlat ×Tlon,as showninFig.6.
Since thebestvalidtrajectory describesthetracking refer-
ence forafeedbackcontroller,weneedto derivethehigher
orderinformation of~x(t),that istheheading θx(t),curvature
κx(t),velocityvx(t),and acceleration ax(t).Asformost
setupsthe centerlineisassumed not to be availableina
closedformbutrepresented by presampledcurvepointswith
orientation θr(s),curvatureκr(s),and changeofcurvature
overarclengthκ
r(s),therequiredinterpolation makesit im-
possibleto derivethehigherorderinformation numerically.
Thederivationsoftherequiredclosedformtransformations
canthereforebefound inApp.I.The curvatureκx(t)is
then usedforexcluding trajectoriesexceeding themaximum
turnradiusofthe car.Inalaststep,the conjointcostsof
eachtrajectoryiscalculatedastheweightedsumCtot=
klatClat +klon Clon.
Asforcollision detection,wewouldliketoavoidadding
heuristicpenaltytermstothe costfunctionalsinthevicinity
ofotherobstacles,astheytend toleadtocomplex parameter
adjustmentsaswell asunpredictablebehavior.Insteadwe
add a certainsafety distance tothesize ofourcaron each
side and make a hierarchicalzero/onedecision interms
ofinterference with otherobstacles similarto[16].Our
solution to preventing the car frompassing otherobstacles
unnecessarilycloselywithout increasing thesafety distance
in general,isas simple aseffective:The collision-checked
contouriscontinuouslyexpandedalittlebit towardsthetime
horizon,so obstaclesofany kind seemtocontinuously back
off aswegetcloser.
Everytimeweutilize a newreference asthe centerline,such
asduring initialization and lane changes,orwhenweswitch
betweenlowand high speedtrajectories,wehaveto project
the currentend point(x,θx,κx,vx,ax)(t0)on thenewcenter
line and determinethe corresponding [s0,˙s0,¨s0,d0,˙
d0,¨
d0]or
[s0,˙s0,¨s0,d0,d
0,d′′
0]respectively.Forthisreason,thetrans-
formationsinthe appendixcaneasily beinvertedinclosed
form,exceptfors0,aswedo notrestrict the centerline~r(s)
toa certainshape6.Howevertheinversion can berestated
astheminimization problems=argmin
σkxr(σ)k,for
whichefcientnumericalmethodsexist.
0 5 10 15 20 25 30 35 40
−14
−12
−10
−8
−6
−4
−2
0
2
x/m
y/m
Fig.6.Resulting trajectoryset in globalcoordinatesforvelocity keeping:
The colormap visualizestheincreasing costsofboththereactivelayerwith
3.0slookaheadfromredto yellowand the alternativesforthelong-term
objectivesformgrayto black.Asthere areno obstacleswithinthe3.0s
horizon,theoptimal trajectory ofthefree problemischosen(green,light
gray),whichleadsthevehiclebacktothe centerline and tothedesired
speed.
VII.CHOOSING THERIG HTSTRATE GY
Asfarasourexperience goes,it is sufcientforhigh-
waytrajectory generation toclassifyall trafcscenariosas
merging,following anothercar,keeping a certainvelocity,
stopping ata certain point,and all combinationsthereof,
whichare conicting mostofthetime.Incontrol the-
ory,override control[6]isawell-knowntechnique,which
choosesamong multiple controlstrategiesaccording toa
scheme,prevalentlythemostconservativeonevia a max-
oramin-operator.Thistechniquehasbeensuccessfully
implementedin numerousautonomouscarson the control
level(AdaptiveCruiseControl),but,to ourbestknowledge,
noton thetrajectory generation level,asweproposehere.
Atany time,thelateral trajectoryset iscombinedwiththe
onesofeveryactivelongitudinal trajectory generation mode
according toSec.VI.Thenthe collision-free trajectorywith
thelowestconjointcostfunctionalsCtotofeachactivemode
iscomparedtotheotherones,and thetrajectorywiththe
smallest initial jerk value...
s(t0)isnally put through to
thetracking controller.Typicalcombinationsofactivemodes
arevelocity keeping and following (AdaptiveCruiseControl,
lane changesinsparce trafc),merging (lane changesin
densetrafc),and velocity keeping and stopping (intersection
withtrafclights).
VIII.EXPERIMENTS
Arstversion ofthe algorithmwasimplementedand
testedwithoutobstacles(long-termobjectives)on the au-
6Forastraight lineora circulararc closed-formsolutionsexist.
991
tonomousvehicleJUNIORwithaplanning cycleof100 ms.
Thetrajectorie