Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

MPC-Based Trajectory Tracking Control for 2016-01-0452

Intelligent Vehicles Published 04/05/2016

Tingyou Ming, Weiwen Deng, Sumin Zhang, and Bing Zhu

State Key Lab of ASCL, Jilin University

CITATION: Ming, T., Deng, W., Zhang, S., and Zhu, B., "MPC-Based Trajectory Tracking Control for Intelligent Vehicles," SAE
Technical Paper 2016-01-0452, 2016, doi:10.4271/2016-01-0452.

Copyright © 2016 SAE International

Abstract This paper focuses on trajectory tracking control of intelligent

vehicles. It has attracted researchers’ attention for years. In general,
In this paper, a model predictive control (MPC) based trajectory
the trajectory tracking control was converted to longitudinal control
tracking scheme utilizing steering wheel and braking or acceleration
and lateral control. R. S. Sharp et al. [4] proposed a mathematic
pedal is proposed for intelligent vehicles. The control objective is to
model for steering control using linear optimal discrete time preview
track a desired trajectory which is obtained from the trajectory
control theory to achieving the path tracking. The feedback control is
planner. The proposed control is based on a simplified third-order
computed using the current lateral displacement error and heading
vehicle model, which consists of longitudinal vehicle dynamics along
error. The feed-forward control input is computed using the preview
with a commonly used bicycle model. A nonlinear model predictive
road information. C. I. Chatzikomis et al. [5] presented a combined
control (NMPC) is adopted in order to follow a given path by
longitudinal-lateral controller that is regulating the steering angle and
controlling front steering, braking and traction, while fulfilling
throttle/brake levels by previewing the path ahead of the vehicle, and
various physical and design constraints. In order to reduce the
a simple PD controller with gain scheduling depending on the
computational burden, the NMPC is converted to a linear time-
longitudinal speed to control the steering angle is proposed. T. Lee et
varying (LTV) MPC based on successive online linearization of the
al. [6] proposed a controller based on linear quadratic optimal
nonlinear system model. Two different test conditions have been used
predictive control. Given the high nonlinearities of the vehicle system
to verify the effectiveness of the proposed approaches through
on one hand, and the uncertainties and disturbance of such a system
simulations using Matlab and CarSim. The results show that the
on the other hand, a very important issue to be considered in the
desired speed and path are well tracked.
control design is the robustness. The controller should be able to
reject the disturbances caused by wind, coefficient of friction of the
Introduction road and many other reasons, and able to deal with parameter
uncertainties and variations encountered in automotive applications.
Technological advances in recent years have greatly driven
progresses on intelligent vehicles to accomplish partial or even full
MPC appears to be well suited to the trajectory following [7, 8, 9]. It
autonomous driving [1, 2]. The major research of intelligent vehicles
allows considering the problem of trajectory tracking for nonlinear
has been focused on the following areas [3]:
systems taking into account the constraints on the state variables and/
or control inputs. In addition, this control technique is proved to be
1. Environment perception, aimed to detect traffic signs, obstacles
robust against system parameter variations. However, for autonomous
and other vehicles on road. An environmental sensing system
driving at high speed, the computation time (non-linear optimization
composed of radars, cameras, GPS, etc. are usually used to
algorithms) becomes very large for real-time operation [10]. Falcone
achieve this goal. It provides a dynamic map of near driving
et al. [11] presented an MPC controller based on successive online
region of the autonomous vehicle.
linearization of the nonlinear plant model for active steering system
2. Trajectory planning and decision, which forms a cluster of safe
of autonomous vehicle and its effectiveness has been validated by
and feasible trajectories, and then determine an optimal one
experimental test. In view of that, a MPC-based control scheme has
(reference trajectory) in the available space.
been proposed to achieve the trajectory tracking of intelligent vehicle
3. Vehicle control system, aimed to control vehicles to travel along as well as to make sure robustness of the system.
the reference trajectory (trajectory tracking) using actuators
like brakes or powertrain and steering wheel. Vehicle control The contributions of this paper are: (1) the formulation of combined
includes path following and speed following. longitudinal and lateral control for trajectory tracking of intelligent
vehicle using MPC approaches, (2) the NMPC problem is solved by a
 Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

