Project:P1

Ding Zhao
24-677 Special Topics: Linear Control Systems

Due: Nov 12, 2019, 5:00 pm

• You need to upload your solution to Gradescope ( https://www.gradescope.com/) to

be graded. The link is on the panel of CANVAS. If you are not familiar about the tool,
post your questions on Piazza or ask during the office hours. We will use the online
submission time as the timestamp.

• Submit controller.py and BuggyStates.npz to Gradescope under Programming P1

and your solutions in .pdf format to Project-P1. Insert the Buggy Simulator perfor-
mance plot image in the .pdf. We will test your controller.py and manually check all
answers.

• You are recommended to test your codes in Google Colab before submission, to ensure it
executes with standard python compilers. Please refer to (https://drive.google.com/open?id
=1g12AAAATJUSY0gRrZ0C2GTM6Y0MGndTkjNOavd41ExY) for documentation on
how to use Colab.

• Right after each week’s recitation, we have a half-hour Q&A session. You are re-
quested to work on the assignment early and bring your questions to this session to
take advantage of this support.

• You can also post your questions on the Piazza. We will try our best to give feedback
within 24 hours during the workday and within 48 hours during the weekend. We will
keep answering questions until 8:00 pm, Monday.

1
1 Introduction
This project is an individual project in which you are asked to design a controller for your
Buggy (autonomous) team (go to https://www.springcarnival.org/buggy.shtml, if you would
like to find out more about Buggy).
A control engineer would take the following steps for designing a controller for this system:

1. Examine control model (nonlinear) [already provided to you].

2. Linearize the state space system equations. [P1]

3. Develop a PID controller for the system. [P1]

4. Check the controllability and stabilizability of the system. [P2]

5. Design feedback controller using Pole Placement. [P2]

6. Design optimal controller. [P3]

7. Design stochastic controller. [P4]

This project is designed to get you well acquainted with the controller designing process for
a 4 wheeler vehicle. The project is sub-divided into 4 assignments as:

P1 (a) Linearize the state space model.

(b) Design a PID lateral and PID longitudinal controller.

P2 (a) Check the controllability and stabilizability of the linearized system.

(b) Pole placement.

P3 (a) Design an optimal controller for the vehicle.

P4 (a) Design a Kalman Filter to filter disturbances in the states.

(b) Race with other Buggy teams in the class

The deadlines of the 4 sub-parts will be :

• P1 is due at 5:00 PM on N ov.12th

• P2 is due at 5:00 PM on N ov.19th

• P3 is due at 5:00 PM on N ov.26th

• P4 is due at 5:00 PM on Dec.10th

2
2 Model

Figure 1: Bicycle model[2]

Figure 2: Tire slip-angle[2]

Here you will use the 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 in two
degree of freedom, described in longitudinal and lateral dynamics separately. The model
parameters are defined in Table 1.

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

d2 y
 
where ay = is the inertial acceleration of the vehicle at the center of geometry
dt2 inertial
in the direction of the y axis, Fyf and Fyr are the lateral tire forces of the front and rear

3
wheels respectively and δf is the front wheel angle which will be denoted as δ later. Two
terms contribute to ay : the acceleration ÿ which is due to motion along the y axis and the
centripetal acceleration . Hence
ay = ÿ + ψ̇ ẋ
Combining the two equations, the equation for the lateral translational motion of the vehicle
is obtained as
1
ÿ = −ψ̇ ẋ + (Fyf cos δ + Fyr )
m
Moment balance about the axis yields the equation for the yaw dynamics as

ψ̈Iz = lf Fyf − lr Fyr

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 ẋ ≥ 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
!
ẏ + lf ψ̇
Fyf = 2Cα δ −
ẋ
!
ẏ − lr ψ̇
Fyr = 2Cα −
ẋ

where Cα is called the cornering stiffness of tires. If ẋ < 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

ẍ = ψ̇ ẏ + ax
max = F − sign(ẋ)Ff
Ff = f mg

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 ẋ ≥ 1 otherwise -1.

4
2.3 Global coordinates
In the global frame we have

Ẋ = ẋ cos ψ − ẏ sin ψ
Ẏ = ẋ sin ψ + ẏ cos ψ

2.4 System equation

Gathering all the equations, if ẋ ≥ 0.5 m/s we have:
!
2Cα ẏ + lf ψ̇ ẏ − lr ψ̇
ÿ = −ψ̇ ẋ + (cos δ δ − − )
m ẋ ẋ
1
ẍ = ψ̇ ẏ + (F − f mg)
m ! !
2lf Cα ẏ + lf ψ̇ 2lr Cα ẏ − lr ψ̇
ψ̈ = δ− − −
Iz ẋ Iz ẋ
Ẋ = ẋ cos ψ − ẏ sin ψ
Ẏ = ẋ sin ψ + ẏ cos ψ

otherwise since the lateral tire forces are zeros we only consider the longitudinal model.

2.5 Measurements
The observable states are with some Gaussian noise  = N (0, σ), where
 
0.5 ··· 0
 
ẋ
 ẏ  
 0.5 

0.05
 
 ψ̇   
y=  + , σ =  
X   .. 
   . 0.05 
Y  
 1


ψ 0 0.5
.

2.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 ẋ 6 100 m/s
|ẏ| 6 10m/s

5
 Table 1: Model parameters.
Name Description Unit Value
(ẋ, ẏ) Vehicle’s velocity along the direction of m/s State
vehicle frame
(X, Y ) Vehicle’s coordinates in the world m State
frame
ψ, ψ̇ 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 m 1.7
mass
lf Length from front tire to the center of m 1.1
mass
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

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 generates the
buggy state after the given fixed time step (fixed fps) as output. 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 3.

6
 Figure 3: Buggy track[3]

4 P1:Problems [Due 5:00 PM, November 12]

Exercise 1. Model Linearization
As mentioned in class, model linearization is always the first step for non-linear control.
During this assignment, you will approximate the given model with a linear model.
Since the longitudinal term ẋ is non-linear in the lateral dynamics, we can simplify the con-
troller by controlling the lateral and longitudinal states separately. You are required to write
the system dynamics in linear forms as s˙1 = A1 s1 + B1 u and s˙2 = A2 s2 + B2 u in terms of
the following given input and states:
 
  y  
δ  ẏ  x
u= , s1 =   , s2 =
 
F ψ ẋ
ψ̇

Solution:

This is a recommended linearization. If you linearize your system with rea-

sonable assumptions, you will get full points.
Under the small angle assumption, we have cos(δ) = 1. Then cluster the varibles with
respect to the variables within state variables, we have
For s1 :

7
      
−4Cα 2Cα (lr − lf ) 2Cα
ÿ = ẏ + ψ̇ −ẋ + +δ
mẋ mẋ m

!
2 lf2 + lr2 Cα
   
2(lr − lf )Cα 2lf Cα
ψ̈ = ẏ + ψ̇ − +δ
Iz ẋ Iz ẋ Iz

For s2 :

 
1
ẍ = F + ψ̇ ẏ − f g
m

Write them in matrix form:

  0 1 0 0
   
y y 0 0
−4C 2C (l −l )
0 0 −ẋ + αmrẋ f  2Cα
α
 
d d  ẏ  
mẋ   ẏ   m
   0 δ
s1 =   =   +  0
dt dt ψ  0 0 0 1  ψ 0 F
2(l −l f )Cα 2(lf +lr )Cα
2 2 2lf Cα
ψ̇ 0 r
0 − Iz ẋ ψ̇ Iz
0
Iz ẋ

         
d d x 0 1 x 0 0 δ 0
s2 = = + 1 +
dt dt ẋ 0 0 ẋ 0 m
F ψ̇ ẏ − f g

8
Exercise 2. Introduction to the Simulator. The Buggy Simulator has been implemented
as a python code in the BuggySimulator.py, takes the steering, brake and longitudinal force
as input.
For this question, you have to design a PID longitudinal controller and a PID lateral con-
troller for the vehicle. PID is an error based controller, that requires tuning of the propor-
tional, derivative and integral gains.
Design the controllers in controller.py.
[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 and the response plot
with the code.
Your controller is required to achieve the following performance criteria:

1. Time to complete the loop = 450 s

2. Maximum deviation from the reference trajectory = 8.0 m

3. Average deviation from the reference trajectory = 4.5 m

Solution:

from BuggySimulator import *

import numpy as np
import scipy
from scipy.ndimage import gaussian_filter1d
from util import *
import scipy.signal

class controller():

def __init__(self,traj,vehicle):
self.vehicle=vehicle
self.traj=traj
self.prev_vx_error=0
self.integral_vx_error=0
self.integral_phi_error=0
self.prev_phi_error=0

def control_update(self):

traj=self.traj
vehicle=self.vehicle

lr = vehicle.lr
lf = vehicle.lf

9
Ca = vehicle.Ca
Iz = vehicle.Iz
f = vehicle.f
m = vehicle.m
g = vehicle.g

delT = 0.05

#reading current vehicle states

X = vehicle.state.X
Y = vehicle.state.Y
xdot = vehicle.state.xd
ydot = vehicle.state.yd
phi = vehicle.state.phi
phidot = vehicle.state.phid
delta = vehicle.state.delta

mindist, index = closest_node(X, Y, traj)

if index<len(traj)-100:
idx_fwd = 100
else:
idx_fwd = len(traj)-index-1

Vx = 6 # constant desired longitudinal velocity of the vehicle

# ---------------|Lateral Controller|-------------------------
kp=1
kd=0.05
ki=0.001

phides = np.arctan2((traj[index+idx_fwd][1]-Y),(traj[index+idx_fwd][0]-X))
delta_error=(phides-phi)
self.integral_phi_error+=delta_error
derivative_error=delta_error-self.prev_phi_error
deltades=kp*delta_error + ki*self.integral_phi_error*delT + kd*derivative_error/
deltades=wrap2pi(deltades)
deltad = (deltades - delta)/delT

#--------|Longitudinal Controller|------------------------------
kp=400
kd=300
ki=-0.1

10
# Computing the errors
vx_error=Vx-xdot
self.integral_vx_error+=vx_error
derivative_error=vx_error-self.prev_vx_error
F=kp*vx_error + ki*self.integral_vx_error*delT + kd*derivative_error/delT
# print(kp*vx_error, ki*self.integral_vx_error*delT, kd*derivative_error/delT)

controlinp = vehicle.command(F,deltad)

# Storing the parameters for the next epoch

self.prev_vx_error=vx_error
self.prev_phi_error=delta_error

return controlinp,Vx

11
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

12