IN DEGREE PROJECT MECHANICAL ENGINEERING,SECOND CYCLE, 30 CREDITS

, STOCKHOLM SWEDEN 2018

Model Predictive Control for path tracking and obstacle avoidance of autonomous vehicle

AHMED HATEM

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF ELECTRICAL ENGINEERING AND COMPUTER SCIENCE

Sammanfattning

Konceptet autonoma fordon har pa senare tid blivit brett utforskat av blandannat bilindustrin som ett satt att exempelvis forbattra bransleekonomi eller fatillgang till miljoer som utgor en fara for manskliga operatorer. Modell-prediktivreglering (MPC) har traditionellt anvants for att styra system med langsamdynamik men i och med uppkomsten av kraftfullare datorer anvands det nui system med avsevart snabbare dynamik. En av de huvudsakliga styrkornahos MPC ar dess formaga att hantera restriktioner som finns i alla fysikalis-ka system. Malet med den har uppsatsen var att utveckla en linjar regulatorbestaende av ett lager for vagfoljning och undvikning av hinder for en auto-nom bil. Dess formaga att minimera avvikelser till referensbanan och samtidigtundvika statiska hinder utvarderades. Fokus placerades pa sparningen av refe-rensbanan foljaktligen implementeras inte nagot system for planering av banan.Istallet anvandes en fordefinierad bana. Simuleringar utvecklades i matlab base-rat pa den kinematiska cykelmodellen. Regulatorns prestanda utvarderas vidarepa Smart Mobility Lab (SML) pa KTH dar en modifierad radiostyrd bil styrdesvia Robotics Operting System (ROS). Resultaten fran experimenten visade paatt bilen klarar av att undvika hindren samtidigt som den foljde banan. Dockmisslyckades bilen i experimenten att respektera kraven pa maximal avvikelsefran hindren och banan.

1

1

Model Predictive Control for path tracking andobstacle avoidance of autonomous vehicle

Ahmed Hatem

Abstract—The concept of autonomous vehicles has been widelyexplored lately by, among others, automotive companies as away to for example improve fuel efficiency or to gain accessto environments which pose a danger to human operators.Model Predictive Control (MPC) has traditionally been used tocontrol systems with slower dynamics but with the emergence ofmore powerful computers it is now being used in systems withconsiderably faster dynamics as well. One of the main strengthsof MPC is its ability to handle constraints which are present inall physical systems. The aim of this thesis was to develop a singlelayer linear controller for path tracking and obstacle avoidanceof an autonomous car. Its ability to minimize the deviations tothe reference path while clearing static obstacles was evaluated.Focus was placed on the tracking problem hence no trajectoryplanning system was implemented. Instead a predefined pathwas used. Simulations were developed in MATLAB based on thekinematic bicycle model. The performance of the controller wasfurther tested at Smart Mobility Lab (SML) in KTH where amodified R/C car was controlled through Robotics OperatingSystem (ROS). The results from the experiments showed that itwas able to successfully evade the obstacles while tracking thepath. However, in the experiments the vehicle failed to respectthe requirements on maximum deviation from the obstacles andthe path.

I. INTRODUCTION

THE concept of autonomous vehicles was for not so longago an unrealistic technology. Today autonomous road,

surface and aerial vehicles are reality. One of the challengesthat engineers face is the ability of the vehicle to track a givenpath, i.e. the tracking problem. To this end Model PredictiveControl (MPC) has been employed partly due to its abilityto handle constraints which are necessary in the modeling ofall physical systems. The main idea of MPC is to solve afinite horizon optimal control problem at certain time steps andcompute an optimal control sequence of which only the firstcontrol action is implemented. The procedure is then repeatedat the next sampling step as new information about the statehas been obtained from a model.

Due to an increase in the number of road vehicles in today’smodern society safety has become a major issue. Studies haveshown that by 2006 road traffic accidents was the leadingcause of death by injury as well as the tenth-leading sourceto all deaths globally. Estimations suggest that 1.2 millionpeople are killed in road crashes each year. Furthermore, asmany as 50 million are injured [1]. If current trends persistroad traffic injuries are predicted to become the third-leadingcontributor to the global burden of disease and injuries by2020 [2]. Although passive as well as active systems havebeen developed and implemented in vehicles over the years[3], there is a desire to further reduce the number of trafficaccidents. Since a great deal of light-duty vehicle crashes occur

due to human error and distracted driving partially-automatedcrash avoidance systems can offer a potential reduction in boththe frequency and the severity of accidents [4]. This suggeststhat the implementation of fully-automated road vehicles couldpossibly provide an even greater improvement.

Recently predictive control for autonomous vehicles hasalso been studied, [5] is an example where the vehicle wasinstructed to follow a given reference path. In this specific casethe path is assumed to be collision free, a scenario which maynot be useful in case of unknown environments. Nevertheless,it can be utilized in for instance mining networks which arewell mapped.

Experiments with autonomous vehicles have been success-fully conducted over the years, [6] and [7] are two suchexamples. Although they proved to be successful there are stillsome years before the concept of autonomous road vehicleswill be deployed on a large scale. Both tests where performedon days which were sunny and clear, in the absence of anyheavy rain or snow. Another issue is that some maneuvers wereperformed with safety in mind and could sometimes deviatefrom common human behaviour. This needs to be consideredin the future to avoid that the behavior of the vehicle becomestoo inefficient when compared to a piloted vehicle. A betterunderstanding of human behavior still remains a major diffi-culty to tackle.