linear time-varying (LTV) MPC based on successive online

linearization of the nonlinear system model; (3) the effectiveness has
been verified through simulation.

The paper is organized as follows: In section 2, general vehicle

dynamic model is presented. The proposed MPC approach is
discussed in section 3 and simulation and analysis is presented in
section 4.

Vehicle Model where νx, νy and wr are respectively the longitudinal velocity, lateral
velocity and yaw rate of the vehicle, as shown in figure 1. In addition,
In this section, we will describe the vehicle model used for control
a and b are the distances of the front and rear axles to the vehicle
design and simulation. It contains a three degree vehicle model for
center of gravity, mCG is the vehicle mass, Iz is the yaw moment of
the trajectory tracking of intelligent vehicle. The longitudinal and
inertia, δ is the steering angle of the front wheel, and Caf , Car are total
lateral vehicle controls are treated as being completely uncoupled.
cornering stiffness of the front and rear tires, respectively.
The vehicle longitudinal control or the longitudinal velocity was
given as a function of the position along the planed path and a
As we know, during the process of trajectory tracking, the
simplified longitudinal model was presented. A bicycle model was
longitudinal velocity may not be constant. In our model, during each
used for the lateral vehicle dynamic control.
predictive horizon, the longitudinal velocity was considered as a
constant variable, and in the next predictive horizon, the longitudinal
The Longitudinal Vehicle Dynamic velocity is updated according the real vehicle speed. So it is
The longitudinal control variable is the longitudinal acceleration. reasonable to use the bicycle model to represent the lateral dynamic
According to the Newton’s second law and the vehicle dynamic, the behavior during trajectory tracking.
following equation can be obtained:

(1)

where a is the longitudinal acceleration; νx is the longitudinal

velocity; Ft is the traction force or braking force, which is derived
from the powertrain or the braking system; Ff and Fw are the rolling
resistance and aerodynamic drag respectively; m is the mass of the
vehicle and g is the acceleration of gravity; f is the rolling resistance
coefficient; CD is the aerodynamic drag coefficient and A is the
frontal area of the vehicle.
Figure 1. The bicycle model

The Lateral Vehicle Dynamic The Coordination Transformation

The main goal of trajectory tracking of intelligent vehicle is to make Considering that the reference trajectory obtained from trajectory
sure that the vehicle travels along the reference trajectory with planner is given in an absolute inertial, so the real-time position of
minimum tracking error. To represent the lateral vehicle dynamic the vehicle in the vehicle axis system should be transformed into the
behavior, the well-known bicycle model is used, as shown in (2). The absolute inertial. The vehicle’s equations of motion in an absolute
model has the lateral and yaw degrees of freedom under the inertial frame can be expressed as follows:
consumption of a constant longitudinal velocity.

(3)
(2)
where (X, Y) and ψ are respectively the position coordination and
Where: heading angle of the vehicle in the absolute inertial frame, as shown
in figure 2.
 Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

According to (1) and (3), the nonlinear three-degree-of-freedom

vehicle model can be obtained in the following compact form:

To minimize the position error and the speed error as well as the
control energy during the trajectory tracking process, the following
cost function was considered:

(4)
(6)
where u = [a, δ] denote the longitudinal acceleration and the
steering angle. The state and input vectors are x = where, as in standard MPC notation, Δut = [Δut,t, …, Δut+Hc-1,t] is the
[X, Y, ψ, νx, νy, wr ] and u respectively. optimization vector at time t and yt+i denotes the output vector predicted
at time t + i obtained by starting from the state yt,t = y(t) and applying to
system (5) the input sequence [Δut,t, … , Δt+i-1,t]. Hc and Hp denote the
output prediction horizon and the control horizon, respectively. As in
standard MPC scheme, we use Hp > Hc and the control signal is
assumed constant for all Hp ≥ i ≥ Hc, i.e. Δut + i,t = 0 ∀ i ≥ Hc. The
reference signal yref,i = [Xref Yref ψref νxref] represents the desired outputs,
Q, R and S are weighting matrix of appropriate dimensions. The first
summand reflects the desired performance on target tracking; the
second and third summands weight the actuators’ effort.

