Two-dimensional vehicular movement modelling at intersections based on optimal control

Two-dimensional vehicular movement modelling at intersections based on optimal control

Zhao, Jing; Knoop, Victor L.; Wang, Meng

DOI

10.1016/j.trb.2020.04.001

Publication date

2020

Document Version

Final published version

Published in

Transportation Research Part B: Methodological

Citation (APA)

Zhao, J., Knoop, V. L., & Wang, M. (2020). Two-dimensional vehicular movement modelling at intersections

based on optimal control. Transportation Research Part B: Methodological, 138, 1 - 22.

https://doi.org/10.1016/j.trb.2020.04.001

Important note

To cite this publication, please use the final published version (if applicable).

Please check the document version above.

Copyright

Other than for strictly personal use, it is not permitted to download, forward or distribute the text or part of it, without the consent of the author(s) and/or copyright holder(s), unless the work is under an open content license such as Creative Commons. Takedown policy

Please contact us and provide details if you believe this document breaches copyrights. We will remove access to the work immediately and investigate your claim.

This work is downloaded from Delft University of Technology.

ContentslistsavailableatScienceDirect

Transportation

Research

Part

B

journalhomepage:www.elsevier.com/locate/trb

Two-dimensional

vehicular

movement

modelling

at

intersections

based

on

optimal

control

Jing

Zhao

a,b,∗

_,

_Victor

_L.

_Knoop

b

_,

_Meng

_Wang

b

a Department of Traffic Engineering, University of Shanghai for Science and Technology, Shanghai, China b Transport & Planning, Delft University of Technology, Delft, Netherlands

a

r

t

i

c

l

e

i

n

f

o

Article history: Received 17 September 2019 Revised 8 February 2020 Accepted 3 April 2020 Keywords:

Traffic flow model Intersections Optimal control Vehicular trajectory

a

b

s

t

r

a

c

t

Modelingtrafficflowatintersectionsisessentialforthedesign,control,andmanagement ofintersections.Achallengingfeatureofmicroscopicmodelingvehicularmovementat in-tersectionsis thatdrivers canchoose amonganinfinite numberofalternative traveling pathsandspeeds.Thismakesitfundamentallydifferentfromstructuredstraightroad sec-tionswithlanes.Thisstudyproposesanovelmethodtomodelthetrajectoriesofvehicles intwo-dimensionalspaceandspeed.Basedonoptimalcontroltheory,itassumesdrivers scheduletheirdrivingbehavior, includingthesteeringand acceleration,tominimizethe predictedcosts.Thecostscontaintherunningcosts,whichconsistofthetraveltimeand drivingsmoothness(longitudinallyand laterally),andtheterminal cost,whichpenalizes thedeviationsfromthedesiredfinalstate.Differentthanconventionalmethods,the vehi-clemotiondynamicsareformulatedindistanceratherthanintime.Themodelissolved byaniterativenumericalsolutionalgorithmbasedontheMinimumPrincipleof Pontrya-gin.Thedescriptivepower, plausibility,and accuracyoftheproposedmodel are investi-gatedbycomparingthecalculatedresultsunderseveralcases,whichcanbesolvedfrom symmetryoranalytically.The proposed modelis further calibratedand validated using empiricaltrajectorydata,andthequalityofthepredictedtrajectoryisconfirmed. Qualita-tively,theoptimaltrajectorychangesintherangeoftheshortestpathandsmoothestpath underdifferentweightsoftherunningcost.Theproposedmodelcanbeusedasastarting pointandextendedwithmoreconsiderationsofintersectionoperationintherealworld forfutureapplications.

