Description
24-677 Special Topics: Linear Control Systems
1 Introduction
This project will entail using Webots, an open-source robotics simulation software, to learn
more about control methods for autonomous vehicles. Webots was chosen because of its ease
of use, flexibility, and impressive rendering capability. A brief guide for Windows, Mac OS,
and Linux follows. Please see the links on the previous page for a link to documentation and
more information.
For Windows:
1. Download and install Webots R2021a (latest release).
2. Download and install Python natively from Python’s official webpage. 3.9 is recommended, but any version from 3.7 – 3.9 will work.
3. Open a Command Prompt window (right-click and select “Run as Administrator”). On
the command line, install numpy, scipy, and matplotlib with the following command:
python -m pip install .
4. If you encounter issues installing matplotlib, please try upgrading pip and setuptools::
python -m pip install –upgrade pip
python -m pip install –upgrade setuptools
For Mac:
1. Download and install Webots R2020b.
2. Download and install Python natively from Python’s official webpage. 3.8 is recommended, but 3.7 will also work.
3. In a Terminal window, install numpy, scipy, and matplotlib with the following command:
pip3.x install where x is the version number you installed.
4. Check where your Python is installed with:
which python3.x
Copy the path that appears and open Webots. In the Webots menu at the top, open
Tools → Preferences, and paste the copied path under “Python command”.
For Linux:
1. Download and install Webots R2021a (latest release).
2. In a terminal window, make sure you have numpy, scipy, and matplotlib with the
following command:
pip install .
2
As you might already know, Buggy is a time-honored CMU tradition (you can learn more
about it here and here). Unfortunately, it was not able to be held in-person last year due to
the pandemic. To help make up for this, you will be asked to create a controller that helps a
vehicle to complete the same course in simulation. However, instead of controlling an actual
Buggy, you’ll be controlling a Mercedes-Benz Spring van. It’s a bit more fun and stylish (we
hope)!
We will study this problem from various angles as we move through the rest of the course,
applying newly learned control techniques on the autonomous vehicle system. We will follow
the steps listed below to learn more about the system and synthesize several different kinds
of controllers:
1. Examine the provided nonlinear control model
2. Linearize the state space system equations [Project 1]
3. Develop a PID controller for the system [Project 1]
4. Check the controllability and stabilizability of the system [Project 2]
5. Design a full-state feedback controller using pole placement [Project 2]
6. Design an optimal controller [Project 3]
7. Implement an extended Kalman filter (EKF) for simultaneous localization and mapping
(SLAM) [Project 4]
The project has been divided into 4 parts to reflect this:
Project 1 (a) Linearize the state space model
(b) Design a PID lateral and PID longitudinal controller
Project 2 (a) Check the controllability and stabilizability of the linearized system
(b) Design a full-state feedback controller
Project 3 (a) Design an optimal controller
Project 4 (a) Implement EKF SLAM to control the vehicle without perfect sensor input
(b) Race with other Buggy competitors in the class
3
2 Model
Figure 1: Bicycle model[2]
Figure 2: Tire slip-angle[2]
We will make use of a bicycle model for the vehicle, which is a popular model in the
study of vehicle dynamics. Shown in Figure 1, the car is modeled as a two-wheel vehicle
with two degrees of freedom, described separately in longitudinal and lateral dynamics. The
model parameters are defined in Table 2.
2.1 Lateral dynamics
Ignoring road bank angle and applying Newton’s second law of motion along the y-axis:
may = Fyf cos δf + Fyr
where ay =
d
2
y
dt2
inertial
is the inertial acceleration of the vehicle at the center of geometry
in the direction of the y axis, Fyf and Fyr are the lateral tire forces of the front and rear
4
wheels, respectively, and δf is the front wheel angle, which will be denoted as δ later. Two
terms contribute to ay: the acceleration ¨y, which is due to motion along the y-axis, and the
centripetal acceleration. Hence:
ay = ¨y + ψ˙x˙
Combining the two equations, the equation for the lateral translational motion of the vehicle
is obtained as:
y¨ = −ψ˙x˙ +
1
m
(Fyf cos δ + Fyr)
Moment balance about the axis yields the equation for the yaw dynamics as
ψI¨
z = lfFyf − lrFyr
The next step is to model the lateral tire forces Fyf and Fyr. Experimental results show that
the lateral tire force of a tire is proportional to the “slip-angle” for small slip-angles when
vehicle’s speed is large enough – i.e. when ˙x ≥ 0.5 m/s. The slip angle of a tire is defined
as the angle between the orientation of the tire and the orientation of the velocity vector of
the vehicle. The slip angle of the front and rear wheel is
αf = δ − θV f
αr = −θV r
where θV p is the angle between the velocity vector and the longitudinal axis of the vehicle,
for p ∈ {f, r}. A linear approximation of the tire forces are given by
Fyf = 2Cα
δ −
y˙ + lfψ˙
x˙
!
Fyr = 2Cα
−
y˙ − lrψ˙
x˙
!
where Cα is called the cornering stiffness of the tires. If ˙x < 0.5 m/s, we just set Fyf and Fyr both to zeros. 2.2 Longitudinal dynamics Similarly, a force balance along the vehicle longitudinal axis yields: x¨ = ψ˙y˙ + ax max = F − Ff Ff = fmg where F is the total tire force along the x-axis, and Ff is the force due to rolling resistance at the tires, and f is the friction coefficient. 5 2.3 Global coordinates In the global frame we have: X˙ = ˙x cos ψ − y˙ sin ψ Y˙ = ˙x sin ψ + ˙y cos ψ 2.4 System equation s Gathering all of the equations, if ˙x ≥ 0.5 m/s, we have: y¨ = −ψ˙x˙ + 2Cα m (cos δ δ − y˙ + lfψ˙ x˙ ! − y˙ − lrψ˙ x˙ ) (1) x¨ = ψ˙y˙ + 1 m (F − fmg) (2) ψ¨ = 2lfCα Iz δ − y˙ + lfψ˙ x˙ ! − 2lrCα Iz − y˙ − lrψ˙ x˙ ! (3) X˙ = ˙x cos ψ − y˙ sin ψ (4) Y˙ = ˙x sin ψ + ˙y cos ψ (5) Otherwise, since the lateral tire forces are zeros, we only consider the longitudinal model. 2.5 Measurements The observable states are: y = x˙ y˙ ψ˙ X Y ψ 2.6 Physical constraints The system satisfies the constraints that: |δ| 6 π 6 rad F > 0 and F 6 16000 N
x˙ > 10−5 m/s
6
Table 1: Model parameters.
Name Description Unit Value
( ˙x, y˙) Vehicle’s velocity along the direction of
vehicle frame
m/s State
(X, Y ) Vehicle’s coordinates in the world
frame
m State
ψ, ψ˙ Body yaw angle, angular speed rad,
rad/s
State
δ or δf Front wheel angle rad Input
F Total input force N Input
m Vehicle mass kg 4500
lr Length from rear tire to the center of
mass
m 3.32
lf Length from front tire to the center of
mass
m 1.01
Cα Cornering stiffness of each tire N 20000
Iz Yaw intertia kg mˆ2 29526.2
Fpq Tire force, p ∈ {x, y},q ∈ {f, r} N Depends on input force
f Rolling resistance coefficient N/A 0.028
delT Simulation timestep sec 0.032
3 Resources
3.1 Simulation
Figure 3: Simulation code flow
7
Several files are provided to you within the controllers/main folder. The main.py script
initializes and instantiates necessary objects, and also contains the controller loop. This loop
runs once each simulation timestep. main.py calls your controller.py’s update method
on each loop to get new control commands (the desired steering angle, δ, and longitudinal
force, F). The longitudinal force is converted to a throttle input, and then both control
commands are set by Webots internal functions. The additional script util.py contains
functions to help you design and execute the controller. The full codeflow is pictured in
Figure 3.
Please design your controller in the your controller.py file provided for the project part
you’re working on. Specifically, you should be writing code in the update method. Please
do not attempt to change code in other functions or files, as we will only grade the
relevant your controller.py for the programming portion. However, you are free to
add to the CustomController class’s init method (which is executed once when the
CustomController object is instantiated).
3.2 BaseController Background
The CustomController class within each your controller.py file derives from the BaseController class in the base controller.py file. The vehicle itself is equipped with a
Webots-generated GPS, gyroscope, and compass that have no noise or error. These sensors
are started in the BaseController class, and are used to derive the various states of the
vehicle. An explanation on the derivation of each can be found in the table below.
Table 2: State Derivation.
Name Explanation
(X, Y ) From GPS readings
( ˙x, y˙) From the derivative of GPS readings
ψ From the compass readings
ψ˙ From the gyroscope readings
3.3 Trajectory Data
The trajectory is given in buggyTrace.csv. It contains the coordinates of the trajectory as
(x, y). The satellite map of the track is shown in Figure 4.
8
Figure 4: Buggy track[3]
4 Problems [Due Mar. 25, 2021]
Exercise 1. Model Linearization. As mentioned in class, model linearization is usually
the first step in control design for non-linear systems. During this assignment, you will
approximate the given model with a linear model. For this we will use the following state
vector
χ =
x˙
y˙
ψ
ψ˙
X
Y
.
For the below assume that for values of ˙x < 0.5 m/s that the system is identical, i.e. there
is no need to linearize two separate systems.
• Using the given nonlinear state equations, derive the linearized state equations for the
arbitrary operating point χ0.
• Use the parameters in Table 2 and the prior results to find the linearized model about
operating point ˙y = ψ˙ = ψ = 0, ˙x = 6. At this operating point the car is driving straight at the track center, and something interesting happens to the dynamics.
NOTE: If you are having extra constant terms in your state equation, be sure you are
properly using and applying the equilibrium inputs.
• Convert the model you previously found to a transfer function matrix assuming that
the system outputs are ψ and ˙x, the heading angle and longitudinal speed. You should
9
find that you have two decoupled transfer functions, i.e. at this point the steering
angle affects only ψ and the force affects only the longitudinal velocity ˙x. What are
the poles and zeros of the two transfer functions?
10
Exercise 2. Controller Synthesis in Simulation. The driver functions that control the
car take the desired steering angle δ and a throttle input – ranging from 0 to 1 – which is
derived from the desired longitudinal force F.
For this question, you have to design a PID longitudinal controller and a PID lateral controller for the vehicle. A PID is an error-based controller that requires tuning proportional,
integral, and derivative gains. As a PID allows us to minimize the error between a set point
and process variable without deeper knowledge of the system model, we will not need our
result from Exercise 1 (though it will be useful in future project parts, and you could certainly design by hand at least for the simple longitudinal dynamics model).
Design the two controllers in your controller.py. You can make use of Webots’ builtin code editor, or use your own.
Check the performance of your controller by running the Webots simulation. You can press
the play button in the top menu to start the simulation in real-time, the fast-forward button
to run the simulation as quickly as possible, and the triple fast-forward to run the simulation
without rendering (any of these options is acceptable, and the faster options may be better
for quick tests). If you complete the track, the scripts will generate a performance plot via
matplotlib. This plot contains a visualization of the car’s trajectory, and also shows the
variation of states with respect to time.
Submit your controller.py and the final completion plot as described on the title page.
Your controller is required to achieve the following performance criteria to receive full
points:
1. Time to complete the loop = 350 s
2. Maximum deviation from the reference trajectory = 8.0 m
3. Average deviation from the reference trajectory = 4.0 m
Some hints that may be useful:
• Using a PID controller requires storing variables between method calls. Python allows
this through use of the self preface (in other words, making them class member
variables). Think about which variables you use in your controller that you will need
to store, and be sure to add them to CustomController’s init method so they are
initialized.
• You will need to use the sensor measurements to compute the error in heading angle and
error in longitudinal velocity. While the longitudinal velocity error is straightforward to
compute based on your measured states, you will have to think about how to compute
the heading error given your current heading ψ and the relationship between your
global position and the given trajectory points.
• If you are tuning your lateral controller for a point on the trajectory that is the closest
to the vehicle, the controller may struggle on sudden turns. Think about how to
11
add some mechanism that “looks ahead” of your current position before calculating a
control command.
• Functions in util.py might be useful. For example, closestNode returns the absolute
value of the cross-track error, which is the distance from the vehicle’s center of gravity
to the nearest waypoint on the trajectory. In addition, the function also returns the
index of the closest waypoint to the vehicle.
• If your car does not perform well, it may just be that it requires tuning. You may already have some experience tuning PID controllers, but if not, viewing online resources
is recommended. See this Wiki link [4] for more background in the process of manual
tuning.
12
5 Reference
1. Rajamani Rajesh. Vehicle Dynamics and Control. Springer Science & Business Media,
2011.
2. Kong Jason, et al. “Kinematic and dynamic vehicle models for autonomous driving
control design.” Intelligent Vehicles Symposium, 2015.
3. cmubuggy.org, https://cmubuggy.org/reference/File:Course_hill1.png
4. “PID Controller – Manual Tuning.” Wikipedia, Wikimedia Foundation, August 30th,
2020. https://en.wikipedia.org/wiki/PID_controller#Manual_tuning
13