Model Predictive Control Based Trajectory Generation forAutonomous Vehicles – An Architectural Approach

Marcus Nolte1, Marcel Rose1, Torben Stolte1 and Markus Maurer1

Abstract— Research in the field of automated driv-ing has created promising results in the last years.Some research groups have shown perception systemswhich are able to capture even complicated urbanscenarios in great detail. Yet, what is often missing aregeneral-purpose path- or trajectory planners whichare not designed for a specific purpose. In this paperwe look at path- and trajectory planning from an ar-chitectural point of view and show how model predic-tive frameworks can contribute to generalized path-and trajectory generation approaches for generatingsafe trajectories even in cases of system failures.

c© 2017 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any currentor future media, including reprinting/republishing this material for advertising or promotional purposes, creating newcollective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in otherworks.

I. IntroductionIn recent years, the field of automated driving has

gained much attention not only from research groups,but also from industry and even the public. In publicreception, the timespan until ”autonomous” vehicles willbe ready for market seems to be a matter of years ratherthan decades, also due to the high expectation raised bymarketing strategies in industry.

The field of environment perception has recently seenimpressive progress, also boosted by the developmentsin the field of machine learning. This has increased thelevel of detail of the representation of the vehicle’s envi-ronment as a basis for decision making and, eventually,for trajectory generation. However, in most cases, thedriver is still considered to be the ultimate fallback incase of system failures, so that current systems have tobe considered partially automated (level 2) according tothe definition of SAE [1].

When moving towards fully automated (level 5) or evenautonomous vehicles, safety requirements emerge as thesystem must be able to handle system failures withoutexternal intervention. Thus, a detailed representation ofthe vehicle’s environment alone is not sufficient. Fur-thermore the external representation must be combinedwith a detailed representation of the vehicle’s internalstates and its internal capabilities which are requiredto fulfill the vehicle’s mission, as already demanded byDickmanns [2], Maurer [3], Pellkofer [4], Siedersberger[5], and Reschka [6]. By fusing the internal represen-tation, e.g. about available actuators, with the externalcontext extracted from sensor data, the vehicle becomesself-aware and is enabled to take safe driving decisionsat any time, even in case of system failures.

1All authors are with the Institute of Control Engineering, Tech-nische Universitat Braunschweig, 38106 Braunschweig, Germany{nolte,stolte,maurer}@ifr.ing.tu-bs.de

Extracting the required information about the internalstate and the external context is only one part of therealization of safe driving strategies. Decision algorithmsand eventually trajectory generation and control algo-rithms must be designed in such a way, that availableinformation can be utilized efficiently at each level ofabstraction. As an example, a degradation of the steeringsystem due to a failure has immediate impact on vehiclestability when turning so that control algorithms must beimmediately reparameterized in order to keep the vehiclestable. Additionally, a trajectory generation module –which is running at a lower frequency – must also plantrajectories which conform to the resulting actuator ca-pabilities after degradation e.g. by generating trajectorieswith reduced curvature. Eventually, decision algorithmsmust be aware of possible degradation effects in order togenerate reachable target poses for the vehicle.

In this paper, we propose model predictive control(MPC) as a framework which can be utilized for reflect-ing performance degradation and mitigating the emerg-ing degradation effects at the lower levels of a systemarchitecture. Corresponding to the above example, wewill map decisions about constraints and optimizationparameters to a functional system architecture and de-scribe an actually implemented MPC framework for ourfull-by-wire research vehicles MAX and MOBILE.

This paper is structured as follows: Section II givesa short overview over related work regarding functionalsystem architectures, the demand for self-awareness, andrecent approaches to MPC for automated road vehicles.After this, Section III describes the utilized functionalsystem architecture and how MPC can be mapped to it incombination with information about the internal vehiclestate. Section IV presents a multi-stage MPC frameworkfor autonomous vehicles, considering active rear steering.

II. Related WorkFor the development of complex vehicle systems, the

ISO 26262 standard requires architectural designs thatfeature modularity, adequate level of granularity, andsimplicity [7, Part 3, 7.4.3.7]. Functional system archi-tectures provide the possibility to fulfill these designrequirements demanded by the ISO26262. In the fieldof automated driving, architectures which relate to therobotics sense, plan, act scheme are utilized.

One of the more recent approaches to a functionalsystem architecture for level 3 to level 5 automatedvehicles is presented by Matthaei and Maurer [8]. Their

arX

iv:1

708.

0251

8v2

[cs

.SY

] 1