© 2020TheAuthor(s).PublishedbyElsevierLtd. ThisisanopenaccessarticleundertheCCBYlicense. (http://creativecommons.org/licenses/by/4.0/)

1. Introduction

Modelingthetrafficflow witha realisticdescriptionofthevehicularmovement isessentialinthedesign,control,and managementofintersections,andmoregenerallyatweakenedlaneorlane-freeroadspace.Modelingtoolscanhelp infras-tructuredesignerstoevaluatethesafetyandefficiencyperformanceofdifferentdesignschemesandtorefineandoptimize theschemes.

∗ _{Corresponding author.}

E-mail address: jing_zhao_traffic@163.com (J. Zhao).

https://doi.org/10.1016/j.trb.2020.04.001

0191-2615/© 2020 The Author(s). Published by Elsevier Ltd. This is an open access article under the CC BY license. ( http://creativecommons.org/licenses/by/4.0/ )

Sincethe 1950s,manytrafficflow modelshavebeenestablishedto analyzevariouscomplextraffic phenomena,which can be divided intotwo groups: microscopicmodels andmacroscopic models. Themicroscopic traffic flow modelfor an intersectionisanimportantmethodtoexplorethemechanismoftheinfluencingfactors onthemicroscopictraffic opera-tions. Invariousprevious studies,the interactionbetweenvehicles andthe causedtrafficflow changeswere analyzed in-depthwiththeconsiderationofbothinternalandexternalinfluencingfactors(BrackstoneandMcDonald,1999;Gipps,1981;

Rahmanetal.,2013;Zheng,2014).

Inliterature, themethods inmodelingthe microscopicvehiculartraffic flow atintersectionsmainly includefour cate-gories, includingcar-followingmodels, cellularautomata models,social force theory models,andoptimalcontrol models. Thefollowingparagraphsmadeabriefreviewoftheexistingstudiesinthatorder.

Forthecar-following modelatintersections,the existingstudies mainlyfocus ontheeffectof signalcontrol and traf-ficmanagement onthecar-followingbehavior. Ahnetal.(2004)verifiedNewell(2002)’s simplecar-followingby compar-ing withthe time-space trajectoryof dischargingvehicles atsignalizedintersections. Sasaki andNagatani(2003) studied thetraffic flowona single-laneroadwaywiththeconsiderationofsignal controlbasedontheoptimalvelocity model,in which threesignal control strategies were analyzed: thesimple synchronized,green wave, andrandom switching strate-gies.Tangetal.(2008)extendedthefull velocitydifference(FVD)model(Jiangetal., 2001) toexploretheeffectofsignal control on the traffic flow at the intersections, including the traffic clustering, dissipating, andthe propagation of stop-ping andstartingwave.Yu andShi (2014)proposedan extendedcar-followingmodelatsignalizedintersections consider-ing theaccelerationof twoleadingcars basedon theFVDmodel.The modelwascalibratedandverified withfield data. They(YuandShi,2015) alsoanalyzedthe impacts ofthegreen signalcountdowndevice on car-followingbehaviors dur-ing the phase-change periods.On thisbasis, Tang etal.(2017a) modeled the driving behavior atsignalizedintersections withtheinformationofremaining greentime.Theinfluenceofsignalcontrolontheaggregationanddissipationoftraffic flow is studied. Withthe improvement ofthe automation andcommunication technology,some studies analyzed its ef-fectonthecar-followingbehavioratintersections(Hoogendoornetal., 2014).ZhaoandLi(2016)proposedacar-following modelwiththeconsiderationofspeedguidanceatintersections.Theeffectsofthespeedguidanceontwodifferentvehicle types,namelyintelligent vehiclesandtraditionalvehicles,werediscussed.Tangetal.(2017b)alsoproposeda speed guid-ancemodeltoreflectthedrivers’rationalityonreducingthefuelconsumptionandemissionsatthesignalizedintersection. Threeparameters,includingtheresponsetime,acceptancethresholdvalue,andexecutionlevel,wereconsidered.However, inthesecar-followingmodels,vehicleshavetorunalongthegiventrafficlane.

Forthecellular automata(CA)model,it canbe usedto modeltheheterogeneity ofvehicles andthecomplex interac-tions at intersections (Tonguzet al., 2009). Several models were developed forthe unsignalized intersection to describe the conflicts betweenthe vehicles fromtwo crossing streets(Foulaadvand andBelbasi, 2007;Li etal., 2009; Ruskin and Wang,2002).TheCAmodelcanbeusedtoanalyzetheoperationalefficiencyandsafetyoftheintersection.E.g.,fromthe runningefficiencyaspect,Spyropoulou(2007)discussedtheinheritmodelpropertiesandtheir dynamicsinrespecttothe modelproducedsaturationflowatsignalizedintersections;andfromtherunningsafetyaspect,ChaiandWong (2014) es-tablishedaCAmodeltosimulatevehicularinteractionsinvolvingleft-turnvehicles.Therelationshipsbetweenconflict occur-renceswithtrafficvolumeandleft-turnmovementcontrolstrategiesareanalyzed.ChaiandWong(2015)furtherproposeda fuzzycellularautomatamodelatsignalizedintersectionsbycombiningthecellularautomataandfuzzylogic,whichisable toreplicatedecision-makingprocessesofdriver.TheCAmodelwasalsousedtosimulatethemixedtrafficflowat intersec-tions.Zhaoetal.(2007)simulatedthemixedtrafficflowoftheright-turningvehicleandthestraight-goingbicycle,inwhich thedelayrules ofthebicyclethroughvehicle,thegaprules ofvehiclethroughbicycle,andthedisposalrulesofthe occu-piedconflictzonewereconsidered.VasicandRuskin(2012)simulatedcombinedcarandbicycletrafficatintersections.The conflictsbetweencarsandbicycleswerepresentedandanalyzed.Tangetal.(2018)establishedacellularautomatamodel to describe the lane-changing andretrogradebehaviors ofelectricbicycles near a signalizedintersection,andto analyze theimpacts onthesignalizedintersectioncongestions.The effectofthenewly developedautomationandcommunication technologieswere alsodiscussedusingtheCAmodel.ZhuandUkkusuri(2018)proposedacell-basedsimulationapproach fordescribingtheproactivedrivingbehaviorofconnectedvehicles atthesignalizedintersections.Theeffectsofconnected vehiclesonsmoothingthetrafficflowandreducingthevehicularemissionwereanalyzed.However,sincethelatticeofcells hasequalsize,vehicleshavetorunalongthegiventracksofcells.

To describe the vehicular movement in the inner area of the intersection, the description model should be two-dimensional.Severalworkshavebeendoneusingvariousmethodsbasedonthegranularflowconcept(Helbingetal.,2005;

HoogendoornandDaamen,2005).Oneapproachistoextendthetraditionalone-dimensionalcar-followingmodeltoa two-dimensionalmodel.Nakayamaetal.(2005)constructedatwo-dimensional optimalvelocity modelforpedestrians. Pedes-triansaretreatedasidenticalparticlesmovinginthetwo-dimensionalspace,andeachparticledecidesitsoptimalvelocity dependingondistancestootherparticles.Onthisbasis,Xieetal.(2009)proposed atwo-dimensionalcar-followingmodel todescribethemixedtrafficflowatunsignalizedintersections.Theinteractionbetweenavehicleandother motorized/non-motorizedvehicles were considered. Anotherapproachis thesocial force-based model,whichcan be appliedto describe thepedestrianflow (Zeng etal.,2014),cyclistflow (Huang etal.,2017)andvehicular flow(Maetal., 2017b) at intersec-tions. Maetal.(2017a)proposed athree-layered “plan-decision-action” modeltosimulatethemovingofturning vehicles atmixed-flowintersectionsandfurtherused thesocialforcemodeltogeneratevehiclemovementsintheoperationlayer (Maetal., 2017b).Moreover, severalsocialforce-based vehiclemoving modelswere established tosimulatethevehicular trafficflowwiththeconsiderationofnolanedivision(Fellendorfetal.,2012;Huynhetal.,2013;Yangetal.,2018).The

tra-jectoryisobtainedbythecomprehensiveresultofseveraltypesofforces,suchastheself-drivenforce,therepulsiveforce, theattractiveforce,andotherforcesforspecificconditions.

Moreover,theresultsoftheoptimaltrajectoryguidanceforautonomousvehiclesalsodescribethevehicularmovement atintersections.Numerous worksconsidered theoptimaltrajectory guidancefor autonomousvehicles traversingcrossing the intersection. Some studies focused on establishing cooperative vehicle intersection control algorithms to enable au-tonomousvehicles topass theintersectioncooperativelywithouttraffic signals(Ahmane etal., 2013;LeeandPark,2012;

Yuetal.,2019),whilesomeothersaimedtooptimizethevehiculartrajectoryundertheconsiderationofthesignal control (Jiangetal.,2017;Kamalanathsharmaetal.,2015)ortooptimizeofthetrajectoryandsignalcontrolinaunifiedframework (Fengetal., 2018;Liuetal., 2019; Xuetal., 2018; Yuetal.,2018).Dresner andStone(2004)propose areservation-based systematintersectionsunder theconditionoffully autonomousvehicles.The systemaims tomaximize the efficiencyof movingcarsthrough intersectionswith0probability ofcollisions.Thereservation-basedmethodwasfurtherenhanced to improvevarious operational issues (Dresner andStone, 2008; Li et al., 2013). Medina et al.(2015) developed a cooper-ativeintersection control method by defining a virtual intervehicle distance betweenvehicles driving on differentlanes. Based on the virtual platooning concept, the model can guide the vehicles to cross the intersection safely. Zohdy and Rakha(2016) developeda system,namelyiCACC(intersectioncooperativeadaptivecruisecontrol),tooptimizesthe move-mentofCACCequippedvehicles withtheobjectiveofminimizingtheintersectiondelaywhileensuringoperationalsafety.

Lietal.(2019)proposedaspeedguidancealgorithmforautomatedvehiclestoensuresafetyandoperationefficiencyat in-tersections.Thetrajectoriesofautomatedvehicleswereformulatedina3-dimensionalway,inwhichthevehiclewidthand lengthandtimecontinuitywereconsidered.Guoetal.(2019)furtherproposedanalgorithmtooptimizethetrajectoriesof automatedvehicleandsignalcontrolsimultaneouslyforbestsystemperformanceintermsoftraveltime,fuelconsumption, andsafety.However, inthese models,the vehiculartrajectories areassumedto be along thepredeterminedtraffic lanes.

BichiouandRakha(2018)developedanovelalgorithmistoobtainanoptimalsolutionforeachpossiblescenarioavehicle mayencounterwhilecrossinganintersection.Thedetailedmotionofthevehicleandtheconstraintsonavoidingcollisions wereconsidered(FadhlounandRakha,2019;Rakhaetal.,2001,2009).Thepromisingbenefitsofthealgorithminreducing delayandemissionswere analyzedby comparing itwithother intersectioncontrol strategies.The algorithm wasfurther simplifiedtomakeitpracticalforreal-timeimplementations(BichiouandRakha,2019).Thesemodelsalsousetimeasthe mainindependentvariable.The controlinputsarethe rateofchangeofthe velocityanditsvector orientation,which are appropriate forvehicle control.However, drivers cannot change the angularvelocity ofthe vehicle directly,butthey can adapttheirsteeringwheelangle,whichchangesthecurvatureofthetrajectory.

Comparedto the urban street segments orfreeways, a challengingfeature of microscopic modeling vehicular move-mentatthe innerarea ofintersections isthat driverscan chooseamongan infinitenumberof alternativerunningpaths andspeeds according to their desires. The traditional microscopic models are lane-based (Munigety andMathew, 2016;

Rahmanetal.,2013),i.e., vehiclesshouldtravel inapre-determinedtraffic laneormovefromonelane toanother,which maynotaccordwiththefactthattheconceptofthetrafficlaneissignificantlyweakenedattheinnerareaofintersections. Somenewlyproposed methods,includingthetwo-dimensional car-followingmodel,thesocialforce-basedmodel,andthe optimaltrajectoryguidancemodel,havethepossibilityofdescribingthetwo-dimensionalmovingprocessofvehicles. How-ever,thetrajectoryisnotobtaineddirectlyfromthebehaviorofdrivers,suchasturningthesteeringwheelandpushingthe brakeorthrottlepedals.Therefore,itisstillachallengetomakearealisticdescriptionofthevehiculartrajectorydrivenby thehumandriveratintersections.

Themotivationofthepaperistomodelthetrajectoryofthemanual drivingvehicleatintersections,wherestructured lanesareabsent.Atwo-dimensionalvehicularmovementmodelisproposedbasedonoptimalcontrol.Inmethodology,the studyincorporatesbothsteeringandaccelerationasmodelinputsandchoosesthetravelleddistanceasthemain indepen-dentvariableinstead ofamoreintuitive variable– time. Inthisway,the longitudinalandlateralcontrolsare decoupled. Thatmeansthataparticularheadingangleasafunctionofthedistanceleads,independentoftheaccelerationfunction,to aparticularpathinspace.Therefore,themodelresembleshumandrivinginputs,consistingofsteering(steeringwheel)and longitudinalacceleration(pedals).ThemodelissolvedbyaniterativenumericalsolutionalgorithmbasedontheMinimum PrincipleofPontryagin.Thedescriptivepower,plausibility,andaccuracyoftheproposedmodelarevalidatedbycomparing thecalculatedresultsforseveralcaseswithexpectedoranalyticallycalculatedresults.Determiningtheexacttrajectorycan helpthe researchersandengineersto evaluatethe intersectiondesign andcontrolscheme moreaccurately.Forexample, basedon theproposed model,thedistribution ofthe moving trajectorycan be obtainedunderdifferentdesign schemes. Furthermore,theproposed modelcan beusedinthemicroscopicsimulationsoftwaretoimprovethereality oftheresult, insteadofpredeterminingtherunningpath.

Thearticleisorganizedasfollows.Section2establishes theutility-basedtwo-dimensionalvehicular movementmodel.

Section3analyzestwospecificconditions,namelythefastestpathandthesmoothestpath.Section4validatesthe descrip-tivepower,plausibility,accuracy,andthequalityofthepredictedtrajectoryoftheproposedmodel.Section5discussesthe expansionof themodel withsome considerations oftheoperation inthe realworld. Section 6summarizes theresearch resultsanddiscussesfutureresearchdirections.

Fig. 1. Problem description.

2. Optimalvehicularmovementmodelformulation

Atintersections,vehicles movefroman approachlanetoan exitlane,asshowninFig.1.Incontinuousspace, an infi-nite numberofpaths fromorigintodestinationispossible.Inthisresearch,we assumethat thedriverwillminimizethe predicted disutility/costwhile simultaneously choosing the path andspeed. The remainder ofthis section describesthis problem;wefirstdefinethemodelingvariables,thenthecostfunctions,followedbyconstraintsandthesolutionalgorithm.

2.1. Vehiclemotiondynamics

We definestate Xasa functionofthe distancetraveled(s) bythe subjectvehicle; thestate containsthex-coordinate

x(s), y-coordinatey(s), theheadingangle

θ

(s),andthepace p(s), i.e.,the reciprocalof thevehicularvelocity, asshownin

Eq.(1).Choosingthetravelleddistance(s)asthemainindependentvariablecomparedtoamoreintuitivevariable– time hasconsiderableadvantages.First,thelongitudinalandlateralcontrolsaredecoupled.Thatmeansthataparticularsteering functionasfunctionofsleads,independentoftheaccelerationfunction,toaparticularpathinspace.Moreover,withinthis lateralmovement,theconceptofthe dθ

ds isequaltothecurvatureofthepathofthevehicle.

X

(

s

)

=

⎡

⎢

⎢

⎢

⎣

x

(

s

)

y

(

s

)

θ

(

s

)

p

(

s

)

⎤

⎥

⎥

⎥

⎦

(1)

where, X denotes the state of a vehicle at moving distance s; x andy are the plane coordinate of a vehicle at moving distances,inm;

θ

istheheadingangleatmovingdistances,inrad;pispace,i.e.,thereciprocalofthevelocityofavehicle atmovingdistances,ins/m.

In this study,it is assumed that the initial anddesired terminal states are given. The initial state of a vehicle X₀ is describedwith: X0 =

⎡

⎢

⎢

⎢

⎣

x0 y0

θ

0 p0

⎤

⎥

⎥

⎥

⎦

(2)

whilethedesiredterminalstateofthevehicleX_DisdescribedwithEq.(3),inwhichtheterminalpaceisfree:

XD =

⎡

⎣

xD yD

θ

D

⎤

⎦

(3)

Todescribe thetrajectory choice behavior,the driverusesa utility modelto estimateandpredict thetrajectory costs. Then, the driver will determine his future state using the state prediction model (the system dynamics), which can be expressedbyEq.(4).AccordingtoEq.(4),

κ

and

α

constitutethecontrolU,asshowninEq.(5).Thecontrolvariable

κ

can be seenasthecontrol ofthesteeringwheel, whileanothercontrol variable

α

canbe seenasthecontrol ofthebrakeor

throttlepedals.Inthisway,wecontrolthevehicleasthehumandriverdoes. d d sX= d d s

⎡

⎢

⎢

⎢

⎣

x y

θ

p

⎤

⎥

⎥

⎥

⎦

=

⎡

⎢

⎢

⎢

⎣

cos

θ

sin

θ

κ

α

⎤

⎥

⎥

⎥

⎦

(4)

where,

κ

isthecurvature(reciprocaloftheturningradius)ofatrajectoryatmovingdistances,inrad/m;

α

istheparameter indicatingtheaccelerationofavehicleatmovingdistances,ins/m2_{,apositivevalueindicatesdecelerating,anegativevalue}

indicatesaccelerating.

U=

κ

α

(5)

where,Udenotesthecontrolvectorofavehicleatmovingdistances.

2.2.Movingcost

ConsiderthecontrolUforatrajectoryfromtheinitialstateX₀totheactualterminalstateX_f.Giventhecontrolfunction

U(s),thestateofthevehiclealongthefulltrajectorycanbedetermined,whichwillalsoyieldthedisutility(cost),asshown inEq.(6). C

(

X,U

)

= K

sf,Xf



+  sf s0 L

(

s,X,U

)

ds (6)

Inthisequation,Kdenotestheterminalcost,whichreflectsthecostsattheendofthetrajectory;sfdenotesthemoving

distanceattheendofthetrajectory; X_f denotesthe actualterminal state;Ldenotesthe runningcost,whichreflects the costsoccurringduringatrajectory.Wewillnowelaborateonthesecosts.

2.2.1. Compositionofterminalcost

Theaim oftheterminal cost K isto geta trajectory closetothe desiredposition andheadingangle.In thisline,the terminal cost can be specified asEq.(7) where deviationsof the actual final state (indicated withsubscript f) from the desiredfinalstate(indicatedwithsubscriptD),inducecost:

K=1 ₂ b

xf − xD

2

+

yf − yD

2

+

θ

f −

θ

D

2



(7)

Inthisequation, bdenotes therelative weights oftheterminal cost,whichcan be setasa relativelylarge numberto ensurethevehiclesreachthedesiredfinalstate(positionandangle).

2.2.2. Compositionofrunningcost

TherunningcostLreflectsthedifferentcostaspectsconsideredbythedriversduringdriving.Wemodelitasasumof differentelements:

L= 

β

jLj (8)

where,Ljdenotesthecontributionofthecostaspectj;

β

jdenotestherelativeweightsoftherunningcostaspectj.

We willconsider 3 runningcost terms. Firstly, the travel time of thetrajectory is one of the contributingcosts. This isformulated as Eq.(9).Since the pace isintegrated (Eq.(6)) over the trajectory, thisyields the travel time to the final position.

L1 = p (9)

Theremainingtwotermsreflectthedrivingsmoothness.Driversmightnotprefertodrivewithasharpturn,norprefer todrive withfrequentacceleration andbraking.Thedegree ofthe driving smoothnessisdefinedasa quadratic function ofthevehicularlateralacceleration(centripetalacceleration)andlongitudinalacceleration,asshowninEqs.(10)and(11), respectively. L2 = 1 2 a 2 c = 1 2

κ

p−2

2

= 1 2

κ

2_p−4 ₍₁₀₎ L3 = 1 2 a 2 l = 1 2

−

α

p−3

2

= 1 2

α

2_p−6 ₍₁₁₎

2.3. Constraints

Toconform toreality,therunningspeed,thecurvature,andtheaccelerationshouldbe limitedaccordingto thetraffic ruleandthecharacteristicofavehicle.Therunningspeedshouldberestrictedwithinareasonableminimumandmaximum speedlimitation,andthereversingisprohibited,asshowninEq.(12).Thecurvatureofamovingpathisboundedbyagiven minimumturningradius,asshowninEq.(13).Theothercontrolvariable

α

thatindicatingthevehicularaccelerationshould alsobeinareasonablerange,asshowninEq.(14).Pleasenotethattheproposedmodelaimsatgeneratingmotionsin 2-dimensionalspaceandtimewithaminimalsetofparametersthatcanbeusedtodescribetrafficoperationsatintersections. Inregularhumandriving,thelimitsofthevehicularphysicalcapabilitiesarenotreached. Therefore,thesimplifiedvehicle constraintsonitsmotionareconsideredinourwork;thismightdifferforautomateddriving.

1

v

max ≤ p ≤ 1

v

min (12) −_r1 min ≤

κ

≤ 1 rmin (13)

α

min ≤

α

≤

α

max (14)

where,vmaxandvminarethespeedlimitation,inm/s;rmin istheminimumturningradiusofavehicle,inm;

α

minand

α

max

aretheminimumandmaximumvalueof

α

,ins/m2_. 2.4. Solution

Inthissection,wedescribethesolutionmethod.Aniterativenumericalsolutionalgorithmwasusedbasedonthe Mini-mumprincipleofPontryagin.

2.4.1. MinimumprincipleofPontryagin

Theutilityoptimizationparadigmimpliesthatthedriverswillchoosethecontrolthatcanminimizethetotalcost,which isafunctionoftheinitialstate,thedesiredterminalstate,andthecontrol.LetU∗denotestheoptimalcontroltominimize thepredictedcost,asshowninEq.(15).LetC∗denotesthevaluefunctionofthecostofavehiclewhentheoptimalcontrol

U∗isused,asshowninEq.(16).

U∗= argmin C

(

X,U

)

(15)

C∗= min

U C

(

X,U

)

= C

(

X,U

∗

₎

₍₁₆₎

The minimum principle of Pontryagin isused to solve the optimal control problem. The methodentails defining the HamiltonianH,asshowninEq.(17),whichcanbespecifiedasEq.(18).

H

s,X,U,

λ



= L

(

s,X,U

)

+

λ

_f

₍

_s,X,U

₎

₍₁₇₎ H=

β

1p+ 1 2

β

2

κ

2_p−4₊ 1 2

β

3

α

2_p−6₊

_λ

1cos

θ

+

λ

2sin

θ

+

λ

3

κ

+

λ

4

α

(18)

whereH denotesthe Hamiltonian;

λ

 isthe transposeof

λ

,whichdenotes theso-calledco-stateormarginal costofthe stateX.Thesecostsreflecttherelativeextracostduetomakingasmallchange



X onthestate X.

λ

1,

λ

2,

λ

3,and

λ

4are

theco-statescorrespondingtothefourelementsofthestatevectorx,y,

θ

,andp,respectively. UsingtheHamiltonian,wecanderivethefollowingnecessaryconditionfortheoptimalcontrolU∗:

H

s,X,U∗,

λ



≤ H

s,X,U,

λ



(19)

Fortherunning costfunction showninEq.(7),usingthe necessarycondition(19)we canfind theexpressionforthe optimalcontrol,asshowninEqs.(20)-(21).

∂

H

∂κ

= 0 ⇒

κ

= −

λ

3p 4

β

2 (20)

∂

H

∂α

= 0 ⇒

α

= −

λ

4p 6

β

3 (21)

Moreover, forthe co-state

λ

, we can determine the costate dynamics as shown in Eq. (22), subjectto the terminal conditionsat theendof thecontrol horizon, asshowninEq.(23).Then, we getspecific co-statedynamics,asshown in

Eq.(24),andthetransversalconditions,asshowninEq.(25).

−d

λ

d s =

∂

H

∂

X =

∂

L

∂

X+

λ ∂

f

∂

X (22)

λ

sf



=

∂

_∂

K X

sf,Xf



(23) d

λ

d s =

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

d

λ

1 d s = −

∂

H

∂

x = 0 d

λ

2 d s = −

∂

H

∂

y = 0 d

λ

3 d s = −

∂

H

∂θ

=

λ

1sin

θ

−

λ

2cos

θ

d

λ

4 d s = −

∂

H

∂

p = −

β

1+ 2

β

2

κ

2_p−5_{+ 3}

_β

3

α

2p−7 (24)

λ

sf



=

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

λ

1

sf



=

∂

K

∂

x = b

xf− x D



λ

2

sf



=

∂

K

∂

y = b

yf− y D



λ

3

sf



=

∂

_∂θ

K = b

θ

f −

θ

D



λ

4

sf



=

∂

_∂

K p= 0 (25)

2.4.2. Iterativenumericalsolutionalgorithm

Tosolve themodel mentioned above, we presenta numericalsolution.This solution approachis based oniteratively solvingthestatedynamicequationforwardinthedistance,andsubsequentlysolvingtheco-stateequationbackwardinthe distance(Hoogendoornetal.,2012;Wangetal.,2012).Thefollowingproceduresummarizesthealgorithm:

Step1:Initialization.Inputtheinitialanddesiredterminalstates(X0 andXD),choosetheadjustmentfactor

γ

(0.999is

setastheinitialvalue,anditcanbedynamicallyenlargedwhenthealgorithmcannotconverge),settheiterationnumber

n₌1,settheinitialco-state



0_{=0,andsetthecontrolstep.}

Step2:SolvethestatedynamicequationforwardintimeaccordingtoEqs.(2),(4),(20)and(21).Gotostep3. Step3:Solvetheco-statedynamicequationbackwardintimeaccordingtoEqs.(22)and(23).Gotostep4. Step4:Updatetheeffectiveco-stateaccordingtoEq.(26).Gotostep5.



n₌

_γ

_

n−1₊

_{1 −}

_{γ λ}

n



₍₂₆₎

Step5:Stoppingcriteria.Theiterationwillbestoppedifthedifferenceoftheco-statesofthebackwardandforwardis lessthanthethreshold

ξ

(0.1isset),asshowninEq.(27);otherwise,setn=n+1,andgobacktostep2.

||



n₋

_λ

n

_||

_<

_ξ

₍₂₇₎

3. Specificconditionanalysis

Theiterativenumericalsolutionalgorithmisapplicableforawiderangeofconditions,buttheresultisanapproximate onebecausetheprecisionofthecontrolstepandconvergencethresholdislimited.Inthissection,wesolvetheproposed modelanalyticallyundersomespecificconditions.Twospecificconditionswillbeanalyzed,namelythefastestpathandthe smoothestpath.ThenumericalalgorithmresultswillbecomparedandverifiedwiththeanalyticalresultsinSection4.This willyieldafacevalidationforthesolutionmethod.

3.1. Thefastestpath

Inthiscase,theexpectedtraveltimeistheonlyrunningcostconsidered.Therefore,theHamiltonianfunctionisspecified asEq.(28),andtheco-statedynamicsasEq.(29).Wecanderivethenecessarycondition(19)foroptimalcontrol.

TominimizetheHamiltonian,forthecurvature(

κ

),eitheroneofthefollowingtwocasesholds.(1)Caseone:curvature

∂H

∂κ =

λ

3=0.Then, d_dλ_s3 =

λ

1sin

θ

−

λ

2cos

θ

=0,accordingtoEq.(29).Wealsoknow d_dλ_s1 =0andd_dλ_s2 =0,whichindicatethat

λ

1and

λ

2areconstants,accordingtoEq.(29).Then,thepathisalinesegmentwithheadingangle

θ

=arctanλ_λ2

1.Therefore, thecurvatureshould equal to0(

κ

= 0). (2)Case two: ∂_∂κH=0. Then,in orderto minimizethe H shown inEq.(28),we shouldminimize

λ

3

κ

.Therefore,weshouldselecttheboundary ofthecurvature(

κ

=±r_min1 ) accordingtotheplus-minus

signofthe

λ

3,asshowninEq.(30).

Forthecontrolvariableofacceleration(

α

),weshouldminimize

λ

4

α

.AccordingtoEq.(29),weget ddλs4 =−

β

1<0.

Mean-while,wehavetheterminal stateof

λ

4(sf) =0accordingtoEq.(25).Thismeans

λ

4 isalways nonnegative(

λ

4 ≥ 0). The

optimalcontrolof

α

shouldselecttheminimalvalueundertheconstraintsEqs.(12)and(14),asshowninEq.(31). Insummary,Eqs.(30)-(31)istheanalyticalsolutionoftheproposedmodelundertheconditionofthefastestpath.The conclusionisthattheoptimalcontrolofthefastestpathistheconcatenationofthegivenmaximumcurvature(

κ

=± 1

andfixed heading angle

θ

withthe curvature equals to0 (

κ

₌ 0) withthe maximum acceleration(the minimum value of

α

) undertherestrictions ofthespeed andacceleration.Thisconclusionwill beused tovalidate theplausibilityofthe numericalalgorithmresultsinSection4.2(seetheredlineinFig.3(g)and(i)).Moreover,basedontheanalyticalsolution, theaccuracyofthenumericalalgorithmresultswillbeverifiedinSection4.3(seeTable3).

H=

β

1p+

λ

1cos

θ

+

λ

2sin

θ

+

λ

3

κ

+

λ

4

α

(28) d

λ

d s =

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

d

λ

1 d s = −

∂

H

∂

x = 0 d

λ

2 d s = −

∂

H

∂

y = 0 d

λ

3 d s = −

∂

H

∂θ

=

λ

1sin

θ

−

λ

2cos

θ

d

λ

4 d s = −

∂

H

∂

p = −

β

1 (29)

κ

=

⎧

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎩

1 rmin,

λ

3< 0 − 1 rmin ,

λ

3>0 0 ,

λ

3 =0 (30)

α

=

⎧

⎪

⎨

⎪

⎩

α

min, p+

α

mind s≥ 1

v

max 1 vmax− p d s , p+

α

mind s< 1

v

max (31)

3.2. Thesmoothestpath

Inthiscase,thedrivingsmoothnessistheonlyrunningcostconsidered.Therefore,theHamiltonianfunctionturnstobe

Eq.(32),andtheco-state dynamicsturnsto beEq.(33).Then, usingthenecessary condition(19), theexpressionforthe optimalcontrolforthecurvature(

κ

)andtheacceleration(

α

)canbedrawn,asshowninEqs.(34)and(35),respectively.

They are thesame as Eqs.(20)-(21), whichis very difficult to obtain a full analytical solution. However, we can still analyzesomegeneralresultsforthecontrolvariableofacceleration(

α

).Weget dλ4

ds ≥ 0accordingtoEq.(33),because

β

2,

β

3,

κ

2,

α

2,andpareallnon-negative.Itmeansthevalueof

λ

4 willnotdecrease.Meanwhile,wehavetheterminalvalue

of

λ

4(sf)=0accordingtoEq.(25).Therefore,

λ

4shouldbenolargerthan0(

λ

4≤ 0).Additionally,

β

3andparelargerthan

0.Then,

α

₌₋λ4p6

β3 isnonnegative,asshowninEq.(36).

Therefore,apartialanalytical solutioncanbedrawn thattheoptimalcontrolof

α

isnonnegativeunderthesmoothest path condition,asshownin Eq.(36). Theconclusion isthat theoptimalcontrol ofthe smoothestpath isdecelerating or keeping the same speed. This conclusion will be used to validate the plausibility of the numerical algorithm results in

Section4.2(seetheredlineinFig.3(i)). H= 1 2

β

2

κ

2_p−4₊ 1 2

β

3

α

2_p−6₊

_λ

1cos

θ

+

λ

2sin

θ

+

λ

3

κ

+

λ

4

α

(32) d

λ

d s =

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

d

λ

1 d s = −

∂

H

∂

x = 0 d

λ

2 d s = −

∂

H

∂

y = 0 d

λ

3 d s = −

∂

H

∂θ

=

λ

1sin

θ

−

λ

2cos

θ

d

λ

4 d s = −

∂

H

∂

p = 2

β

2

κ

2_p−5_{+ 3}

_β

3

α

2p−7 (33)

∂

H

∂κ

= 0 ⇒

κ

= −

λ

3 p4

β

2 (34)

∂

H

∂α

= 0 ⇒

α

= −

λ

4 p6

β

3 (35)

α

≥ 0 (36)

Table 1 Tested cases.

Scenario Movement Initial state Desired terminal state Scenario Movement Initial state Desired terminal state A U-turn [0, 6, 0, 1/8] [0, 16, π] I Left-turn [0, 6, 0, 1/8] [10, 10, π/8] B Left-turn [2, 16, 9 π/10] J Through [10, 8, 0] C Left-turn [4, 16, 4 π/5] K Through [10, 6, 0] D Left-turn [6, 16, 7 π/10] L Through [10, 4, 0] E Left-turn [8, 16, 3 π/5] M Right-turn [10, 2, −π/8] F Left-turn [10, 16, π/2] N Right-turn [10, 0, −π/4] G Left-turn [10, 14, 3 π/8] O Right-turn [8, 0, − 3 π/8] H Left-turn [10, 12, π/4] P Right-turn [6, 0, −π/2] Table 2

Input parameter for model validation.

Parameter Value

Boundary of the speed, v min and v max 5 m/s and 12 m/s

Minimum turning radius, r min 4 m

Boundary of the α, αmin and αmax −0.01 s/m 2 and 0.01 s/m 2

Weight of the terminal cost, b 100 s/m 2

Weight of the running cost, β1 , β2 , and β3 1, 0.001 s 5 /m 3 , and 0.01 s 5 /m 3

Precision of the path (control step) 0.1 m Iteration stopping threshold 0.1

Fig. 2. Optimal trajectories of the tested scenarios.

4. Modelvalidation

Thedescriptive power,plausibility,accuracy, andthe qualityofthe predictedtrajectory oftheproposed modelwillbe validatedinthissectionaccordingtothefollowingfoursteps:(1)validatethedescriptivepowerofasolutiontothemodel byenumeratingcommonmovingcasesintheintersection;(2)validatetheplausibilityofthesolutionofmodelbehaviorby analyzingchangetendencyofthepath,thetwocontrolvariables(

κ

and

α

),andtherunningcostsunderdifferentweights oftherunningcosts;(3)validate theaccuracyofthe solutionalgorithmby comparingthedifference betweentheresults oftheproposed algorithm andtheanalytical solution;(4)calibratethe modelparameters andvalidatethe quality ofthe predictedtrajectoryusingempiricaldata.

4.1. Descriptivepowervalidation

Tovalidatetheproposedsolutionalgorithmcanbeusedtosolveallconditions,thecommonmovingcasesinthe inter-sectionareenumeratedinTable1,whichcontainsU-turn,leftturnswithvariousangles,throughmovementswith/without anoffset,andrightturns withvarious angles.Theother inputparameters areshowninTable2.Notethatwe choosethe normalizethe cost C of Eq. (6)in units of time. The results are illustrated in Fig. 2, which is a manifestation that the proposedsolutionalgorithmcangenerateoptimalcontrolresultsunderalltestedscenarios.

Table 3

Accuracy validation for the fastest path scenario.

Absolute error Mean Minimum Maximum Std. Deviation (RMSE) Path ( x, y ), m 4.02E-02 0.00E + 00 7.97E-02 2.58E-02

Pace ( p ), s/m 7.25E-17 0.00E + 00 5.00E-16 1.15E-16

Note: the standard deviation of the absolute error equals the RMSE (root-mean- square error).

4.2. Plausibilityofmodelbehavior

For reasons of simplicity in explanation, the left-turn scenario (Scenario F in Table 1) will be used in the hereafter discussion.The path(x,y), theheadingangle(

θ

), thepace(p),thecurvaturecontrol (

κ

), andthe accelerationcontrol (

α

) are analyzed, asillustrated inFig. 3. Twosub-scenarios are discussed in thissection. Scenario 1 is a free terminal pace scenario,whichisthesameasScenarioFinTable1.Scenario2isarestrictedterminalpacescenario,inwhichtheterminal pace isrestrictedto beequalto theinitial one.The terminalstate inScenario2 turnstobe [10, 16,

π

/2,1/8].The other inputparametersareshowninTable2.

Generally,thepath(seeFig.3(b)),therunningstate(seeFigs.3(d)and3(f)),andthecurvatureandaccelerationcontrols (seeFigs.3(h)and3(j))aresymmetricundertherestrictedterminalpacescenario.Itindicatesthatthetrajectoriesarethe same whendriving fromone pointto anotherand backwardwhen theinitial state andthe terminalstate are the same, whichisinaccordancewiththeexpectation.However, thecurvatureandaccelerationcontrols, therunningstate,andthe patharenonsymmetricwhenthepaceischangeable.Itisbecauseofallthethreeaspectsoftherunningcost(L1,L2,andL3)

arerelatedtothepace.Thetwocontrolvariables,

κ

and

α

,willadjustaccordingtothepace statetoobtaintheminimum runningcosts.

For the runningpath, as we expected, it isalways within the boundary of the fastestpath and the smoothest path. The path becomesstraighter and the turning becomes sharper withthe increase of the weight of the travel time cost. Accordingly,thelengthoftherunningpathdecreaseswiththeincreaseoftheweightofthetravel timecost.Itisbecause whentheweightofthetraveltimecostincreases,thevehicletendstoselectthefastestpath(theblackline);onthecountry, thevehicletendstoselectthesmoothestpath(theredline)whentheweightofthedrivingsmoothcostincreases.

Forthechange tendencyoftheheading angle,it changesmore homogeneouswiththeincrease ofthe relativeweight oftheunsmoothrunningcost.AstheredlineshowninFig.3(c)and(d),itisalmoststraight,whichindicates theheading anglechangesuniformly.Itisinaccordancewiththedefinitionofthe“smoothestpath”.Onthecontrary,theblacklineis themostunsmoothpath.It iscomposedoftwosegments withthelargestslopeandone segmentwithaslopeof0.The twosegmentswiththelargestslopeindicatethevehiclerunswiththelargestcurvature,whilethesegmentwithaslopeof 0indicatesthevehiclerunsstraightahead.ItcanbefurtherverifiedbyFig.3(g)and(h),wherewecanseethechangeof thecurvaturecontrolalongthepath.Theredline(smoothestpath)isthemoststableone.Theblackline(thefastestpath) hasthe largestchanges (maximum curvatureor0), whichisin accordancewiththeanalytical resultsof thefastestpath showninSection3.1.

Forthechangetendencyofthepace(p),Asexpected,thechangesofpunderthefastestpathisthemostdramatic.Thep

ofthefastestpath(blackline)decreases(accelerationinvelocity)withasharpslopethenmaintainsattheminimumvalue (themaximumvelocitylimitation).Itisinaccordancewiththechangeofthe

α

inFig.3(i)and(j),whereonlytheminimum value orthe0ofthe

α

isselected.ItisinaccordancewiththeanalyticalresultsofthefastestpathshowninSection 3.1. Moreover,fromFig.3(j),wecanfindthevalue of

α

alwayslargerorequalto0forthesmoothestpath(redline),whichis inaccordancewiththeanalyticalresultsofthesmoothestpathshowninSection3.2.Forotherpathsexceptforthefastest andsmoothestpaths,thechangeoftherunningpacedependsontheweightsofthetraveltimecostandunsmoothrunning cost.When thetraveltime costmakes aprimary contributiontothetotalcost, thevehiclepreferstoaccelerate(negative valuein

α

,henceareductionofpaceandincreaseofspeed))tominimizethetotalcost.

Fig. 4shows thechange in thetravel time cost andthe driving smoothnesscost withthe change ofthe weights. As expected,thetraveltimecostdecreasesandthedrivingsmoothnesscostincreaseswiththeincreaseoftheweightoftravel timecost.

Insummary,theanalysisshowsthattheproposedmodelissensible.Thevehicledynamicsareshowntohavesymmetry whentheterminalpaceisrestrictedtobeequaltotheinitialone.Qualitatively,theoptimaltrajectorychangesintherange oftheshortestpathandsmoothestpathunderdifferentweightsoftherunningcost.

4.3. Accuracyvalidation

The result ofthe fastest path is further analyzed in thissection by comparing the results drawn fromthe analytical solutioninSection3tovalidatetheaccuracyoftheproposedalgorithm.AccordingtotheinputparametersshowninTable 1,theexpressionsoftherealfastestpathoftheleft turn(ScenarioFinTable1)are showninEq.(37).Wecancheck the gap(the Euclideandistanceforthepath andthe absoluteerrorofthepace)betweenthecalculated resultsbasedonthe

Fig. 4. Running cost analysis.

Fig. 5. Comparison between the algorithm result and analytical solution .

proposedalgorithmandtherealonesobtainedfromtheanalyticalsolutionateachcalculatedstep.Thecomparisonresults areshowninFig.5andTable3.

The resultsshow that the differencesare small.In thistest case, theaverage error,maximum error,andRMSE (root-mean-square error) in path (position state) of the real fastest path are 0.04 m, 0.08 m, and 0.03 m, which very small compared to of the width of the traffic lane. Additionally, there is no observable difference in the pace. Therefore, the accuracyoftheproposedalgorithmisacceptable.

⎧

⎨

⎩

y= 10 −√ 16 − x 2_{, 0 ≤ x ≤ 2} √₂ y= x+ 10 − 4 √2 , 2 √2 <x<6 + 2 √2 y= 16 −



16 −

(

x− 6

)

2_{, 6 + 2} √ _{2 ≤ x ≤ 10} (37)

4.4. Validationwithempiricaldata

Inthissection,theproposedmodelisvalidatedusingempiricaldata.Themodelparametersare firstlycalibratedusing therealvehiculartrajectoriescollectedinanintersection.Then, thecalibratedmodelisvalidatedusingthe datacollected inanotherintersection.

Twointersectionslocated inShanghai, China,namelyYouyiRoad – TieliRoad andZuchongzhiRoad – GaosiRoad,are selectedforparametercalibrationandmodelvalidation,respectively,asshowninFig.6.Theoperation oftheintersection isrecordedbytheunmannedaerialvehicle.Thepositionofeachvehicleateachframe(1/24s)canbecollectedbyusinga speciallydevelopedvideorecognitionsoftware.Then,therealvehiculartrajectorycanbeobtained.

TheRMSE(root-mean-squareerror)ofthetrajectoryatalltimestepswillbeusedastheindicatoroftheaccuracyofthe model.Thetrajectoryerrorateachtimestep(1/24s)isdefinedastheEuclideandistancebetweenthegeneratedtrajectory andtherealtrajectory,asshowninFig.7(a).

Forparametercalibration, thethreerelative weights oftherunningcost,

β

1,

β

2,and

β

3,need tobe calibratedinthe

model.Since they arerelative weights, withoutlossof generality,

β

1 isset tobe 1.Then, thereare onlytwo parameters

tocalibrate,

β

={

β

2,

β

3}.Foreach collectedvehicle,the

β

wascalibratedusingtheenumerationmethodaccordingtothe

Fig. 6. Surveyed intersections.

Fig. 7. Example of parameter calibration.

Table 4

Results of parameter calibration.

Left-turn Through movement

Vehicle

number β2 (s

5 /m 3) _β

3 (s 5 /m 3 ) RMSE of

trajectory (m) RMSE of path (m) Vehicle number β2 (s

5 /m 3 ) _β

3 (s 5 /m 3 ) RMSE of

trajectory (m) RMSE of path (m)

1 0.068 0.087 1.7493 0.6427 21 0.021 0.129 0.4935 0.1723 2 0.070 0.105 1.7105 0.5612 22 0.027 0.032 0.6161 0.2799 3 0.020 0.070 0.9695 0.2324 23 0.009 0.013 0.3399 0.3137 4 0.026 0.031 1.7490 0.9033 24 0.015 0.052 0.4714 0.2311 5 0.047 0.049 1.8327 1.1397 25 0.019 0.075 0.5174 0.3966 6 0.023 0.071 1.0432 0.3604 26 0.006 0.026 0.3711 0.1352 7 0.017 0.031 1.7029 1.1981 27 0.045 0.045 0.4391 0.2668 8 0.054 0.052 1.7067 1.5648 28 0.008 0.019 0.4496 0.1429 9 0.015 0.013 1.1516 1.8955 29 0.033 0.064 0.5479 0.1732 10 0.037 0.064 1.5589 0.2223 30 0.005 0.005 0.7082 0.2101 11 0.026 0.048 0.6749 0.4099 31 0.020 0.050 0.4628 0.3704 12 0.045 0.049 1.9261 1.0918 32 0.003 0.059 0.4044 0.1551 13 0.008 0.076 1.5516 0.1759 33 0.032 0.025 0.2672 0.2393 14 0.027 0.016 1.0216 0.1946 34 0.024 0.024 0.3435 0.2490 15 0.010 0.045 0.6944 0.3596 35 0.005 0.009 0.5221 0.1914 16 0.033 0.039 1.9809 0.9256 36 0.012 0.028 0.3226 0.2578 17 0.049 0.079 1.1081 0.8237 37 0.049 0.014 0.6836 0.2558 18 0.030 0.028 0.5428 0.1572 38 0.005 0.040 0.3536 0.2542 19 0.040 0.034 1.4750 0.8956 39 0.007 0.032 0.5488 0.2242 20 0.015 0.002 1.4607 0.8992 40 0.004 0.081 0.4422 0.2760

s5_/m3 _{withaninterval of0.001 s}5_/m3_{.(3)Generatethe trajectoryunder each}

_β

_{usingthe proposedmodel.(4) Calculate}

thecorrespondingfitnessvalue(RMSE).(5)Obtaintheoptimalvalueof

β

accordingtothefitness(smallestRMSE).Fig.7(b) and(c)illustrateanexampleoftheparametercalibrationforleft-turnandthroughvehicles,respectively.Theresultsofthe parametercalibrationof40vehiclesareshowninTable4.OnecanfindthatthevaluesofRMSEaresmallerthan2.0mforall thetestedvehicles.TheaverageRMSEoftrajectoryis0.923m.Itmeansthedistancebetweenthegeneratedtrajectoryand therealtrajectoryateachtimeisapproximatelyonemeter.Ifonlyconsideringthepathandneglectingthetimeandspeed, theaverageRMSEofpathis0.486m.Giventheinitialandterminalstates,therealtrajectoriescanbewellrepresentedby

Fig. 8. Example of model validation.

Table 5

Results of model validation.

Left-turn Through movement

Vehicle number β 2 (s 5 /m 3 ) β3 (s 5 /m 3 ) RMSE of trajectory (m) RMSE of path (m) Vehicle number β 2 (s 5 /m 3 ) β3 (s 5 /m 3 ) RMSE of trajectory (m) RMSE of path (m) 1 0.023 0.071 1.4358 1.2253 11 0.030 0.028 0.6049 0.2325 2 0.004 0.081 1.9354 1.4839 12 0.015 0.002 2.2613 0.3610 3 0.070 0.105 1.9610 1.8657 13 0.021 0.129 0.4356 0.3046 4 0.070 0.105 2.8646 1.2076 14 0.021 0.129 0.2634 0.1420 5 0.070 0.105 1.8124 1.4233 15 0.005 0.005 1.0374 0.2946 6 0.033 0.064 1.8546 1.3855 16 0.005 0.005 1.1096 0.2419 7 0.027 0.016 0.7366 0.1885 17 0.070 0.105 0.2683 0.2556 8 0.068 0.087 1.0164 0.6140 18 0.005 0.005 0.4490 0.2882 9 0.068 0.087 1.3310 1.0924 19 0.032 0.025 0.2410 0.2093 10 0.070 0.105 2.2098 0.8055 20 0.015 0.002 0.9099 0.3394

adjustingtheweightsofthecosts.Itindicatesthattheproposedmodelhasastrongperformanceindescribingthemoving trajectoryofthemanualdrivingvehicle.

Formodelvalidation,itwasconductedby comparingtherealvehiculartrajectoriesandthepredictedtrajectorieswith thecalibrated

β

.Foreachvehicle,theaccuracyofthemodelwastestedaccordingtothefollowingfoursteps.(1)Extractthe initialandterminalstatesofthevehicle.(2)Generatethetrajectoryundereachcalibrated

β

inTable4usingtheproposed model.(3)Calculatethecorrespondingfitnessvalue(RMSE).(4)Obtaintheaccuracyofthemodelandtheoptimalvalueof

β

accordingtothefitness(smallestRMSE).Fig.8(a)and(b)illustrateanexampleofthemodelvalidationforleft-turnand throughvehicles,respectively.Theresultsofthemodelvalidationof20vehiclesareshowninTable5.TheaverageRMSEof trajectoryis1.237m.Althoughitisalittlelargerthantheparametercalibrationcases,theaccuracyisstillacceptable.Please notethisistheresultofthetrajectory usingother driver’spreferences.Inapplication,numerousrealvehiculartrajectories can becollectedtodrawthe distributionofthecalibratedparameters (

β

). Then,we canrepresentthedistributionofthe trajectoriesbyrunningthemodelmanytimesusingdifferentvaluesof

β

drawnfromthedistribution.

5. Modelextensions

Inthispaper,weestablishautility-basedtwo-dimensionalmodeltodescribethetrajectoryofthemanualdrivingvehicle atintersections.Themodelisasimplifiedrepresentationofreal-worlddriving,ofwhichrealismcanbeextendedwith ele-mentssuchastheuncertaintyofthemanualdriving,thepreferenceofthedrivingpath,andtheavoidanceofstationaryand movingobstacles.Thissectiondiscussesthesepossibilitiesfurther.Inparticular,theframeworkofcontrolactions(splitting longitudinalandlateralcontrols)andsolutionapproach(applyingaPontryaginprincipleforoptimization)isthesame,but additionalcostsareincludedfortheseconsiderations.

5.1. Uncertaintyofmanualdriving

Forahumandriver,evenifweassumethatthedriverhasperfectknowledgeonthesystemdynamics,itisstilldifficult orevenimpossibleto controla vehicleexactlythe wayhewants.In thehierarchicalframework ofstrategic,tactical,and operationaldriving,asdefinedbyHoogendoornandBovy(1999),thevehicleoperationisnotnecessarilyaccurate.Theactual

Fig. 9. Trajectories under each control period.

controlU∗ofmanualdrivingcontainstheuncertainty.Butadriverwillrealizethisinaccuracyincontrolandre-planhis/her desiredtrajectory(inthetacticallayer).Toreflectthisbehavioralcharacteristic,wecanadd noisetothemovingdynamics to reflectthe uncertainty of the manual driving, asshown in Eq.(38). Additionally,the idea of re-planningthe optimal trajectoryateachdiscretizedcontrolperiodcanbeapplied.Itmeansadriverwillplantheentirecontrolstrategyfromthe currentstate to theterminal state, butonlythe first periodofthe control decisionwill be applied,i.e.model predictive control(Wang,2018; Wangetal.,2014a,Wangetal., 2014b).After that,the driverwillre-plan thecontrol strategyfrom theupdatedstate totheterminalstate.Thediscretized controlperiodcanbepre-determinedwiththeunit ofdistanceor time.Inthisway,theuncertaintyofmanualdrivingcanbereproduced.Theactualrunningtrajectorieschangefordifferent drivers,eventheirweightstothemovingcostsarethesame.Wemodelitas:

˙ X = f

s,X,U∼∗



=

⎧

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎩

˙ x = cos

θ

˙ y = sin

θ

˙

θ

=

(

1 +

ξ

1

)

κ

∗ ˙ p=

(

1 +

ξ

2

)

α

∗ (38)

where,

κ

∗and

α

∗aretheoptimalcontrolof

κ

and

α

;

ξ

1and

ξ

2 representtheuncertaintyofthehumandrivercontroland

theresultingeffectsonthesubject’skinematics.

Usingthesame casestudyasinSection 4.2(Scenario FinTable 1),therandom variables

ξ

1 and

ξ

2 obeythenormal

distributionwiththeexpectationof

μ

1=

μ

2=0,andthestandarddeviationof

σ

1=

σ

2=1.Thediscretizedcontrolperiod

issettobe4m.Theweightsoftherunningcost

β

1,

β

2,and

β

3aresettobe1,0.001s5/m3,and0.01s5/m3,respectively.

OtherparametersremainthesameasthoseinTable2.

Fig.9illustrates the optimalcontrol andthemanual drivingof eachcontrol period.One canfind that thetrajectories ofthemanualdriving arenot thesameastheoptimalcontrol.There aresome errors.However,atthebeginning ofeach controlperiod,theoptimalcontrolpathwillbere-planned.Then,thehumandriveradjuststhecontrolaccordingtothe re-planedrouteandreachestheterminal.Pleasenotetheuncertaintyofmanualdrivingchangesfordifferentdrivers.Therefore, theactualrunningtrajectorieschangefordifferentdrivers,eventhedistributionoftherandomvariables

ξ

1 and

ξ

2arethe

same.Asanexample,Fig.10illustrates5manualdrivingtrajectorieswiththesamedistributionof

ξ

1and

ξ

2. 5.2.Preferenceofthedrivingtrajectory

Humandriversalsomayprefertodriveinsomespaceinsteadofothersaccordingtothelayoutoftheintersection,which canbecalledthepreferenceofthedrivingtrajectory.Thepreferencemightbefollowingtheguidelines,ortravellingcloser tothesideoftheroad.Anadditionalrunningcosttermreflectingthisphenomenoncanbeaddedinthisframework,which isrelatedtotheposition(coordinates)ofthevehicle,asshowninEq.(39).

L4= O

(

x,y

)

(39)

where,O(x,y)isthecostfunctionrelatedtothecoordinates.

TheexplicitformulationofEq.(39)isrelatedtothelayoutoftheintersectionandthespecificgeometricdesignelement. As an example,we show a casewith a turningmovement, like introduced in Section 4.2. In thiscase, we consider two setsofline-markingsthatare usedtoguidevehicles.One makesasmooth turn,andthe othera sharpturn(Fig.11). The equationsareasfollows.

Fig. 10. Trajectories with the consideration of manual driving uncertainty.

Fig. 11. Two scenarios of guide lines.

Assumingthecurveoftheguidelineisgiven,thecostfunctioncanbedefinedasthequadraticfunctionofthedistance betweenthevehicularposition(x,y)andtheguidelinecurveg(x,y),asshowninEq.(40).

L4 = 1 2

x− x 0

2

+

y− y 0

2



(40)

where,

(

x₀,y₀

)

isthenearestpointontheguidelinecurvetothevehicularposition(x,y),whichcanbeobtainedaccording toEqs.(41)-(42). g

x,y



= 0 (41)

∂

g

∂

y

x− x 0



−

_∂

∂

_xg_

y− y 0



= 0 (42)

AsshowninFig.11,thetwoguidelinecurvescanbeexpressedbyEqs.(43)and(44)forthesmoothguidelineandthe sharpguideline,respectively.

y₁= 16 −



100 − x 1 2

, 0 ≤ x ≤ 10 for the smooth line (43)

⎧

⎪

⎨

⎪

⎩

y₂= 6 , 0 ≤ x 2≤ 6 y₂= 10 −



16 −

x₂− 6

2

, 6 <x₂<10 x₂= 10 , 10 ≤ y 2≤ 16

for the sharp line (44)

For the smooth guide linescenario, the running cost of driving preferenceequal to Eqs. (45).Then, the Hamiltonian functioncanbespecifiedasEq.(46),andtheco-statedynamicsturnstobeEq.(47).

L4= 1 2



x2₊

(

_{y− 16}

)

2_{− 10}

2

(45)

H =

β

1p+ 1 2

β

2

κ

2_p−4₊ 1 2

β

3

α

2_p−6₊ 1 2

β

4



x2₊

₍

_{y− 16}

₎

2_{− 10}

2

+

λ

1cos

θ

+

λ

2sin

θ

+

λ

3

κ

+

λ

4

α

(46) d

λ

d s =

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

d

λ

1 d s = −

∂

H

∂

x1 =

β

4 x

10

x2+

(

y− 16

)

2

−

12 − 1



d

λ

2 d s = −

∂

H

∂

x2 =

β

4

(

y− 16

)

10

x2₊

₍

_{y− 16}

₎

2

−

12 − 1



d

λ

3 d s = −

∂

H

∂

x3 =

λ

1 sin

θ

−

λ

2cos

θ

d

λ

4 d s = −

∂

H

∂

x4 = −

β

1+ 2

β

2

κ

2p−5+ 3

β

3

α

2p−7 (47)

Forthesharpguidelinescenario,therunningcostofdrivingpreferenceequaltoEqs.(48).Then,theHamiltonian func-tioncanbespecifiedasEq.(49),andtheco-statedynamicsturnstobeEq.(50).

L4 =

⎧

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎩

1 2

(

y− 6

)

2_{, x≤ 6 ,x+ y ≤ 16} 1 2



(

x− 6

)

2₊

₍

_{y− 10}

₎

2_{− 4}

2

, x> 6 ,y< 10 1 2

(

x− 10

)

2_{, y ≥ 10 ,x+ y>16} (48) H =

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

3  j=1

β

jLj+

β

4 1 2

(

y− 6

)

2₊

_λ

_f

₍

_s,X,U

₎

_{, x≤ 6 ,x+ y≤ 16} 3  j=1

β

jLj+

β

4 1 2



(

x− 6

)

2₊

₍

_{y− 10}

₎

2_{− 4}

2

+

λ

_f

₍

_s,X,U

₎

_{, x> 6 ,y<10} 3  j=1

β

jLj+

β

4 1 2

(

x− 10

)

2₊

_λ

_f

₍

_s,X,U

₎

_{, y ≥ 10 ,x+ y> 16} (49) d

λ

d s =

⎧

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎨

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎪

⎩

d

λ

1 d s = −

∂

H

∂

x1 =

⎧

⎪

⎨

⎪

⎩

0 , x ≤ 6 ,x+ y ≤ 16

β

4

(

x− 6

)

4

(

x− 6

)

2₊

₍

_{y− 10}

₎

2

−

12 − 1



, x>6 ,y< 10

β

4

(

10 − x

)

, y≥ 10 ,x+ y> 16 d

λ

2 d s = −

∂

H

∂

x2 =

⎧

⎪

⎨

⎪

⎩

β

4

(

6 − y

)

, x≤ 6 ,x+ y≤ 16

β

4

(

y− 10

)

4

(

x− 6

)

2₊

₍

_{y− 10}

₎

2

−

12 − 1



, x> 6 ,y<10 0 , y ≥ 10 ,x+ y>16 d

λ

3 d s = −

∂

H

∂

x3 =

λ

1 sin

θ

−

λ

2cos

θ

d

λ

4 d s = −

∂

H

∂

x4 = −

β

1+ 2

β

2

κ

2p−5+ 3

β

3

α

2p−7 (50)

We can derive the necessary condition(19) for the optimal control forboth guideline scenarios. The results of the optimalcontrol for the smooth and sharp guideline scenarios under various weights of the driving preferencecost are showninFig.12andFig.13,respectively.Theweightsoftherunningcost

β

1,

β

2,and

β

3aresettobe10,0.001s5/m3,and

0.01s5_/m3_{,respectively.Theweightsoftherunningcost}

_β

4 changesfrom0s/m3to5s/m3.Other parametersremainthe

sameasthoseinTable2.

Onecanfindthatinbothguidelinescenarios,withtheincreaseoftheweightofthedrivingpreferencecost(

β

4),the

runningtrajectory becomes closerandcloser tothe designed guideline,while the value ofthe velocity almost remains thesame.Comparingthe two scenarios oftheguideline,the sharpguideline ismoredifficult toapproach. Driverswill not follow thesharp guide lineunless a huge weight is given forthe running cost of driving preference, whichcan be a traffic rule inreality. Moreover, we noticed that the runningcost Eq. (48)is a piecewise continuous function.It leads toapiecewise continuoustheHamiltonianfunction(49).The resultsshow thattheproposed iterativenumericalsolution algorithmcanhandlethiscondition.

5.3.Interferencewithstationaryandmovingobstacles

Intherealworld,driversshouldavoidobjectsduringdriving.Thiscanalsobe capturedby addinganewrunningcost term. The generalizedobstacles can be the separationfacilities, the piers of theelevated highways andpedestrian

over-Fig. 12. Trajectory under smooth guide line scenario.

Fig. 13. Trajectory under sharp guide line scenario.

passes,andother vehicles.Theycanbetreatedasstationaryormovingobstacles.Sincethedriversaimtostayclearofthe obstacles, therunningcostcausedbythe obstaclescanbe definedasa monotonicallydecreasingfunctionofthedistance betweenavehicleandtheobstacle,asshowninEq.(51).Then,theHamiltonianfunctioncanbespecifiedasEq.(52),and the co-state dynamicsturns to be Eq. (53). The difference between thestationary andmoving obstacles liesin that the positionofthe movingobstacle (xb,yb)will changealongthe timewhile thestationaryobstaclepositionwill not.Please

note,sincethestate Xofavehicleisdescribedbyusingthemovingdistancesasthedependentvariableintheproposed model,themoving distancesshouldbetransformedtothetimetwhenupdatingthepositionofthemoving obstacles,as showninEq.(54).Inaquiteshortdistance,Eq.(54)turnstoEq.(55)byusingtheEulerforwarddifference.

L5= e −(x−xb) 2 +(y−yb) 2 2D2 b (51)

where,x_b andy_barethecoordinateoftheobstacleb,m;D_bisthescaleparameterthat isreflectingtheinfluencerangeof theobstacleb,m. H=

β

1p+ 1 2

β

2

κ

2_p−4₊ 1 2

β

3

α

2_p−6₊

_β

5e −(x−xb)2+(y−yb)2 2D2 b +

λ

₁cos

θ

+

λ

₂sin

θ

+

λ

₃

κ

+

λ

₄

α

(52)