Real-Time NMPC for Semi-AutomatedHighway Driving of Long Heavy Vehicle

Combinations ?

Niels van Duijkeren ∗ Tamas Keviczky ∗∗ Peter Nilsson ∗∗∗

Leo Laine ∗∗∗

∗ KU Leuven, B-3001 Heverlee, Belgium(e-mail: Niels.vanDuijkeren@kuleuven.be).

∗∗ Delft University of Technology, 2628 CD Delft, The Netherlands(e-mail: T.Keviczky@tudelft.nl)

∗∗∗ Volvo Group Trucks Technology, SE-405 08 Goteborg, Sweden(e-mail: {Peter.q.Nilsson, Leo.Laine}@volvo.com)

Abstract: This work presents a Nonlinear Model Predictive Control (NMPC) approach to real-time trajectory generation for highway driving with long truck combinations. In order to considerall relevant information for the road and the surrounding traffic, we formulate a finite-horizonoptimal control problem (OCP) that incorporates a prediction model in spatial coordinatesfor the vehicle and the surrounding traffic. This allows road properties (such as curvature) toappear as known variables in the prediction horizon. The objective function of the resultingconstrained nonlinear least-squares problem provides a trade-off between tracking performance,driver comfort, and keeping a comfortable distance from fellow road users. The OCP is solvedwith a direct multiple shooting solution method implemented in a real-time iteration schemeusing ACADO code generation and compared with a feedback scheme that solves the entirenonlinear program in each time step with the interior point solver IPOPT. To illustrate theefficacy and the real-time implementability of the methodology, simulation results are presentedfor a high way merging scenario of a long heavy vehicle combination.

Keywords: predictive control, model-based control, automotive control, autonomous vehicles,real-time MPC, control engineering, nonlinear control

1. INTRODUCTION

The so-called long heavy vehicle combinations (LHVCs)are trucks that are widely used in Canada and Australiatoday, and will be more abundant on the road also inEurope in the near future (Kindt et al. (2011)). Theyhave the potential to decrease road transport costs, trafficcongestions, and generate lower emissions than currentmeans of road freight transport. However, an undesiredeffect of the added towed units is the increase in difficultyto maneuver these trucks on roads and in busy traffic. Theincreased complexity for truck drivers to handle tasks suchas changing lane and maneuvering in tight corners, callfor automatic driver assistance functions. Development ofsuch advanced driver assistance systems, or potentially au-tonomously functioning trucks, are essential to safeguardtraffic safety with an increasing use of LHVCs. One crucialelement to increase autonomy in completing the complex

? The research leading to these results was partly funded by thePeople Programme (Marie Curie Actions) of the European Union’sSeventh Framework Programme FP7/2007-2013/ under REA grantagreement n◦ 607957. This work also benefits from Center-of-Excellence Optimization in Engineering (OPTEC), the Belgian Pro-gramme on Interuniversity Attraction Poles, initiated by the Bel-gian Federal Science Policy Office (DYSCO). The applied researchon commercial heavy vehicles was funded by Volvo Group TrucksTechnology (VGTT) and Virtual Prototyping Centre (ViP).

Fig. 1. LHVC in the A-double configuration.

driving task of LHVCs is the online trajectory generationalgorithm. The main challenge for this trajectory gener-ator is to combine the information about the state ofthe truck, the road properties, and the behavior of thesurrounding traffic, and decide on a desired steering andvelocity profile.

We propose to use predictive control techniques as thetrajectory generator for the following reasons. First andforemost, such techniques can plan safe vehicle maneu-vers ahead to avoid need for conservatism. Secondly, bymeans of optimization, incorporating models for the truck,the road and surrounding traffic, the most preferentialpath can be selected from the set of safe trajectories.We highlight three types of control techniques that canfulfill the need for a predictive strategy with real-timeperformance. Nonlinear model predictive control (NMPC),randomized sampling-based planning strategies (LaValle(1998); Karaman and Frazzoli (2011)) and state-lattice

approaches combined with motion primitives (see e.g.,Pivtoraiko et al. (2009)).

This paper presents an NMPC technique for longitudinaland lateral motion control of the A-double combinationduring highway driving in presence of surrounding vehi-cles. The A-double combination LHVC, depicted in Fig. 1,is currently the longest LHVC that is allowed on roads inEurope in the very near future. First, an optimal controlproblem (OCP) is formulated that reflects the desiredmotion of the truck over a finite future horizon. Theprediction model considers the nonlinear articulated truckdynamics, the geometry of the road, and the behaviorof the surrounding traffic. The OCP is transcribed asa Nonlinear Program (NLP) using a multiple shootingprediction model integration technique. In order to achievereal-time computations when solving this complex NLPonline, the real-time iteration (RTI) scheme is employed(Diehl et al. (2005)). Since the real-time iteration scheme isa suboptimal solution technique, for comparison purposes,an interior-point NLP optimizer (IPOPT) is used to solvethe NLP as well. Closed-loop simulation results are pre-sented to show that the trajectory generator can handlecomplex situations while achieving real-time performance,thus enabling predictive advanced driver assistance func-tions to be implemented. The presented solution techniquehas been successfully implemented on a motion simulatorplatform operated by professional truck drivers at theSwedish National Road and Transport Research Institute(VTI) in Gothenburg, Sweden.

