Implementation of Model PredictiveControl for Path Following with the KTH Research Concept Vehicle

(1)

IN

DEGREE PROJECT DEGREE PROGRAMME IN ELECTRICAL ENGINEERING , SECOND CYCLE

300 CREDITS ,

STOCKHOLM SWEDEN 2015

Implementation of Model Predictive

Control for Path Following with the

KTH Research Concept Vehicle

PONTUS BELVÉN

(2)

Acknowledgments

To start with, I want to thank my supervisor Jonas M˚artensson for giving me the opportunity to do this master’s degree project. Furthermore, I would also like to thank Jonas for patiently giving valuable guidance and insights during the entire process of finishing this thesis.

(3)

Abstract

Trends in research show the interest of autonomously driving cars. One topic within au-tonomous driving is path following, which is the topic studied in this project. More specifically, development and implementation of model predictive control for path following. A number of model predictive controllers are developed. Their path following performance is studied and evaluated in simulations on flat, banked, and sloped road surfaces. The effects of using different levels of prediction model complexities are studied, and also the effects of two wheel steering compared to four wheel steering. The main focus with the design of the controller is for it to keep the vehicle as close as possible to the path while the ride comfort is maintained at an acceptable level. The simulation results show the advantages of using more complex pre-diction models, as well as the advantages of four wheel steering. The studies of the different model predictive control designs, result in that the most suitable controller is implemented, and the path following performance with the KTH Research Concept Vehicle is studied. The studies of the path following with the KTH Research Concept Vehicle show the great potential of using model predictive control for path following, and further improvements are proposed.

(4)

Sammanfattning

Trender inom forskning visar att det finns ett stort intresse för självkörande bilar. Ett omr˚ade inom självkörande fordon är “path following”, vilket är det som studeras i detta projekt. Mer specifikt utveckling och implementering av “model predictive control” för “path follow-ing”. Ett antal kontroller utvecklas och deras prestanda testas p˚a b˚ade platta och sluttande underlag. Effekterna av att använda olika niv˚aer av komplexitet hos prediktionsmodeller-nas och tv˚ahjulsstyrning jämfört med fyrhjulsstyrning studeras. Huvudfokuset med designen p˚a kontrollerna är att h˚alla fordonet s˚a nära den tänkta körvägen som möjligt samtidigt som ˚akkomforten h˚alls p˚a en acceptabel niv˚a. Simuleringsresultaten visar fördelarna med att använda en mer komplex prediktionsmodell och fyrhjulsstyrning. Studierna av de olika kon-trollerna resulterade i att den mest lämpliga implementerades och “path following” prestandan med KTH Research Concept Vehicle studerades. Resultaten med KTH Research Concept Ve-hicle visade stor potential och ytterligare förbättringar föresl˚as.

(5)

Nomenclature

αf, αr Front Wheel Slip Angle, Rear Wheel Slip Angle

δr

f, δrr Front Wheel Steering Angle References, Rear Wheel Steering Angle References

Φ, ¯Φ0 _{Objective Function, QP Objective Function}

ψr _{Yaw or Heading Angle References}

A, B Jacobian with Respect to z, Jacobian with Respect to u ar

x Longitudinal Acceleration References

Q, P , R State Weight Matrix, Terminal State Weight Matrix, Control Input Weight Ma-trix

u∗ _{Optimal Control Sequence}

vr

x, vry Longitudinal Velocity References, Lateral Velocity References

Xr_{, Y}r _{X-axis Coordinate References, Y-axis Coordinate References}

z, u State Vector, Control Input Vector

zr_{, u}r _{State References, Control Input References}

∆δf, ∆δr Front Wheel Steering Angle Change Rate, Rear Wheel Steering Angle Change

Rate

∆ax, ∆ay Longitudinal Jerk, Lateral Jerk

δf, δr Front Wheel Steering Angle, Rear Wheel Steering Angle

∆(X,Y ) Euclidean Distance Between Reference Points

˙

ψr _{Yaw Rate References}

˙

ψ Yaw Rate

Euclidean Distance Between the Current Position and the Closest Reference Point

ˆ

z Estimated Current States

φ, θ Road Banking angle, Road Slope Angle ψ Yaw or Heading Angle

ψ0 Initial Yaw or Heading Angle

ρ Air Density ˜

(7)

Af Projected Frontal Area of the Vehicle

ax, ay Longitudinal Acceleration, Lateral Acceleration

Cd Aerodynamic Drag Coefficient of the Vehicle

Cf, Cr Front Axle Cornering Stiffness, Rear Axle Cornering Stiffness

Fa Equivalent Longitudinal Aerodynamic Drag Force

Fb, Fs Force Due to Road Banking, Force Due to Road Slope

Ff, Fr Lateral Tire Force on the Front Wheel, Lateral Tire Force on the Rear Wheel

Fm Force Due to Motion Resistance

fr Tire Rolling Resistance

g Gravitational Constant

Hp, Hc Prediction Horizon, Control Horizon

Jz Inertia Around the Vehicle z-axis

lf, lr Length from the CoG to the Front Axle, Length from the CoG to the Rear Axle

m Total Mass of the Vehicle mj Unsprung Mass

nz, nu Number of States, Number of Control Inputs

Ts Sampling Time

vw Wind Speed

vx, vy Longitudinal Velocity, Lateral Velocity

X, Y X-axis Coordinates, Y-axis Coordinates

(8)

Chapter 1

Introduction

The trend in the automotive industry shows an increase in electronics and computers in cars. At the moment they are mostly used for active safety systems such as the electronic stability program (ESP) [1], or other driving assistance systems like the adaptive cruise control (ACC). Trends in research show that, in the future, these rather simple driving aids will be replaced by entirely automatic driving controls. Cars driving autonomously in urban environments is one of the most challenging topics in the field of intelligent transportation systems. However, the concern with safety and efficiency on the roads means that this is one of the most extensively researched areas in the aforementioned field [2].

1.1 Path Following

One of the topics of research within autonomous driving, is path following. This means that a predetermined path, constituted by checkpoints, is generated either offline or online. Each point on the path includes the needed information, for example position, speed and heading, to move the vehicle along the path in the desired way.

The information in each checkpoint can be recalculated online, by a path planner, based on measurements and the current state of the vehicle [3]. In the event that the path has to be changed, for example due to obstacles, the path planner could also account for that and change the path as necessary.

1.2 Motion Control

The goal for the motion control of the vehicle is to make the vehicle converge to the references of path and follow them as closely as necessary by the circumstances. This can be done by for example adjusting the steering angle of the wheels or the speed of the vehicle [3].

The motion control of the vehicle can be designed and implemented in many different ways. In [2], three fuzzy controllers are used to keep the vehicle on the path. One controller was used to adjust the lateral position deviation, and another to adjust the heading error from the reference path. The third controller is for controlling the longitudinal movements of the vehicle, and used the speed and acceleration of the vehicle as inputs. This is done to keep the reference speed of the path and to improve the comfort during speed changes. In [4], the authors use constant speed and calculate the difference between the heading of the vehicle and the bearing of the next checkpoint on the path. They then use a simple Proportional-Derivative (PD) controller to change the steering angle of the wheels to drive the difference between the heading of the vehicle and the bearing of the next checkpoint to zero.

(9)

1.3 Model Predictive Control

Due to advances in theory and computing systems, more advanced controllers such as model predictive control (MPC), can be used in real-time in a wider range of applications [3].

