24-677 Project 3 Solved

1 Introduction

In this project, you will complete the following goals: 1. Design an lateral optimal controller
2. Implement A* path planning algorithm

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

2 Model

The error-based linearized state-space for the lateral dynamics is as follows.
e1 is the distance to the center of gravity of the vehicle from the reference trajectory. e2 is the orientation error of the vehicle with respect to the reference trajectory.

e10 1 0 0e100  o  d e ̇ 0 −4Cα 4Cα −2Cα(lf−lr) e ̇ 2Cα 0 􏰇δ􏰈 −2Cα(lf−lr)−x ̇

1mx ̇ m mx ̇1mmx ̇  ̇

=
dte2 0 0

e ̇ 2Cα(lf −lr) 2 0 − Izx ̇

0

2Cα(lf −lr) Iz

1

2Cα(lf2 +lr2) − Izx ̇

+ + ψdes e20 0F  0 

e ̇ 2Cαlf 0 2Cα(lf2 +lr2) 2 Iz − Izx ̇

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.

e10 1 0 0e100
d e ̇ 0 −4Cα 4Cα −2Cα(lf−lr) e ̇ 2Cα 0 􏰇δ􏰈

1mx ̇ m mx ̇1m

= dte20 0

0

+  1e200F

Iz

e ̇ 2 C α ( l f − l r ) 2 0 − Izx ̇

2 C α ( l f − l r ) Iz

2 C α ( l f2 + l r2 ) e ̇ 2 C α l f 0

− Izx ̇ 2
d􏰇x􏰈 􏰇0 1􏰈􏰇x􏰈 􏰇0 0􏰈􏰇δ􏰈 􏰇 0 􏰈

For the longitudinal control:
dt x ̇ = 0 0 x ̇ + 0 m1 F + ψ ̇y ̇−fg

Assuming ψ ̇ = 0:

d􏰇x􏰈 􏰇0 1􏰈􏰇x􏰈 􏰇0 0􏰈􏰇δ􏰈 dtx ̇=00x ̇+0m1 F

2

3 P3: Problems
Exercise 1. [50pts] All the code related to Exercise 1 is under P3 student/P3-LQR/. For

the lateral control of the vehicle, design a discrete-time infinite horizon LQR controller.

You can reuse your longitudinal PID controller from part 1 of this project, or even im- prove 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’ built- in 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 and report your score. Your controller is required to achieve the following performance criteria to receive full points:

1. Time to complete the loop = 250 s
2. Maximum deviation from the reference trajectory = 7.0 m
3. Average deviation from the reference trajectory = 3.5 m

Some hints that may be useful:

• Make sure to discretize your continuous state-space system with the provided timestep (delT) prior to solving the ARE.
• Using LQR requires manually creating two matrices Q and R. Q works to penalize state variables, while R penalizes control input. Try to think about what form your Q and R matrices should take for good performance.
  • –  For Q, large values will heavily restrict changes to the respective states, while small values will allow the states to easily change.
  • –  Similarly, in R, large values will heavily restrict control input, while small values will allow the control input to vary widely.
  • –  One idea for tuning is to set the relevant indices of Q and R to 1(max value of the corresponding state/input)2
    in order to normalize the value. Make sure to experiment outside of this guideline

    to determine the best performance. 3

– There is a relationship between Q and R, though it is subtle. For example, if you increase weights in Q, you are more heavily penalizing changes in the states, which will require more control input. This would imply that you should decrease weights in R to see an effect. Due to this, it may be helpful to keep either Q or R fixed while varying the other during tuning.

[10% Bonus]: Complete the loop within 130 s. The maximum deviation and the average deviation should be within in the allowable performance criteria mentioned above.

Exercise 2. [50pts] A* Planning
All the code related to Exercise 1 is under P3 student/P3-AStar/. In this exercise, we will implement A* path planning algorithm. Now there is another vehicle driving on the track. If the ego vehicle follows the original trajectory (which aligns with the track), it is likely to crash into the other vehicle. In order to perform the overtaking, we need to re-plan the trajectory. In this problem, we will implement A* path planning algorithm.

To simplify the problem, we initialize the other vehicle on the straight section of the track. You will overtake it on the straight section as well. We assume the other vehicle will drive in a straight line so we do not need to worry about trajectory prediction too much. As you get close to the other car, our controller will use A* algorithm to re-plan the path. The predicted trajectory of another vehicle is treated as obstacle in the cost map. In addition, we do not want to drive too far away from the track therefore we treat the area outside a certain distance from the center of the track as obstacle. In your implementation, you only need to find a path given a static and discretized map.

You only need to complete the plan function in P3 student/P3-AStar/AStar-scripts/ Astar script.py. To test the algorithm you implemented, simply run Astar script.py file, which will generate 3 figures of path planning results using the function you implemented. The maps are shown in Figure.1. Yellow region represents the obstacle. Red Crosses repre- sent the start and red dots represent the goal. If you think it plans reasonable paths, you can copy the code from your Astar script.py to P3 student/P3-AStar/controllers/main/Astar.py and then open Webots world file P3 student/P3-AStar/worlds/Project3.wbt. and try it with the Webots simulator and the controller you implemented in Exercise 1.

