Description
This project will allow you to practice the following skills
• Checking controllability and observability
• Design a state feedback controller
• Design a full state observer
As in Project 1, this project consists of a written component along with a Webots controller
design component.
2 Model
State feedback control is used to drive the states to the origin, but in this case we are following
a trajectory. For this project we will follow the lead of [1] and redefine the state space system
developed in Project 1 to be based on the error in lateral position. Like Project 1, we will
assume that the dynamics are decoupled between the lateral and longitudinal directions,
which allows us to independently design lateral and longitudinal controllers.
We will define the following states to capture the error (see Section 2.5 in [1] for more
detail):
• e1 – the distance of the vehicle mass center from the track centerline. Note that this
was absent in the first Project!
• e2 – the orientation of the vehicle with respect to the road. This is what we controlled
with a PID controller in Project 1.
Using state feedback we will be able to account for BOTH position errors relative to the
center and the heading – this might make state feedback more successful than PID control
for this system.
Assume that the longitudinal velocity Vx is constant and the radius of the road R is
constant and large. Then we can define the desired rate of orientation change as
ψ˙
des =
Vx
R
.
The desired acceleration is then given by
alatdes =
V
2
x
R
= Vxψ˙
des.
Now we can compute the error in lateral acceleration
e¨1 = ¨y + Vxψ˙
| {z }
alat
−
V
2
x
|{z}
R
alatdes
= ¨y + Vx
ψ˙ − ψ˙
des
2
With Vx and a constant we can integrate this easily
e˙1 = ˙y + Vx (ψ − ψdes) (1)
Using the definition of the heading error (e2 = ψ − ψdes) along with the dynamics in Project
1 we can derive the following error dynamics equation.
d
dt
e1
e˙1
e2
e˙2
=
0 1 0 0
0 −
4Cα
mx˙
4Cα
m −
2Cα(lf −lr)
mx˙
0 0 0 1
0 −
2Cα(lf −lr)
Izx˙
2Cα(lf −lr)
Iz
−
2Cα(l
2
f +l
2
r
)
Izx˙
e1
e˙1
e2
e˙2
+
0 0
2Cα
m
0
0 0
2Cαlf
Iz
0
δ
F
+
o
−
2Cα(lf −lr)
mx˙ − x˙
0
−
2Cα(l
2
f +l
2
r
)
Izx˙
ψ˙
des
In lateral vehicle dynamics, ψ˙
des is a time-varying disturbance in the state space equation.
Its value is proportional to the longitudinal speed when the radius of the road is constant.
When deriving the error-based state space model for controller design, ψ˙
des can be safely
assumed to be zero.
d
dt
e1
e˙1
e2
e˙2
=
0 1 0 0
0 −
4Cα
mx˙
4Cα
m −
2Cα(lf −lr)
mx˙
0 0 0 1
0 −
2Cα(lf −lr)
Izx˙
2Cα(lf −lr)
Iz
−
2Cα(l
2
f +l
2
r
)
Izx˙
e1
e˙1
e2
e˙2
+
0 0
2Cα
m
0
0 0
2Cαlf
Iz
0
δ
F
For the longitudinal control:
d
dt
x
x˙
=
0 1
0 0 x
x˙
+
0 0
0
1
m
δ
F
+
0
ψ˙y˙ − fg
Assuming ψ˙ = 0:
d
dt
x
x˙
=
0 1
0 0 x
x˙
+
0 0
0
1
m
δ
F
On the sensing side, we typically know the location of the car (GPS) with its heading (compass) and longitudinal velocity (tachometer). In other words, we cannot directly
measure the full state in the error dynamics model.
3
3 Project 2: State Space Control [Due April 8, 2021]
Exercise 1. To warm up for the exercises in the project (and help prep for the 2nd exam),
here are several typical homework problems on controllability, observability, controller design,
and observer design.
1. For the following systems determine controllability and observability.
• x˙ =
0 1 0
0 0 1
−1 −3 −3
x +
1
0
0
u, y =
1 1 1
x
• x˙ =
0 1 0
0 0 1
0 2 −1
x +
0 1
1 0
0 0
u, y =
1 0 1
x
• x˙ =
2 1 0 0 0 0 0
0 2 0 0 0 0 0
0 0 2 0 0 0 0
0 0 0 2 0 0 0
0 0 0 0 1 1 0
0 0 0 0 0 1 0
0 0 0 0 0 0 1
x +
2 1 0
2 1 1
1 1 1
3 2 1
−1 0 1
1 0 1
1 0 0
u, y =
2 2 1 3 −1 1 1
1 1 1 2 0 0 0
0 1 1 1 1 1 0
x
2. Find a state space realization for G(s) = 1
s
s+3
s+1
1
s+3
s
s+1
.
3. Consider the discrete time system xk+1 =
1 1 −2
0 1 1
0 0 1
xk +
1
0
1
uk. Design a state
feedback control matrix K such that the closed loop system has all poles at 0.
4. Consider the discrete time system xk+1 =
−2 4
−3 9
x +
0
1
u, y =
3 1
x. Design an
observer matrix L such that the observer has poles at −0.5 ± 0.5j.
Exercise 2. Considering the linearized, error-based state space system for the vehicle in
the Model section above:
1. Check the controllability of the system at the following longitudinal velocities: 2 m/s,
5 m/s and 8 m/s.
2. For longitudinal velocities v from 1 m/s to 40 m/s, plot the following:
(a) log10(
σ1
σn
) versus v (m/s), where σi
is the ith singular value of the controllability
matrix P (i = 1, 2, …, n). (In other words, what is the logarithm of the greatest
singular value divided by the smallest?)
4
(b) Re(pi) versus v (m/s), where Re is real part and pi
is the ith pole of the continuous
state space system. [Use 4 subplots, one for each of the 4 poles]
What conclusions can you draw about the overall controllability and stability of the
system in observing these two plots?
3. Assuming that we can directly measure both lateral error terms but not their derivatives, check the observability of the lateral error dynamics at the following longitudinal
velocities: 2 m/s, 5 m/s and 8 m/s. If we measure ONLY the heading error (as in
Project 1) what is the conclusion on observability?
4. Show your design for the pole placement controller that you used in Exercise 2. How
did you select your desired pole locations?
5. Given measurements of only the two error terms, how would you reconstruct the error
derivative states to enable your state feedback controller?
Submit you answers in the .pdf file and also submit the Matlab live script you used to
generate it.
5
Exercise 3. For the lateral control of the vehicle, design a state feedback controller using
pole placement. Tune the poles of the closed loop system such that it can achieve the performance criteria mentioned below.
You can reuse your longitudinal PID controller from part 1 of this project, or even improve upon it. However, it may require retuning based on observed performance.
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:
• You should use Matlab to perform your controller design, then implement it in Python.
It is certainly possible to solve the pole placement problem directly in Python but that
is not necessary for this problem.
• It is somewhat difficult to tune pole-placement controllers. Learning optimal control
in the next submodule will fortunately make this task much easier. Some tips to help
for this assignment follow.
– Poles must be negative if the system is stable.
– Poles can be complex, where an imaginary number is denoted with j, e.g. -3+1j .
If you use a complex pole, you must also include its complex conjugate.
– Don’t use the poles from Exercise 1 as a starting point – these are the system’s
open-loop poles. Your goal is to select new positions for the closed-loop poles.
– Poles placed closer to the imaginary axis (in other words, closer to 0 on the real
axis) will dominate the system response. Recall that the system responds faster
the farther the poles are from the imaginary axis (provided they are stable).
6
– Having at least one dominant pole to help the system to converge is recommended.
The placement of your other poles is up to you based on your performance. Alternatively, you can also place a pair of conjugate poles closer to the imaginary
axis, and keep the other two away from it.
• The controller itself can be continuous or discrete – it is your choice whether to discretize
the system or not.
• You need not design and implement an observer for this problem.
7
4 Appendix
(Already covered in Project 1)
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.
4.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
8
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.
4.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.
9
4.3 Global coordinates
In the global frame we have:
X˙ = ˙x cos ψ − y˙ sin ψ
Y˙ = ˙x sin ψ + ˙y cos ψ
4.4 System equation
Gathering all of the equations, if ˙x ≥ 0.5 m/s, we have:
y¨ = −ψ˙x˙ +
2Cα
m
(cos δ
δ −
y˙ + lfψ˙
x˙
!
−
y˙ − lrψ˙
x˙
)
x¨ = ψ˙y˙ +
1
m
(F − fmg)
ψ¨ =
2lfCα
Iz
δ −
y˙ + lfψ˙
x˙
!
−
2lrCα
Iz
−
y˙ − lrψ˙
x˙
!
X˙ = ˙x cos ψ − y˙ sin ψ
Y˙ = ˙x sin ψ + ˙y cos ψ
otherwise, since the lateral tire forces are zeros, we only consider the longitudinal model.
4.5 Measurements
The observable states are:
y =
x˙
y˙
ψ˙
X
Y
ψ
4.6 Physical constraints
The system satisfies the constraints that:
|δ| 6
π
6
rad
F > 0 and F 6 16000 N
x˙ > 10−5 m/s
10
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 State
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 inertia 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
4.7 Simulation
Figure 3: Simulation code flow
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
11
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).
4.8 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
4.9 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.
12
Figure 4: Buggy track[3]
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