2. FORMULATION OF OPTIMAL CONTROLPROBLEM

When studying highway maneuvers of the A-double com-bination, additional factors need to be taken into consider-ation compared to regular passenger cars. One importantaspect is to prevent the truck or one of its trailers to rollover due to excessive acceleration levels. Other aspectsinclude the difficulty of keeping all trailers inside the laneboundaries and the limited agility due to the size andweight of the truck. In this section, we mathematicallyformulate comfortable driving behavior and incorporatethe limits on agility and safety effectively as the objectiveand the constraints of the OCP, respectively. Additionallywe present an approach to include surrounding traffic inthe OCP employing a prediction of their motion.

2.1 Prediction Model

In vehicle dynamics research and engineering a wide vari-ety of vehicle models are used. These range from non-slipkinematic car models, neglecting tire properties, to multi-body dynamical systems accurately modeling the effectsof chassis dynamics and dynamic nonlinear tire models.Choosing the most appropriate model for prediction pur-poses naturally highly depends on the expected situationsthat need to be predicted. From experience we know thatfor mild high-speed driving maneuvers the so-called singletrack model for the lateral dynamics, often referred to asa bicycle model, with a linear tire slip balances complexityand accuracy very well. The most important properties ofthe single-track model, schematically depicted in Fig. 2,are:

• The vehicle model is planar, effects such as roll andpitch are not modeled.

• The wheels of the vehicle are lumped in the center ofeach axle.

• Each unit is a rigid body mass without suspension,chassis, and cabin dynamics.

• The lateral tire stiffness is assumed to be linear in thelateral tire slip.

• Coupling between longitudinal and lateral dynamicsis neglected.

The lateral vehicle model and parameters used for thiswork, considering a fully loaded A-double combination, arereported in Nilsson and Tagesson (2013). For a more com-prehensive overview of different types of vehicle modelsand their properties see e.g., Rajamani (2012).

In addition to the lateral vehicle dynamics of the A-double combination, the prediction model also models thelongitudinal dynamics, the position on the road, and thebehavior of fellow road users. Vehicle dynamics are oftenexpressed in the non-inertial reference frame fixed in thecenter of gravity of a car. In order to avoid ambiguity inwhich frame certain measurements are defined, we denotea measurement a in point v with respect to reference frameo expressed in reference frame f by av f

o . The subscript isreserved to refer to a specific component of the variable aand to distinguish between actual measurements and e.g.,reference signals. Throughout the paper, the identifier ref(for reference) denotes a reference signal for the NMPCcontroller and the identifier des (for desired) denotesa reference signal for the low-level control algorithmssuch as the cruise controller. See Fig. 2 for a schematicrepresentation of the single-track model for the A-doublecombination and important points and reference frameson the truck. The four units the A-double combinationconsists of: the tractor, the first semi-trailer, the converterdolly and the second semi-trailer are named unit A, B, Cand D respectively.

θ2

X

Y

O δ

A

B

C

D

θ1

θ3

φ

{Dr}

{Ac}

s

{Φ}(af)

{Φ}(dr)

vac AcZ y

vac AcZ x

φR(s)

dr

af{Z}

Fig. 2. Schematic representation of the single-track modelof the A-double combination and an illustration fora selection of reference frames. The road curvature isexaggerated for clarity of the illustration.

The state vector ξ of the prediction model can be dividedinto four groups for illustrative purposes. States describing

the lateral dynamics of the truck

ξlat =(vac Ac

Z y φ φ θ1 θ1 θ2 θ2 θ3 θ3 δ)T

(1)

are governed, as previously introduced, by a single trackvehicle model. The longitudinal part of the dynamics

ξlong =(vac Ac

Z x aac AcZ x aac Ac

Z x,des

)T(2)

essentially models the closed-loop cruise-controlled systemas a first-order system on the desired longitudinal accelera-tion with a time constant of 0.25s. Another group of statesmodeling the distance of the front axle of the tractor (pointaf) and the rear-most axle of the second semi-trailer (pointdr) from the lane center is described as

ξroad =(

saf ΦΦ daf Φ

Φ sdr ΦΦ ddr Φ

Φ

)T. (3)

Finally, the states that model the position of the truckwith respect to surrounding traffic are denoted by

ξtraffic = (∆so,1 ∆so,2 ∆so,3)T. (4)

In this work, the traffic model assumes that all vehiclesremain driving at the measured velocity along the lanecenter.