MPC is a type of controller in which the control inputs are acquired by solving a finite horizon optimal control problem, online, during each sampling period. It uses a model of the system to predict the evolution of the system, for a certain number of time steps, the prediction horizon. Based on the predictions, an objective function is minimized with respect to the future inputs and constraints. This results in an optimal control sequence of which the first control action is applied to the system. During the next sampling period this is repeated with updated system states and a shifted prediction horizon.

MPC is a controller that is widely used in motion control of autonomous vehicles. This is because of its capability to systematically handle nonlinear models and constraints, and operate close to the limits of admissible states and inputs [5].

1.4 Vehicle Modeling

There are several ways in which a vehicle can be modeled. One vehicle model is the so called bicycle model which is used in [3]. With the bicycle model it is assumed the right and left wheels can be modeled as a single wheel, which means that there is one wheel on the front and one on the rear axle. There are also four wheel vehicle models such as the modified bicycle model presented in [5]. At low speed however, a reasonable assumption is that the slip angles, lateral speed, and yaw rate are zero, and in this case a kinematic vehicle model is used instead [6].

1.5 KTH Research Concept Vehicle

In the vehicle industry there has also been great developments toward alternative fuels, among them electricity. Furthermore, there is also a need for alternative ways to transfer the power from the engine, and the actuation from the steering wheel, to the wheels. An alternative way to do this is to fit the vehicle with independently controllable actuators and motors on each wheel, and this is called autonomous corner module (ACM). It requires the propulsion to be electric and the actuation to be steer-by-wire. This means, that in addition to two wheel driving (2WD) and two wheel steering (2WS), this allows for four wheel driving (4WD) and four wheel steering (4WS). 4WD and 4WS give theoretical advantages with regards to vehicle controllability [7], which can become very helpful in an autonomous path following situation. The KTH Research Concept Vehicle (RCV) is a research and demonstration platform built and developed at the KTH Integrated Transport Research Lab as a collaboration between students and researchers from several schools and departments at KTH Royal Institute of Technology [8]. The RCV employs ACM [7], which, as stated above, can be advantageous in autonomous path following.

1.6 Related Work

A lot of work has been published were MPC is used for the steering of autonomous vehicles. Nonlinear MPC is used in some of the work, for example [1] and [9]. Although, linear time-varying (LTV) MPC, where the nonlinear vehicle models are continuously linearized, is the choice in a majority of the work. LTV MPC is, for example, used in [3], [5], and [10], and in these cases it is also common to recast the optimization problem on a quadratic programming (QP) form.

Another trend in using MPC for the steering of an autonomous vehicle is to use a bicycle model as the prediction model of the MPC, which is the case in [10], [11], and [12], and also,

(10)

in these cases the path following is performed at constant speed, and the vehicle steered only with the front wheels.

One thing that all of the aforementioned work has in common is that all the studies have been performed on flat surfaces only.

1.7 Problem Formulation

In this project, different strategies for designing and implementing MPC for path following are tested and evaluated. Four different MPC are designed and implemented. They differ in that they use different vehicle models as the prediction model, and they will use both 2WS and 4WS. With the 2WS the MPC controls the steering angle of the front wheels, and with 4WS the MPC controls the steering angle of the front and rear wheels separately. The vehicle models that are used are a kinematic bicycle model, and a dynamic bicycle model. This means that the first MPC uses a kinematic vehicle model and 2WS, the second a dynamic model with 2WS, the third a kinematic model with 4WS, and lastly, the fourth a dynamic model with 4WS.

Simulations are used to study the path following performance of the controllers. The simulated paths include paths on flat, as well as, sloped and banked road surfaces. The flat paths are used to test what affects the ride comfort to find out how to best design the MPC so that the ride comfort can be maintained. The sloped and banked paths are used to test how the vehicle is affected by it, and how well the MPC can control a vehicle when the road is no longer flat. The controllers are evaluated based on how comfortable the ride can be kept while the vehicle is following the path without deviating from it, more than accepted.

The most suitable, of the studied MPC designs, is implemented to control the RCV. It is done to test the performance of MPC for path following, in reality compared to simulations, and to find out what can be done to achieve acceptable path following if this is not the case.

(11)

Chapter 2

Model Predictive Control

In MPC the control action is obtained by, at each sampling instant, solving an optimal control problem. The optimal control problem is solved online over a finite horizon for which the initial state is the current state of the plant [13]. A model of the plant is used to predict the evolution of the system, for a certain number of time steps, the prediction horizon. Based on the predictions, an objective function is minimized with respect to the future inputs and constraints. This results in an optimal control sequence of which the first control action is applied to the system. During the next sampling period this is repeated with updated system states and a shifted prediction horizon, which is why it is also called receding horizon control (RHC) [14].

MPC is a controller that is widely used in motion control of autonomous vehicles. This is because of its capability to systematically handle nonlinear models and constraints, and operate close to the limits of admissible states and inputs [5]. Almost every system imposes constraints. The constraints are often related to physical limitations of the vehicle, such as the maximum steering angle of the wheels, but the constraints can also be with regards to safety and comfort. When handling constraints, MPC is one of few suitable controllers, which makes it a very important tool [13].

Although nonlinear MPC has been used in the literature, [14,15], only linear MPC is considered in this project. The computational complexity is much higher in the nonlinear case, which significantly reduces the prediction and control horizons which can be implemented. This limits the MPC to be used only with lower vehicle speeds. That is because the nonlinear programming problem that needs to be solved is nonconvex, has a larger number of decision variables, and a global minimum is generally impossible to find. Online successive linearizations of a nonlinear model can be performed, thus creating an LTV model. In the linear case, the optimization problem can be put on a QP form, with the control inputs as the decision variables, and since it is convex, it is easy and fast to solve [14]. The increased performance allows for longer prediction and control horizons, and the vehicle can be driven at higher speeds.

2.1 General Model Predictive Control

In MPC, the system to be controlled is usually described or approximated by an ordinary differential equation. Since the control usually is piecewise constant, the system is modeled by the difference equation

z(k + 1) = f (z(k), u(k)), (2.1) where z(k)∈ Rnz _{is the state vector, and u(k)}∈ Rnu _{is the control input vector [}₁₃_{]. This}

(12)

The MPC is based on solving an optimization problem during each sampling period. The optimization problem is generally on the form

min

u(k) Φ(k), (2.2a)

s. t. z0= z(k), (2.2b)

z(k + 1) = f (z(k), u(k)) , (2.2c)

u(k)∈ U. (2.2d)

Φ is the objective function to be minimized, the initial condition z0is the state measured at the

current instant, (2.2c) represents the model used for system behavior prediction, and (2.2d) rep-resents constraints on the control inputs [14]. Minimizing the objective function, with control horizon Hc, will result in the optimal control sequence u∗(k) ={u∗(0), u∗(1), . . . , u∗(Hc−1)},

where the first control u∗_{(0) is applied to the system.}

The control objective is usually to steer the states to the origin, or to steer it to reference states zr_{, often referred to as reference tracking [}₁₃_].

2.2 Reference Tracking Model Predictive Control

To use MPC for reference tracking, the formulation in (2.2) has to be modified. This is done making the following modifications. When MPC is used for tracking, an error model of the system is used, and in discrete time it can be expressed as

˜

z(k + 1) = A(k) ˜z(k) + B(k) ˜u(k). (2.3) The error between the states z(k) and the state references zr_{(k), is represented by ˜}_{z(k) =}

z(k)_{− z}r_{(k), and ˜}_{u(k) = u(k)}

− ur_{(k) represents the error between the control inputs u(k)}

and the control input references ur_{(k). A(k)}

∈ Rnz×nz _{and B(k)}_{∈ R}nz×nu_{, which are given}