0 A

ug 2

017

Global Localization& Mapping

Environment & SelfPerception

MissionAccomplishment

Road LevelLocalization & Mapping

Road LevelEnvironment Modeling

Navigation

Lane LevelLocalization & Mapping

Context / SceneModeling

Guidance

Feature LevelLocalization & Mapping

Feature Extraction &Model-Based Filtering

Stabilization

Environ.Sensors

VehicleSensors

Actuators

LocalizationSen

sors

Communication

(HMI,V2X)

focus ofexample

Fig. 1. Functional system architecture based on [8] and [19]: This paper focuses on Guidance and Stabilization in Mission Accomplishment.

contribution is based on earlier works of Rasmussen [9],Donges [10], Dickmanns et al. [11], and Maurer [3]. Amore detailed presentation of this contribution [8] ispresented in Section III. A deeper review of existingarchitectural approaches for automated road vehicles hasalso been published by Matthaei and Maurer [8]. Taset al. [12] compare architectures of selected automatedvehicles in a unified graphical representation and alsostate the necessity for self- and performance monitoring.

With regard to the internal representation of capabili-ties of automated vehicles, first demands for representingthe ability to accelerate, brake and turn have been statedby Dickmanns [2]. Based on Passino and Antsaklis [13],Maurer provides a first concept for a runtime represen-tation of the performance of control algorithms in auto-mated vehicles [3]. Further developing and implementingthe idea, Pellkofer [?] and Siedersberger [5] introducethe concept of skill networks. A further differentiationbetween skills and abilities is made by Bergmiller [14] andReschka [15], the latter introducing the concept of abilityand skill graphs for runtime monitoring and support ofdevelopment processes.

With regard to MPC, several contributions have re-cently been made for trajectory generation and controlin the field of automated driving. An approach for lateralvehicle guidance, using MPC for trajectory planning andcontrol is presented and evaluated in simulation by Gotteet al. [16]. A similar approach utilizing more advancedcollision avoidance constraints is described by Gutjahrand Werling [17]. Yi et al. [18] propose an approachto MPC-based trajectory planning for critical drivingmaneuvers. They introduce a quadratic MPC problemfor considering friction limits in evasion maneuvers.

III. MPC-Based Trajectory Generation in aFunctional System Architecture

Model predictive control aims at generating optimalcontrol inputs for a given plant. Thus, a weighted costfunction as well as optimization constraints are required

in order to fully describe the related optimization prob-lem. In case of trajectory generation for automatedvehicles, optimization constraints are e.g. based on thevehicle’s environment, as the generated trajectory mustalways be collision free. Moreover, the internal state ofthe vehicle can introduce additional constraints, e.g. byincorporating actuator limitations such as the maximumangular steering change rate. Weights in the cost functionemerge from additional mission parameters (e.g. comfort,actuator wear or energy consumption).

In the following subsections, we will describe how theseconstraints and weights are generated at different levelsof the functional system architecture. For this, we will re-late to the architectural approach proposed by Matthaeiand Maurer [8]. The resulting architecture is displayedin Fig. 1. The authors consider the sense, plan, actscheme and derive a three-layer architecture operatingat road level (navigation), lane level (guidance), and ata quasi-continuous level (stabilization). The three levelsare divided into three columns each: global localization &mapping, environment and self-perception, and missionaccomplishment.

Fig. 2 depicts a closer view at the guidance andstabilization level for mission accomplishment, which isthe focus in the following. However, a short descriptionfor the navigation level is given, as the planned routeprovides the basis for selecting a target pose at lowerarchitecture levels.

A. NavigationAt the navigation level (cf. Fig. 1), the most recent

available information about the road network is providedfrom the environment perception column supported bya-priori external data (maps) according to Matthaei andMaurer [8]. Applied algorithms for route planning aretypical graph-based approaches such as Dijkstra or A*derivatives. Mission goals as well as possible informationfrom self-representation modules can be represented asedge weights or costs in the respective graph. Thus, only

AbilityMonitoring

SituationAssessmentExperts

Situation Assessment

ExecutionMonitoring

Constraint& WeightGeneration

Target PoseSelection

ReferencePath

Generation

Behavior Generation

Guidance

TrajectoryCalculation

CoordinateTransform

Trajectory Generation

ExecutionMonitoring

Low-Level Control

Stabilization

Actuators

Target Pose, Reference Path,Weights & Constraints

FeasibilityInformation

Reference Values

Planned Route Status Information

Scene