(a) (b) (c)

Figure 1: A* Testing cases

In your submission, 1. Save and attach the figures generated by runing Astar.py in your submission and 2. report the time of completion. As long as the generated figures show the

4

successful path planning and the car completes the racing without crashing, you will receive full marks for this problem.

Here is the pseudocode for A* path planning algorithm (Source: https://en.wikipedia. org/wiki/A*_search_algorithm).

   function calculate_path(node):
       path_ind.append(node.pose)
       while current.parent is not empty:

           current := current.parent

           path_ind.append(current.pose)
       return path_ind

5

// A* finds a path from start to goal.
// h is the heuristic function. h(n) estimates the cost to reach goal from

􏰘→ node n.
function A_Star(start, goal, h)

    // Initialize start node and goal node class

    // Calculate h_value and f_value of start node
    // For node n, g(node) is the cost of the cheapest path from start to n

􏰘→ currently known
// f(node) = g(node) + h(node), which represents ourcurrent best guess

􏰘→ asto
// how short a path from start to finish can be if it goes through n

    // The set of discovered nodes that may need to be (re-)expanded.
    // Initially, only the start node is known.
    // This is usually implemented as a min-heap or priority queue rather

􏰘→ than a hash-set. openSet := {start}

// For node n, closedSet[n] is the node immediately preceding it on the 􏰘→ cheapest path from start

    // to n currently known.

    closedSet := an empty map

while openSet is not empty
// Current is the node in open_list that has the lowest f value
// This operation can occur in O(1) time if openSet is a min-heap or

􏰘→ a priority queue
current := the node in openSet having the lowest fScore[] value openSet.Remove(current)

        // Append current node into the closed_list

        if current = goal
            return reconstruct_path(closedSet, current)

for each neighbor of current (can be computed by calling the 􏰘→ get_successor method):

// d(current,neighbor) is the weight of the edge from current to 􏰘→ neighbor

            // g(successor) = g(current) + d(current, successor)
            // h(successor) can be computed by calling the heuristic method
            // f(successor) = g(successor) + h(successor)
            // Don't forget to push the successor into open_list

    // Open set is empty but goal was never reached

    return failure

6

4 Appendix (Already covered in P1)

where ay = dt2

inertial

Figure 2: Bicycle model[2]

Figure 3: 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 2, 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

􏰒d2y􏰓

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, 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:

a y = y ̈ + ψ ̇ x ̇
Combining the two equations, the equation for the lateral translational motion of the vehicle

is obtained as:

y ̈=−ψ ̇x ̇+m1(Fyf cosδ+Fyr)
Moment balance about the axis yields the equation for the yaw dynamics as

ψ ̈Iz =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 =δ−θVf αr =−θVr

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

􏰔 y ̇ + l f ψ ̇ 􏰕 Fyf=2Cα δ− x ̇

􏰔 y ̇−lrψ ̇􏰕 Fyr=2Cα − 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 ̇ + a x 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.

8

4.3 Global coordinates

In the global frame we have:

4.4 System equation

X ̇ =x ̇cosψ−y ̇sinψ Y ̇ =x ̇sinψ+y ̇cosψ

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

2 C 􏰔 y ̇ + l ψ ̇ 􏰕 y ̈=−ψ ̇x ̇+α(cosδδ− f − r)

y ̇ − l ψ ̇ m x ̇ x ̇

x ̈ = ψ ̇y ̇ + m1 (F − fmg)
2lC􏰔 y ̇+lψ ̇􏰕2lC􏰔y ̇−lψ ̇􏰕

ψ ̈= f α δ− f − r α − r Iz x ̇ Iz 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:

4.6 Physical constraints

The system satisfies the constraints that:

 x ̇ 

 y ̇ 

  ψ ̇   y = X  Y 

ψ

|δ|􏰖π6 rad
F 􏰗0andF 􏰖15736N x ̇ 􏰗 10−5 m/s

9

Name

δ or δf F
m

Cα Iz

Fpq
f delT

Table 1: Model parameters. Unit

Description

Front wheel angle Total input force Vehicle mass

Cornering stiffness of each tire Yaw intertia
Tire force, p ∈ {x, y},q ∈ {f, r} Rolling resistance coefficient Simulation timestep

rad N kg

N
kg mˆ2 N
N/A sec

Value

Input Input 1888.6

20000
25854
Depends on input force 0.019
0.032

4.7 Simulation

Figure 4: 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

10

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

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 Base- Controller 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.

Name (X, Y ) (x ̇,y ̇) ψ

ψ ̇
4.9 Trajectory Data

Table 2: State Derivation.

Explanation
From GPS readings
From the derivative of GPS readings From the compass readings
From the gyroscope readings

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

11

5 Reference

Figure 5: Buggy track[3]

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

12