by A(k) = ∂f (z(k), u(k)) ∂z(k) z(k)=zr_(k) u(k)=ur_(k) , (2.4a) B(k) = ∂f (z(k), u(k)) ∂u(k) z(k)=zr_(k) u(k)=ur_(k) , (2.4b)

are the Jacobians with respect to z(k) and u(k), respectively, of f (z(k), u(k)), evaluated around the references zr_{(k) and u}r_{(k) [}₁₄_].

The use of MPC for reference tracking, requires the cost function to be formulated such that the deviation from reference states and control inputs is penalized. This leads the cost function to be formulated as ˜ Φ(k) = H_Xp−1 i=1 ˜ z|_{(k + i)Q ˜}_{z(k + i) + ˜}_z|_{(k + H} p)P ˜z(k + Hp) (2.5) + HXc−1 i=0 ˜

u|_{(k + i)R ˜}_{u(k + i),}

where Hp and Hc is the prediction horizon and control horizon, respectively. Furthermore,

Hc < Hp, and the control input is assumed constant for all i ≥ Hc [16]. Q ∈ Rnz×nz, is

the state penalization matrix, P ∈ Rnz×nz _{is the terminal state penalization matrix, and}

(13)

The control input constraints also have to be modified. With uminas the minimum control

input and umaxas the maximum control input, the constraints can be expressed as

˜

umin(k) = umin− ur(k), (2.6a)

˜

umax(k) = umax− ur(k). (2.6b)

The rate at which the control inputs are allowed to change can also be constrained. This is done by introducing

∆ ˜u(k) = ˜u(k)_{− ˜}u(k_{− 1),} (2.7a) ∆ ˜umin(k) = ∆umin− (ur(k)− ur(k− 1)), (2.7b)

∆ ˜umax(k) = ∆umax− (ur(k)− ur(k− 1)), (2.7c)

where ∆umin and ∆umax are the lower and upper bounds, respectively, for how much the

control inputs are allowed to change from one sampling instant to the next. The control input constraints can now be formulated as

˜

umin≤ ˜u(k)≤ ˜umax, (2.8a)

∆ ˜umin ≤ ∆ ˜u(k)≤ ∆ ˜umax. (2.8b)

With these modifications, the problem can now be formulated on the standard MPC form as min ˜ u(k) ˜ Φ(k), (2.9a) s. t. z˜0= ˜z(k), (2.9b) ˜ z(k + i + 1) = A(k + i) ˜z(k + i) + B(k + j) ˜u(k + j), (2.9c) ˜

umin≤ ˜u(k + j)≤ ˜umax, (2.9d)

∆ ˜umin ≤ ∆ ˜u(k + j)≤ ∆ ˜umax, (2.9e)

(2.9f) where i _{∈ [0, H}p− 1], j ∈ [0, Hc− 1], and the initial condition ˜z0, is the error between the

reference states and the measured states, ˜z0= z(k)− zr(k), at the current instant k.

When the MPC is formulated in this manner, it becomes evident that the objective function is minimized when the deviation from the reference states and control inputs is minimized. This results in ˜u∗_{(k) =}_{{ ˜}_u∗_{(0), ˜}_u∗_{(1), . . . , ˜}_u∗_(Hc_{− 1)}, and the first control u}∗_{(0) = ˜}_u∗_{(0) + u}r₍₀₎

is applied to the system.

2.3 Quadratic Programming Form

Optimization problems where the objective function is convex and quadratic, and the con-straints are linear, are referred to as QP problems. They are generally on the form

min v 1 2v |_{Hv + f}|_v, _(2.10a) s. t. Av≤ b, (2.10b) Aeqv = beq, (2.10c)

(14)

To put the MPC for tracking formulation on QP form, the following vectors need to be introduced ˜ Z(k + 1) =      ˜ z(k + 1) ˜ z(k + 2) .. . ˜ z(k + Hp)     , U (k) =˜      ˜ u(k) ˜ u(k + 1) .. . ˜ u(k + Hc− 1)     . (2.11) Also, by introducing ¯Q = diag (Q; . . . ; P ), and ¯R = diag (R; . . . ; R), the objective function in (2.5) can be rewritten as

Φ(k) = ˜Z|_{(k + 1) ¯}_{Q ˜}_{Z(k + 1) + ˜}_U|_{(k) ¯}_{R ˜}_{U (k),} _(2.12)

[14]. The predicted states over the prediction horizon according to (2.3), can be shown to be

˜

z(k + 1) =A(k) ˜z(k) + B(k) ˜u(k), ˜

z(k + 2) =A(k + 1) ˜z(k + 1) + B(k + 1) ˜u(k + 1)

=A(k + 1)A(k) ˜z(k) + A(k + 1)B(k) ˜u(k) + B(k + 1) ˜u(k + 1), ..

. (2.13)

˜

z(k + Hp) =A(k + Hp− 1)A(k + Hp− 2) . . . A(k)˜z(k)

+ A(k + Hp− 1)A(k + Hp− 2) . . . A(k + 1)B(k) ˜u(k)

+ A(k + Hp− 1)A(k + Hp− 2) . . . A(k + 2)B(k + 1) ˜u(k + 1)

+ . . .

+ A(k + Hp− 1) . . . A(k + Hc)B(k + Hc− 1) ˜u(k + Hc− 1).

[19]. This implies that, by introducing

¯ A =        A(k) A(k + 1)A(k) .. . γ(k, 2, 0) γ(k, 1, 0)        , (2.14a) ¯ B =        B(k) 0 . . . 0 A(k + 1)B(k) B(k + 1) . . . 0 .. . ... . .. ... γ(k, 2, 1)B(k) γ(k, 2, 2)B(k + 1) . . . 0 γ(k, 1, 1)B(k) γ(k, 1, 2)B(k + 1) . . . γ(k, 1, Hc)B(k + Hc− 1)        , (2.14b) where γ(k, j, l) = l Y i=Hp−j A(k + i), (2.15)

(2.3) can be rewritten as a function of the initial state error ˜z(k), as

˜

(15)

After some algebraic manipulations with the help of (2.12) and (2.16), the objective func-tion (2.5) can be rewritten on standard QP form as

¯ Φ(k) = 1 2U˜ |_{(k)H(k) ˜}_{U (k) + f}|_{(k) ˜}_{U (k) + d(k),} _(2.17) where H(k) = 2 ¯B|_{(k) ¯}_{Q ¯}_{B(k) + ¯}_R_, _(2.18a) f (k) = 2 ¯B|(k) ¯Q ¯A(k) ˜z(k), (2.18b) d(k) = ˜z|_{(k) ¯}_A|_{(k) ¯}_{Q ¯}_{A(k) ˜}_z(k). _(2.18c)

H(k) is a positive definite matrix and describes the quadratic part of the objective function, and f (k) describes the linear part. The term d(k) is independent of the decision variable

˜

U (k), hence it will not influence the minimization of the objective function. This means that (2.17) can be rewritten as ¯ Φ0(k) = 1 2U˜ |_{(k)H(k) ˜}_{U (k) + f}|_{(k) ˜}_{U (k),} _(2.19) [14].

The control input constraints in (2.8) will also have to be written on QP form. This is done by rewriting them on the form

D ˜U (k)_{≤ d(k),} (2.20) where D =     I −I I −I     , (2.21a) d(k) =      ˜ Umax(k) − ˜Umin(k) ∆ ˜Umax(k) + ˜U (k− 1) −∆ ˜Umin(k) + ˜U (k− 1)     , (2.21b) and I is the identity matrix.