While the two above mentioned studies targeted urban areasoff-road path following also receives some attention. Scaniahas shown interest in developing autonomous vehicles withspecialized applications such as for deployment in miningsites. In [8] the authors propose an Economic Model PredictiveControl (EMPC) for smooth path following. The objectivebeing to track a path while at the same time minimizing thefirst as well as the second derivative of the road curvature. Theproposed controller is implemented on a construction truckand its performance in terms of path tracking accuracy iscompared to a Pure Persuit Controller (PPC). Results showthat the suggested EMPC outperforms the PPC with an averagedeviation from the road of 6 cm. Hence the area of autonomousroad vehicles has been widely studied over the years, althoughmajor challenges still remain to be tackled before it can bewidely used in society.

A. Thesis outline

The objective of this thesis is to develop a simple MPCfor path tracking and obstacle avoidance for autonomousroad vehicle. The controller will be developed and tested insimulations performed in MATLAB. To assess the performanceof the controller it will be implemented on a modified R/C car

2

with a scale of measure of approximately 1:10. The tests arecarried out at Smart Mobility Lab in KTH. The vehicle shouldbe able to track a straight line with a velocity of 0.5 /s whileavoiding obstacles placed along the path. More specifically thevehicle should fulfill the following requirements:• Handle disturbances in the form of initial lateral devia-

tions from the path on up to 0.4 m.• In the absence of any obstacles the settling distance must

not exceed double the initial lateral deviation. This isthe distance projected along the path that the vehiclehas to travel in order to recover to the path with anover/undershoot no larger than 0.1 m.

• Avoid 3 static obstacles by driving around them.• The vehicle should clear the obstacles with a distance not

larger than 0.07 m.• Over/undershoots from the path must not exceed 0.07 m.

Moreover, following assumptions are made:• The vehicle travels with a constant velocity.• The obstacles are identical with the dimension 0.14 ×

0.14 m.• The shape and position of the obstacles are known to the

controller.• The estimations of the states are free from errors and

noise. No filters are used to deal with uncertainties andnoise.

• No trajectory planning system is available, instead thepath is predefined.

II. BACKGROUND

A control system for an autonomous vehicle can be regardedas a system composed of two components. Firstly there isthe path planning algorithm which generates a collision freetrajectory. Work in this area dates back to the 1980s andfocused mainly on calculating a time-optimal and collision-free path reaching from one point to another [9], [10].Ever since then many different methods and implementationshave been reported including the Rapidly-exploring RandomTree Algorithm (RRT), generalized Voronoi diagram and thevisibility graph method. The second component is the pathtracking algorithm which ensures that the vehicle follows theplanned trajectory. In order to achieve this one has to take intoconsideration that vehicles are made out of mechanical andelectrical systems which are subjected to physical constraints.Furthermore, the dynamics of the vehicles are nonlinear. Withits ability to handle both linear as well as nonlinear constraintsin a systematic manner MPC is an attractive method fortackling this problem. However, the usage of MPC for systemswith fast dynamics is challenging especially for nonlinearsystems. Since an optimization problem has to be solved theprocess is computationally heavy and therefore requires pow-erful computers. Even though there are examples of successfullimplementations of MPC controllers on test vehicles [11],[12], they are not nonlinear but linear. Hence, implementationsof nonlinear MPC on autonomous vehicles still pose somechallenges [13]. A possible solution to this issue could be todivide the control structure into two layers where one layerdeals with the nonlinear vehicle model along with constraints

and trajectory generation while the design of the second layeris based on a linearization around a trajectory provided bythe first layer. This layer is used to stabilize the vehicle.An additional design technique that can be used to furtherdecrease the computational demand necessary to obtain fastfeedback required by vehicle stabilization is to use a lowersampling rate for the former control layer and a higher for thelatter. These measures along with others were investigated in[14] with the aim to develop an MPC for obstacle avoidance.For the obstacle avoidance ability the authors suggest anapproach where the obstacles are enclosed in circles and aterm is added to the objective function which when penalizedappropriately makes sure that the vehicle does not enter theregion in which the obstacle lays. The results suggest thatthe two-level control framework overcomes the issue relatedto computational burden and the vehicle is not only able totrack the reference path but also able to evade the obstaclessuccessfully.

Studies have also been conducted regarding single layercontrollers for nonlinear vehicle models with the aim of track-ing a reference. In [15] the performance of a nonlinear vehiclemodel is compared to another formulation where the nonlinearmodel is successively linearized around the current operatingpoint. The findings in this study show that the computationalburden poses a problem for the former formulation while thelatter is able to stabilize the vehicle for moderate speeds. Thishighlights the challenges present for implementation of anMPC with nonlinear dynamics for obstacle avoidance system.In this thesis it will be investigated, on the basis of therequirements, how well a simple linear single layer MPCperforms.

A. Model Predictive Control