Taking into account the constraints on the state variables and control
inputs are one of the biggest advantages of MPC controller. During
Figure 2. The coordination transformation the trajectory tracking process, some constraints have been
considered. First, due to the characteristic of the actuators, the
magnitude and rate of steering wheel angel and acceleration should
The MPC-Based Trajectory Tracking Controller be limited, as showed in (7):
The goal of trajectory tracking control is to achieve the speed control
and path following control. The basis of the MPC is to predict the
evolution of the vehicle model over a finite horizon based on a
sequence of future inputs in order to optimize the system cost under
the given constraints. In this section, we design the MPC controller (7)
computing the steering wheel angle and the longitudinal acceleration,
so that a desired trajectory is followed as close as possible. The The stability of the vehicle is another important issue that should be
controller is based on the vehicle model presented in (4). considered in the trajectory tracking, especially in the extreme
conditions, for instance, on the icy road. In that case, the reason for
the instability is tire force saturation. According to the tire model, the
MPC Controller Based on the Nonlinear Vehicle Model tire slip angle should be limited to the maximum value in order to
Before the formulation of the MPC problem, the continuous vehicle avoid it. So the following constraints were considered:
model presented in (4) should be discretized first. Based on Shannon
Sampling Theorem, the following discretized model was obtained:
(8)

where αf,r is the front or rear tire slip angle; σ is the slack variable and
σ ≥ 0. Equation (8) is a soft constraint, which makes sure that the
(5) optimization problem described in (6) always has a solution. Due to
the existence of the slack variable σ, the cost function should be
Where:
expressed in the following way:
 Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

According to the linearization process in (11), (12), (13), the

nonlinear model presented in (4) can be linearized as follows:

(9)
(14)
where ρ is the weighted matrix.
Where:
By combined (5), (6), (7), (8), (9), the following optimization
problem was solved at each time step:

(10)

The LTV MPC Controller

In addition, in the linear process, something should be noted: during
For nonlinear model predictive control (NLMPC), the computational
the predictive horizon, the longitudinal velocity was a constant and
burden is still a serious barrier for a wide class of “fast’ applications.
the vehicle model was replaced by the linear model on the state (x0,
This has motivated the study of alternative MPC approaches,
u0). At the next sampling point, the model is linearized over the
requiring the solution of simpler optimization problems in real time.
shifted horizon based on the new state.
Most of these approaches are based on linear or previse-linear
approximations of the nonlinear model of the plant. Studies on linear
According to the theory of MPC [14], the system must be
time-varying (LTV) or linear parameter-varying MPC schemes can be
controllable. Considering the following matrix:
found in [12, 13, 14].

Here we proposed a MPC controller based on successive online linear

of the nonlinear model present in (4). A general linear process is as The rank of Matrix S is equal to the row number of matrix A, so
follows: considering a nonlinear system, as showed in (11): system (14) is controllable according to the linear continuous system
theory [15].

Simulation and Results

The proposed LTV MPC controller was implemented in Matlab/
(11)
Simulink and CarSim in order to perform a trajectory tracking
It can be approximated by the following LTV system: maneuver. The MPC problem (10) has been transformed into a
standard Quadratic Problem (QP) and has been resolved using an
interior-point algorithm. Two different test scenarios were used to
validate the effectiveness and robustness of the MPC controller.
(12)
Some of the vehicle and controller parameters are listed in table 1.
Where:
Bounds on the longitudinal acceleration depend on the vehicle
dynamics. In particular, upper bounds depend on the engine and
driveline dynamics, while lower bounds depend on the braking
system. Bounds on the steering wheel angle depend on the constraints
of the actuator used to control the steering wheel angle. Here the
(13) following constraints on input and input rates are used:

The LTV system (12) describes the deviations of the nonlinear system
(11) from the state trajectory x0(k).
 Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

The simulation test was conducted on an icy road with a constant