The MPC problem can now be put on the standard QP form:

min ˜ U(k) ¯ Φ0(k), (2.22a) s. t. D ˜U (k)_{≤ d(k).} (2.22b) Constraints for initial condition and model will no longer have to be included because they are implicitly included in the objective function (2.19) [14]. The objective function is minimized by ˜U∗_{(k), and u}∗_{(0) = ˜}_u∗_{(0) + u}r_{(0) is applied to the system.}

(16)

Chapter 3

Vehicle Modeling

An MPC requires a model of the system, to use as the prediction model, which is used to predict the evolution of the system so that an optimal control input can be applied.

There are several different models to describe the dynamics of a car-like ground vehicle. There are two and four wheel models, kinematic and dynamic models, and the dynamics of the tires can be described by linear and nonlinear models [5,6].

3.1 Bicycle Model

A two wheel vehicle model is the so called bicycle model. In the bicycle model, the left and right wheels at the front and the rear are represented by one wheel on the front axis and one wheel on the rear axis as seen in Figure3.1. In practice, this means that it is assumed that the distance from the ground to the center of gravity (CoG) is zero [20].

3.1.1 Kinematic Vehicle Model

A kinematic vehicle model provides a mathematical description of a vehicle without taking into account the dynamic forces that affect its motion. It purely describes the geometric relationships governing the system. For lower vehicle velocities, from around 5 ms−1 _and

lower, it can be assumed that the lateral slip angles of the wheels are equal to zero. This means that the velocity vector at each wheel is in the direction of the wheel. A kinematic vehicle model can be described by the following equations:

˙

ψ = vx lf+ lr

(tan (δf) + tan (δr)) , (3.1a)

˙

X = vxcos (ψ) , (3.1b)

˙

Y = vxsin (ψ) , (3.1c)

[6]. ψ is the yaw angle, or heading, which is the angle between the X axis and the orientation of the vehicle, X and Y are the global X and Y axis coordinates, respectively. vx is the

longitudinal velocity, and lf and lrare the distances from the front and rear axles to the CoG,

(17)

X Y CoG lr lf ψ vx δf δr

Figure 3.1: Kinematic bicycle model of vehicle motion, which shows how vx, ψ, X, Y , δf, δr,

lf, and lr are defined.

3.1.2 Dynamic Vehicle Model

For higher velocities, from around 5 ms−1 _{and higher, the assumption that the lateral slip}

angles are zero can no longer be made. Instead the slip angles are given by

αf = tan−1 vy+ lf ˙ ψ vx ! − δf, (3.2a) αr= tan−1 vy− lr ˙ ψ vx ! − δr, (3.2b)

vy is the lateral velocity, and ˙ψ is the yaw rate.

For these cases the kinematic model is no longer sufficient and a dynamic vehicle model must be used instead. With nonzero slip angles, the lateral tire forces Ff and Fr, seen in

Figure3.2, arise. The lateral tire forces are assumed to be linear functions of the slip angle α, which has proven to be accurate for small slip angles, and given by

Ff =−Cfαf, (3.3a)

Fr=−Crαr, (3.3b)

where Cf and Cr are the cornering stiffness of the front and rear axles, respectively. The

(18)

X Y CoG lr lf ψ ˙ ψ vx vy δf αf δr αr Fr Ff

Figure 3.2: Dynamic bicycle model of vehicle motion, which shows how vx, ψ, X, Y , vy, ˙ψ,

δf, δr, αf, αr, Ff, Fr, lf, and lr are defined.

˙vx= −F fsin (δf) + Frsin (δr)− Fa− Fm m + ˙ψvy, (3.4a) ˙ ψ = vx lf+ lr (tan (δf) + tan (δr)) , (3.4b) ˙ X = vxcos (ψ)− vysin (ψ) , (3.4c) ˙ Y = vxsin (ψ) + vycos (ψ) , (3.4d) ˙vy= Ffcos (δf) + Frcos (δr) m − ˙ψvx, (3.4e) ¨ ψ = lfFfcos (δf)− lrFrcos (δr) Jz , (3.4f)

defined as in Figure3.2[7]. Fa is the equivalent longitudinal aerodynamic drag force, and Fm

is the force due to motion resistance. They are given by

Fa = 1 2ρCdAf(vx+ vw) 2 , (3.5a) Fm= mgfr, (3.5b)

where ρ is the air density, Cd is the aerodynamic drag coefficient of the vehicle, Af is the

projected frontal area of the vehicle, vwis the wind speed, g is the gravitational constant, and

fr is the tire rolling resistance of the vehicle [6].

3.2 KTH Research Concept Vehicle

The RCV, shown in Figure3.3, is a research and demonstration platform concerning sustain-able transport. It was built and developed at the KTH Integrated Transport Research Lab as a collaboration between students and researchers from several schools and departments at KTH Royal Institute of Technology [8]. The RCV employs ACM, which means that motors,

(19)

Figure 3.3: The KTH Research Concept Vehicle.

actuators, suspension, and gear are integrated in one modular unit which is fitted in each cor-ner. This makes the RCV very versatile, because each wheel can be controlled independently. This allows for, for example, 2WD, 4WD, 2WS, and 4WS [7]. The vehicle specifications for the RCV are listed in Table3.1.

Table 3.1: The RCV vehicle specifications.

Symbol Nomenclature Value Unit m Total mass including two passengers 600 kg

mj Unsprung mass 25 kg

Jz Inertia around the z-axis 1181 kgm2

Cf/Cr Cornering stiffness 50 kN/rad

lf Length from the CoG to the front axle 1.1 m

lr Length from the CoG to the rear axle 0.9 m

fr Tire rolling resistance 0.0175

Af Projected frontal area 2.25 m2

Cd Aerodynamic drag coefficient 1.3

δf/δr Steering angle [-0.24, 0.24] rad

(20)

Chapter 4

Path Following

One of the topics of research within autonomous driving, is path following. This means that a predetermined path, constituted by reference points, is generated offline or online. Each point on the path includes the needed information, for example position, velocity, and heading, to move the vehicle along the path in the desired way.

Different MPC designs are implemented to study how they perform when following differ-ent paths. The first MPC uses the kinematic vehicle model, as the prediction model, with longitudinal acceleration axand front wheel steering angle δf as the control inputs, which will

be referred to as the Kinematic 2WS MPC. The second one uses the dynamic vehicle model in the MPC with ax and δf as the control inputs, the Dynamic 2WS MPC. The third one

uses the kinematic vehicle model with ax, δf, and rear wheel steering angle δr as the control

inputs, the Kinematic 4WS MPC. Finally, the fourth one uses the dynamic vehicle model with ax, δf, and δr, as the control inputs, the Dynamic 4WS MPC.

The different designs are evaluated based on how small the deviation can be kept, while keeping the ride comfortable, on flat, sloped, and banked roads. The different tests and simulation results are presented in Section4.6.

The most suitable of the studied MPC designs is implemented for path following with the RCV, to study how it performs in reality compared to in simulation, and the studies are presented in Section4.7.

4.1 Model Predictive Control

MPC is a good choice for path following, because of its ability to predict the evolution of the system to determine optimal control inputs, and the ease of implementing constraints.

4.1.1 Formulation

To use MPC for path following it needs to be formulated in the same way as the reference tracking MPC in Section2.2, and the problem is also put on QP form as described in Section

2.3. All simulations are run in Matlab, and the QP problem is solved using the tool CVXGEN [21].

4.1.2 Vehicle Models