MPC is an optimal control strategy which traditionally hasbeen applied to systems with rather slow dynamics, such asprocess control plants. However, as more powerful computinghardware have emerged over time it is today used in systemswith considerably faster dynamics such as on flight controlcomputers in airplanes as well as software for combustionengines. As mentioned above the main strength of the methodlies in its ability to control linear and nonlinear systems whiletaking into account state as well as input constraints. Themethod uses a predictive control strategy where the futureresponse of the controlled plant is predicted using a discretelinear time invariant dynamic model. The system is representedin state space form

xt+1 = Axt +But

yt = Cxt + ut(1)

where xt ∈ IRn is the state vector, ut ∈ IRm is the controlinput and yt ∈ IRp is the output vector. The time index t ∈ Zis integer valued. The principle is to find an open loop controlsequence which minimizes a certain cost function over a finite

3

time horizon. The problem can be formulated as

minu

J(x(t),u)

subject to xt+k+1 = Axt+k +But+k, ∀k = 0, . . . , Np − 1

xt+k ∈ X, ∀k = 0, . . . , Np − 1

ut+k ∈ U, ∀k = 0, . . . , Np − 1

xt+Np ∈ Xfxt = x(t),

(2)

where u = (ut, . . . , ut+Np−1) is a sequence of control inputs,xt+k denotes the state at time t+k as predicted at time t andNp is the prediction horizon. The sets X ∈ Rn and U ∈ Rpare the constraints on the state and input respectively. Theterminal constraint set in the state is Xf ⊆ X. Now if the goalis to steer system (2) to the origin the cost function J(x(t),u)can be chosen as a quadratic function

J(x(t),u) = xTt+NpPfxt+Np+

Np−1∑k=1

xTt+kQxt+k+uTt+kRut+k,

(3)where Pf , Q are positive semidefinite (Pf , Q ≥ 0) and Ris positive definite (R > 0). The algorithm starts with ameasurement of the state x(t). Then optimization problem (2)is solved. In the optimal control sequence found in the previousstep only the first control is applied. The system then evolvesone time step after which the procedure is repeated.

Since there exist standard solvers for convex optimizationproblems it is desirable to make (2) convex. This is achievedif the cost function is convex, the system dynamics are linearand the constraint sets X,U are convex as well.

III. VEHICLE MODEL

A kinematic model representing the lateral motion of avehicle can under some specific assumptions described belowbe developed. In contrast to a dynamic model where the forcesacting on the system are taken into account the kinematicmodel is derived from mathematical relationships governingthe system. Furthermore, compared to higher fidelity vehiclemodels the system identification on the kinematic model iseasier because of the low number of system parameters neededto describe the system. In addition to that studies have shownthat the kinematic model is under some conditions able toactually perform similarly well compared to a more complexand computationally heavy dynamic model [16]. For thisapplication where the velocity of the vehicle as well as thelateral acceleration are low the kinematic model performs well[17]. This motivates the design of the controller based on akinematic model.

A. Kinematic model

Consider a bicycle model of the vehicle where the twowheels in each axle are represented by a single wheel alignedwith the center of gravity of the vehicle, see Figure 1. The

nonlinear continuous time equations that describe this modelin an inertial frame rate are [18]

x = vcos(ψ) (4a)y = vsin(ψ) (4b)

ψ =v

ltan(δ) (4c)

where x and y are the coordinates of the vehicle given in theglobal coordinate system. The yaw angle is denoted ψ and thewheelbase is given by l. The longitudinal component of thevelocity is v. In the kinematic bicycle model it is assumedthat the slip angles at both wheels are zero. Consequently thelateral component of the velocity is zero. This is a reasonableassumption to make for low velocities [19]. The steering angleof the front wheels with respect to the longitudinal axis of thecar is denoted δ.

Fig. 1. The nonlinear kinematic bicycle model.

Note that in this model it is assumed that both left and rightfront wheel have the same steering angle. In reality the innerwheel turns with a greater angle due to the fact that it coversa shorter distance than that of the outer wheel. However, thisdifference is in this case small enough to be neglected.

B. Road-aligned coordinates

In order to formulate the tracking problem a model trans-formation from time-dependent vehicle dynamics to a track-dependent (also known as Frenet-Serret frame or curvelinearframe) dynamics is proposed. Similar ideas have previouslybeen developed in the area of robotics [20]. To this end a newvariable s, which represents the the distance traveled along thereference path, is introduced. The system dynamics presentedin the previous section are modeled in terms of the independentvariable s. In this frame of reference the states of interest arethe lateral deviation to the reference path and the error in theheading angle here denoted ey and eψ respectively. Figure 2shows the road-aligned coordinate system where ρs is the roadradius and ψs is the heading angle of the reference path. Thetwo components of the velocity x and y are in the body frameof reference. The velocity projected along the direction of thecenter line, represented in Figure 2 by a solid arrow, is denotedvs.

4

Fig. 2. The road-aligned coordinate system. The dynamics are derived abouta curve defining the centerline of a path.

With the assumption that the lateral component of the vehiclevelocity is zero, i.e y=0 the following equations can be derivedfrom Figure 2

ey = vsin(ψs), (5a)

eψ = ψ − ψs, (5b)

s =ρsvcos(eψ)

ρs − ey. (5c)

In order to derive the spatial dynamics the following relationis used

d(·)ds

=d(·)dt

dt

ds. (6)

If s 6= 0 is assumed at any time, by the inverse functiontheorem it holds that

dt

ds=

1

s. (7)