The control inputs to this system are the steering anglerate for the front wheels of the tractor, and the desired lon-gitudinal jerk (i.e., the change in acceleration) requestedfrom the cruise-controller.

2.2 Spatial Reformulation of Prediction Model

In order to model the position of the vehicle in thecurvilinear coordinate system of the road, we requireknowledge on the relative heading and the curvature ofthe road in our prediction horizon. It is common to choosea certain time span of interest for the prediction horizon.In case of our trajectory generator, we would then needto know the road heading and curvature as a functionof the vehicle states. As suggested in previous work byGao et al. (2012); Frasch et al. (2013), a more naturalchoice is to take the traveled distance of the vehicle alongthe road geometry as a pseudo-time variable in a spatialhorizon. The main advantage is that the road geometry canthen be algebraically defined in the prediction horizon. Theprice to pay is a nonlinear transformation of the predictionmodel equations and the loss of the ability to explicitlydefine the position of fellow road users in the prediction

horizon. Suppose that saf ΦΦ represents the position of the

front axle of the tractor along the road geometry, then thetime transformation will have the form dξ/ds = dξ/dt · dt/ds.The transformation consists of dividing the system of first-order differential equations by

d saf ΦΦ

dt=

vaf AfZ x cos(φ− φR)− vaf Af

Z y sin(φ− φR)

1− κR daf ΦΦ

(5)

with κR = dφR/ds as the local curvature of the road at saf ΦΦ .

An explicit overview of the prediction model equations anda more extensive treatment of its spatial reformulation isreported in Van Duijkeren (2014).

It is important to note that the heading and curvature ofthe road in the prediction horizon remains fixed only forone point of the vehicle. The heading and the curvatureof the road at e.g., the position sdr Φ

Φ of the rear traileris not algebraically defined. This matter is dealt with

by a crucial assumption: the distance between any twopoints of the truck along the road geometry is assumedto be constant. Considering the highway driving task, thisassumption is not problematic but it makes our imple-mentation incompatible with high curvature conditions,such as city driving. With this assumption, it also opensthe possibility to describe the position ddr Φ

Φ in terms ofthe position of the tractor and the articulation anglesof the truck. However, in this work, the position ddr Φ

Φ iskept as a state so that lane departure constraints can beformulated as state box constraints. With the predictionmodel introduced, we describe the remaining elements ofthe OCP formulation next.

2.3 Cost Function & Constraints

For our trajectory generator we observe that we requirethree distinct (possibly conflicting) objectives. The firstis a tracking objective, the second part promotes smoothand comfortable trajectories, and the last part penalizesdriving close to other vehicles. On the one hand, aneconomic-MPC (EMPC) can be formulated to optimizea generic cost function. Although this may be attractiveconceptually, finding efficient solution strategies to theresulting NLPs is nontrivial. On the other hand, efficientalgorithms are available for constrained least-squares opti-mization problems, such as the generalized Gauss-Newtonalgorithm. The latter type of solution strategy is especiallywell-suited for tracking-MPC applications because of itsleast-squares nature. Since real-time performance is ofcrucial importance, we formulate our economic objectiveas a least-squares optimization problem.

Tracking Objective In (12) the tracking objective isrepresented by three terms in the Lagrange-term of thecost function:

Kd1( daf Φ

Φ − daf ΦΦ ref)

2 +Kd4( ddr Φ

Φ − ddr ΦΦ ref)

2+

Kvx( vac AcZ x − vac Ac

Z x,ref)2

(6)

Firstly, the lateral distance offset from the lane centerfor points af and dr are penalized. Secondly, to achieveefficient driving behavior (i.e., fast transportation) it isdesired to drive close to the legal speed limit. A weightedquadratic penalty is placed on the tracking error of bothstates.

Driver Comfort Objective Previous research, such asNilsson et al. (2014), supports the generally acceptedobservation that the human perception of smooth andcomfortable driving is closely related to the jerk levelsthat arise over a driven trajectory. Hence an importantpart of the cost function promotes driver comfort byrewarding driving at low jerk levels. Additionally, thelongitudinal acceleration levels and rapid steering arepenalized resulting in the Lagrange term:

Kjx( jac AcZ x,des)

2 +Kjy ( jac AcZ y )2 +Kax( aac Ac

Z x,des)2 +Kδ δ

2 (7)

The desired longitudinal jerk is a control input to the ve-hicle prediction model. The lateral jerk however is neithera state nor an input, thus we must find a closed-formexpression as a function of the states and inputs. Thefollowing expression can be derived for this relationship

∆so,1

∆so,2 ∆so,3

Fig. 3. Distance keeping scenario in a lane change to theright and labeling of fellow road users.

jac AcZ y = 2 φac Z

Z · vac AcZ x + φac Z

Z · vac AcZ x +

−(

φac ZZ

)2

· vac AcZ y + vac Ac

Z y

(8)