friction coefficient 0.3 and the vehicle speed is 20m/s. Firstly, no tire
slip angle constraints was considered. Then, as a contrast, in order to
improve the stability of vehicle, the soft constraints of tire slip angle
have been applied with the range [-1.0,1.0] degree to make sure that
the tires are in the linear range and the weighted matrix of the slack
variable is 1000. The simulation results are shown in figure 4 to
(15) figure 6.
Table 1. Lists of vehicle and controller parameters.
Figure 4 showed the comparison of the actual trajectory and the
reference trajectory. In view of the extreme driving condition, the
vehicles follow the reference trajectory well at both cases. Figure 5
and Figure 6 showed the front wheel angel and the tire slip angle
respectively. It can be seen that when the tire slip angle was not
constrained, the actual tire slip angle exceeds the bounds of the linear
region, up to 10 degree. At the same time, when the tire slip angle
was constrained, the tire was in the linear region, which can improve
the stability of the vehicle when there are some disturbances.

Results Analysis
The first test maneuver that is used to evaluate the performance of the
LTV MPC controller is a steady-date constant radius cornering a.
maneuver. The vehicle is driven through a circular path with a radius
of 152.4m. The controller attempts to keep the vehicle on the
reference trajectory while the vehicle speed is continuously
increasing from an initial speed of 30km/h to a maximum speed of
90km/h. As shown in figure 3.1(b), the actual vehicle speed precisely
follows the desired speed. Figure 3.1(c) shown the comparison of the
actual trajectory and desired trajectory, it can been seen that the
controller is able to keep the vehicle on the desired circular trajectory
even at high speed and the maximum position error is less than 0.6m, b.
as shown in Figure 3.1(c). Figure 3.1(a) shows the steering wheel
angle that was applied to the keep the vehicle on the circular
trajectory, it can be seen that as the vehicle speed is increased, the
controller applied a larger steering wheel angle, thereby generating
larger lateral forces on the front axle in order to compensate for the
larger centripetal acceleration.

The second test maneuver is the double lane change (DLC) test which
presents an obstacle avoidance emergency maneuver. The objective c.
of the test is to validate the stability of the vehicle with the proposed
MPC controller in critical situations. The lateral displacement and
yaw angle of the reference trajectory are described as a function of
the longitudinal X:

d.

Figure 3. Simulation results in first test maneuver. (a) steering wheel angle;
(b) comparison of vehicle speed and desired speed; (c) comparison of vehicle
(16) trajectory and desired trajectory; (d) the partial enlarged view of the vehicle
trajectory and desired trajectory.
where z1 = 0.096(X - 27.19)-1.2, z2, = 0.11(X - 56.46) - 1.2, dx1 = 25,
dx2 = 21.95, dy1 = 4.05, dy2 = 5.7.
 Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