The derivatives of (5a) - (5b) with respect to s, representedby (·)′

, are

e′

y =deyds

=eys

=ρs − eyρs

tan(eψ), (8a)

e′

ψ =deψde

=eψs

=(ρs − ey)tan(δ)

ρslcos(eψ)− ψ

′

s, (8b)

which is a spatial-based representation of the vehicle dynam-ics. In order to switch from the spatial coordinates to theglobal coordinates X , Y and ψ the following transformationis necessary:

X = Xs − eysin(ψs), (9a)Y = Ys + eycos(ψs), (9b)ψ = ψs + eψ, (9c)

where Xs, Ys and ψs denote the the global coordinates as wellas the heading angle of the desired path [21],[22].

C. Linearization

The proposed models are both nonlinear which results ina non-convex optimal open-loop control problem. One canexpect conventional solvers for nonlinear problems to findlocal rather than global solutions. On the other hand for linearsystems, quadratic cost functions and constraint sets which arepolyhedral the problem reduces to a quadratic one for whichefficient solvers exist. Furthermore, these solvers yield a globaloptimal solution [23]. In order to obtain good performancefor the tests it is necessary to keep the solving time for theoptimization down. This will be easier achieved with the useof a linear model. Consequently, it is highly preferable to workwith linear systems. It is therefore necessary to linearize thesystems around an appropriate point. For simplicity this pointwas chosen as ey = 0, eψ = 0 and δ = 0. The systemwas discretized using forward Euler’s method with samplingdistance ∆s = vTs where Ts is the sampling time.

IV. OBSTACLE AVOIDANCE SYSTEM

The size and position of the obstacles are as previouslymentioned assumed to be known. The system works in sucha way that as soon as a collision is predicted a soft constraintwill be set on the lateral deviation ey . This allows the vehicleto pass the obstacle either by making a left or right hand turn.How the vehicle chooses to act is predefined in the algorithm.If the center of the obstacle is placed above the path the vehiclewill make a right hand turn to pass it. Likewise, if the obstacleis centered below the path the vehicle is instructed to make aleft hand turn. If on the other hand the obstacle is placed alongthe path the vehicle will make a turn based on the placementof the upcoming obstacle. In other words the controller willchoose the way which minimizes the lateral distance to thereference path. In the simulations the center of gravity ofthe vehicle was chosen as the point which must avoid theobstacles. In reality the vehicle cannot be represented by apoint so to take the width of the vehicle into consideration thesize of the obstacles were simply increased.

V. CONTROL PROBLEM FORMULATION

Based on the spatial representation of the vehicle model andthe objective to follow a desired path while evading obstaclesthe following MPC formulation was formulated:

minNp∑t=1

Q1ey2t +Q2eψ

2t +Q3ε

2t +Q4ut−1

2 +R1∆u2t

(10a)s.t xt+1 = Axt + B(ut−1 + ∆ut) + C ∀t = 0, . . . , Np

(10b)|ut| − εt ≤ umax ∀t = 0, . . . , Np (10c)|∆ut−1| ≤ ∆umax ∀t = 0, . . . , Np (10d)εt >= 0 ∀t = 0, . . . , Np (10e)

To achieve the goal of following the desired path the objectivefunction is defined such that the square of ey and eψ areminimized. As mentioned in the previous section a bound is seton ey in order for the vehicle to avoid the obstacles. To prevent

5

the optimization problem from becoming infeasible thoseconstraints along with the hard constraints on the steeringangle ut had to be softened with a slack variable εt. As inany physical system the control input ut at step t and thecontrol correction variable ∆ut−1 at step t − 1 are limited.In this case the maximum steering angle is upper bounded byumax while the change of rate is limited by ∆umax. In thisformulation the control correction variable is treated as theinput to the system while the steering angle is considered asa state. With this formulation the control input at time t canbe computed recursively with

ut = ut−1 + ∆ut. (11)

The state vector [ey eψ s]T is denoted xt and the matrix A, the

vectors B and C are obtained by linearizing and discretizing(5c), (8a) and (8b). Moreover, since the intention is to drivein a straight line the limit of the above mentioned equationsas ρ→∞ had to be calculated. The final results are

A =

1 vTs 00 1 00 0 1

, B =

0vTsl0

, C =

00vTs

. (12)

The system dynamics can now as described in [24] be writtenas [

xt+1

ut

]=

[A B0 I

]︸ ︷︷ ︸

A

[xtut−1

]+

[BI

]︸︷︷︸B

∆ut +

[CI

]︸︷︷︸C

. (13)

VI. EXPERIMENTAL SETUP

The simulations were initially performed in MATLAB us-ing YALMIP, a toolbox for modeling and optimization. Thetoolbox uses quadprog as standard for solving quadraticprogramming problems. At a later stage this simulation wasreplaced with a more efficient one where the problem wasreformulated as a quadratic programming problem. The resultsare presented in the following sections. To assess the perfor-mance of the controller it was implemented and tested on amodified R/C car, see Figure 3. It is equipped with an NvidiaJetson TX2 embedded computer. The position, orientation aswell as velocity of the vehicle were tracked using a set ofmotion capture cameras (MOCAP) from Qualisys.

Fig. 3. Test vehicle.

A. Robotics Operating System