We note that the only unknown term is vac AcZ y . Fortunately,

in highway driving scenarios that same term is not thegreatest contributor to the lateral jerk. Since we solelyneed to capture the trend for the jerk level in order topenalize it, we choose to approximate the jerk with theremaining known terms.

Distance Keeping Objective Keeping distance to othertraffic is a tasks that involves heuristics. For instance, oneis usually insensitive to the behavior of a car far away.In a normal lane keeping scenario, only a vehicle in thenear vicinity and in the same lane is of our interest. Infact by assuming that the trailing vehicle in the samelane adjusts its speed to the truck, only the vehicle infront of us needs to be taken into account. The situationbecomes slightly more complex for a lane change, since twoextra vehicles must be considered: the nearest precedingvehicle and the nearest trailing vehicle in the target lane.Fig. 3 illustrates this concept for a lane change scenario.The main part of the lane-changing logic behavior iscaptured in a preprocessing stage to the OCP solver,and sensitivity to the ∆so,1, ∆so,2, ∆so,3 parameters isdetermined beforehand according to the scenario. In casea lane change is not desired or infeasible, ∆so,2 and ∆so,3are not of interest to the trajectory generator and thusremoved from the optimization.

The logic in the sensitivity of the distance to other vehiclesis captured in the optimization problem. We choose aquadratic penalty in the Lagrange term of the OCP:

3∑k=1

K∆so,k

(fdk(∆so,k, vac Ac

Z x ))2

(9)

where the function fdk, assuming saf ΦΦ x ≈ vac Ac

Z x , approxi-mates

fdk =

0, if ∆so,k > τo,k vac Ac

Z x

τo,k vac AcZ x −∆so,kτo,k vac Ac

Z x

, otherwise(10)

with τo,k denoting the temporal gap at which the respec-tive vehicle k is taken under consideration. Two alternativemethods have been explored to implement this logic be-havior. One is to introduce a slack variable that becomesactive when crossing a distance threshold, similarly to thetechnique described in Borrelli et al. (2004). The secondmethod is to use a sigmoid function to closely resemblebinary behavior. We experienced no obvious differences inthe resulting open-loop optimal trajectories and the closedloop behavior. In our implementation, the latter techniqueusing a sigmoid function, resulted in significantly shortersolution times and was therefore preferred. The employed

sigmoid function is

fdk(∆so,k, vac AcZ x ) =

τo,k vac AcZ x −∆so,k

τo,k vac AcZ x

(1 + e−co,k(τo,k−∆so,k/ vac Ac

Z x ))

(11)with co,k a tuning variable for the slope of the sigmoidfunction in the transition region.

Complete Optimal Control Problem With the three dif-ferent elements in the objective function introduced, wedefine our OCP.

minξ(·),u(·)

sf∫s=0

(6) + (7) + (9)dσ (12)

subject to

dξ

ds= f(s, ξ, u) (13)

ay ≤ aaf AfZ y (s) ≤ ay (14)

ay ≤ adr DrZ y (s) ≤ ay (15)

d ≤ daf ΦΦ (s) ≤ d (16)

d ≤ ddr ΦΦ (s) ≤ d (17)

ax,des ≤ aac AcZ x,des(s) ≤ ax,des (18)

δ ≤ δ(s) ≤ δ (19)

δ ≤ δ(s) ≤ δ (20)

∆s0,k ≤ ∆s0,k k = 1, . . . , 3 (21)

∀s ∈ [0, sf ]

The equations of motion (13) depend on the position of

the front axle of the tractor safΦ along the road geometry,

the state, and the control input. Constraints (14) and (15)limit the lateral accelerations of two points of the truckto prevent the truck to tip over. Constraints (16) and(17) enforce two points of the truck to stay inside thelane boundaries. Constraint (18) informs the trajectorygenerator of the physical maximum longitudinal acceler-ation of the truck that may depend on the position onthe road (slope of road). Constraints (19) and (20) reflectthe maximum road wheel angle and its time derivative,respectively, reflecting physical actuation constraints. Fi-nally, (21) prohibits a trajectory that collides with that ofone of the surrounding vehicles.

3. IMPLEMENTED SOLUTION TECHNIQUES

The OCP problem introduced in Section 2 is transcribedinto an nonlinear program (NLP). The spatial prediction

horizon in saf ΦΦ is discretized using 50 intervals with a step

size of ∆s of 2 meters. The system dynamics constraintsare enforced with a multiple-shooting integration schemeusing a fourth order explicit Runge-Kutta update rule tak-ing 5 intermediate integration steps in each discretization

interval of the OCP. The control inputs δ and jaf AfZ x,des are

held constant over each interval. The objective function isapproximated by the sum of the Lagrange terms at eachshooting node of the transcribed OCP. Likewise, constraintsatisfaction is enforced at each shooting node. In a lanechange the reference transitions to the other lane witha minimum jerk path for a point-mass. The referencefor vx,meas = vac Ac