2. Li, L., Wen, D., Zheng, N.-N., and Shen, L.-C., “Cognitive
Cars: A New Frontier for ADAS Research,” IEEE Trans.
Intell.Transp. Syst. 13(1):395-407, 2012,doi:10.1109/
TITS.2011.2159493.
3. Tagne G., Talj R., and Charara A., "Higher-order sliding mode
control for lateral dynamics of autonomous vehicles, with
experimental validation," in 2013 IEEE Intelligent Vehicles
Symposium, IEEE IV 2013, June 23, 2013 - June 26, 2013,
Gold Coast, QLD, Australia, 2013, pp. 678-683.
4. Sharp R. S., Casanova D., and Symonds P., "Mathematical
model for driver steering control, with design, tuning and
performance results," Vehicle System Dynamics, vol. 33, pp.
289-326, 2000.
5. Chatzikomis C. I. and Spentzas K. N., "A path-following driver
Figure 4. The Trajectory.
model with longitudinal and lateral control of vehicle's motion,"
ForschungImIngenieurwesen-Engineering Research, vol. 73, pp.
257-266, Dec 2009.
6. Lee T., Kang J., Yi K., and Noh K., "An investigation on the
integrated human driver model for closed-loop simulation of
intelligent safety systems," Journal of Mechanical Science and
Technology, vol. 24, pp. 761-767, 2010.
7. Kim, D. and Kim, H., "Lateral Control for Automated Vehicle
Following System in Urban Environments," SAE Int. J. Trans.
Safety 2(2):199-206, 2014, doi:10.4271/2014-01-0161.
8. Chen, B.-C., Luan, B.-C., and Lee, K., “Design of lane keeping
Figure 5. The front steer angle δf.
system using adaptive model predictive control,” Automation
Science and Engineering (CASE), 2014 IEEE International
Conference on, IEEE: 922-926, 2014.
9. Qu, T., Chen, H., Cao, D., Guo, H., and Gao, B., “Switching-
Based Stochastic Model Predictive Control Approach for
Modeling Driver Steering Skill,” IEEE Trans. Intell. Transp.
Syst. 16(1):365-375, 2015, doi:10.1109/TITS.2014.2334623.
10. Keviczky T., Falcone P., Borrelli F., Asgari J., and Hrovat D.,
"Predictive control approach to autonomous vehicle steering,"
in 2006 American Control Conference, June 14, 2006 - June 16,
2006, Minneapolis, MN, United states, 2006, pp. 4670-4675.
Figure 6. The front and rear tire slip angle.
11. Falcone P., Borrelli F., Tseng H. E., Asgari J., and Hrovat
D., "Linear time-varying model predictive control and its
Conclusions application to active steering systems: Stability analysis and
A trajectory tracking control based on MPC was proposed to achieve experimental validation," Southern Gate, Chichester, West
the path tracking and speed tracking for intelligent vehicles. The Sussex, PO19 8SQ, United Kingdom, 2008, pp. 862-875.
control was developed under a simulation environment. In order to 12. Falcone P., Tufo M., Borrelli F., Asgarit J., and Tseng H. E., "A
reduce the computational burden, the LTV MPC based on successive linear time varying model predictive control approach to the
online linearization of the nonlinear system model was used. integrated vehicle dynamics control problem in autonomous
Simulation results under different scenarios showed the effectiveness systems," in 46th IEEE Conference on Decision and Control
and robustness of the proposed MPC controller. Experiments on a test 2007, CDC, December 12, 2007 - December 14, 2007, New
vehicle will be performed in future work. Orleans, LA, United states, 2007, pp. 2980-2985.
13. Chen Bo-Chiuan, Luan Bi-Cheng, Lee Kangwon, "Design
of Lane Keeping system using adaptive model predictive
References
control," in 2014 IEEE International Conference on Automation
1. Levinson, J., Askeland, J., Becker, J., Dolson, J., Held, D., Science and Engineer (CASE), Aug. 18, 2014 - Aug. 22, 2014,
Kammel, S., Kolter, J.Z., Langer, D., Pink, O., Pratt, V., and Taipei,Taiwan, China, 2014, pp.922-926.
others, “Towards fully autonomous driving: Systems and
14. Rawlings James B., Mayne David Q., "Model Predictive
algorithms,” Intelligent Vehicles Symposium (IV), 2011 IEEE,
Control: Theory and Design,", Nob Hill Publishing, 2009.
IEEE: 163-168, 2011.
15. Shousong Hu, "Automatic Control Theory," the Fifth Edition,
Science Press, 2007.
 Downloaded from SAE International by Univ of California Berkeley, Wednesday, August 01, 2018

Contact Information
For questions or to contact the authors, please email to the
corresponding author Sumin Zhang at:
[email protected]

Acknowledgments
The authors wish to acknowledge the sponsorship of National Natural
Science Foundation of China under grant 51175215.

The Engineering Meetings Board has approved this paper for publication. It has successfully completed SAE’s peer review process under the supervision of the session organizer. The process
requires a minimum of three (3) reviews by industry experts.

All rights reserved. No part of this publication may be reproduced, stored in a retrieval system, or transmitted, in any form or by any means, electronic, mechanical, photocopying, recording, or
otherwise, without the prior written permission of SAE International.

Positions and opinions advanced in this paper are those of the author(s) and not necessarily those of SAE International. The author is solely responsible for the content of the paper.

ISSN 0148-7191

http://papers.sae.org/2016-01-0452