What is Wrong With My Stanley Controller for Car Steering Control

Question

Hope everyone is doing well.

I am just trying out Microsoft Airsim for few days and few hours ago I wanted to implement a Path Tracking Algorithm for an Autonomous Car. For the lateral control, I wanted to use Stanley Controller. Here is the basic idea of the Stanley Controller:

Here is the Stanley Controller part of my script:

##############################
#  LATERAL POSITION CONTROL  #
##############################

# STANLEY GAIN:
K = 0.78

# READING CAR POSITION AND ORIENTATION FROM THE SIMULATOR:
pos = car_state.kinematics_estimated.position
orientation = car_state.kinematics_estimated.orientation

x = pos.x_val
y = pos.y_val
yaw = orientation.z_val

if v < 0.5 and v > -0.5:
    index += 1  
else:
    arr = np.zeros(ref.shape[0], dtype=np.float32)

    for i in range(ref.shape[0]):
        arr[i] = np.sqrt((x - ref[i][0])**2 + (y - ref[i][1])**2)
    index = np.argmin(arr)


#print(index)

# I APPROXIMATED HERE, NOT SURE IF IT IS REALLY CORRECT:
xc = x + (1.5 * np.cos(yaw))
yc = y + (1.5 * np.sin(yaw))

# SLOPE OF THE LINE CONTAINING TWO CONSECUTIVE WAYPOINTS:
ref_path_x = ref[index + 1][0] - ref[index][0]
ref_path_y = ref[index + 1][1] - ref[index][1]


# TO AVOID ZERO BY ZERO DIVISION:
if ref_path_x == 0:
    ref_path_x = 0.000001     

slope = ref_path_y / ref_path_x  

# X INSTERSECTION OF THE LINE: 
# Constant = n:

constant = ref[index + 1][1] - (slope * ref[index + 1][0])

# CONTANTS OF THE LINE EQUATION:
a = -1 * slope
b = 1
c = -1 * constant

# CROSS TRACK ERROR CALCULATION (AS MENTIONED ON THE SCREENSHOT):

e_cross = ((a * xc) + (b * yc) + c) / np.sqrt(a**2 + b**2)

# HEADING ERROR CALCULATION (AS MENTIONED ON THE SCREENSHOT):
e_heading = np.arctan2(-a, b) - yaw

#e_heading = (e_heading + np.pi) % (2 * np.pi) - np.pi

# if e_heading > np.pi:
#     e_heading -= 2 * np.pi
# if e_heading < - np.pi:
#     e_heading += 2 * np.pi
# if e_heading > 0:
#     e_cross = abs(e_cross)
# else:
#     e_cross = - abs(e_cross)

# CROSS-TRACK STEERING (AS MENTIONED ON THE SCREENSHOT):
steer_cross = np.arctan(K * e_cross / v + 0.000001)

# TOTAL STEERING (AS MENTIONED ON THE SCREENSHOT):
steering_out = e_heading + steer_cross

#steering_out = (steering_out + np.pi) % (2 * np.pi) - np.pi
# if steering_out > np.pi:
#     steering_out -= 2 * np.pi
# if steering_out < - np.pi:
#     steering_out += 2 * np.pi

However, when I run the simulation, the car starts normally but then towards the turn (before the reference steering), it steers around 20 - 30 degrees abnormally, causing collision. I tried different gains but received the same result. I suspect that there is a mistake in my code. Can you please direct me towards a solution?

Answer 1

While matching up your code against the Stanford paper I did some rewriting, which I pasted below. The handling of ref and arr were pretty confusing, so I introduced somewhat more transparent terms. Also I tried to follow the paper's nomenclature although I chose names for some anonymous quantities.

It's generally a "code smell" when you find yourself worrying about dividing by zero when it's just an artifact of the coordinate system that something has infinite slope. Like if you could rotate everything by $45^o$ the problem would go away (or move somewhere else!) A related issue is doing atan of a quotient, which is better done with atan2.

# from elsewhere
v = <speed of car>
k = <tracking control coefficient>

# location of car
xc, yc = <location of car>

# orientation of car aka yaw
theta_c = <orientation of car>

# previous and next waypoints
x1, y1 = <previous waypoint>
x2, y2 = <next waypoint>

# cross track error: distance of car xc,yc from the line defined
# by the two waypoints.
# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
e_numerator = (x2-x1) * (y1-yc) - (x1-xc) * (y2-y1)
e_denominator = sqrt((x2-x1)**2 + (y2-y1)**2)
e = e_numerator / e_denominator

# direction of correct track
theta_track = atan2(y2-y1, x2-x1)

# heading error
psi = theta_track - theta_c

# cross track steering correction
theta_xc = atan2(v, k * e)

# steering
delta = psi + theta_xc