The optimization problem was solved on a separate com-puter using MATLAB. The communication between the com-puter, the vehicle and the sensors was carried out through theRobotics Operating System (ROS). Each of the componentsconstitute a node in the system. A ROS master is set upmaking sure that the individual nodes are able to locateeach other. The nodes communicate with each other throughmessages. As soon as the sensors publish new data to thetopic /qualisys_odom the computer, being a subscriberto that topic, receives the data. The computer then solves theoptimization problem and publishes the control input in thetopic /cmd to which the vehicle is a subscriber. See Figure 4for a schematic of the setup.

Fig. 4. Schematics of ROS setup.

B. MPC formulated as Quadratic Programming

In order to obtain good performance in the experimentsthe sampling time for the controller had to be minimized.The sensors fed the controller with new data with a rate of20 Hz meaning that the optimization could not take longerthan 50 ms to complete. Otherwise no feedback will beprovided to the vehicle which could lead to slow responsesand collision with obstacles. The simulations developed inYALMIP proved not to fulfill this requirement. To improvethe efficiency of the controller the problem was rewritten

6

as a quadratic programming problem [25]. To start with theoptimization problem was rewritten on the form

min1

2zTHz

subject to Aeqz = beq

Az =≤ b

(14)

where z = (xTt+1|t, . . . , xTNp|t, ut−1|t, . . . , ut+Np−1)T . The

matrix H associated with the objective function is a diagonalsquare matrix of the size 4Np +Np + 1× 4Np +Np + 1. Thestructure of the matrix is

H =

Q 0 . . . . . . 00 Q 0 . . . 0... 0

. . ....

......

... . . . R 00 0 . . . . . . wε

where the submatrices are defined as

Q = diag(wey , weψ , ws, wu), R = diag(r, . . . , r). (15)

The size of Q is 4× 4 while R is Np×Np. Note also that wεis the weight for the slack variable and is a scalar. The matrixAeq along with the vector beq are attained by considering thesystem dynamics as

Z = Azt|t + BU + C (16)

where Z = (zTt+1|t,...,zT

Np|t)T and U = (ut|t, . . . , ut+Np−1|t).

The matrices A and B and the vector C are given as

A =

A

A2

...ANp

, B =

B 0 . . . . . . 0

AB B 0 . . . 0... AB

. . ....

......

. . . . . .ANp−1B ANp−2B . . . AB B

,

C =

C

C + AC...

C + AC, . . . ,+ANp−1C

equation (16) can be rewritten as

Z − BU = Azt|t + C. (17)

Note that z = (ZTUT )T hence the matrix Aeq and the vectorbeq can now be identified as

Aeq = [I− B], beq = Azt|t + C. (18)

As for the limitation on the input and steering angle the matrixA and the vector b are introduced. These are defined as

A =

0 . . . I0 . . . −I. . . A . . .

. . . A . . .0 . . . −1

, b =

umax...b

∆umax...−1

. (19)

The identity matrices are of size Np × Np. The last elementin both the matrix A and the vector b is for the non-negativityconstraint on the slack variable. The submatrix A of sizeNp × NpNs + Np + 1 along with the subvector b of sizeNp × 1 define the upper or lower constraints on the lateraldeviation necessary for for the vehicle to evade an obstacle. Iffor instance the center of the obstacle is aligned with the pathand the goal is to pass it from above there will be a positivebound on the lateral deviation equal to half of the width ofthe obstacle. As for the constraint on the control correctionvariable they are represented by the matrix A which is of sizeNp ×NpNs +Np + 1.

It is evident from Figure 5 and Table I that the QP formula-tion of the MPC problem is significantly faster than the initialone developed in YALMIP. It shows the time to completean optimization step for sampling time 0.05 s and with aprediction horizon of 20. Note that the first two iterationshave been excluded since they take significantly longer to runbecause of the initialization in MATLAB. Because of their shortduration they do not affect the performance of the controller.

0 50 100 150 200 250

Iteration [-]

0

20

40

60

80

100

120

140

160

180T

ime to r

un [m

s]

QP

YALMIP

Fig. 5. Time to complete optimization for each sampling step.

YALMIP QPMax [ms] 166 37Mean [ms] 142 18

TABLE ICOMPARISON OF PERFORMANCE OF SCRIPTS.

C. Evaluation of performance

In this section the results from both the simulation as wellas the experiments are presented. The performance of thecontroller was evaluated on basis of the requirements givenin the introduction. To start with the ability of the controllerto track a reference path was investigated. In these tests noobstacles were present.

1) Reference tracking: Figure 6 shows the path of thevehicle starting with a lateral offset of approximately 0.4 mfrom the reference line being the x axis. The experiments were

7

conducted for a set of different weights on the control input. Tobegin with the control correction variable was not limited, theprediction horizon was set to 25 and the mean speed, excludingthe short time where the vehicle accelerates, was about 0.55m/s. Only the lateral deviation was penalized with the weightwey=0.8. The objective of this test was to investigate the effectdifferent weights on the steering angle had on the performance.

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-0.4

-0.35

-0.3

-0.25

-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

y

Wu=0

Wu=0.01

Wu=0.1

Wu=0.3

Wu=0.5

Fig. 6. Experiment showcasing the effect an increasing weight on the controlinput has on the ability of the vehicle to track the reference.