Z x is pre-processed to prevent a high

velocity set-point in curves and to adapt the velocity toleading vehicles close-by. To attain real-time performance,the main challenge is to find a trade-off between a longprediction horizon and a fast control rate. In this work,the horizon length and the control frequency are 100mand 2m−1 respectively. The physical control frequencyis synchronized with the NMPC control intervals of thetranscribed OCP and is triggered every 2 meters traveledalong the lane center. Driving at 20m/s this agrees to acontrol rate of 10Hz.

Two different techniques are implemented to solve theNLP. Both solution schemes share the same base of heuris-tics and are entirely written in the C/C++ programminglanguage.

(1) The so-called real-time iteration (RTI) scheme isimplemented using the open-source ACADO toolkit(Houska et al. (2011)). Using the code generationfunctionality of ACADO, tailored C-code is gener-ated for the SQP-type solution scheme. Both thestructure of the RTI scheme, explained in e.g., Diehlet al. (2002, 2005) and the auto-generated code forthe NLP solver enable very fast execution times forthe NMPC solution steps. Previous results alreadyshowed solution time scales for NMPC that are wellwithin the region that allows their application to fastmechatronic systems, see e.g., Vukov et al. (2012);Frasch et al. (2013); Verschueren et al. (2014). Theactive-set QP solver qpOASES is used to solve eachintermediate condensed QP (Ferreau et al. (2014)).

(2) Solving the NLP to a certain prescribed accuracy withthe state-of-art interior point NLP solver IPOPT, seeWachter and Biegler (2006). The NLP is formulatedin the symbolic framework for algorithmic differen-tiation (AD) and numerical optimization CasADi,see e.g., Andersson (2013). The toolkit implementsefficient techniques for AD to supply IPOPT withthe gradient of the objective, the Jacobian of theconstraints and the Hessian of the Lagrangian.

We note that the NLP was identical in both cases, and thatthe first method using the RTI scheme was fast enoughfor real-time application. The second implementation withIPOPT is used to compare the closed-loop performanceof a suboptimal and an optimal solution technique. Thenext section presents the numerical results of closed-loopsimulation experiments on a high-fidelity model of the A-double LHVC in a realistic highway driving scenario.

4. SIMULATION EXPERIMENTS

In order to evaluate and illustrate the results in this work,we simulate the controlled A-double combination situatedin a highway scenario. A high-fidelity two-track modellibrary developed by Volvo Trucks is used simulate the“real” truck plant model dynamics. The model includesdetailed sub-models of the vehicle chassis, cab suspensions,steering system, power train, and brakes. The frame tor-sion flexibility of the tractor and semi-trailers is consid-ered by using multiple frame bodies connected throughsprings. The Magic Formula tire model, Pacejka (2006),with combined slip, dynamic relaxation, and rolling re-sistance, is used for all tires in the vehicle combination.In this framework, control allocation has been used for

∆so,1 = 45mso,1 = 19m/s

Trucks = 23m/s

∆so,3 = 60mso,3 = 15m/s

∆so,2 = −25mso,2 = 23m/s

(a) Start of scenario, merge into highway from left and request lanechange. Obstacles are numbered.

Trucks = 20m/s

∆so,1 = 36mso,1 = 19m/s

∆so,3 = 35mso,3 = 15m/s

∆so,2 = −40mso,2 = 15m/s

(b) Lane change opportunity detected and initiated.

Trucks = 15m/s

∆so,1 = 30mso,1 = 15m/s

(c) Lane change is completed. Obstacles are renumbered.

Fig. 4. Main phases of the lane change scenario.

coordinating propulsion, braking and steering as describedin Laine (2007). The control allocation weighting for, e.g.braking in-between axles, has been adapted to commercialheavy vehicles (see Tagesson et al. (2009)). This simulationmodel is much more accurate than the prediction modelused in the NMPC formulation and serves as a benchmarkfor the performance assessment of the NMPC applied to arealistic system.

The simulation experiments are executed in Simulink.The control algorithms, written in C/C++, are interfacedthrough a Matlab s-function. We consider full-state feed-back control, i.e., the required knowledge of the vehicle, theroad and surrounding traffic is assumed to be available.In the simulations the truck drives autonomously and thepresence of a driver in the loop is neglected.

The considered scenario is based on actual real-life con-cerns that arise with the introduction of the longer truckcombinations. The scenario begins with the truck being inthe left lane of a highway in busy traffic. The controlledvehicle can arrive there e.g., when two highways merge.The truck needs to transition to a lane to the right in orderto take the next exit, but the size of the truck makes itdifficult to keep an overview of the situation. We considerthat a vehicle in the lane to the right is blocking the waydriving with the same speed as the truck. Another obstaclein front of that vehicle drives significantly slower. Out ofcourtesy, the vehicle to the right slows down early to makespace for the truck. In the truck its own lane, a vehicle infront drives slightly slower thus the truck speed needs tobe adjusted while looking for a lane-change opportunity.In order to move to the right lane, the controller mustrecognize that the vehicle in the right lane keeps a gap. Assoon as the gap complies with the minimum requirements,the truck needs to slow down to match the velocity ofthe vehicles in the adjacent lane. Finally, it can transitionto the new lane and complete the merging maneuver. In

