Description
1 Introduction
In this project, you will complete the following goals:
1. Design a Kalman Filter to de-noise.
2. Race with other Buggy teams.
[Remember to submit the write-up and codes on the Gradescope]
2
2 Model
The error-based linearized state-space for the lateral dynamics:
e1: distance of the c.g. of the vehicle from the reference trajectory
e2: the orientation error of the vehicle with respect to the reference trajectory
d
dt
e1
e˙1
e2
e˙2
=
0 1 0 0
0 −
4Cα
mVx
4Cα
m −
2Cα(lf −lr)
mVx
0 0 0 1
0 −
2Cα(lf −lr
IzVx
2Cα(lf −lr
Iz
−
2Cα(l
2
f +l
2
r
IzVx
e1
e˙1
e2
e˙2
+
0 0
2Cα
m
0
0 0
2Cαlf
Iz
0
δ
F
+
o
−
2Cα(lf −l)r)
mVx
− Vx
0
−
2Cα(l
2
f +l
2
r
IzVx
ψ˙
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 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α
mVx
4Cα
m −
2Cα(lf −lr)
mVx
0 0 0 1
0 −
2Cα(lf −lr
IzVx
2Cα(lf −lr
Iz
−
2Cα(l
2
f +l
2
r
IzVx
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
3
3 Resources
3.1 Buggy Simulator
A Buggy Simulator designed in python has been provided along with the assignment. The
simulator takes the control command[steering, longitudinal Force] and then outputs the
buggy state after the given fixed time step (fixed fps). Additional script util.py contains
functions to help you design and execute the controller. Please design your controller in
controller.py. After the complete run, a response plot is generated by the simulator. This
plot contains visualization of the buggy trajectory and variation of states with respect to
time.
3.2 Trajectory Data
The trajectory is given in buggyTrace.csv. It contains the coordinates of the trajectory:
(x, y). The satellite map of the track is shown in Figure 1.
Figure 1: Buggy track[3]
4
4 P4:Problems [Due 5:00 PM, December 10]
Exercise 1. Stochastic noise has been added to all the observable states of the vehicle.
Design a Kalman Filter to de-noise the measurement of the buggy states, allowing more
accurate estimation and robust control. Assume the noise in the measurements to be Gaussian. Implement a Discrete Time, Infinite Horizon LQR controller for the lateral control and
a PID controller for the longitudinal control of the vehicle.
Design the Kalman filter and the controllers in controller.py.
Table 1: Noise Parameters.
Name Description Unit Mean Variance
( ˙x, y˙) Vehicle’s velocity along the direction of
vehicle frame
m/s 0 0.5
(X, Y ) Vehicle’s coordinates in the world
frame
m 0 1
ψ, ψ˙ Body yaw angle, angular speed rad 0 0.5,
0.05
δ or δf Front wheel angle rad 0 0.05
[You have to edit only the controller.py python script]
Execute the main.py python script to check your controller. It generates a performance plot
and saves the vehicle states in a .npz file. Submit the Buggy states in .npz file, the response
plots in pdf file, and your controller in the [andrew id] controller.py script.
Your controller is required to achieve the following performance criteria:
1. Time to complete the loop = 300 s
2. Maximum deviation from the reference trajectory = 10.5 m
3. Average deviation from the reference trajectory = 3.0 m
5
Exercise 2. For this section, you will get additional points for competing with your colleagues. The score of each individual will be decided by their rank in the class.
The BuggySimulator environment for testing your controller will be same as the one used
for Exercise 1.
Submit the response plots in the pdf file, and your controller in [andrew id] race.py script.
Scoring metric:
1. Bronze (top 20-30% of the class): 10% additional points for P4.
2. Silver (top 10-20% of the class): 20% additional points for P4.
3. Gold (top 10% of the class): 30% additional points for P4.
6
5 Appendix
(Already covered in P1)
Figure 2: Bicycle model [2]
Figure 3: Tire slip-angle [2]
Here you will use the same bicycle model introduced in P1. We will work with the
linearized version of the model for all the questions that you formulated for P1. Shown in
Figure 2, the car is modeled as a two-wheel vehicle in two degree of freedom, described in
longitudinal and lateral dynamics separately. The model parameters are defined in Table 2.
5.1 Lateral dynamics
Ignoring road bank angle and applying Newtons second law for 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
7
wheels respectively and δf is the front wheel angle whichi 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 transnational 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, let’s say 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 wheel. the slip angle of the front and rear wheel is
αf = δ − θV f
αr = −θV r
where θV p is the angle that the velocity vector makes with the longitudinal axis of the vehicle
forp ∈ {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 tires. If ˙x < 0.5 m/s, we just set Fyf and Fyr
both to zeros.
5.2 Longitudinal dynamics
Similarly, a force balance along the vehicle longitudinal axis yields
x¨ = ψ˙y˙ + ax
max = F − sign( ˙x)Ff
Ff = fmg
where F is the total tire force along x axis, Ff is the force due to rolling resistance at the
tires, and f is the friction coefficient. sign function returns +1 when ˙x ≥ 1 otherwise -1.
8
5.3 Global coordinates
In the global frame we have
X˙ = ˙x cos ψ − y˙ sin ψ
Y˙ = ˙x sin ψ + ˙y cos ψ
5.4 System equation
Gathering all 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.
5.5 Measurements
The observable states are with some Gaussian noise = N(0, σ), where
y =
x˙
y˙
ψ˙
X
Y
ψ
+ , σ =
0.5 · · · 0
0.5
0.05
.
.
. 0.05
1
0 0.5
.
5.6 Physical constraints
The system satisfies the constraints that:
|δ| 6
π
6
rad/s
|
˙δ| 6
π
6
rad/s
|F| 6 10000 N
0 m/s 6 x˙ 6 100 m/s
|y˙| 6 10m/s
9
Table 2: 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 State
δ or δf Front wheel angle rad State
˙δ Steering Rate rad Input
F Total input force N Input
m Vehicle mass kg 1000
lr Length from front tire to the center of
mass
m 1.7
lf Length from front tire to the center of
mass
m 1.1
Cα Cornering stiffness of each tire N 15000
Iz Yaw intertia kg mˆ2 3344
Fpq Tire force, p ∈ {x, y},q ∈ {f, r} N Depend on input force
m vehicle mass Kg 2000
f Friction coefficient 1 0.01
6 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
10