In MPC a model of the system is used to predict the evolution over the prediction horizon. Two different vehicle models are used as the MPC prediction model, and they are the discrete time representation of the kinematic vehicle model in (3.1), and the discrete time representation of the dynamic bicycle model in (3.4), with the RCV specifications in Table3.1.

Because ax is one of the control inputs, a state for the longitudinal velocity is added to

(21)

[X0, Y0, ψ0] _Path

Generator

[Xr_{, Y}r_]

Controller u∗ Vehicle z _EstimatorState

ˆ z Controller [Xr_{, Y}r ] Reference Generator [Zr_{, U}r_] ˆ z Ur Zr

Linearize [A, B] QP Form ¯Φ 0_{, D, d}

QP Solver U˜∗ + u∗

Figure 4.1: An overview of the system, showing inputs to, and outputs from, all of the parts of the system.

longitudinal dynamics. Both 2WS and 4WS are used, which means that δr= 0 when 2WS is

used, and both δf and δr are control inputs for 4WS.

4.1.3 Discretization and Linearization

All of the vehicle models used are on discrete time form. The discrete time vehicle models are approximated from (3.1) and (3.4) by using the Euler method, with sampling time Ts,

resulting in the discrete time kinematic vehicle model

vx(k + 1) = vx(k) + axTs, (4.1a) ψ(k + 1) = ψ(k) + vx(k) lf+ lr(tan (δf)− tan (δr)) Ts, (4.1b) X(k + 1) = X(k) + (vx(k) cos (ψ(k))) Ts, (4.1c) Y (k + 1) = Y (k) + (vx(k) sin (ψ(k))) Ts, (4.1d)

and the discrete time dynamic vehicle model vx(k + 1) = vx(k) + ax+−F fsin (δf) + Frsin (δr)− Fa− Fm m + mj + ˙ψ(k)vy(k) Ts, (4.2a) ψ(k + 1) = ψ(k) + _v x(k) lf+ lr (tan (δf)− tan (δr)) Ts, (4.2b) X(k + 1) = X(k) + (vx(k) cos (ψ(k))− vy(k) sin (ψ(k))) Ts, (4.2c) Y (k + 1) = Y (k) + (vx(k) sin (ψ(k)) + vy(k) cos (ψ(k))) Ts, (4.2d) vy(k + 1) = vy(k) + Ffcos (δf) + Frcos (δr) m + mj − ˙ ψ(k)vx(k) Ts, (4.2e) ˙ ψ(k + 1) = ˙ψ(k) + lfFfcos (δf)− lrFrcos (δr) Jz Ts. (4.2f)

To put the MPC on an LTV form, the discrete vehicle models are linearized. This is done, every sampling period, by calculating the jacobians as in (2.4), evaluated around the reference states and control inputs for the entire prediction and control horizons, respectively. Linearizations are performed in the the Linerize block in the Controller, as seen in Figure4.1.

(22)

−0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 0 0.5 1 1.5 2 2.5 3 3.5 4 [Xr_{(n), Y}r_(n)] [X0, Y0] X [m] Y [m]

Figure 4.2: An example of a path from initial position [X0, Y0], generated with ∆(X,Y ) = 0.1

m, showing the position references [Xr_{(n), Y}r_(n)].

4.1.4 Reference Generation

Proper references for each state and control input are needed, every sampling period over the horizon, so that the deviation from them can be minimized in the MPC. Poor references means that the control sequence from the MPC will not minimize the deviation from the desired path, hence leading to sub-optimal path following.

The reference path used in the path following is generated offline, and in the overview of the system, in Figure 4.1, it is referred to as the Path Generator, but the path can also be generated online by continuously generating parts of the path for a certain distance ahead. It is done by arbitrarily placing reference points, uniformly spaced with the euclidean distance ∆(X,Y ) m, into the desired path. This results in the position references [Xr, Yr], in meters

from the initial position [X0, Y0], as illustrated by Figure4.2.

From the position references, the remaining state and control input references are calculated online during each sampling period, in what is referred to as the Reference Generator, in Figure

4.1. They are derived from the discrete-time representation of the kinematic vehicle model (4.1). The references for the heading, or yaw angle ψr_{, are obtained by calculating the angle}

between subsequent reference points as

ψr(n) = tan−1

_Yr_{(n + 1)}_{− Y}r_(n)

Xr_{(n + 1)}_{− X}r_(n)

. (4.3)

From ψr_{, the yaw rate references ˙}_ψr_{can be calculated as}

˙ ψr(n) = (ψ r_{(n + 1)} − ψr_{(n)) v}r x(n) ∆(X,Y ) . (4.4) With δr

r= 0, (4.1b) is used to get the front wheel steering angle references δfr, as

δfr(n) = tan−1 _(l f+ lr) (ψr(n + 1)− ψr(n)) ∆(X,Y ) , (4.5)

(23)

and similarly, δr r=−δfr, leads δfr to be δfr(n) = tan−1 (lf+ lr) (ψr(n + 1)− ψr(n)) 2∆(X,Y ) . (4.6)

The references for the lateral velocity vr

y(n) = 0, because keeping the lateral velocity down

means that the deviation from the path is easier to keep down and the ride is more comfortable. For the purpose of comparing the different MPC design strategies, the longitudinal velocity references vr

x(n) are kept constant for the entire paths, and the longitudinal acceleration

references ar

x(n) = 0.

By using the equations in (4.3) - (4.6), it is possible to acquire references for all of the states and control inputs for each of the reference points. To find the appropriate references, at each sampling instant, the reference point on the path with the shortest euclidean distance to the current position is found. When the closest reference point on the path is known, Hp reference points ahead are used for calculating the state and control input references for

the entire prediction horizon. When finding the Hp reference points, not every consecutive

reference point is used. Based on the current velocity vx(k), the distance between the reference

points ∆(X,Y ), and the sampling time Ts, the reference points to use are determined by

ζ = _v x(k)Ts ∆(X,Y ) . (4.7)

With [Xr_{(n), Y}r_{(n)] as the closest reference point to the current position, the reference points}

[Xr_{(n + ξ), Y}r_{(n + ξ)], where ξ}

∈ [0, ζHp− ζ], are used to calculate references for the states

and control inputs with (4.3) - (4.6). This results in Zr₌

{zr_{(n), z}r_{(n + ζ), . . . , z}r_{(n + ζH} p−

ζ)_{}, and U}r₌

{ur_{(n), u}r_{(n+ζ), . . . , u}r_(n+ζH

p−ζ)}, which are outputted from the Reference

Generator block, as shown in Figure4.1.

4.1.5 State Estimation

The MPC relies on getting the current state of the vehicle at each sampling instant, because it is used as the initial condition for the optimization problem. At the beginning of each sampling period the current state is sent to the controller. In simulations the current states can be sent flawlessly from the simulation model to the controller. However, this is not the case in reality, where the states have to be measured with different kinds of sensors. The sensors include, for example, GPS, which can give the velocity, heading, and position of the vehicle. The sensor data from a GPS can be combined with the sensor data from, for example, an inertial measurement unit (IMU) to get more accurate estimations of the current state of the vehicle. State estimations are performed in the State Estimator block, in Figure4.1, which sends the estimated current states ˆz(k) to the controller.

4.2 Ride Comfort

Ride comfort is hard to quantify, because it is subjective what is comfortable and what is not. When designing highways, two key aspects of keeping the ride comfort, is to design them so that the lateral acceleration and lateral jerk are kept below certain values. According to [22], for the lateral acceleration it varies between 1.47_{− 2.45 ms}−2_{. For the lateral jerk, which is}

