Intersection-Traffic Control of Autonomous Vehicles using Newton-Raphson Flows and Barrier Functions
Shashwat Shivam, Yorai Wardi, Magnus Egerstedt, Aris Kanellopoulos, Kyriakos G. Vamvoudakis
IIntersection-Traffic Control ofAutonomous Vehicles usingNewton-Raphson Flows and BarrierFunctions
S. Shivam ∗ , Y. Wardi ∗ , M. Egerstedt ∗ , A. Kanellopoulos ∗∗ , K. G. Vamvoudakis ∗∗∗
School of Electrical and Computer Engineering, Georgia Institute ofTechnology, Atlanta, GA 30332, USA.e-mail: [email protected], [email protected],[email protected]. ∗∗ The Daniel Guggenheim School of Aerospace Engineering, GeorgiaInstitute of Technology, Atlanta, GA, , USA.e-mail: [email protected], [email protected].
Abstract:
This paper concerns an application of a recently-developed nonlinear trackingtechnique to trajectory control of autonomous vehicles at traffic intersections. The techniqueuses a flow version of the Newton-Raphson method for controlling a predicted system-outputto a future reference target. Its implementations are based on numerical solutions of ordinarydifferential equations, and it does not specify any particular method for computing its futurereference trajectories. Consequently it can use relatively simple algorithms on crude modelsfor computing the target trajectories, and more-accurate models and algorithms for trajectorycontrol in the tight loop. We demonstrate this point at an extant predictive traffic planning-and-control method with our tracking technique. Furthermore, we guarantee safety specifications byapplying to the tracking technique the framework of control barrier functions.1. INTRODUCTIONIn a recent work (Wardi et al. (2019)), we proposed anew approach to output tracking of dynamical systemsthat appears to be effective while requiring modest com-puting efforts. Its underscoring technique is based on astandalone integrator with a variable gain, designed forstability and small tracking errors. The integrator is de-fined by a flow version of the Newton-Raphson method forsolving algebraic equations. These equations are definedby attempting to match a predicted system’s output toa predicted value of the reference target. Furthermore,increasing the controller’s rate can stabilize the system,increase its stability margins, and reduce its tracking erroreven (in some cases) if the plant-subsystem is unstable andnot of a minimum phase.The proposed tracking technique may not be as generalor powerful as some of the existing nonlinear regulationtechniques like the Byrnes-Isidori regulator (Isidori andByrnes (1990)), Khalil’s high-gain observers for outputregulation (Khalil (1998)), or Model Predictive Control(MPC) (Rawlings et al. (2017)). However, the effectivenessof these methods is partially due to their computationalsophistication such as, respectively, nonlinear inversions,the appropriate nonlinear normal form, and real-time op- (cid:63)
This work was supported in part by the National Science Foun-dation under grant Nos. S&AS-1849264, CPS-1851588, and by ONRMinverva under grant No. N00014-18-1-2874. timal control. As for our tracking technique, its controlleris defined by an ordinary differential equation which canbe solved numerically in real time and hence, as arguedin Wardi et al. (2019), may be implementable by simplealgorithms.Thus far, the development of the proposed technique hasfocused on its fundamental structure, theoretical conver-gence results, and various examples including an invertedpendulum and motion control in platoons (Wardi et al.(2019)). Presently our main interest is in applications toautonomous vehicles, and especially in trajectory controlof swarms and platoons. Such problems often are ad-dressed by MPC or related techniques; see e.g., (Konget al. (2015); Plessen et al. (2018); Kim and Kumar (2014))and references therein. Like MPC, our technique is basedon predictive control, but it is different from MPC inseveral ways. It is not based on optimal control nor does itspecify a particular framework for computing future targettrajectories. Once the target trajectories become available,it uses a fluid-flow version of the Newton-Raphson methodfor tracking control.The primary objective of this paper is to investigate howthe proposed tracking technique can complement otherprediction-based approaches. To this end we consider thetrajectory-control technique developed in Malikopouloset al. (2018) for traffic management of autonomous vehiclesin urban road-intersections. This technique is slated to a r X i v : . [ ee ss . S Y ] A p r ptimize motion-energy consumption of each vehicle whileguaranteeing safety constraints. It is in the flavor of MPCin that it solves optimal control problems for computingfuture trajectories, but unlike MPC it does not considerrolling horizons but a single optimal control program foreach vehicle approaching an intersection. A salient featureof this technique is that is uses a simple dynamic modelfor the vehicles, comprised of a double integrator, therebyenabling closed-form solutions to the optimal control prob-lems. This gives an efficient trajectory-computation forevery vehicle, which scales well with traffic loads at theintersections.Our tracking technique complements the traffic controlframework of Malikopoulos et al. (2018) in the follow-ing way. We first compute the target-trajectories of thevehicles using the simple model and formula derived inMalikopoulos et al. (2018), then we apply our techniqueto a more complicated and realistic model for trackingof the target trajectory. To this end we use a dynamicbicycle model for the vehicles’ motion, a sixth-order non-linear model that has been extensively used in control ofautonomous vehicles (see, e.g., Kong et al. (2015); Plessenet al. (2018) and references therein). Furthermore, weextend the applications domain of of Malikopoulos et al.(2018) from a straight road to a curved road. Lastly, weexamine the treatment of safety constraints in the tightcontrol loop by incorporating control barrier functionswith the tracking technique.The rest of the paper is organized as follows. Section 2formulates the problem. Section 3 summarizes the trackingtechnique that will be used in the sequel. Section 4 presentssimulation results, and Section 5 concludes the paper andoutlines directions for future research. Statement of contributions.
The contribution of the presentpaper is twofold. The first contribution extends the frame-work of prediction-based nonlinear tracking in the contextof trajectory control of autonomous vehicles at trafficintersections, while the second is in the incorporation ofsafety measures through the use of barrier functions. Re-garding the first contribution, the tracking technique hasbeen applied to the dynamic bicycle model (Shivam et al.(2019b); Wardi et al. (2019)), and the relevant contribu-tion in this paper is in a proof of concept regarding theway it complements the control framework of Malikopouloset al. (2018). Regarding safety guarantees, control barrierfunctions have not been applied to the tracking techniqueor, to our knowledge, to a dynamic bicycle model.A reduced version of this paper will appear in the
Proc.21st IFAC World Congress , Berlin, Germany, July 12-17,2020. 2. PROBLEM FORMULATIONOur work is concerned with the management and control ofvehicle-flows at traffic intersections. Each one of the roadscomprising an intersection consists of two zones: a controlzone and a merging zone. The merging zone is at the centerof the intersection, where lateral accidents are possible.The control zone is a stretch of the road approachingthe merging zone, where the scheduling, planning andcontrol of vehicles’ trajectories are performed. Once a vehicle enters the merging zone its speed or lane cannotbe changed.Whenever a vehicle enters the control zone, a schedulercomputes the time and speed at which it has to enterthe merging zone based on the current and future states(positions and velocities) of all the other vehicles concur-rently in the intersection. Subsequently the trajectory ofthe newly-arrived vehicle is computed by minimizing itsprojected motion energy while maximizing the throughputat the intersection, subject to safety and operational con-straints. The safety constraints include a minimum inter-vehicle distance and a maximum deviation from a lane-center, while the operational constraints include boundson speed and acceleration. This trajectory-planning prob-lem is formulated as an optimal control problem whichis parameterized by the states of all the other vehiclesconcurrently at the intersection, hence it is different fromone vehicle to the next and consequently must be solvedin real time.The contribution of this paper is not in the aforementionedscheduling and trajectory planning-and-control problem,but in a tracking of its computed solution. Thus, thetracking control is at a lower level than the optimal controlproblem, and for that we use a bicycle model for thevehicles’ dynamics, which is more accurate and detailedthan the double-integrator model.The bicycle model that we use is the six-degree nonlinearsystem described in Kong et al. (2015). Its state variableis x = ( z , z , v (cid:96) , v n , ψ, ˙ ψ ) (cid:62) , where z and z are the planerposition-coordinates of the center of gravity of the vehicle, v (cid:96) and v n are the longitudinal and lateral velocities, ψ isthe heading of the vehicle and ˙ ψ is its angular velocity. Theinput, u = ( a (cid:96) , δ f ) (cid:62) , consists of the longitudinal accelera-tion and steering angle of the front wheels, respectively,and the output, y = ( z , z ) (cid:62) , is the center of gravity ofthe vehicle. The dynamic equations are (see Kong et al.(2015)): ˙ z = v (cid:96) cos ψ − v n sin ψ, ˙ z = v (cid:96) sin ψ + v n cos ψ, ˙ v (cid:96) = ˙ ψv n + a (cid:96) , ˙ v n = − ˙ ψv (cid:96) + 2 ( F c,f cos δ f + F c,r ) /m, ¨ ψ = 2 ( l f F c,f cos δ f − l r F c,r ) /I z , (1)where m is the mass of the vehicle, l f and l r are the frontand back axles’ distances from the vehicle’s center of mass, I z is the yaw moment of inertia, and F c,f and F c,r are thelateral forces on the front and rear wheels. These forcesare approximated by the following equations, F c,f = C α,f (cid:16) δ f − tan − (cid:0) ( v n + l f ˙ ψ ) /v (cid:96) (cid:1)(cid:17) ,F c,r = − C α,r tan − (cid:16) ( v n − l r ˙ ψ ) /v (cid:96) (cid:17) , where C α,f and C α,r are the cornering stiffness of the frontand rear tires, respectively.Our tracking technique will be tested first on a curvedroad, which does not quite fit in the framework of Ma-likopoulos et al. (2018) due to its one-dimensional trafficmodel of motion. Then we make a more careful examina-tion of safety constraints which are addressed in real time (t) e(t) � Controller u(t) Plant y(t) � � + . - Fig. 1.
Basic control system. by control barrier functions, and for that we use a straightroad in order to highlight the effects of the safety controls.3. TRACKING TECHNIQUE AND CONTROLBARRIER FUNCTIONSThis section serves to explain the tracking technique andthe salient features of control barrier functions that willbe used in the sequel.
An extension of the material in this subsection can befound in Wardi et al. (2019).Consider the system depicted in Figure 1, where thereference input r ( t ), the control signal u ( t ), and the output y ( t ) are in R m for a given m = 1 , , . . . . The task of thecontroller is to ensure thatlim t →∞ || r ( t ) − y ( t ) || < ε (2)for a given (small) ε > y ( t ) = g ( u ( t )) (3)for a continuously differentiable function g : R m → R m .The controller that we use is defined by the differentialequation ˙ u ( t ) = α (cid:16) d g d u ( u ( t )) (cid:17) − (cid:0) r ( t ) − g ( u ( t )) (cid:1) , (4)with an initial condition u (0) ∈ R m , where α > dgdu ( u ( t )) is nonsingular throughout the trajectorydetermined by this equation.To see the effects of this controller on asymptotic tracking-convergence, define the Lyapunov function V ( u ( t )) = 12 || r ( t ) − g ( u ( t )) || . (5)Define η := lim sup t →∞ || ˙ r ( t ) || . Then some algebra revealsthat˙ V ( u ( t )) = (cid:10) r ( t ) − g ( u ( t )) , ˙ r ( t ) − α ( r ( t ) − g ( u ( t )) (cid:11) , (6)and hence lim sup t →∞ || r ( t ) − y ( t ) || < ηα ; (7)see Wardi et al. (2019) for details. Stability is not a concernas long as the Jacobian dgdu ( u ( t )) is nonsingular throughoutthe trajectory { u ( t ) : t ≥ } since (7) is guaranteed by (5)and (6). Therefore, ε > α large enough in (4).Suppose next that the plant (in Figure 1) is a dynamicalsystem with the state equation˙ x ( t ) = f ( x ( t ) , u ( t )) , x (0) := x , (8) and the output equation y ( t ) = h ( x ( t )); (9)here the state variable is x ( t ) ∈ R n , x ∈ R n is a giveninitial state, and the input and output are u ( t ) ∈ R m and y ( t ) ∈ R m , respectively. The dynamic response functionis f : R n × R m → R n , and the output function is h : R n → R m . The following assumption, implicitlymade in the forthcoming discussion, ensures that for everybounded, piecewise-continuous control function u ( t ), andfor every x ∈ R n , there exists a unique continuous,piecewise continuously-differentiable solution x ( t ) to Eq.(8). Assumption 1. f : R n × R m → R n is continuously differentiable, and for every compact setΓ ⊂ R m there exists K > x ∈ R n and for every u ∈ Γ, (cid:107) f ( x, u ) (cid:107) ≤ K (cid:0) (cid:107) x (cid:107) + 1 (cid:1) . (10)2). The function h : R n → R m is continuously differen-tiable.By Eqs. (8)-(9), x ( t ) and hence y ( t ) are not functions of u ( t ) but rather of { u ( τ ) : τ ∈ [0 , t ) } and x . Therefore Eq.(3) is no longer true, and a controller cannot be definedby (4). To get around this difficulty we use an outputpredictor and attempt to match it to a future target-reference. Given a fixed time horizon T >
0, at every time t ≥ y ( t + T ), denoted byˆ y ( t + T ). We assume that it is a function of x ( t ) and u ( t ),hence has the functional formˆ y ( t + T ) = g ( x ( t ) , u ( t )) (11)for a suitable function g : R n × R m → R m . We hence-forth implicitly assume that g ( x, u ) is continuously dif-ferentiable. The future reference r ( t + T ) may have to beestimated as well by a suitable predictor (e.g., Wardi et al.(2019)), but we assume here that it is known exactly attime t in order to simplify the discussion.The objective of the controller, next defined, is to haveˆ y ( t + T ) track r ( t + T ). Thus, in analogy with (4), it isdefined by the following equation,˙ u ( t ) = α (cid:16) ∂g∂u ( x ( t ) , u ( t )) (cid:17) − (cid:0) r ( t + T ) − g ( x ( t ) , u ( t )) (cid:1) (12)with given α > u := u (0); weimplicitly assume that the partial Jacobian ∂g∂u ( x ( t ) , u ( t ))is nonsingular for every t ≥
0. By (11), it can be seen thatthis controller attempts to have ˆ y ( t + T ) match r ( t + T ).The output predictor that we use is based on the stateequation (8) in the interval τ ∈ [ t, t + T ] with theinput control u ( τ ) ≡ u ( t ) and initial state x ( t ). Formally,denoting by ξ ( τ ) the variable representing the predictedstate, it satisfies the equation˙ ξ ( τ ) = f ( ξ ( τ ) , u ( t )) , ξ ( t ) = x ( t ) , (13)then we define ˆ y ( t + T ) = h ( ξ ( t + T )) . (14)We typically solve Eq. (13) by the Forward Euler method.The following discussion implicitly assumes that this is thepredictor used.The state equation (8) and control equation (12) togetherdefine a dynamical system with input r ( t ) and statevariable z ( t ) := ( x ( t ) (cid:62) , u ( t ) (cid:62) ) (cid:62) ∈ R n + m . Suppose thathe initial state z := z (0) is confined to a given compactset Γ ⊂ R n + m . Ref. Wardi et al. (2019) defines thefollowing notion of BIBS stability, called α -stability. Definition 1.
The system is α -stable if there exist ¯ α ≥ K functions, β ( s ) and γ ( s ) such that, forevery initial state z ∈ Γ and an input { r ( t ) } , for every α ≥ ¯ α , || z ( t ) || ≤ β ( (cid:107) z (0) (cid:107) ) + γ ( || r || ∞ ) , (15)where || r || ∞ is the L ∞ norm of { r ( t ) : t ≥ } .Essentially α -stability implies BIBS stability for large gain α . Ref. Wardi et al. (2019) derives verifiable sufficientconditions for α stability of linear systems, which coversituations where the plant subsystem is neither stable norof a minimum phase. Simulation and experimental resultsof various linear and nonlinear systems indicate stabilityat large gains. Also, a theoretical result guarantees anextension of Eq. (7) from the case where the plant is amemoryless nonlinearity to the present case of a dynamicalsystem. It states that if the system is α stable then thereexist η > α ≥ α ≥ ¯ α ,lim sup t →∞ || r ( t + T ) − ˆ y ( t + T ) || < ηα . (16)In this case, speeding up the controller by increasing α can serve the dual purpose of stabilizing the closed-loopsystem and reducing the tracking error. We point out,though, that “tracking” means here that the future targetreference, r ( t + T ), is approached by the predicted output,ˆ y ( t + T ), not the actual output, y ( t + T ). In fact, definingthe asymptotic prediction error by µ := lim sup t →∞ || y ( t ) − ˆ y ( t ) || , Eq. (7) is extended tolim sup t →∞ || r ( t + T ) − y ( t + T ) || < µ + ηα . (17)We see that speeding up the controller does not reducethe effects of the asymptotic prediction error on theasymptotic tracking error. This comes at no surprise sincethe prediction error is akin to a measurement error inclassical control systems.Finally, a word must be said about the relationship be-tween this tracking-control method and MPC. MPC ar-guably is the most-commonly used control technique basedon prediction. It solves optimal control problems in realtime, and these serve the dual purpose of computing thefuture trajectory and controlling the system. In contrast,our technique does not specify how to compute the futuretarget trajectory. In principle it may be given a-priori,computed once at the start of the control action as inMalikopoulos et al. (2018) and in this paper, or computedon-line by data interpolation (as in Wardi et al. (2019))or neural nets (see Shivam et al. (2019b)). The controllaw we use is based on a real-time numerical solution of adifferential equation but not on optimal control. The authors of Ames et al. (2014) laid the groundworkfor ensuring safety in the design of control systems. Theapproach combines control barrier functions with controlLyapunov functions to achieve the dual purpose of effectivecontrol and satisfying hard safety constraints. Shortly thereafter it was applied to the control of multi-agentsystems and networks, with applications to mobile robotsand autonomous vehicles; see Wang et al. (2017) for aninitial work, and Ames et al. (2019) for a recent survey.The essential elements of this approach are summarized inthis subsection, and more-extensive expos´es can be foundin Ames et al. (2014, 2019); Wang et al. (2017).Consider a dynamical system defined by Eq. (8), where x ∈ R n and u ∈ R m , and suppose that Assumption 1is in force. Let S ⊂ R n be a closed set called a safeset , and suppose that it is desirable to design a feedbackcontrol such that S is forward invariant and asymptoticallystable for the system. This requirement means that (i) if x ( t ) ∈ S for some t ≥
0, then x ( t ) ∈ S for every t ≥ t ;and (ii) the following limit holds,lim t →∞ dist (cid:0) x ( t ) , S (cid:1) = 0 , (18)where dist( x, S ) is the point-to-set Euclidean distance.A continuously-differentiable function h : R n → R is saidto be a barrier function for S if there exists a Lipschitz-continuous, extended class K function κ : R → R (monotone increasing, κ (0) = 0) such that h ( x ) < x outside S , h ( x ) > x in the interior of S , and h ( x ) = 0 for all x in the boundary of S ; and forevery t ≥
0, dd t h ( x ( t )) + κ (cid:0) h ( x ( t )) (cid:1) ≥ . (19)Now suppose that a control u ( t ) is determined such that,at each time t ≥
0, Eq. (19) is satisfied. Then the set S isforward invariant and asymptotically stable for the system.That is, the state trajectory is driven towards S , and onceentering, cannot escape from it. The main question now ishow to design a controller guaranteeing (19) for all t ≥ u (cid:63) ( t ) be a state-feedback control law that has beendesigned for a suitable performance (e.g., tracking) butwithout regard for safety. For every x ∈ R n , denote by S ( x ) the set S ( x ) = (cid:8) u ∈ R m : ∂h∂x ( x ) f ( x, u ) + κ ( x ) ≥ (cid:9) . (20)Note that dd t h ( x ( t )) = ∂h∂x ( x ) f ( x, u ). Therefore, if u (cid:63) ( t ) ∈ S ( x ( t )) for every t ≥
0, then the requirements for thesafety set S are met. However, in the event that u (cid:63) ( t ) / ∈ S ( x ( t )) for some t ≥
0, it is reasonable to modify thecontrol from u (cid:63) ( t ) to u ( t ), defined as follows: u ( t ) := argmin {|| u − u (cid:63) (t) || : u ∈ S(x(t)) } . (21)In other word, u ( t ) is the point in S ( x ) closest to u (cid:63) ( t ).With the feedback control defined by (21) for all t ≥
0, therequirements of the safety set S are met, and it is hopedthat the performance according to which u (cid:63) ( t ) was definedis changed in a minimal way.Observe that this procedure requires an optimization prob-lem to be solved at each t ≥
0. However, if the system’sdynamic equation is control-affine, then this optimizationproblem is a quadratic program for which there are effi-cient computational techniques. To see this point, supposethat f ( x, u ) = f ( x ) + f ( x ) u (22)for functions f : R n → R n and f : R n → R n × m , whichmeans that the plant system is control affine. Then, by21), given t ≥ x ( t ) ∈ R n and u (cid:63) ( t ) ∈ R m , u ( t ) is thesolution of the optimization problemmin (cid:8) || u − u (cid:63) ( t ) || : ∂h∂x ( x ( t )) (cid:0) f ( x ( t )) + f ( x ( t )) u + κ (cid:0) h ( x ( t )) (cid:1) ≥ (cid:9) ; (23)the constraints on u are linear hence this is a quadratic-programming problem in u .We mention that the control barrier function method,in more-general setting than described above, has hadsuccessful applications in several areas; see, e.g., (Ameset al. (2014, 2017); Wang et al. (2017); Ames et al. (2019))and references therein.4. SIMULATION RESULTSWe consider the control zone of a road approaching anintersection, and as in Malikopoulos et al. (2018), assumethat vehicles do not change lanes and hence we focuson a single lane. The motion dynamics of the vehiclesfollow the bicycle model discussed in Section 2 with thefollowing parameter values as in Shivam et al. (2019a): m = 2 , kg , I z = 3 , kg · m , l f = 1 . m , l r =1 . m , C α,f = 57500 N/rad , and C α,r = 92500 N/rad .Two experiments are conducted. In the first experiment weconsider only tracking without regard to safety constraintsin the tight loop. We compute the trajectories of the vehi-cles by the formula derived in Malikopoulos et al. (2018),then apply the tracking technique to ensure that the com-puted trajectories are followed. In the second experimentwe define safety constraints in terms of minimum inter-vehicle distance and maximum lateral deviations from thelane’s center, and apply Control Barrier Functions (CBF)to ensure that they are satisfied in the face of unexpectedchanges to traffic conditions. We consider a road (lane) approaching an intersection,consisting of a control zone of 400m and merging zoneof 30m. It comprises a 430 m-long, 30 o segment of a circledefined by the following equation, z + ( z − R ) = R , where R = π/ = 821 . z and z are planer co-ordinates of points on the circle, z ∈ [0 , R sin( π/ , . z ∈ [0 , R (1 − cos( π/ , . o with respectto the direction of the lane, and therefore, if the controlgives effective tracking, they are expected to remain closeto the lane’s center and maintain a heading of near 0 o (with respect to the road) throughout the control zone.The arrival times of the vehicles to the merging zone andthe vehicle’s trajectories in the control zone are computed, Safety has been guaranteed in the trajectory-planning stage bythe scheduler and the optimal control problem. Here we considerunforeseen situations that may arise in real time.
Fig. 2.
Distances traveled vs. time, barely distinguishable from thecorresponding target trajectories.
Fig. 3.
Tracking errors vs. time, under 6 cm during initial transientphase, and under 2 cm thereafter. respectively, by the scheduling procedure and the optimalcontrol algorithm proposed in Malikopoulos et al. (2018).Now it must be pointed out that that algorithm is ap-plicable to straight roads since it is underscored by astraight-line model of motion. Therefore, we compute thetrajectories as if the road is straight, and map the resultsto the curved road according to the distance travelled. Thisno-longer results in minimum-energy trajectories since thedynamic vehicle-model is two-dimensional, but the approx-imation errors are minor.To test the robustness of the controller with respect tomodeling variations, we induce an error of 100% in thevehicles’ mass. Thus, the predictor equation (13) usestwice the “real” weight of the cars which is used in thesimulations.All the differential equations for the simulation and thecontroller are solved by the Forward Euler method withstep-size of d t = 0 .
005 for the simulations, and ∆ t = 0 . α = 100.The results are shown in Figures 2-4. Figure 2 depicts thegraphs of the distance (arc-length) travelled by the fivevehicles through the control zone and merging zone, asfunctions to time. The color-coded legend indicates theorder of the vehicles, where car i refers to the i th vehiclethat arrives to the control zone. The vehicles travel 430mthrough the control and merging zones, but we extendthe graphs past their departures from the merging zoneby holding the Distance-Travelled variables to a constant(430m), for ease of a better presentation. Apparently Car1 moves at a constant velocity. In contrast, subsequentvehicles slow down in order to meet the computed scheduleof entering the merging zone, which is more sparse thantheir arrival schedule to the control zone. These graphswill be used to explain some of the phenomena indicatedin the figures below.ig. 4. Vehicles’ longitudinal accelerations vs. time, under 5% of g . The tracking error for each vehicle, defined by the Eu-clidean distances between its position and target referenceas computed by the optimal control program, is depictedin Figure 3. Following an initial error of about 6 cm,the vehicles settle, in about 3 seconds, to a steady-stateerror of under 2 cm. The initial error is due to transientsassociated with discrepancies between the vehicles’ ini-tial poise and their corresponding reference points. Thesetransients appear identical for all the vehicles, becausetheir respective initial positions, velocities and steeringangles are identical at the respective times they enter thecontrol zone. The asymptotic tracking errors are due to theprediction errors and the curvatures of the road. We testedthis hypothesis by trying the following modifications one ata time: (i) Increasing the predictor’s step size in (13) from0 .
001 to 0 . .
34 cm.Since the tracking errors are small relative to the distancetravelled, the graphs in Figure 2 are indistinguishable fromcorresponding graphs of the reference trajectories, whichconsequently are not presented in the paper.Figure 4 depicts the graphs of the longitudinal acceler-ations of the vehicles vs. time, and we observe initialtransients of under 0 .
48 m/s . The graphs of later vehiclesare below the graphs of earlier vehicles. This is due to thefact that all of the vehicles travel the same distance butlater ones do it in more time, hence have to deceleratemore than earlier vehicles. We also note that each graphreaches zero acceleration at the final point of the controlzone, Which is in compliance with the constraint that itmust travel at a constant speed at the merging zone.Finally, we tested the tracking algorithm on a straightroad, all other parameters unchanged. The results arenot shown here. Those of the distance travelled andlongitudinal accelerations are barely distinguishable fromthe graphs in Figure 2 and Figure 4, respectively. Theonly noticeable differences are in the asymptotic trackingerrors, whose maximum is reduced from around 2 cm asindicated in Figure 3, to 1 .
34 cm.
We next extend the simulation setting described in thelast subsection to include safety constraints that may haveto be addressed in real time, and apply to them controlbarrier functions. To highlight the role of the CBF weassume that the road is straight and consider only two vehicles in the forthcoming simulations. The first vehicleserves only as a reference for controlling the second vehicle,hence we preset its trajectory and do not control it. Thesecond vehicle has the same dynamic bicycle model as inSubsection 4.1, and it is controlled by the same trackingtechnique described there.The first vehicle enters the control zone at time t = − t := 0. The firstvehicle travels along the straight road and its speed profileis shown in the blue graph of Figure 5. The constantvelocities in the figure are 2 m/s, 1 m/s, and 2 m/s,respectively, its deceleration commences at time t :=50 s,and its acceleration starts at time t :=75 m. We do notapply the tracking control or CBF to the first vehicle, andassume that it maintains the above speed profile whilemoving along the lane without deviations.At the time the second vehicle enters the control zone(road), t = 0, it is 10m behind the first vehicle. Its targetreference trajectory is r ( t ) := r ( t ) = (2 t, (cid:62) , lying alongthe horizontal road. However, it enters the road at theinitial steering angle of 20 o , or 0.35 rad from the directionof the road. Therefore initially the second vehicle veers offthe lane, but is pulled back to it by the tracking control.Thereafter it stays on the lane while tracking its targettrajectory. Without an application of the CBF to thesecond vehicle, maximum deviation (lateral distance) fromthe lane’s center is about 1.6m, which practically may beunacceptable. Furthermore, after returning to the lane, itruns into the first vehicle shortly after its slowdown.To avoid the collision and limit the lateral deviation fromthe lane, we impose the following two safety constraints:(i) the second vehicle must maintain a distance of at least5m from the first vehicle, and (ii) the lateral deviation ofthe second vehicle from the center-lane must not exceed0.5m. We label these the longitudinal constraint and the lateral constraint . We design two corresponding CBF andapply them jointly with the tracking controller. We pointout that the longitudinal dynamics of the second vehi-cle are control-affine while its lateral dynamics are notcontrol affine; see the state equation (1). Therefore theCFB for the longitudinal constraint can rely on quadraticprogramming for computing the control defined by Eq.(21), while the lateral-safety control cannot use quadraticprogramming and has to be ad hoc. We next explain thetwo control barrier functions.Let us denote the position and velocity of the i th vehicle, i = 1 ,
2, by p i ∈ R and v i ∈ R , respectively.Furthermore, define the relative displacement and relativevelocity of the second vehicle with respect to the first oneby ∆ p := p − p and ∆ v := v − v . The purpose ofthe CBF is to ensure that || ∆ p || ≥ d for a given d > d = 5m in our experiments). Therefore it is tempting todefine the safe set as S := { x ∈ R : || ∆ p || ≥ d } , where x is the state variable of the dynamic bicycle model definedin Section 2 (recall Eq. (1) for its state equation). However, We are aware of a transformation of the system that renders itsstate equation affine with respect to both input controls (Rajamani(2012)). However, we prefer to work with the current system in orderto test the controller in an environment where the state equation isnot control affine. The results, presented in Figure 7, below, suggestthat the CBF works well. his can be problematic because if || ∆ p || is nearly d and∆ v projected on the direction of relative displacement isnegative, it may be impossible to guarantee the forwardinvariance of the safe set. Furthermore, according to thedefinition of the state variable, the position of a vehicleis expressed in terms of its Cartesian coordinates whileits velocity is characterized by its longitudinal and lateralcomponents, which can make it complicated to describethe safe set in simple terms.To get around this difficulty we use an idea, developed inWang (2018), of defining a CBF in terms of the relativevelocity along the relative displacement. Denoted by ˆ v , itis defined by ˆ v = (cid:10) ∆ p (cid:107) ∆ p (cid:107) , ∆ v (cid:11) . (24)Let ¯ a > k := (2¯ a ) − .Recall that the longitudinal acceleration is denoted by a (cid:96) and it is a part of the input (see (1)). Now, a simplealgebra shows that for every interval [ t, t ] where thefirst vehicle has a constant velocity, if a (cid:96) ( τ ) ≡ − ¯ a then || ∆ p ( τ ) || ≥ || ∆ p ( t ) || − k ˆ v ( t ) . Therefore, to ensure theforward invariance of the constraint set { x ∈ R : (cid:107) ∆ p (cid:107) ≥ d } , we impose the condition that (cid:107) ∆ p ( t ) (cid:107) − k ˆ v ( t ) ≥ d . (25)This leads us to define the barrier function by h ( x ) = (cid:112) k ( (cid:107) ∆ p (cid:107) − d ) − || ˆ v || . As a part of the safety control weenforce the condition ddt h ( x ( t ))+ h ( x ( t )) ≥ ∀ t ≥
0, whichimplies that the set defined by (25) is forward invariant(see Ames et al. (2014, 2017, 2019)). Therefore, we considerthe set defined by (25) as the safe set. Finally, we notethat the dynamic equation (1) is control affine in thelongitudinal acceleration, and hence the input control canbe computed by a quadratic program.To define the lateral CBF, we only need the lateraldeviation of the vehicle from the lane’s center and itsvelocity. Denote by y the lateral deviation, and let y max be the maximum allowed deviation. In analogy to (25),the following condition ensures the maximum deviationconstraint, y max ≥ | y + k y | y | ˙ y | , (26)where k := (2˜ a ) − with ˜ a denoting the maximum lateralacceleration. We define the safe set to be the set satisfyingthe inequality in (26). Correspondingly, we define thebarrier function h ( x ) := y max − | y + k y | y | ˙ y | , and define κ ( y ) = γh for a constant γ > γ = 15). Takingderivatives and using (1), it can be seen that Eq. (19) issatisfied.The input involved with the lateral deviation is the steer-ing angle of the front wheels, δ f . Unlike the longitudinalcontrol, the dynamic equation is not control affine in δ f ,and hence we cannot compute the control by a quadraticprogram. Instead, a linear search is performed by a bisec-tion algorithm over a set of admissible δ f , which is theinterval [ − π , π ], and the closest value to the desired input(computed by the tracking controller) which satisfies the(26) is chosen.The simulation results are depicted in Figures 5-10. Figure5 depicts the speeds of the first and second vehicles in blue Vehicle 1Vehicle 2
Fig. 5.
Vehicles’ velocities vs. time. The change in velocity of Car2 is due to the action of the CBF.
Fig. 6.
Distance between the two vehicles. The decline starting at50s is due to the action of the CBF. and red, respectively. The visible initial transient of the redgraph is due to the heading of the car when it enters theroad. Note the delay in the slowdown of the second vehicleafter the first vehicle; it is due to the fact that the secondvehicle starts reducing its speed not immediately but whenits distance from the first vehicle approaches the minimumof 5 m. In contrast, there is no such delay in the speedupsince that would violate the minimum-delay constraints.Figure 6 shows the graph of the inter-vehicle distance, andwe clearly see that it retains its minimum value throughand following the speedup of the first vehicle.Figure 7 depicts the graph of the lateral (normal) deviationof the second vehicle from the lane’s center, which islargely due to its initial heading of 20 o . We mentionedthat without the lateral CBF the maximum distance is1.6 m, and we observe that with the CBF, the maximumdistance is about 0.27 m. Figure 8 shows the graph ofthe distance between the position of the second vehicleand its target trajectory. Following an initial transientthe vehicle catches up and tracks its target trajectoryuntil the first vehicle slows down. It then rises during theslowdown period due to the action of the CBF. After thefirst vehicle speeds up, the second vehicle cannot closedown its tracking error since it is forced by the CBF tokeep the inter-vehicle distance of 5 m, hence the trackingerror assumes a constant value.The next two figures show the two controls. Figure 9depicts the longitudinal acceleration, and we notice jumpsthat are due to initial transients as well as the slowdownand speedup of the first-vehicle. Figure 10 depicts thegraph of the steering angle of the second car, and itdisplays a transient due to the initial heading of the car.Neither figure displays surprising results.
10 20 30 40 50 60 70 80 90 100-0.2-0.100.10.20.3
Fig. 7.
Distance of second vehicle from the lane-center. Without theCBF the maximum distance is about 1.6m.
Fig. 8.
Tracking error of second vehicle. It cannot be reduced dueto the CBF.
Fig. 9.
Longitudinal acceleration of second vehicle.
Fig. 10.
Steering angle of second vehicle
5. CONCLUSION AND FUTURE WORKThis work extends the framework of prediction-basednonlinear tracking in the context of trajectory control ofautonomous vehicles at traffic intersections. We presentresults that use a flow version of the Newton-Raphsonmethod for controlling a predicted system-output to afuture reference target. Furthermore, we guarantee safetyspecifications by applying to the tracking technique theframework of control barrier functions.Future work will focus on developing robustness guaran-tees will allow for more realistic scenarios, where noise andexternal disturbances are taken into consideration. REFERENCESAmes, A., Coogan, S., Egerstedt, M., Notomista, G.,Sreenath, K., and Tabuada, P. (2019). Control barrierfunctions: Theory and applications. In
European ControlConference,
Napoli, Italy, June 25-28.Ames, A., Grizzle, J., and Tabuada, P. (2014). Controlbarrier function based quadratic programs with applica-tion to adaptive cruise control. In
IEEE Conf. Decisionand Control (CDC),
Los Angeles, California, December15-17, 62716278.Ames, A., Xu, X., Grizzle, J., and Tabuada, P. (2017).Control barrier function based quadratic programs forsafety critical systems.
IEEE Trans. Automatic Control ,62, 38613876.Isidori, A. and Byrnes, C. (1990). Output regulation ofnonlinear systems.
IEEE Transactions on AutomaticControl , 35, 131–140.Khalil, H. (1998). On the design of robust servomecha-nisms for minimum phase nonlinear systems.
Proc. 37thIEEE Conference on Decision and Control,
Tampa, FL,3075–3080.Kim, K.D. and Kumar, P. (2014). Am mpc-based ap-proach to provable system-wide safety and liveness ofautonomous ground traffic.
IEEE Transactions on Au-tomatic Control , 59(12), 3341–3356.Kong, J., Pfeiffer, M., Schildbach, G., and Borrelli, F.(2015). Kinematic and dynamic vehicle models forautonomous driving control design. In
Proc. IEEEIntelligent Vehicles Symposium (IV) .Malikopoulos, A., Cassandras, C., and Zhang, Y. (2018). Adecentralized energy-optimal control framework for con-nected automated vehicles at signal-free intersections.
Automatica , 93, 244–256.Plessen, M., Bernardini, D., Esen, H., and Bemporad, A.(2018). Spatial-based predictive control and geometriccorridor planning for adaptive cruise control coupledwith obstacle avoidance.
Transactions on Control Sys-tems Technology , 26(4), 38–50.Rajamani, R. (2012).
Vehicle Dynamics and Control .Springer, New York, New York.Rawlings, J., Mayne, D., and Diehl, M. (2017).
ModelPredictive Control: Theory, Computation, and Design,2nd Edition . Nob Hill, LLC.Shivam, S., Buckley, I., Wardi, Y., Seatzu, C., and Egerst-edt, M. (2019a). Tracking control by the Newton-Raphson flow: Applications to autonomous vehicles. In
Napoli,Italy, June 25-28.Shivam, S., Kanellopoulos, A., Vamvoudakis, K., andWardi, Y. (2019b). A predictive deep learning approachto output regulation: The case of collaborative pursuitevasion. In .Wang, L., Ames, A., and Egerstedt, M. (2017). Safety bar-rier certificates for collisions-free multi-robot systems.
IEEE Transactions on Robotics , 33(3), 661–674.Wang, L. (2018).