Fig. 4 three main phases of this scenario are depicted withdistance and velocity observations.

Fig. 5 presents several relevant signals from the closed-loop simulation results in the introduced scenario. Theemployed controller parameters are outlined in Section A.We note that the legend entries d1, d4 and vx refer to

daf ΦΦ , ddr Φ

Φ and vac AcZ x , respectively. The reference signals

(ref) to the NMPC controller are shown with virtualmeasurements (meas) originating from the high-fidelitymodel of the A-double combination. From Fig. 5b we canobserve that before the lane change the truck slows downslightly to adapt to the velocity of the leading car. InFig. 5d we can observe that the main trade-off is betweentracking and comfortable driving. At around 21 secondsa lane change opportunity is detected and immediatelythe truck slows down to find the optimal distance to theobstacles in the adjacent lane. The distance to surroundingvehicles is the main contributor to the cost function ofthe NLP. Fig. 5a illustrates the lane transition that a fewseconds later smoothly brings the truck to the right lane.When the lane change is completed, the truck slows downto track the velocity reference and to increase the gap withthe new leading vehicle.

Recall that the RTI scheme, implemented by ACADO, is asuboptimal solution method. Since only one Newton stepis taken for an SQP in each control interval, we do not nec-essarily achieve a local minimum of the NLP at all times.Additionally, the Gauss-Newton Hessian approximationemployed for intermediate QPs is in general not ideal foreconomic type cost functions such as our distance keepingobjective. In order to illustrate the effect of such a sub-optimal strategy on the closed-loop performance, we showa comparison with the total objective value of NLP duringthe same scenario in Fig. 6a. In addition, the solution timesper control iteration are provided in Fig. 6b. From the evo-lution of the total cost we can clearly observe a very similartrend between the RTI scheme and IPOPT. The differencebetween the two varies on the same order of magnitude asthe objective value. The state trajectories of the system,not shown in this paper, are nearly identical as well. At thesame time we can see that the RTI scheme shows solutiontimes that are by a factor of 90 faster than IPOPT. Thisillustrates well the main philosophy of suboptimal methodsfor NMPC: it is not always necessary to have the optimalcontrol input available. Instead, it is important to achievereal-time performance and to converge to a local optimumwith a high rate that allows intermediate SQP solutionsto be applied. Notice from Fig. 6b that our bound on thecomputational time is not constant, since it depends onthe spatial control update frequency (i.e., the speed of thetruck).

The scenario described above considered a straight road.We chose to present the results of this scenario for easeof interpretation. The implemented control algorithmshowever are entirely compatible with curved roads. Thesolution times that are presented in this paper naturallydepend on many factors. The experiments were executedon a notebook with a mobile Intel i5-4300M CPU and16GB of RAM memory. In addition to the hardwarespecifications, the reader may take the following commentsunder consideration:

10 20 30 40 50 60

−4

−2

0

Time [s]

[m]

d1,refd1,meas

d4,refd4,meas

(a) Distance to road geometry with the pre-optimized minimum jerkreference. The lane boundaries are indicated in red.

10 20 30 40 50 60

15

20

Time [s]

[m/s]

vx,refvx,meas

(b) Longitudinal velocity of the tractor and the reference.

10 20 30 40 50 60−100

0

100

200

Time [s]

[m]

∆ so,1∆ so,2∆ so,3

(c) Longitudinal distance to fellow road users with the truck lengthrepresented by the gray region. Lane change occurs between dottedlines.

10 20 30 40 50 6010−9

10−5

10−1

103

Time [s]

[-]

TrackingComfortDistance

(d) Cost decomposition, normalized with total cost.

Fig. 5. Closed-loop simulation results for ACADO NMPCwith high-fidelity plant.

• In the auto-generated RTI scheme from ACADO,the intermediate QPs are condensed and solved withan active-set QP solver. With short horizons this isoften the fastest strategy. However, with an increasinghorizon, sparsity exploiting QP solvers may becomea better alternative. The reader is referred to Fraschet al. (2014) for more details on this issue.• CasADi provides the option to perform code-generation

for the algorithmic differentiation employed for thesensitivity information used in IPOPT. This can nat-urally speed up computations significantly. This op-tion is not taken into consideration for the presentedresults.• In both solution techniques we can parallelize certain

elements of the code, e.g., for the multiple-shootingintegration step. The computations for results pre-sented in this paper are all serially executed.

10 20 30 40 50 60

10−4

10−2

100

102

104

Time [s]