StateVariables

Situation Aspects

Trajectory

Current Abilities

Fig. 2. Detailed view at Guidance and Stabilization

routes will be planned, which conform with mission goalsand the vehicle’s abilities (e.g. sloped roads could beavoided if the braking system can only provide reducedfunctionality). Typical cycle times at this level are severalten seconds or even minutes. Finally, the planned routewill then be input for the algorithms at guidance level.

B. Guidance

At the guidance level, all tactical decisions whichconcern the immediate surroundings of the vehicle aremade. The goal at the guidance level is twofold: Onthe one hand, a target pose as input for trajectorygeneration shall be generated. This target pose mustbe reachable and it must conform to all (i.e. legal)given constraints. As an example, in ordinary drivingscenarios, the target pose will be located either in theego-lane or in a neighboring lane in case a lane changeis desired. On the other hand, a unique reference pathwhich leads to that target pose must be calculatedto facilitate the application of MPC. In a structuredenvironment, this typically is the lane center line. Bythis, a trajectory can be generated along that referencepath, making the trajectory a function of lateral distancefrom the reference. The reference path does not need tobe smooth, as smoothness of the resulting trajectory willbe ensured by the trajectory generation modules at thestabilization level. The reference generation (cf. Fig. 2)can be seen as a coarse path planning in the immediatestatic environment of the vehicle with a horizon of up toseveral hundred meters. Thus, graph based approaches

can be applied at this level of granularity, e.g. based onvisibility graphs, cf. [20].

In order to generate a target pose and a referencepath, the scene (cf. [19] for definition), which is theresult from context and scene modeling, is combinedwith the planned route, mission aspects (e.g. drivablearea or cooperation aspects), as well as constraints andparameters generated from self-representation (cf. Fig.2,Ability Monitoring).

In terms of MPC, the static as well as the dynamicenvironment will be transformed to spatial constraintsin the vehicle’s coordinate frame. This does not onlyinclude rigid objects, but also those areas which mustnot be crossed due to legal regulations (e.g. solid lines).However, all decisions about constraints at this levelare of tactical implication. Actuation constraints canbe imposed at this level, if they are part of an op-timization process. Furthermore, actuator degradationcan be considered at this level. In case degrading ac-tuator performance has been detected, an immediatereaction to stabilize the vehicle must be performed atthe stabilization level. However, the system must alsoplan degraded trajectories, which account for degradedactuation abilities (e.g. constraints on maximal availablesteering angles).

Additionally, tactical decisions about optimizationcosts will be made here. For instance, if the vehicle isdriving with passengers, optimization weights for limit-ing jerk and yaw rate will be increased, in order to in-crease passenger comfort. If the vehicle is driving withoutpassengers, a higher maneuverability might be desirable,such that those weights will be lowered.

Eventually, the guidance level must ensure that at leastone trajectory is available which can be used for safelystopping the vehicle. This can be reached by making useof the least valid trajectory in case of infeasible opti-mization problems as visualized in Fig. 3. In emergency

cycle time

cycle time

trajectory T1

trajectory T2

e.g. 100 ms

several secondshorizon

prediction

non-feasible trajectory T2 at t = t2

timet1 t2

continue on trajectory T1low level

MPC

Fig. 3. Sequential display of a low-level controller using a fallbacktrajectory (e.g. for braking the vehicle) in case of infeasibleoptimization problem at time t2.

situations, the above requirement can also demand a re-

parameterization of optimization constraints. To clarifythis, consider the following situation: An imminent crashcan only be avoided by performing an evasion maneu-ver. The target pose is located in a valid place, butit is not reachable because a solid line would need tobe crossed. Under normal conditions, legal constraintswould prohibit this maneuver, which would be reflectedin the spatial optimization constraints. The resultingoptimization problem would be over-constraint, withoutpossessing a feasible solution. This is why the guidancelevel needs additional feasibility information from thestabilization level.

If a non-feasible optimization problem is detected,several strategies could be applied. Providing severallevels of drivable area could be considered: Normally,solid lines must not be crossed, as this is prohibitedby law. In emergency situations, this constraint couldbe lifted at the tactical level if no feasible trajectory isfound. In the following iteration, with changed spatialconstraints, the subordinate MPC framework could thenfind a feasible solution.

C. StabilizationTaking the reference path and the spatial constraints

from the guidance level as an input, Matthaei and Mau-rer [8] divide the stabilization level into two sub levels.These levels are responsible for trajectory generation andfor trajectory following control. Typical cycle times forthe generation layer are multiple ten to hundred millisec-onds and several milliseconds for the control layer.