In the first test represented by the blue line the controlinput was not penalized but limited by the steering servo to amaximum of 30◦. The result suggests that the vehicle travelswith an oscillatory movement along the path. This is causedby the fact that since the input is not penalized even a smalldeviation from the desired path will result in the controllerasking for maximum steering angle. This in combination withthe fact that the steering servo in reality has a limitation onthe change of rate cause the oscillations. If on the other handthe weight on the control input is increased the oscillationswill be dampened. However, it is evident that even a too largevalue will result in oscillations although with longer periods.Note that the size of the oscillations visible in curves forwu = 0.01 to wu = 0.3 can be deceiving. In reality theyare small and hardly visibly to the eye because of the smallsteering angles typically 1-2◦. The results indicate that bychoosing the weights carefully it might be possible to bringthe vehicle to the reference path and stabilize it without takingthe limitation in change of control input into account. Surelyas can be seen in Figure 7 this is in fact possible. In this casewu=0.0015, and the weight on the error in heading angle hadto be increased to 0.03. From these results it is evident thatthe controller is indeed able to cope with a disturbance in theform of an initial lateral deviation from the path. Furthermore,the settling distance is just about the required 0.8 m. Hence,the vehicle fulfills the requirements although with no marginof safety.

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-0.4

-0.35

-0.3

-0.25

-0.2

-0.15

-0.1

-0.05

0

0.05

0.1

y

Experiment

Simulation

Fig. 7. Comparison of experiment and simulation, vehicle starting with alateral offset of roughly 0.4 m.

In Figure 8 the control input and the error in headingangle as well as the velocity of the vehicle from both theexperiment and simulation are presented. Since the vehicledoes not accelerate in the simulation but starts instantly totravel with a constant velocity it reached the path faster. Thiscould also explain why it changes steering earlier than in theexperiment.

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-50

0

50

δ [°]

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-50

0

50

eψ [°]

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

0

0.5

1

v [m

/s]

Fig. 8. Comparison of steering angle, error in heading angle and velocity ofvehicle in both experiment (blue) and simulation (orange).

2) Obstacle avoidance: Having shown that the vehicle isable to successfully track a reference the next step is tointroduce obstacles in its path. Three boxes in the shape ofrectangles were used for this purpose.

Although the results so far suggest that it is possible totrack a reference and stabilize the vehicle without takingthe dynamics of the steering, namely limitation in change ofrate of steering, into account this will as shown here pose aproblem. In Figure 9 the path of the vehicle is illustrated for3 different cases. While the weights on the control input aswell as the states remain the same as in the previous tests i.e

8

wey = 0.8, weψ = 0 and wu = 0.1 the parameter that waschanged was the limitation on the control correction as well asthe prediction horizon. It is evident that an increase in ∆umaxwill cause a slower change of rate in the steering resulting ina larger lateral deviation when evading the obstacles. As theorange curve suggests the vehicle will at some point not beable to recover fast enough to avoid a collision with the nextobstacle. An increase in prediction horizon is on the otherhand enough to solve the problem.

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

y

umax

= , Np=25

umax

= /90, Np=25

umax

= /90, Np=35

Fig. 9. Experiment with obstacles for showing the effect the limitation onthe rate of change of the steering has on the vehicle. Red rectangles representthe obstacles while the line encircling them show the region which the centerof gravity of the vehicle must not enter to avoid collision.

However, the increase in prediction horizon with 10 unitscomes with the cost of a rise in the time to solve theoptimization problem. The average time increases from 28 msto 37 ms. While the mean value is below the limit of 50 msthere were still peaks which surpassed it. In this case theywere not many enough to affect the controller. However, if theprediction horizon is further increased the controller might notbe able to solve the optimization problem in time which willresult in no feedback and possibly a collision. With the samesets of parameters but an increase in the maximum allowedcontrol correction to pi/60 the vehicle was able to evade theobstacles. The results from the experiments along with thesimulation are presented in Figure 10.

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

y

Experiment

Simulation

Fig. 10. Comparison between results obtained from experiment and simula-tion.

The obtained results suggest that although the vehicle wasable to clear the obstacle there is a significant difference be-tween what the simulation predicts and the experiment. Whilethe simulation determines that the vehicle should be able tomeet the requirements on maximum clearing distance fromthe obstacles and the over/undershoot this is obviously notthe case in the experiment. The differences in the trajectoriesbetween the simulation and the experiments could be causedby modelling errors or inaccuracy in the linearized modelthat arise when the vehicle moves further away from thelinearization point. While the wheelbase could be determinedwith a fairly high accuracy the maximum rate of change inthe steering angle could not be decided with high accuracy.Therefore, it had to be estimated. Because of the uncertaintyit is likely subjected to error and could therefore be a possibleexplanation to the differences in the different trajectories. Toinvestigate the effect it has on the behaviour of the vehicle aseries of tests were run in the simulation. To further ensurethat the linearization is not to blame the tests were run withtwo different methods for linearization. The first being the lin-earization performed around a fixed point as described earlierwhile the second was performed around the last calculatedstate. As Figure 11 shows an increase in ∆umax results inlarger oscillations when turning. If increased too much thevehicle started departing further away from the linearizationpoint which resulted in the simulation crashing. On the otherhand, if the the linearization was performed for every samplinginstance the vehicle proved, as expected, to be able to copewith larger values on ∆umax. The difference in trajectoriesobtained with the two methods of linearization turned out notto be significant for ∆umax = π/60, the value used for thetest shown in Figure 9. Moreover, the differences between thetrajectories are most evident only for the last obstacle.