[-]

IPOPTACADONorm. diff.

(a) Evolution of the total objective value of both optimizationalgorithms and the difference normalized with the cost from IPOPT.

10 20 30 40 50 6010−3

10−2

10−1

100

101

Time [s]

[s]

IPOPTACADO

(b) Time consumed by the optimization, with the velocity depen-dent bound for real-time performance indicated in red.

Fig. 6. Comparison of results for simulations with IPOPTand ACADO.

5. CONCLUSIONS

In this paper we have shown that nonlinear MPC (NMPC)is an appealing method for the trajectory generation ofdriver assistance systems in long heavy vehicle combina-tions (LHVCs). All necessary information, such as thegeometry of the road and models for the surrounding

traffic, can be represented in a natural framework usingoptimization-based control. From simulation experimentresults we observed that real-time performance can beattained for a nonlinear optimization problem. The posedproblem was to minimize a cost function that represents atrade-off between driver comfort, reference tracking per-formance, and distance keeping from surrounding traf-fic. Simultaneously, constraints on the lateral accelera-tion, states and controls were enforced. The real-timeperformance was mainly attributed to the real-time iter-ation (RTI) scheme and the auto-generated code by theACADO toolkit. The same problem was also implementedand solved using the interior point NLP-solver IPOPT.We created an interface to IPOPT using the symbolicoptimization framework CasADi, employing algorithmicdifferentiation for all required sensitivity information. Weshowed that similar closed-loop performance is achievedfor the optimal and sub-optimal solution strategies.

The optimal control problem (OCP) in this paper is formu-lated with a nonlinear least-squares objective. This formis necessary in order to use the generalized Gauss-Newtonalgorithm in the SQP. Other types of cost functions andsolution strategies may be better suited for economic ob-jective functions. Recent work by e.g., Quirynen et al.(2014) present intermediate results to use an exact Hessianin the RTI scheme. These developments are of interestto online nonlinear trajectory generation techniques forLHVCs. The presented control algorithms show that im-plementation on physical systems is within reach. Futurework is aimed at online estimation of the vehicle dynamics,and implementing disturbance estimation to reduce track-ing errors due to model mismatch or external disturbances.Additionally, modeling the behavior of surrounding trafficis an open research question that needs to be addressedin more detail. Finally, we ensured a stable closed-loopresponse by choosing a long enough prediction horizon. Weenvision guaranteeing recursive feasibility and stability bysoftening constraints and including a terminal set in theoptimization.

The current real-time tractability of NMPC using theRTI iteration scheme enabled us to execute experimentswith the presented algorithms in a motion simulator. Thisserved as a basis for a study on driver comfort aimedat introducing truck drivers to future driver assistancesystems that has been performed in cooperation betweenVolvo Group Truck Technology (VGTT) and the SwedishNational Road and Transport Research Institute (VTI) inGothenburg, Sweden.

ACKNOWLEDGEMENTS

Employees of Volvo Group Truck Technology (VGTT) andthe Swedish National Road and Transport Research Insti-tute (VTI) are gratefully acknowledged for their assistancethat led to the results in this paper.

REFERENCES

ACADO toolkit (2014). ACADO toolkit website.acadotoolkit.org.

Andersson, J. (2013). A General-Purpose Software Frame-work for Dynamic Optimization. PhD thesis, ArenbergDoctoral School, KU Leuven.

Borrelli, F., Keviczky, T., and Balas, G.J. (2004).Collision-Free UAV Formation Flight Using Decentral-ized Optimization and Invariant Sets. In proceedingsof the 43rd IEEE Conference on Decision and Control,1099–1104. Nassau, Bahamas.

Diehl, M., Bock, H.G., and Schloder, J.P. (2005). A Real-Time Iteration Scheme for Nonlinear Optimization inOptimal Feedback Control. SIAM J. Control Optimiza-tion, 43(5), 1714–1736.

Diehl, M., Findeisen, R., Schwarzkopf, S., Uslu, I.,Allgower, F., Bock, H.G., Gilles, E.D., and Schloder,J.P. (2002). An Efficient Algorithm for Optimizationin Nonlinear Model Predictive Control of Large-ScaleSystems. Automatisierungstechnik, 50(12), 557–567.

Ferreau, H.J., Kirches, C., Potschka, A., Bock, H.G., andDiehl, M. (2014). qpOASES: A Parametric Active-SetAlgorithm for Quadratic Programming. MathematicalProgramming Computation, 6(4), 327–363.

Frasch, J.V., Gray, A.J., Zanon, M., Ferreau, H.J., Sager,S., Borrelli, F., and Diehl, M. (2013). An Auto-Generated Nonlinear MPC Algorithm for Real-TimeObstacle Avoidance of Ground Vehicles. In proceedingsof the European Control Conference (ECC13), 4136–4141. Zurich, Switzerland.