the time derivative of the lateral acceleration, it is between 0.3_{− 0.9 ms}−3_.

Similarly, longitudinal acceleration and longitudinal jerk also impact the ride comfort. With the longitudinal acceleration ax as one of the control inputs, staying within the comfortable

limits for longitudinal acceleration and jerk is easy. This is because of the ease of directly constraining axand the rate at which it can change per second, ∆ax, in the MPC.

Staying within the limits for lateral acceleration ayand jerk ∆ayis more difficult, because

(24)

speed into corners and by constraining the rate at which the front and rear wheel steering angles are allowed to change, ∆δf and ∆δr, respectively.

4.3 Banked Road

The banking of the road can be modeled as a force acting on the vehicle, which affects the lateral dynamics. This is done by modifying (4.2e) to

vy(k + 1) = vy(k) + _F fcos (δf) + Frcos (δr) + Fb m + mj − ˙ ψ(k)vx(k) Ts, (4.8)

where the force due to banking is

Fb = (m + mj)g sin(φ), (4.9)

g is the gravitational constant, and φ is the banking angle [6].

4.4 Sloping Road

The slope of the road can be modeled as a force acting on the vehicle, which affects the longitudinal dynamics. This is done by modifying (4.2a) to

vx(k + 1) = vx(k) + ax+−Ff sin (δf) + Frsin (δr)− Fa− Fm− Fs m + mj + ˙ψ(k)vy(k) Ts, (4.10) where the force due to sloping is

Fs= (m + mj)g sin(θ), (4.11)

g is the gravitational constant, and θ is the sloping angle [6].

4.5 Sampling Time

The choice of sampling time influences the horizon, which is the distance on the path which is used to predict the behavior of the vehicle. A longer sampling period means that the horizon is increased, but leads to that the discretized prediction model is less accurate. Having a longer sampling period also means that it may be possible to increase the prediction and control horizons to further increase the horizon on the path. This means that a shorter sampling period leads to a shorter horizon, which may be further reduced because it may be necessary to decrease the prediction and control horizons. The benefit of a shorter sampling period is the improved accuracy of the prediction model.

The shortest feasible sampling time, for the implementation with the RCV, is 0.05 s, see Section 4.7.2. The sampling time 0.01 s discarded, because it leads to a horizon that is too short and it is far from a feasible sampling time for the implementation with the RCV. In order to have a more feasible sampling time and a longer horizon, while keeping the prediction accuracy at an acceptable level, 0.03 s is chosen as the sampling time for the simulation studies.

(25)

4.6 Path Following Simulation Results

Two path following simulation tests are performed to evaluate the performance of the various MPC designs, including the Kinematic 2WS MPC, the Dynamic 2WS MPC, the Kinematic 4WS MPC, and the Dynamic 4WS MPC. The tests include tests on a flat, sloped and banked roads, with and without disturbances.

One test is a u-turn test on a flat road. The purpose of the u-turn test on a flat road, referred to as the u-turn test, is done to the evaluate the performance by testing how sharply a u-turn with constant curvature can be made without causing the position deviation to be larger than 0.3 m and keeping the lateral acceleration ay below 1.47 ms−2, which is considered

the maximum to maintain the ride comfort. The deviation is the euclidean distance to the reference point closest to the current position. A u-turn with a constant curvature results in a step in the steering angle reference, and the radius for the different paths is determined by a percentage of the maximum front wheel steering angle, for example 45% of the maximum front wheel steering angle results in the path in Figure4.3, which has 18.45 m radius u-turn. Tests are run at 13.89 ms−1 _{(50 kph), without disturbances, and the steering angle change rates}

∆δf and ∆δrare constrained to make the ride as comfortable as possible without causing the

vehicle to deviate more than allowed.

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30 X [m]

Y

[m]

Figure 4.3: A path with a u-turn equivalent to 45% of a full turn with 18.45 m radius. Roads are not designed as the path in Figure 4.3 where the change in curvature, in the transition from the straight to the turn, results in a step in the steering angle reference. The transition is smooth, the curvature changes linearly, resulting in a steering angle reference that also changes linearly, and results in a path like in Figure4.4.

0

10

20

30

40

50

60

70

80

90 100 110 120 130

0

10

20

30

40 X [m]

Y

[m]

(26)

Other tests are s-turn tests, which are performed on path with linearly changing curvature, referred to as the s-turn tests, with the purpose to test how the MPC implementations handle measurement disturbances. This is done by adding simulated disturbances to the each of the states that are sent to the MPC, corresponding the the accuracy of the sensors used in the experimental test, see Section4.7.1. The standard deviation σ of the added disturbances are shown in Table4.1.

Table 4.1: The accuracy of the sensors used.

vxms−1 ψ [rad] X [m] Y [m] vyms−1 ψ [rad/s]˙

σ 0.1 0.018 0.015 0.015 0.1 0.018

To the position, X and Y , there is also added measurement uncertainty of the network RTK of± 0.025 m.

A test in which a u-turn is performed on a downhill slope is also conducted. The purpose with the u-turn test on a downhill slope is done to study the performance when the road is no longer flat. The performance is evaluated based on how low the deviation can be kept while keeping the lateral acceleration below 1.47 ms−2. Entering at 13.89 ms−1 (50 kph), a u-turn is performed on a 10% downhill slope.

The paths used in the simulations are generated with ∆(X,Y ) = 0.01 m, the sampling time

used is Ts = 0.03 s, and the prediction and control horizons are, Hp = 24 and Hc = 19,

respectively. All simulations are run with the discrete time kinematic vehicle model, in (4.1), for velocities below 5 ms−1_{, and the discrete time dynamic vehicle model, in (}_4.2_{), modified}

to include the forces due to road banking and slope as shown in Section 4.3 and Section

4.4, for velocities above 5 ms−1_{, as the simulation models. The simulation models have the}

specifications of the RCV, in Table3.1, and the wind speed vw= 0. The constraints that are

used on the steering angles δf and δr are due to physical limitations of the RCV, presented

in Section4.1.2, which means that means that−0.24 ≤ δf, δr≤ 0.24 rad. Constraints on the

steering angles change rates ∆δf and ∆δrare determined to make the ride comfort, described

in Section4.2, as good as possible, which is different from case to case. The constraints on the longitudinal acceleration ax are due to ride comfort, presented in Section4.2, which means

that−2.45 ≤ ax≤ 2.45 ms−2, and−0.9 ≤ ∆ax≤ 0.9 ms−3.

4.6.1 Kinematic 2WS MPC and Dynamic 2WS MPC

To study the performance of the Kinematic 2WS MPC compared to the Dynamic 2WS MPC several tests are conducted. They include tests on flat, sloped, and banked roads, with and without disturbances.

U-Turn on a Flat Road

At 13.89 ms−1_{(50 kph) the Kinematic 2WS MPC is able to perform a u-turn which is 45% of}

the maximum steering angle, with the radius 18.45 m, without disturbances. It results in the reference path [Xr_{, Y}r_{] and the driven path [X, Y ], as seen in Figure} _4.5_{, with a maximum}

lateral acceleration ay of 1.46 ms−2, as shown by Figure 4.6, and a maximum deviation of

0.25 m, shown in Figure4.7. To keep ay below the comfort limit, the steering angle change

rate is constrained to_{−0.15 ≤ ∆δ}f ≤ 0.15 rad/s. The MPC is tuned such that the state and

terminal state weight matrices are Q = P = diag ([1, 2000, 100, 100]) and the control input weight matrix is R = diag ([1, 1]). The states are in the same order as in (4.1), and longitudinal acceleration ax is the first control input, and front wheel steering angle δf the second.