9

-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2

x

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

y

∆ umax

= π/60 fix

∆ umax

= π/60 mov

∆ umax

= π/80 fix

∆ umax

= π/80 mov

Fig. 11. Comparison of trajectories for linearization around fixed point andlinearization around last calculated states for different limits on ∆umax.

VII. CONCLUSION

The objective of this thesis was to develop a controller tobe used in an autonomous car for path tracking and obstacleavoidance. The vehicle should as put forward in this paperbe able to deal with initial lateral deviation and recover tothe desired path within a specified settling distance. All thiswhile clearing the obstacles and at the same time keepingthe distance to the reference path to a minimum. The resultsobtained from the simulations as well as the experiments showthat the vehicle in general failed to do so. It did manage tofulfill the requirement on settling distance in absence of anyobstacle. However, although it was able to clear the obstaclesit did not respect the limit on the clearing distance from theobstacles. It was higher than necessary.

This should be regarded as a feasibility study or an inves-tigation of the capabilities of a simple MPC. To determinewhether or not it is possible to control a small scale vehicleusing only a linear MPC, with no other components such asKalman filter, through MATLAB. It can be concluded, on thebasis of the obtained results, that even though it was able totrack the path and avoid the obstacles it did not exhibit thelevel of accuracy required for a real life scenario. The mainreasons why it is not suitable for controlling a vehicle in realityis partly because of the performance and the circumstancesunder which the tests were conducted. If scaled to the sizeof a real vehicle the results from the experiments suggestthat a vehicle would clear obstacles with a distance severaltimes larger than the width or length of the obstacle itself.Hence, the controller does not show the level of precisionrequired for dealing with a traffic situation on a highwaywhere the distance separating vehicles and/or obstacles issometimes less than a meter. Moreover, in a real scenariowhen travelling at high speed one would like to includean additional control variable being the acceleration of thevehicle. Simply relying on lateral control may not be enoughto avoid a collision without performing sudden turns at highspeed which pose a danger not only to passengers but also toother vehicles. Therefore longitudinal control should also be

taken into account for this purpose. Another important pointto be made here is the velocity with which the vehicle wastravelling in the test, around 0.5 m/s. Although the kinematicvehicle model allows for significantly higher velocities beforethe model becomes too inaccurate it is not suitable to use formodelling high speed maneuvers. At that point lateral forcesand interaction between the wheels and the ground would haveto be modelled as well.

Nonetheless, the results should be regarded as relativelygood for several reasons. First and foremost the vehicle thatwas used was an R/C car intended to be controlled manuallyby humans and not by a computer. Therefore one cannotexpect the steering to perform with high accuracy. Secondly,the MPC is sensitive to modeling errors and uncertainties.Recall that the vehicle was controlled solely with a linearMPC neither a robust MPC nor a Kalman filter was usedto deal with uncertainties and disturbances. Despite this thecontroller performed relatively well. If the accuracy of thecontroller, more specifically the large lateral deviations fromthe obstacles could be decreased it could possibly be used asan auto parking systems.

A. Future workAlthough one can further develop the concept by for

instance introducing a lidar device to detect and track theobstacles there is still room for improvements of the controller.First of all further investigation is needed to determine theunderlying cause to the differences in the trajectories betweensimulation and experiment. For this setup the vehicle wascontrolled through MATLAB running on a separate computer.Even though the objective was met controlling the vehicle inthis fashion proved not be optimal since MATLAB is relativelyslow compared to other languages such as Python and C.Therefore with a more efficient solver running onboard thevehicle one can expect the solving time to drop and as aconsequence of that the sampling time can be reduced whilethe prediction horizon can be increased for better performance.Using MATLAB for controlling a real vehicle is because of thecomputational burden not convenient.

In this case the velocity was considered to be constant whilein reality it differed slightly at each time instance. Even ifthe controller handled this well it i worth considering addingthe acceleration as an additional input to the system so thatthe velocity can be controlled. This would give the controlleranother control parameter to use in order to meet the objective.

B. AcknowledgmentFirst and foremost I would like to extend my sincere

gratitude to professor Jonas Martensson for giving me thepossibility to conduct this thesis and for supporting me. Iwould also like to thank Xiao Chen as well as Frank Jiangfor helping me with the implementation. Their expertise andadvice proved crucial for the success of this thesis. It has trulybeen a worthwhile experience. Last but not least thanks tomy friends who have always been by my side throughout theduration of this thesis. In particular, I wish to thank my dearfriend Federico Raiti who provided me with valuable adviceand support through these hard times.

10

REFERENCES

[1] H. Worley, “Road Traffic Accidents Increase DramaticallyWorldwide,” http://www.prb.org/Publications/Articles/2006/RoadTrafficAccidentsIncreaseDramaticallyWorldwide.aspx, 3 2006,[Online; accessed 22-January-2018].

[2] A. D. L. Christopher J.L. Murray, The Global Burden of Disease -A comprehensive assessment of mortality and disability from diseases,injuries, and risk factors in 1990 and projected to 2020. Harvard Schoolof Public Health, 1996.