Frasch, J.V., Vukov, M., Ferreau, H.J., and Diehl, M.(2014). A New Quadratic Programming Strategy forEfficient Sparsity Exploitation in SQP-Based NonlinearMPC and MHE. In proceedings of the 18th IFAC WorldCongress, 2012, 2945–2950. Milano, Italy.

Gao, Y., Gray, A.J., Frasch, J.V., Lin, T., Tseng, H.E.,Hedrick, J.K., and Borrelli, F. (2012). Spatial PredictiveControl for Agile Semi-Autonomous Ground Vehicles.In proceedings of the International Symposium on Ad-vanced Vehicle Control (AVEC12). Seoul, South Korea.

Houska, B., Ferreau, H.J., and Diehl, M. (2011). An Auto-Generated Real-Time Iteration Algorithm for NonlinearMPC in the Microsecond Range. Automatica, 47(10),2279–2285.

Karaman, S. and Frazzoli, E. (2011). Sampling-Based Al-gorithms for Optimal Motion Planning. Int. J. RoboticsResearch, 30(7), 846–894.

Kindt, M., Burgess, A., Quispel, M., Van Der Meulen, S.,and Bus, M. (2011). Monitoring Modal Shift: Longerand Heavier Vehicles, The Follow-Up Measurement.Technical report, Dutch Ministry of Infrastructure andthe Environment.

Laine, L. (2007). Reconfigurable Motion Control Sys-tems for Over-Actuated Road Vehicles. Ph.D. thesis,Chalmers University of Technology.

LaValle, S.M. (1998). Rapidly-Exploring Random Trees:A New Tool for Path Planning. Technical report, De-partment of Computer Science, Iowa State University.

Nilsson, P., Laine, L., and Jacobson, B. (2014). Perfor-mance Characteristics for Automated Driving of LongHeavy Vehicle Combinations Evaluated in Motion Sim-ulator. In proceedings of the IEEE Intellicles VehiclesSymposium (IV2014), 362–369. Dearborn, USA.

Nilsson, P. and Tagesson, K. (2013). Single-Track Models of an A-Double Heavy VehicleCombination. Technical report, ChalmersUniversity of Technology, Goteborg, Sweden. URLhttp://publications.lib.chalmers.se/records/fulltext/192958/local 192958.pdf.

Pacejka, H. (2006). Tyre and Vehicle Dynamics.Butterworth-Heinemann.

Pivtoraiko, M., Knepper, R.A., and Kelly, A. (2009). Dif-ferentially Constrained Mobile Robot Motion Planningin State Lattices. J. Field Robotics, 26(3), 308—-333.

Quirynen, R., Houska, B., Vallerio, M., Telen, D., Logist,F., Van Impe, J., and Diehl, M. (2014). SymmetricAlgorithmic Differentiation Based Exact Hessian SQPMethod and Software for Economic MPC. In proceedingsof the 53rd IEEE Conference on Decision and Control.Los Angeles, USA.

Rajamani, R. (2012). Vehice Dynamics and Control.Springer Science, 2nd edition.

Tagesson, K., Sundstrom, P., Laine, L., and Dela, N.(2009). Real-time Performance of Control Allocation forActuator Coordination in Heavy Vehicles. In proceedingsof the IEEE Intellicles Vehicles Symposium (IV2009),685–690.

Van Duijkeren, N. (2014). Real-Time Receding HorizonTrajectory Generation for Long Heavy Vehicle Combi-nations on Highways. Master’s thesis, Delft Universityof Technology. URL http://resolver.tudelft.nl/uuid:5434052d-b939-4544-92df-ab584b5367d7.

Verschueren, R., De Bruyne, S., Zanon, M., Frasch, J.V.,and Diehl, M. (2014). Towards Time-Optimal RaceCar Driving using Nonlinear MPC in Real-Time. Inproceedings of the 53rd IEEE Conference on Decisionand Control, 2505–2510.

Vukov, M., Van Loock, W., Houska, B., Ferreau, H.J., Sw-evers, J., and Diehl, M. (2012). Experimental Validationof Nonlinear MPC on an Overhead Crane Using Auto-matic Code Generation. In proceedings of the AmericanControl Conference (ACC2012), 6264–6269. Montreal,Canada.

Wachter, A. and Biegler, L.T. (2006). On the Implementa-tion of Primal-Dual Interior Point Filter Line Search Al-gorithm for Large-Scale Nonlinear Programming. Math-ematical Programming, 106(1), 25–57.

Appendix A. CONTROLLER PARAMETERS

The controller tuning parameters employed in the experi-ments presented in this paper.

Symbol Value Symbol ValueKd1

800 / 2 Kd4400 / 2

Kvx 50 / 25 Kjx 50 / 2Kjy 150 / 3 Kax 20 / 1.75Kδ 1 / 0.05 K∆so,k 5000 / 1.2τo,1 1.5s τo,2 2.5sτo,3 1.5s