Third Place Solution in Summer 2023 Simulation Racing Series
Written by Krishay Garg, a junior at American High School (Fremont, CA), this paper describes his third place solution in the Summer 2023 ROAR Competition.
The code for the solution can be found here: https://github.com/krishayg/ROARSummer_Krishay_Garg
Table of Contents
Introduction
The ROAR Racing Competition is an autonomous driving challenge held by UC Berkeley where high school competitors develop algorithms to control a car in a simulated environment with no human intervention. The goal of the competition is to program the car to drive around the racetrack by itself with the fastest possible lap time and without crashing. This competition is at the forefront of computer science, and developments that are made in the simulated environment can be applied to real autonomous vehicles to take technology to the next level.
In every competition, competitors build on top of the winning solutions of the previous competition, which allows the algorithms to continually improve. For the summer competition, I added new algorithms on top of the previous PID solution and improved the racecar’s time by 21.8 seconds, achieving a lap time of 495.1 seconds.
In this solution, four new algorithms were developed:
- Automated Waypoint Shifting: To enable the car to turn at higher speeds, the algorithm automatically shifts waypoints to the inside edge of the track.
- Continuously Variable PID Parameters: A new sigmoid-based function was created to calculate unique PID parameters based on the speed of the car.
- Turn-by-Turn Tuning: The threshold values for each turn were tuned independently to allow the car to go at an optimal speed every turn.
- Hand-Brake Application: The hand brake was used for the first time to improve the car’s ability to decelerate quickly.
Automated Waypoint Shifting
Theory
One area where the car lost time was at the curves in the hills region. At these curves, the car had to slow down considerably so that it would not crash. If the car approached the turn too fast, its momentum would carry it outside the track, where it would crash. Taking this into consideration, it is optimal to move the waypoints to the inside of the track whenever there is a turn, as shown in Figure 1 below. This would allow the car to move at a faster speed while remaining on the track.
Figure 1: Adjusting Waypoints on Turns
Original Waypoints
Inside of track:
One way to move the waypoints to the inside of the track is driving the car manually to set the waypoints. However, this approach is impractical considering the length of the track and the fact that small human errors when setting the waypoints can make the car’s performance very poor, especially when it is driving at high speeds. Instead, it is more efficient and also more generalizable to other racetracks to have an algorithm that can shift the waypoints as needed.
In Figure 2, the car in the waypoint-shifted solution is turning at a faster speed. In addition, notice that in the original solution, the car goes all the way to the edge of the left lane, risking a crash, whereas in the waypoint-shifted solution, the car remains in the center of the road even after the turn.
Figure 2: Comparing Original Solution to Waypoint-Shifted Solution
Original Solution
Waypoint-Shifted Solution
Algorithm and Code
The first challenge is to determine when the waypoints need to be shifted. This can be accomplished by tracking the angle between two closest waypoints and lookahead waypoints. At each turn, based on the speed of the car, the number of timesteps the waypoints should be shifted and the maximum shift distance are determined. The maximum shift distance needs to be smaller when the car is traveling at a higher speed so it does not overshoot and crash. Additionally, the direction the waypoints need to be shifted is chosen based on the direction of the turn. For a smooth transition, the waypoints are shifted gradually from zero to the maximum shift amount for the first half of the turn and are then gradually decreased back to zero for the second part of the turn.
The code is shown below:
def shift_waypoint(point1, point2, direction, amt):
"""
Shifts point2 given:
(1) the previous point point1,
(2) the direction to shift the point
(3) the distance to shift the point
"""
vector_between_points = point2 - point1
length = np.linalg.norm(vector_between_points)
unit_vector = vector_between_points / length
translation_length = amt * direction
translation_vector = np.array([-unit_vector[2], 0, unit_vector[0],0,0,0]) *
translation_length
point2 = point2 + translation_vector
return point2
# The following code is used to determine when to shift the waypoints,
# what direction to shift them in, and how much to shift.
if ((track_error >= 0.75) and region in [2, 3] and time_to_shift == 0):
time_to_shift = int( (0.9 * round(pow(current_speed, 2) * 0.002 + 0.7 *
current_speed)))
time_to_shift_start=time_to_shift
max_shift = max(2.5, 1 / (current_speed + 1e-16) * 225)
if cross[1] > 0:
shift_direction = -1
else:
shift_direction = 1
if time_to_shift > 0:
shiftamt = ((-1 * max_shift * 4) / time_to_shift_start ** 2) *
(time_to_shift - time_to_shift_start / 2) ** 2 + max_shift
next_waypoint =
shift_waypoint(previous_waypoint.to_array(),next_waypoint.to_array(),
shift_direction,shiftamt)
time_to_shift -= 1
Continuously Variable PID Parameters
The previous winning solution used discrete thresholds based on the car’s speed to decide the k-parameters for PID control. Essentially, there were 7 different sets {Kp ,Ki ,Kd} for 7 different speed ranges. However, it is better to have an optimal set of k-parameters depending on the speed instead of mapping a range of speeds to the same set of values. To do this, a function was used to calculate a unique set {Kp ,Ki ,Kd} given the speed of the car. This new continuous function improved the ability of the car to steer and efficiently follow the new path generated by automated waypoint shifting.
A sigmoid function was used to compute the values of Kp and Kd . This function was chosen because
the values of Kp and Kd need to be between 0 and 1, with lower speeds having relatively high values and higher speeds having relatively low values. Ki continued to be computed using the previous algorithm based on speed ranges because it was approximately the same for all speeds and it had very little impact on the car’s performance.
Figure 3: Sigmoid Functions for PID Parameters Kp and Kd
def find_k_values_formula(vehicle: Vehicle, config: dict) -> np.array:
#uses a sigmoid function to calculate k values
e=2.71828
current_speed = Vehicle.get_speed(vehicle=vehicle)
k_p=-0.0225343+(0.6858377+0.0225343)/(1+(current_speed/160.9069)**5.880651)
k_d=-0.01573431+(0.2335526+0.01573431)/(1+(current_speed/113.8959)**3.660237)
k_i=0
#K_i will be computed in a different function
return np.array([k_p, k_d, k_i])
Turn-by-Turn Tuning
In the previous winning solution, a single threshold value for the distance to turn was used to determine when to start braking before each turn in the city region. However, on some turns the car could have started braking later, which would have allowed it to go at a faster speed for longer. For example, prior to the third turn there is a slight slope, meaning the car had to start braking early. This low threshold was then applied for every turn, slowing down the car’s overall speed. To improve the car’s speed, the threshold values for each turn were tuned independently.
Hand Brake Application
To allow the car to decelerate faster, the hand brake was applied in addition to the regular brakes whenever the car needed to slow down. Employing the hand brake enables the car to brake for a shorter amount of time and hence travel at a faster speed for longer, thus reducing its total lap time.
Figure 4: Hand Brake Application on Turn
Conclusion
I really enjoyed working on the ROAR Racing Competition. The challenge of creating advanced autonomous driving algorithms combined with the sophisticated simulation environment has made the ROAR competition one of the most engaging competitions I have participated in. I was very happy that I was able to contribute several new and successful algorithms to the project and improve on the original solution by 21.8 seconds.
I want to thank the entire ROAR team, including Dr. Yang and Mr. Kuan, for providing this opportunity. The ROAR Academy was an amazing learning experience, and it was a privilege to be able to participate and contribute to the project. I look forward to continuing my involvement with the ROAR Program in the future!