[3] S. Chang and T. J. Gordon, “A flexible hierarchical model-basedcontrol methodology for vehicle active safety systems,” Vehicle SystemDynamics, vol. 46, no. S1, pp. 63–75, 2008.

[4] C. D. Harper, C. T. Hendrickson, and C. Samaras, “Cost and benefit es-timates of partially-automated vehicle collision avoidance technologies,”Accident Analysis & Prevention, vol. 95, pp. 104–115, 2016.

[5] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat, “Mpc-based approach to active steering for autonomous vehicle systems,”International Journal of Vehicle Autonomous Systems, vol. 3, no. 2-4,pp. 265–291, 2005.

[6] A. Broggi, P. Cerri, S. Debattisti, M. C. Laghi, P. Medici, M. Panciroli,and A. Prioletti, “Proud-public road urban driverless test: Architectureand results,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE.IEEE, 2014, pp. 648–654.

[7] T. Nothdurft, P. Hecker, S. Ohl, F. Saust, M. Maurer, A. Reschka,and J. R. Bohmer, “Stadtpilot: First fully autonomous test drives inurban traffic,” in Intelligent Transportation Systems (ITSC), 2011 14thInternational IEEE Conference on. IEEE, 2011, pp. 919–924.

[8] P. F. Lima, M. Trincavelli, M. Nilsson, J. Martensson, and B. Wahlberg,“Experimental evaluation of economic model predictive control for anautonomous truck,” in Intelligent Vehicles Symposium (IV), 2016 IEEE.IEEE, 2016, pp. 710–715.

[9] Z. Shiller and Y.-R. Gwo, “Dynamic motion planning of autonomousvehicles,” IEEE Transactions on Robotics and Automation, vol. 7, no. 2,pp. 241–249, 1991.

[10] Y. Kanayama and B. I. Hartman, “Smooth local path planning forautonomous vehicles,” in Autonomous robot vehicles. Springer, 1990,pp. 62–67.

[11] F. Kuhne, W. F. Lages, and J. G. da Silva Jr, “Model predictive controlof a mobile robot using linearization,” in Proceedings of mechatronicsand robotics, 2004, pp. 525–530.

[12] G. Klancar and I. Skrjanc, “Tracking-error model-based predictive con-trol for mobile robots in real time,” Robotics and autonomous systems,vol. 55, no. 6, pp. 460–469, 2007.

[13] K. Kanjanawanishkul and A. Zell, “Path following for an omnidirec-tional mobile robot based on model predictive control,” in Robotics andAutomation, 2009. ICRA’09. IEEE International Conference on. IEEE,2009, pp. 3341–3346.

[14] C. Liu, W.-H. Chen, and J. Andrews, “Optimisation based controlframework for autonomous vehicles: Algorithm and experiment,” inMechatronics and Automation (ICMA), 2010 International Conferenceon. IEEE, 2010, pp. 1030–1035.

[15] P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, and D. Hrovat, “Pre-dictive active steering control for autonomous vehicle systems,” IEEETransactions on control systems technology, vol. 15, no. 3, pp. 566–580,2007.

[16] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic anddynamic vehicle models for autonomous driving control design,” inIntelligent Vehicles Symposium (IV), 2015 IEEE. IEEE, 2015, pp. 1094–1099.

[17] P. Polack, F. Altche, B. d’Andrea Novel, and A. de La Fortelle, “Thekinematic bicycle model: A consistent model for planning feasibletrajectories for autonomous vehicles?” in Intelligent Vehicles Symposium(IV), 2017 IEEE. IEEE, 2017, pp. 812–818.

[18] P. F. Lima, “Optimization-based motion planning and model predictivecontrol for autonomous driving: With experimental evaluation on aheavy-duty construction truck,” Ph.D. dissertation, KTH Royal Instituteof Technology, 2018.

[19] R. Rajamani, Vehicle Dynamics and Control, ser. Mechanical Engineer-ing Series. Boston, MA: Springer US, 2006.

[20] F. Pfeiffer and R. Johanni, “A concept for manipulator trajectoryplanning,” IEEE Journal on Robotics and Automation, vol. 3, no. 2,pp. 115–123, 1987.

[21] J. V. Frasch, A. Gray, M. Zanon, H. J. Ferreau, S. Sager, F. Borrelli,and M. Diehl, “An auto-generated nonlinear mpc algorithm for real-timeobstacle avoidance of ground vehicles,” in Control Conference (ECC),2013 European. IEEE, 2013, pp. 4136–4141.

[22] Y. Gao, Model predictive control for autonomous and semiautonomousvehicles. University of California, Berkeley, 2014.

[23] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert, “Con-strained model predictive control: Stability and optimality,” Automatica,vol. 36, no. 6, pp. 789–814, 2000.

[24] P. Li, X. Yue, G. Luo, and T. Ma, “Rigid spacecraft attitude reorientationcontrol using receding horizon optimization,” in Control Conference(CCC), 2013 32nd Chinese. IEEE, 2013, pp. 2221–2226.

[25] B. P. Otta, “Numerical algorithms for quadratic programming for ap-proximated predictive control,” Ph.D. dissertation, Master Thesis, 2013.

TRITA EECS-EX-2018:654

ISSN 1653-5146

www.kth.se