With regard to MPC, multiple approaches can beconsidered: On the one hand, the MPC framework canbe utilized for model based trajectory generation only incombination with a subordinate lateral and longitudinalcontroller as proposed by Falcone et al. [21]. This is afeasible approach, e.g. if the model reflects basic vehicleproperties, but lacks accuracy to perform actual modelbased control, particularly if appropriate disturbancemodels are lacking. In addition, the problem of long cycletimes due to a slowly solvable optimization problem canbe circumvented using an additional controller: Whilethe MPC framework calculates a new trajectory, thecontroller(s) can always work on the last valid referencevalues. On the other hand, if the MPC algorithm is real-time capable and can be provided with highly accuratevehicle models, the additional controller can be omitted.

Considering data from self-perception modules, actu-ation constraints are modified in order to adopt to theactual system state. For instance, in case of a mechan-ical brake with degraded performance, trajectories withreduced decelerations will be planned.

From a functional safety perspective, MPC as frame-work at the stabilization level allows exploiting func-tional redundancies to compensate for degradation ofthe actuation system due to failures, wear, or insufficientenergy supply. Perceived degradation is mapped to theconstraints related to the affected actuators. In case of

safety relevant degradation, the controller will access allother available actuators in order to follow the plannedtrajectory. For instance, a free-running wheel due to afailure of a mechanical brake leads to reduced decelera-tion capabilities as well as to undesired yaw torque whendecelerating. In this case, the controller compensates forreduced deceleration capabilities by utilizing the drivetrain to reduce speed as well as other available actuatorsin order to compensate for the yaw moment induced bythe degraded brake (e.g. by additional steering angle,differential braking, or torque vectoring).

Output data of the stabilization level consists of di-rect physical reference values (e.g. steering angle, motortorque) for the respective actuators.

IV. MPC Framework for an AutomatedVehicle with Active Rear Steering

The following section will present an actual imple-mentation of an MPC framework for our electric, over-actuated, full-by-wire experimental vehicles. Implemen-tation will be described related to the architecturalconcerns presented in Section III.

A. Experimental Vehicles & Problem StatementThe proposed MPC framework for trajectory planning

was implemented for our research vehicles MOBILE andMAX. Both vehicles are part of a rapid-prototypingtool chain (Fig. 4) developed at the Institute of ControlEngineering. This MATLAB/Simulink tool chain wasimplemented to perform early tests of vehicle controlalgorithms on a 1:5 model vehicle (MAX) in order toallow for safe testing, before moving to full-scale valida-tion (MOBILE).

Idea

Simulation(MATLAB / Simulink)

1:5 ScaleVerification

Full ScaleVerification

Fig. 4. Tool-chain with vehicles MOBILE (left) and MAX (right)

Both vehicles are equipped with the same ECUs andfeature a FlexRay communication backbone as well asCAN bus connections for communication with sensorsand actuators. Regarding the actuator topology, bothvehicles are equipped with four close-to-wheel drivesand four individually steerable wheels. For further de-tails about the drive train and network topologies, referto [14]. Regarding the actuator topology, model basedcontrol strategies can serve as a tool for efficient vehicledynamics control e.g. by utilizing torque vectoring or

force allocation approaches. Moreover, functional redun-dancies can be synthesized e.g. by utilizing single wheelsteering and recuperation to generate additional brakeforce, if the brake system is only available with degradedfunctionality. In addition, the full-scale vehicle MOBILEis equipped with a set of LiDAR Sensors (2× VelodyneVLP-16 PUCK and a Velodyne HDL-32) for basic envi-ronment perception.

For exploiting the vehicle’s capabilities even underordinary driving conditions, coordination between ac-tuators needs to be performed as the driver’s inputcapabilities are limited. E.g. for steering, the driver canonly command a single steering angle δ at the steeringwheel, while the wheels are steered with a front (δf ) anda rear steering angle (δr). By coordinating rear and frontsteering, the turn radius of the vehicle can be modifieddepending on the actual driving scenario. On the onehand, a smaller turn radius can be realized by countersteering (e.g. δf = −δr), thus creating higher yaw ratesand making the vehicle more agile, for instance whenparking. On the other hand, a larger turn radius, or evenparallel steering (e.g. δf = δr) can be used at highervelocities to increase driving comfort for the passengers.