The Dynamic 2WS MPC can perform a u-turn 47% of a full turn, at 50 kph, which gives a curve radius of 17.66 m. The reference path [Xr_{, Y}r_{] and the driven path [X, Y ] are seen}

in Figure4.5. It results in the maximum lateral acceleration ay 1.34 ms−2, as displayed in

Figure 4.6, and the maximum deviation 0.30 m, as displayed in Figure 4.7. The steering angle change rate is constrained to_{−0.14 ≤ ∆δ}f≤ 0.14 rad/s, and the MPC is tuned with the

(27)

state and terminal state weight matrices Q = P = diag ([1, 2000, 100, 100, 10, 0.1]), and the control input weight matrix R = diag ([1, 1]). The states are in the same order as in (4.2), and the control inputs are in the order longitudinal acceleration axfirst, and front wheel steering

angle δf second.

The control inputs ax and δf, along with their respective references, arx and δrf, are

dis-played in Figure 4.8 and Figure 4.9, respectively. The states longitudinal velocity vx and

heading ψ, with references vr

xand ψr, are shown in Figure4.10and Figure4.11, respectively.

The lateral velocity vy and yaw rate ˙ψ, with their respective references vry and ˙ψrare shown

in Figure4.12and Figure4.13, respectively.

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30 X [m]

Y

[m]

Kinematic 2WS MPC

[X, Y ]

_[X

r

_{, Y}

r

_]

0

10

20

30

40

50

60

70

80

90

100

110

0

10

20

30 X [m]

Y

[m]

Dynamic 2WS MPC

[X, Y ]

[X

r

_{, Y}

r

_]

Figure 4.5: The Kinematic 2WS MPC and the Dynamic 2WS MPC performing path following for the paths [Xr_{, Y}r_{] at 13.89 ms}−1_{, resulting in the driven paths [X, Y ].}

(28)

0

2

4

6

8

10

12

14

16

18

20

22 −1

0

1 t [s]

a

y

ms

-2

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22 −1

0

1 t [s]

a

y

ms

-2

Dynamic 2WS MPC

Figure 4.6: The lateral acceleration ay for the Kinematic 2WS MPC and the Dynamic 2WS

MPC.

0

2

4

6

8

10

12

14

16

18

20

22

0

0.1

0.2

0.3 t [s]

[m]

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

0

0.1

0.2

0.3 t [s]

[m]

Dynamic 2WS MPC

(29)

0

2

4

6

8

10

12

14

16

18

20

22

0

1

2

3 t [s]

a

x

ms

-2

Kinematic 2WS MPC

a

_a

xr x

0

2

4

6

8

10

12

14

16

18

20

22

0

1

2

3 t [s]

a

x

ms

-2

Dynamic 2WS MPC

a

x

a

r x

Figure 4.8: The longitudinal acceleration ax with reference arxfor the Kinematic 2WS MPC

and the Dynamic 2WS MPC.

0

2

4

6

8

10

12

14

16

18

20

22 −0.1

0

0.1

0.2 t [s]

δ

f

[rad]

Kinematic 2WS MPC

δ

_δ

fr f

0

2

4

6

8

10

12

14

16

18

20

22 −0.1

0

0.1

0.2 t [s]

δ

f

[rad]

Dynamic 2WS MPC

δ

f

δ

r f

Figure 4.9: The front wheel steering angle δf with reference δfr for the Kinematic 2WS MPC

(30)

0

2

4

6

8

10

12

14

16

18

20

22

0

5

10

15 t [s]

v

x

ms

-1

Kinematic 2WS MPC

v

_v

xr x

0

2

4

6

8

10

12

14

16

18

20

22

0

5

10

15 t [s]

v

x

ms

-1

Dynamic 2WS MPC

v

x

v

r x

Figure 4.10: The longitudinal velocity vx with reference vrxfor the Kinematic 2WS MPC and

the Dynamic 2WS MPC.

0

2

4

6

8

10

12

14

16

18

20

22

0

1

2

3

4 t [s]

ψ

[rad]

Kinematic 2WS MPC

ψ

_ψ

r

0

2

4

6

8

10

12

14

16

18

20

22

0

1

2

3

4 t [s]

ψ

[rad]

Dynamic 2WS MPC

ψ

r

Figure 4.11: The heading ψ with reference ψr_{for the Kinematic 2WS MPC and the Dynamic}

(31)

0

2

4

6

8

10

12

14

16

18

20

22 −0.4

−0.2

0

0.2 t [s]

v

y

ms

-1

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22 −0.4

−0.2

0

0.2 t [s]

v

y

ms

-1

Dynamic 2WS MPC

v

y

v

r y

Figure 4.12: The lateral velocity vy with reference vyr for the Kinematic 2WS MPC and the

Dynamic 2WS MPC.

0

2

4

6

8

10

12

14

16

18

20

22 −1

0

1 t [s]

˙ ψ

[rad/s]

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22 −1

0

1 t [s]

˙ ψ

[rad/s]

Dynamic 2WS MPC

˙

ψ

˙

ψ

r

Figure 4.13: The yaw rate ˙ψ with reference ˙ψr_{for the Kinematic 2WS MPC and the Dynamic}

2WS MPC.

S-Turn on a Flat Road Without Disturbances

At 13.89 ms−1 _{(50 kph) when the Kinematic 2WS MPC performs path following for the}

reference path [Xr_{, Y}r_{] it results in the driven path [X, Y ], as seen in Figure} _4.14_{, with a}

maximum lateral acceleration ay of 0.79 ms−2, as shown by Figure 4.15, and a maximum

deviation of 0.04 m, shown in Figure4.16. To keep ay below the comfort limit, the steering

angle change rate is constrained to_{−0.12 ≤ ∆δ}f ≤ 0.12 rad/s. The MPC is tuned such that

the state and terminal state weight matrices are Q = P = diag ([1, 100, 100, 100]) and the control input weight matrix is R = diag ([1, 1]). The states are in the same order as in (4.1), and longitudinal acceleration ax is the first control input, and front wheel steering angle δf

(32)

The Dynamic 2WS MPC can perform path following for the reference path [Xr_{, Y}r_]

resulting in the driven path [X, Y ] seen in Figure 4.14. It results in the maximum lat-eral acceleration ay 0.51 ms−2, as displayed in Figure 4.15, and the maximum deviation

0.03 m, as displayed in Figure 4.16. The steering angle change rate is constrained to −0.12 ≤ ∆δf ≤ 0.12 rad/s, and the MPC is tuned with the state and terminal state

weight matrices Q = P = diag ([1, 100, 100, 100, 10, 0.1]), and the control input weight matrix R = diag ([1, 1]). The states are in the same order as in (4.2), and the control inputs are in the order longitudinal acceleration ax first, and front wheel steering angle δf second.

dis-played in Figure 4.17and Figure 4.18, respectively. The states longitudinal velocity vx and

heading ψ, with references vxrand ψr, are shown in Figure4.19and Figure4.20, respectively.

0

20

40

60

80

100

120

140

160

180

200 −60

−40

−20

0 X [m]

Y

[m]

Kinematic 2WS MPC

[X, Y ]

_[X

r

_{, Y}

r

_]

0

20

40

60

80

100

120

140

160

180

200 −60

−40

−20

0 X [m]

Y

[m]

Dynamic 2WS MPC

[X, Y ]

[X

r

, Y

r

]

Figure 4.14: The Kinematic 2WS MPC and the Dynamic 2WS MPC performing path following for the paths [Xr_{, Y}r_{] at 50 kph, resulting in the driven paths [X, Y ].}

