In this part of the project, you will complete the following two assignments:

1. Check the controllability and stabilizability of the linearized system
2. Design a lateral full-state feedback controller

[Remember to submit the write-up, plots, and codes on Gradescope.]

1 Model

The error-based linearized state-space for the lateral dynamics is as follows.

e₁ is the distance to the center of gravity of the vehicle from the reference trajectory. e₂ is the orientation error of the vehicle with respect to the reference trajectory.

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.

For the longitudinal control:

Assuming = 0:

2 P2: Problems

Exercise 1. Considering the linearized, error-based state space system for the vehicle in the Model section above:

1. Check the controllability and observability 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:

) 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?)

(b) Re(p_i) versus v (m/s), where Re is real part and p_i 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?

[Submit you answers in the .pdf file and also submit the Python script. The Python script should be named Q1.py]

Exercise 2. 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 cars 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 = 9.0 m
3. Average deviation from the reference trajectory = 4.5 m

Some hints that may be useful:

• The signal subpackage within scipy is required for this part. Please investigate which functions you will need to use. The main goal is to calculate a gain matrix K such that Kx = u, where x is the states and u is the control input.
• It is somewhat difficult to tune pole-placement controllers. Learning optimal control in the next submodule will fortunately make this task much easier. Remember poles should be negative if the system is stable. Poles can also 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.
• The controller itself can be continuous or discrete it is your choice whether to discretize the system or not.

3 Appendix

(Already covered in P1)

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.

3.1 Lateral dynamics

Ignoring road bank angle and applying Newtons second law of motion along the y-axis:

may = Fyf cosf + Fyr

whereis the inertial acceleration of the vehicle at the center of geometry in the direction of the y axis, F_yf and F_yr are the lateral tire forces of the front and rear wheels, respectively, and _f is the front wheel angle, which will be denoted as later. Two terms contribute to a_y: the acceleration y, which is due to motion along the y-axis, and the centripetal acceleration. Hence: a_y = y+ x

Combining the two equations, the equation for the lateral translational motion of the vehicle is obtained as:

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 F_yf and F_yr. Experimental results show that the lateral tire force of a tire is proportional to the slip-angle for small slip-angles when vehicles 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

!

where C is called the cornering stiffness of the tires. If x < 0.5 m/s, we just set F_yf and F_yr both to zeros.

3.2 Longitudinal dynamics

Similarly, a force balance along the vehicle longitudinal axis yields:

x = y + a_x

ma_x = F F_f F_f = fmg

where F is the total tire force along the x-axis, and F_f is the force due to rolling resistance at the tires, and f is the friction coefficient.

3.3 Global coordinates

In the global frame we have:

X = x cos y sin

Y = x sin + y cos

3.4 System equation

Gathering all of the equations, if x 0.5 m/s, we have:

Y = x sin + y cos

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

3.5 Measurements

The observable states are:

x

y

y =

X

Y

3.6 Physical constraints

The system satisfies the constraints that:

F > 0 and F 6 15736 N

x > 10⁵ m/s

Table 1: Model parameters.

Name	Description	Unit	Value
(x, y)	Vehicles velocity along the direction of vehicle frame	m/s	State
(X,Y )	Vehicles 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
	mass
lf	Length from front tire to the center of mass	m	1.55
CIz			25854
Fpq	p {x,y},q {f,r}
f		sec

3.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.pys 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 youre 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 classs init method (which is executed once when the CustomController object is instantiated).

3.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

3.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.

Figure 4: Buggy track[3]

4 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. 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