Our proposed MPC framework shall be able to per-form the above mentioned coordination between rear-and front axle steering, as well as the enforcement ofconstraints derived from self-representation as describedin Section III in a static and unstructured vehicle en-vironment. For this purpose, we focus on the exampleof lateral planning, such that constraints regarding theability to steer the vehicle will be reflected in terms ofavailable steering angle and steering angle rate at thefront and rear axle. Additionally, as this paper shallfocus on the trajectory planning framework. We assumethat our system is provided with information aboutthe static environment and that information about themaximal available steering angles can be derived fromself-representation. As a first step, the implementationis evaluated in simulation. The vehicle model used forsimulation is a continuous time non-linear double trackmodel, while the MPC strategy is based on an LPV singletrack model (equations 4, 5, and 7).

B. ImplementationIn conformance with the proposed functional system

architecture, we propose a two staged approach: At theguidance level (cf. Section III-B) an unambiguous refer-ence path is extracted from the static environment, tak-ing into account all static obstacles. Spatial constraintswhich are required for performing actual MPC at the sta-bilization level (cf. Section III-C) are transformed from avehicle relative cartesian frame to a Frenet frame alongthe reference. Actual trajectory planning is performed atthe stabilization level in Frenet space.

1) Guidance Level: For reference path generation weassume, that the static environment is represented by anoccupancy grid based. By extracting occupied as well as

αs

initial position

initial orientation

Fig. 5. Penalty for vehicle’s orientation relative to first nodestarting from initial position node

free space from this grid based representation we arrive ata polygonal based description of impassable areas aroundthe vehicles.

In a first step, a visibility graph is constructed fromthe polygonal environment utilizing [20]. This visibilitygraph contains all polygon vertices as well as the startand the goal point as nodes. The graph’s edges aredetermined by connecting all nodes which are in lineof sight, to prevent edges going through an obstacle.Weights for this edges are given by euclidean distances between two vertices.

In order to determine the shortest path through thisvisibility graph, we use an A* approach [20]. However,in order to not only consider the initial and terminalposition of the vehicle, but also the initial and terminalorientation, additional edge weights have been definedwhich are added to the initial weight s in the A* imple-mentation. These weights wstart,end penalize the angleαstart,end between the vehicle’s longitudinal axis and theedges connected to the first/last node in the visibilitygraph (cf. Fig. 5). The total cost C for traveling alongthe respective edges is then modified to

C = s+ wstart,end · |αstart,end |. (1)

The resulting shortest path will be utilized as a referencepath and is represented by a polyline through the staticenvironment. In order to derive the spatial constraintsrequired for the MPC framework, a maximal and aminimal deviation emax,min from the reference path aredefined and superimposed with the orthogonal distancesof the given polygon edges from the polyline (cf. Fig 6).

This way, linear spatial constraints for the static en-vironment are generated for each sampling interval k inthe MPC framework.

k 0 1 2 3 4 5

e(5)max

e(12)min

6 7 8 9 10 11 12

Fig. 6. Definition of constraints in relation to reference path

Additional constraints to those already mentioned

(Section IV-A) which are generated at this level areconstraints for maximal side slip angle and maximalyaw rate in order to influence overall driving behavior.In our example, strategic decisions about optimizationweights relate to weighting front and rear axle steeringby formulating penalizing either high yaw rates (for moreside slip angle in the resulting trajectory) or high side slipangle (for more yaw rate in the resulting trajectory).

2) Stabilization Level: The actual MPC for trajectorygeneration is performed at the stabilization level imple-mented with the MATLAB MPC Toolbox. As a first step,we assume that an extended linearized single track modelwith active rear steering is suitable to generate a basiccollision free lateral trajectory. I.e. we assume linear tiredynamics as well as small side slip and heading angles.For the required states we follow an approach presentedby Gutjahr and Werling [17]. Apart from the side slipangle β and the vehicle’s yaw rate ψ, we introduce theheading difference to the reference frame ∆ψ and lateraldeviation in Frenet coordinates e in order to describe themotion of the vehicle. Thus, our state space vector x is:

x =[β ψ ∆ψ e

]T (2)

Inputs to the model are front and rear steering anglesδf , δr, as well as a disturbance term ∆ψref to model thetrajectory’s curvature in the world frame (cf. Fig. 7).

vψ

δf

δr

reference

∆ψ

e

β

Fig. 7. Single track model

The resulting input vector u is

u = [δf δr ∆ψref ]T . (3)