(33)

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −1

−0.5

0

0.5

1 t [s]

a

y

ms

-2

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −1

−0.5

0

0.5

1 t [s]

a

y

ms

-2

Dynamic 2WS MPC

MPC.

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0.00

0.02

0.04 t [s]

[m]

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0.00

0.02

0.04 t [s]

[m]

Dynamic 2WS MPC

(34)

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

1

2

3 t [s]

a

x

ms

-2

Kinematic 2WS MPC

a

_a

xr x

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

1

2

3 t [s]

a

x

ms

-2

Dynamic 2WS MPC

a

x

a

r x

Figure 4.17: The longitudinal acceleration ax with reference arx for the Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −0.2

−0.1

0

0.1

0.2 t [s]

δ

f

[rad]

Kinematic 2WS MPC

δ

_δ

fr f

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −0.2

−0.1

0

0.1

0.2 t [s]

δ

f

[rad]

Dynamic 2WS MPC

δ

f

δ

_fr

Figure 4.18: The front wheel steering angle δf with reference δrf for the Kinematic 2WS MPC

(35)

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

5

10

15 t [s]

v

x

ms

-1

Kinematic 2WS MPC

v

_v

xr x

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

5

10

15 t [s]

v

x

ms

-1

Dynamic 2WS MPC

v

x

v

r x

Figure 4.19: The longitudinal velocity vx with reference vrxfor the Kinematic 2WS MPC and

the Dynamic 2WS MPC.

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −4

−2

0 t [s]

ψ

[rad]

Kinematic 2WS MPC

ψ

_ψ

r

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −4

−2

0 t [s]

ψ

[rad]

Dynamic 2WS MPC

ψ

r

Figure 4.20: The heading ψ with reference ψr_{for the Kinematic 2WS MPC and the Dynamic}

(36)

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −0.4

−0.2

0

0.2

0.4 t [s]

v

y

ms

-1

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −0.4

−0.2

0

0.2

0.4 t [s]

v

y

ms

-1

Dynamic 2WS MPC

v

y

v

r y

Figure 4.21: The lateral velocity vy with reference vyr for the Kinematic 2WS MPC and the

Dynamic 2WS MPC.

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −2

−1

0

1

2 t [s]

˙ ψ

[rad/s]

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −2

−1

0

1

2 t [s]

˙ ψ

[rad/s]

Dynamic 2WS MPC

˙

ψ

˙

ψ

r

Figure 4.22: The yaw rate ˙ψ with reference ˙ψr_{for the Kinematic 2WS MPC and the Dynamic}

2WS MPC.

S-Turn on a Flat Road With Disturbances

At 13.89 ms−1 _{(50 kph) when the Kinematic 2WS MPC performs path following for the}

reference path [Xr_{, Y}r_{] it results in the driven path [X, Y ], as seen in Figure} _4.23_{, with a}

maximum lateral acceleration ay of 0.87 ms−2, as shown by Figure 4.24, and a maximum

deviation of 0.26 m, shown in Figure4.25. To keep ay below the comfort limit, the steering

angle change rate is constrained to_{−0.11 ≤ ∆δ}f ≤ 0.11 rad/s. The MPC is tuned such that

the state and terminal state weight matrices are Q = P = diag ([1, 100, 100, 100]) and the control input weight matrix is R = diag ([1, 1]). The states are in the same order as in (4.1), and longitudinal acceleration ax is the first control input, and front wheel steering angle δf

(37)

The Dynamic 2WS MPC can perform path following for the reference path [Xr_{, Y}r_]

resulting in the driven path [X, Y ] seen in Figure 4.23. It results in the maximum lat-eral acceleration ay 0.79 ms−2, as displayed in Figure 4.24, and the maximum deviation

0.29 m, as displayed in Figure 4.25. The steering angle change rate is constrained to −0.10 ≤ ∆δf ≤ 0.10 rad/s, and the MPC is tuned with the state and terminal state

weight matrices Q = P = diag ([1, 100, 100, 100, 10, 0.1]), and the control input weight matrix R = diag ([1, 1]). The states are in the same order as in (4.2), and the control inputs are in the order longitudinal acceleration ax first, and front wheel steering angle δf second.

dis-played in Figure 4.26and Figure 4.27, respectively. The states longitudinal velocity vx and

heading ψ, with references vxrand ψr, are shown in Figure4.28and Figure4.29, respectively.

0

20

40

60

80

100

120

140

160

180

200 −60

−40

−20

0 X [m]

Y

[m]

Kinematic 2WS MPC

[X, Y ]

_[X

r

_{, Y}

r

_]

0

20

40

60

80

100

120

140

160

180

200 −60

−40

−20

0 X [m]

Y

[m]

Dynamic 2WS MPC

[X, Y ]

[X

r

, Y

r

]

Figure 4.23: The Kinematic 2WS MPC and the Dynamic 2WS MPC performing path following for the paths [Xr_{, Y}r_{] at 50 kph, resulting in the driven paths [X, Y ].}

(38)

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −1

−0.5

0

0.5

1 t [s]

a

y

ms

-2

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −1

−0.5

0

0.5

1 t [s]

a

y

ms

-2

Dynamic 2WS MPC

MPC.

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

0.1

0.2

0.3 t [s]

[m]

Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

0.1

0.2

0.3 t [s]

[m]

Dynamic 2WS MPC

(39)

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

1

2

3 t [s]

a

x

ms

-2

Kinematic 2WS MPC

a

_a

xr x

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30

0

1

2

3 t [s]

a

x

ms

-2

Dynamic 2WS MPC

a

x

a

r x

Figure 4.26: The longitudinal acceleration ax with reference arx for the Kinematic 2WS MPC

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −0.2

−0.1

0

0.1

0.2 t [s]

δ

f

[rad]

Kinematic 2WS MPC

δ

_δ

fr f

0

2

4

6

8

10

12

14

16

18

20

22

24

26

28

30 −0.2

−0.1

0

0.1

0.2 t [s]

δ

f

[rad]

Dynamic 2WS MPC

δ

f

δ

_fr

Figure 4.27: The front wheel steering angle δf with reference δrf for the Kinematic 2WS MPC

Implementation of Model PredictiveControl for Path Following with the KTH Research Concept Vehicle

Implementation of Model Predictive

Control for Path Following with the

KTH Research Concept Vehicle

PONTUS BELVÉN

Acknowledgments

Abstract

Sammanfattning

Contents

Nomenclature

Chapter 1

Introduction

1.1

Path Following

1.2

Motion Control

1.3

Model Predictive Control

1.4

Vehicle Modeling

1.5

KTH Research Concept Vehicle

1.6

Related Work

1.7

Problem Formulation

Chapter 2

Model Predictive Control

2.1

General Model Predictive Control

2.2

Reference Tracking Model Predictive Control

2.3

Quadratic Programming Form

Chapter 3

Vehicle Modeling

3.1

Bicycle Model

3.1.1

Kinematic Vehicle Model

3.1.2

Dynamic Vehicle Model

3.2

KTH Research Concept Vehicle

Chapter 4

Path Following

4.1

Model Predictive Control

4.1.1

Formulation

4.1.2

Vehicle Models

4.1.3

Discretization and Linearization

4.1.4

Reference Generation

4.1.5

State Estimation

4.2

Ride Comfort

4.3

Banked Road

4.4

Sloping Road

4.5

Sampling Time

4.6

Path Following Simulation Results

0

10

20

30

40

50

60

70

80

90

100

110

_[X

_{, Y}

_]