By considering a linear parameter variant (LPV) singletrack model with vehicle mass m and velocity v (v asa time-variant parameter), the front and rear corneringstiffness cα,f and cα,r, front and rear distance from thevehicle’s center of gravity lf and lr as well as the momentof inertia around the vehicle’s z-axis Jz, the resultingsystem matrix A(v) and input matrix B(v) are given by

A(v) =

− cα,f+cα,r

m·vcα,r·lr−cα,f ·lf

m·v2 − 1 0 0− cα,f ·lf−cα,r·lr

Jz − cα,f ·l2f+cα,r·l2rJz·v 0 0

0 1 0 0v 0 v 0

(4)

and

B(v) =

cα,fm·f

cα,rm·v 0

cα,f ·lfJz

cα,r·lrJz

00 0 −10 0 0

. (5)

In order to check for collisions during MPC calculation, itis not sufficient to impose optimization constraints on thecenter of gravity of the vehicle, as this does not considerthe vehicle’s orientation. Hence, we do not only considerthe lateral distance of the vehicle’s center of gravity e,but also the lateral distances of the vehicle’s front ef andthe vehicle’s rear er from the reference path, which aregiven by

ef ,r = e± lf ,r · sin(∆ψ). (6)

By introducing the linearized distances in the measure-ment matrix C, all required dimensions can be constraintduring optimization:

C =

1 0 0 00 1 0 00 0 1 00 0 0 10 0 lf 10 0 −lr 1

(7)

For applying the MPC scheme to the defined system,we formulate a cost function as follows: The total costJ(zk) for a given series of p future system inputs u atthe current control step k

zkT =

[u(k|k)T u(k + 1|k)T . . . u(k + p− 1|k)T

](8)

can be described as the sum of costs of depending onthe following properties (ny = dim(y), wyi,j weight foroutput yj at prediction step i):1) the weighted deviation of measured outputs y fromtheir given reference r

Jy(zk) =ny∑j=1

p∑i=1

(wyi,j [rj(k + i|k)− yj(k + i|k)]

)2, (9)

2) the weighted deviation of calculated inputs u froma targeted series of input vectors ut

Ju(zk) =nu∑j=1

p∑i=1

(wui,j [uj(k + i|k)− ut,j(k + i|k)]

)2,

(10)3) the weighted difference between input vectors in

subsequent time steps

J∆u(zk) =n∆u∑j=1

p∑i=1

(w∆ui,j [uj(k + i|k)− uj(k + i− 1|k)]

)2.

(11)Combined, this yields the complete cost function

J(zk) = Jy(zk) + Ju(zk) + J∆u(zk). (12)

Please note, that slack variables are omitted for brevity(please refer to [22] for further information).

In the actual implementation, we utilize the outputweights wyβ , w

y

ψ, wy∆ψ, and wye for tracking the corre-

sponding output variables. Absolute front and rear steer-ing angles and their corresponding rates are minimizedusing the weights wuδf and wuδr as well as w∆u

δfand w∆u

δrThe full optimization problem, including all con-

straints for states and inputs, results in solving

min(J(zk)) subject to

βmin(i) ≤ β(k + i|k) ≤ βmax(i)ψmin(i) ≤ ψ(k + i|k) ≤ ψmax(i)emin(i) ≤ e(k + i|k) ≤ emax(i)

ef,min(i) ≤ ef (k + i|k) ≤ ef,max(i)er,min(i) ≤ er(k + i|k) ≤ er,max(i)δf,min(i) ≤ δf (k + i|k) ≤ δf,max(i)δr,min(i) ≤ δr(k + i|k) ≤ δr,max(i)δf,min(i) ≤ δf (k + i|k) ≤ δf,max(i)δr,min(i) ≤ δr(k + i|k) ≤ δr,max(i)

. (13)

As already stated, weights will be derived from thetactical level, constraints can originate from either thetactical or the stabilization level. Yet, as the stabilizationlevel is ideally immediately aware of sudden actuatordegradations (e.g. by residual fault diagnosis techniquesas proposed by Ding [23]), while it can take several hun-dred milliseconds until the planning level is aware of suchdegradations, constraints imposed at the stabilizationlevel need to possess higher priority compared to con-straints imposed at the planning level. Regarding MPCthere are two drawbacks, which must be considered:First, if MPC is used for actual control, the performanceis highly dependent of accurate models for predictingmodel states. In addition, MPC can only adapt to newparameters between two optimization cycles. As this cantake several ten to hundred milliseconds, this might beto slow for sudden actuator failures. Thus we requirean underlying low level controller to be present forperforming actual trajectory following control.

C. ResultsThe following results have been obtained in simulation,

using a model of the research vehicle MAX to showseveral scenarios for tactically chosen optimization pa-rameters. For an exemplary parking scenario, the resultsof trajectory generation and according steering angles areshown in Fig. 8a. The LPV model has proven to deal withslowly varying velocities during parking. The optimizedtrajectory is plotted in green, the vehicle contour isplotted in black. It can be stated, that no collision occurswhen following the calculated trajectory. Also, vehicleheading is nearly zero for the final vehicle state in theparking lot. The trajectory is based on a steering config-uration using counter steering and common steering, toachieve optimal results as shown in Fig. 8b.

Fig. 9a and Fig. 9b show the results of a trajectorygeneration in an unstructured environment. In Fig. 9a,the blue line segments depict the shortest path from startto target point, the collision-free trajectory is shown ingreen. Fig. 9b shows the calculated steering angles tofollow the generated trajectory. In this case, commonsteering is applied for the whole trajectory, to minimizeyaw rate and thus increasing driving comfort. It can bestated, that trajectories for a variety of scenarios canbe generated without changing optimization criteria. Ifhigh maneuverability is required, a trajectory based oncounter steering is generated, otherwise common steeringis applied to increase driving comfort. This underlines thegeneric character of the presented approach.

However, weight tuning can be applied if necessary.Fig. 10 shows the effects of different weights appliedfor the steering angles at front and rear axle whenplanning a slalom maneuver. The blue lines result fromoptimization if a high weight on rear axle steering isapplied. The tactical decision to choose a high weight forone axle could be performed due to information from self-representation modules to reduce wear on the actuatorsof one axle. In case high weights are introduced for bothaxles the resulting steering angles switch to continuouscounter steering, as this steering configuration requireslower steering angles to complete the slalom maneuver.

The MPC framework runs with an average cycle timeof ca. 100 ms on an Intel Pentium G3450 (3.4 GHz) usingMathworks’ Embedded Coder for code generation.

V. Conclusion & Future ResearchIn this paper, we presented an architectural approach

to MPC for path planning in autonomous vehicles. Theapproach considers the tactical formulation and refor-mulation of optimization parameters and constraints,

0 1 2 3 4 50

0.5

1

1.5

x in m

yin

m

(a) Trajectory and vehicle contour

0 10 20 30 40 50−0.6

−0.4

−0.2

0

prediction step

stee

ring

angl

ein

rad

frontrear

(b) Steering angles

Fig. 8. Trajectory generation for a parking scenario

0 5 10 15 2002468

10

x in m

yin

m

(a) Trajectory

0 10 20 30 40 50 60 70−0.4

−0.20

0.2

0.4

prediction step

stee

ring

angl

ein

rad

frontrear

(b) Steering angles

Fig. 9. Trajectory generation in an unstructured scenario

0 20 40 60 80 100

0

0.2

prediction step

stee

ring

angl

ein

rad

front steering, high weight rear axlerear steering, high weight rear axlefront steering, high weight both axlesrear steering, high weight both axles

Fig. 10. Influencing trajectory by tuning weights

based on self-assessment and feasibility information fromthe MPC framework. The applicability of the chosenapproach has been demonstrated in a simulative set-upfor several planning scenarios.

Further research will focus on the integration of tiredynamics (e.g. as proposed by Yi et al. [18]). Additionallywe will extend the implementation of the self-awareMPC approach to vehicle dynamics control at the lowerstabilization level (e.g. brake-blending).

Eventually, the presented control concepts shall beevaluated on the research platforms MOBILE and MAX.

AcknowledgmentThe authors would like to thank the MOBILE team,

Leyre Tardio Martos in particular, for their valuableinput. This work is part of the DFG research groupFOR1800 Controlling Concurrent Change (CCC).

References[1] SAE, “Taxonomy and Definitions for Terms Related to On-

Road Motor Vehicle Automated Driving Systems,” SAE In-ternational, Tech. Rep., 2014.

[2] E. D. Dickmanns, “Multisensorielle Fahrzeugfuhrung,” 1987.[3] M. Maurer, “Flexible Automatisierung von Straßenfahrzeu-

gen mit Rechnersehen,” dissertation, Univ. der BundeswehrMunchen, 2000.

[4] M. Pellkofer, “Verhaltensentscheidung fur autonomeFahrzeuge mit Blickrichtungssteuerung,” dissertation, Univ.der Bundeswehr Munchen, 2003.

[5] K.-H. Siedersberger, “Komponenten Zur AutomatischenFahrzeugfuhrung in Sehenden (Semi-)Autonomen Fahrzeu-gen,” dissertation, Univ. der Bundeswehr Munchen, 2003.

[6] A. Reschka, “Fertigkeiten- und Fahigkeitengraphen alsGrundlage des sicheren Betriebs von automatisierten Fahrzeu-gen im offentlichen Straßenverkehr in stadtischer Umgebung,”dissertation, TU Braunschweig, 2017, announced.

[7] ISO, “ISO 26262: Road Vehicles - Functional Safety,” In-ternational Organization for Standardization, Genf, Schweiz,International Standard, 2011.

[8] R. Matthaei and M. Maurer, “Autonomous Driving - A Top-Down-Approach,” at - Automatisierungstechnik, vol. 63, no. 3,2015.

[9] J. Rasmussen, “Skills, rules, and knowledge; signals, signs, andsymbols, and other distinctions in human performance mod-els,” IEEE Transactions on Systems, Man, and Cybernetics,vol. SMC-13, no. 3, pp. 257–266, 1983.

[10] E. Donges, “A Conceptual Framework for Active Safety inRoad Traffic,” Vehicle System Dynamics, vol. 32, no. 2-3, pp.113–128, 1999.

[11] E. Dickmanns, R. Behringer, D. Dickmanns, T. Hildebrandt,M. Maurer, F. Thomanek, and J. Schiehlen, “The seeingpassenger car ’VaMoRs-P’,” in 1994 IEEE Intelligent VehiclesSymposium, 1994, pp. 68–73.

[12] O. S. Tas, F. Kuhnt, J. M. Zollner, and C. Stiller, “Functionalsystem architectures towards fully automated driving,” in2016 IEEE Intelligent Vehicles Symposium, 2016, pp. 304–309.

[13] K. M. Passino and P. J. Antsaklis, “Modeling and analysis ofartificially intelligent planning systems,” P. J. Antsaklis andK. M. Passino, Eds. Norwell, MA, USA: Kluwer AcademicPublishers, 1993, pp. 191–214.

[14] P. Bergmiller, Towards Functional Safety in Drive-by-WireVehicles. Springer, 2015.

[15] A. Reschka, G. Bagschik, S. Ulbrich, M. Nolte, and M. Maurer,“Ability and skill graphs for system modeling, online moni-toring, and decision support for vehicle guidance systems,” in2015 IEEE Intelligent Vehicles Symposium, pp. 933–939.

[16] C. Gotte, M. Keller, C. Rosmann, T. Nattermann, C. Haß,K.-H. Glander, A. Seewald, and T. Bertram, “A Real-TimeCapable Model Predictive Approach to Lateral Vehicle Guid-ance,” in 2016 IEEE International Conference on IntelligentTransportation Systems, 2016.

[17] B. Gutjahr, L. Groll, and M. Werling, “Lateral Vehicle Tra-jectory Optimization Using Constrained Linear Time-VaryingMPC,” IEEE Transactions on Intelligent Transportation Sys-tems, vol. PP, no. 99, pp. 1–10, 2016.

[18] B. Yi, S. Gottschling, J. Ferdinand, N. Simm, F. Bonarens, andC. Stiller, “Real Time Integrated Vehicle Dynamics Controland Trajectory Planning with MPC for Critical Maneuvers,”in 2016 IEEE Intelligent Vehicles Symposium, Gothenburg,Sweden, 2016, pp. 584–589.

[19] S. Ulbrich, T. Menzel, A. Reschka, F. Schuldt, and M. Maurer,“Defining and Substantiating the Terms Scene, Situation, andScenario for Automated Driving,” in 2015 IEEE InternationalConference on Intelligent Transportation Systems, pp. 982–988.

[20] K. J. Obermeyer and Contributors, “The VisiLibity library,”2008, r-1.

[21] P. Falcone, F. Borrelli, H. E. Tseng, J. Asgari, and D. Hrovat,“A hierarchical Model Predictive Control framework for au-tonomous ground vehicles,” in 2008 American Control Con-ference, 2008, pp. 3719–3724.

[22] MATLAB, Version 9.0.0 (R2016a). Natick, Massachusetts:The MathWorks Inc., 2016.

[23] S. X. Ding, Model-Based Fault Diagnosis Techniques, ser.Advances in Industrial Control. London: Springer London,2013.