This paper is a continuation of the introduction to self driving cars final project-w7_pid to realize vehicle lateral and longitudinal control, which aims to change the vehicle lateral controller to Pure pursuit mode. (it is the last project assignment in the first part of Coursera automatic driving series courses)
Introduction to Pure Pursuit
Pure tracking and its variants are often used in mobile robot path tracking problems, and are often used in autopilot cars. The main idea is to calculate the radius of curvature between the rear axle center and the desired tracking point (target point) of the vehicle, and then obtain the steering wheel angle or front wheel angle required for the tracking path.
The target point is determined by the forward looking distance ld from the current rear axle position to the desired path. The target point (gx, gy) is shown in the figure.
Use the target point position and the angle between the vehicle heading vector and the forward-looking vector α To determine the steering angle of the vehicle δ . According to the sine theorem, the following formula is obtained:
Why is the first formula 2 α？ Because in this mathematical model, the vehicle trajectory is approximately an arc. At the position of the rear axle point, the rear axle orientation is tangent to the arc. Because the center of the circle, the center of the rear axle and the target point constitute an isosceles triangle, it can be concluded that the angle is 2 α. In the last equation κ Is the curvature of the arc. According to the bicycle model of Ackerman steering vehicle, the steering angle can be written as:
According to the above two formulas, the rotation angle formula is obtained:
By defining a new variable, the control law can be better understood. eld represents the lateral distance between the heading vector and the target point, and the formula is obtained:
In this case, the curvature formula can be written as:
This formula shows that pure tracking is actually a proportional controller that calculates the angle of rotation according to the tracking error of a certain forward-looking distance ld in front of the vehicle, and the gain of the controller is 2/ld^2.
In practice, the stable gain (forward-looking distance) will be adjusted according to different speeds, which is well understood. Referring to human drivers, when the vehicle speed changes, the distance to be observed is different, which also leads to ld being specified as a function of vehicle speed. Rewrite the control law as follows:
Here, it is a common practice to use the longitudinal speed of the vehicle to control the forward-looking distance in proportion. In addition, the forward-looking distance is usually saturated at the minimum and maximum values.
These are most of the formulas for pure tracking control.
Pure tracking lateral controller in Final project
Similarly, similar to the previous article, the pure tracking controller is separated into a class.
Specific implementation:
#pure_pursuit.py
import numpy as np
class PP(object):
def __init__(self,L, k, k_Ld):
self.L = L
self.k = k
self.k_Ld = k_Ld
def update(self, coeffs, velocity):
'''
coeffs: The coefficient of fitting the tracking curve is used to calculate eld
'''
eps = 0.001
if velocity < eps:
steering_angle = 0.0
else:
# For Pure Pursuit if we look ahead distance changes, then the cte changes
Ld_x = self.k_Ld * velocity
cte = np.polyval(coeffs, Ld_x)
alpha = np.arctan(cte/Ld_x)
sin_alpha = np.sin(alpha)
steering_angle = np.arctan((2*self.L* sin_alpha) / (self.k * velocity))
return steering_angle
contorller2d.py
Similarly, the controlle2d is simply modified, mainly to initialize the PP controller, and then use the PP controller to update the steering wheel angle.
#controller2d.py
#!/usr/bin/env python3
"""
2D Controller Class to be used for the CARLA waypoint follower demo.
"""
import cutils
import numpy as np
from pid import PID
from pure_pursuit import PP
class Controller2D(object):
def __init__(self, waypoints):
self.vars = cutils.CUtils()
self._current_x = 0
self._current_y = 0
self._current_yaw = 0
self._current_speed = 0
self._desired_speed = 0
self._current_frame = 0
self._current_timestamp = 0
self._start_control_loop = False
self._set_throttle = 0
self._set_brake = 0
self._set_steer = 0
self._waypoints = waypoints
self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
self._pi = np.pi
self._2pi = 2.0 * np.pi
## PID
self.throttle_brake_pid = PID(P=7.0, I=1.0, D=1.026185)
self.throttle_brake_pid.setSampleTime = 0.033
## Pure Pursuit
self.pp = PP(L=4.5, k=1.00, k_Ld=1.3)
def update_values(self, x, y, yaw, speed, timestamp, frame):
self._current_x = x
self._current_y = y
self._current_yaw = yaw
self._current_speed = speed
self._current_timestamp = timestamp
self._current_frame = frame
if self._current_frame:
self._start_control_loop = True
def update_desired_speed(self):
min_idx = 0
min_dist = float("inf")
desired_speed = 0
for i in range(len(self._waypoints)):
dist = np.linalg.norm(
np.array([
self._waypoints[i][0] - self._current_x,
self._waypoints[i][1] - self._current_y
]))
if dist < min_dist:
min_dist = dist
min_idx = i
if min_idx < len(self._waypoints) - 1:
desired_speed = self._waypoints[min_idx][2]
self.target_wp = self._waypoints[min_idx]
else:
desired_speed = self._waypoints[-1][2]
self.target_wp = self._waypoints[-1]
self._desired_speed = desired_speed
def update_waypoints(self, new_waypoints):
self._waypoints = new_waypoints
def get_commands(self):
return self._set_throttle, self._set_steer, self._set_brake
def set_throttle(self, input_throttle):
# Clamp the throttle command to valid bounds
throttle = np.fmax(np.fmin(input_throttle, 1.0), 0.0)
self._set_throttle = throttle
def set_steer(self, input_steer_in_rad):
# Covnert radians to [-1, 1]
input_steer = self._conv_rad_to_steer * input_steer_in_rad
# Clamp the steering command to valid bounds
steer = np.fmax(np.fmin(input_steer, 1.0), -1.0)
self._set_steer = steer
def set_brake(self, input_brake):
# Clamp the steering command to valid bounds
brake = np.fmax(np.fmin(input_brake, 1.0), 0.0)
self._set_brake = brake
def map_coord_2_Car_coord(self, x, y, yaw, waypoints):
wps = np.squeeze(waypoints)
wps_x = wps[:, 0]
wps_y = wps[:, 1]
num_wp = wps.shape[0]
## create the Matrix with 3 vectors for the waypoint x and y coordinates w.r.t. car
wp_vehRef = np.zeros(shape=(3, num_wp))
cos_yaw = np.cos(-yaw)
sin_yaw = np.sin(-yaw)
wp_vehRef[0, :] = cos_yaw * (wps_x - x) - sin_yaw * (wps_y - y)
wp_vehRef[1, :] = sin_yaw * (wps_x - x) + cos_yaw * (wps_y - y)
return wp_vehRef
def update_controls(self):
######################################################
# RETRIEVE SIMULATOR FEEDBACK
######################################################
x = self._current_x
y = self._current_y
yaw = self._current_yaw
v = self._current_speed
self.update_desired_speed()
v_desired = self._desired_speed
t = self._current_timestamp
waypoints = self._waypoints
throttle_output = 0
steer_output = 0
brake_output = 0
######################################################
######################################################
# MODULE 7: DECLARE USAGE VARIABLES HERE
######################################################
######################################################
"""
Use 'self.vars.create_var(<variable name>, <default value>)'
to create a persistent variable (not destroyed at each iteration).
This means that the value can be stored for use in the next
iteration of the control loop.
Example: Creation of 'v_previous', default value to be 0
self.vars.create_var('v_previous', 0.0)
Example: Setting 'v_previous' to be 1.0
self.vars.v_previous = 1.0
Example: Accessing the value from 'v_previous' to be used
throttle_output = 0.5 * self.vars.v_previous
"""
self.vars.create_var('v_previous', 0.0)
# Skip the first frame to store previous values properly
if self._start_control_loop:
"""
Controller iteration code block.
Controller Feedback Variables:
x : Current X position (meters)
y : Current Y position (meters)
yaw : Current yaw pose (radians)
v : Current forward speed (meters per second)
t : Current time (seconds)
v_desired : Current desired speed (meters per second)
(Computed as the speed to track at the
closest waypoint to the vehicle.)
waypoints : Current waypoints to track
(Includes speed to track at each x,y
location.)
Format: [[x0, y0, v0],
[x1, y1, v1],
...
[xn, yn, vn]]
Example:
waypoints[2][1]:
Returns the 3rd waypoint's y position
waypoints[5]:
Returns [x5, y5, v5] (6th waypoint)
Controller Output Variables:
throttle_output : Throttle output (0 to 1)
steer_output : Steer output (-1.22 rad to 1.22 rad)
brake_output : Brake output (0 to 1)
"""
wps_vehRef = self.map_coord_2_Car_coord(x, y, yaw, waypoints)
wps_vehRef_x = wps_vehRef[0, :]
wps_vehRef_y = wps_vehRef[1, :]
## fit a 3rd order polynomial to the waypoints
coeffs = np.polyfit(wps_vehRef_x, wps_vehRef_y, 7)
#vel_poly = np.polyfit(wps_vehRef_x, wps_vehRef_y, 3)
## get cross-track error from fit
# In vehicle coordinates the cross-track error is the intercept at x = 0, That means that since we have a fit of the form:
# Y = C0 + C1*X + C2*X^2 + C3X^3 + ....
# when we evaluate using x=0 we just get C0.
# But to understand where this is coming from I like to keep the whole evaluation, even though this is exactly C0
CarRef_x = CarRef_y = CarRef_yaw = 0.0
# For Pure Pursuit if we look ahead a distance Ld = nnnn then the cte changes
cte = np.polyval(coeffs, CarRef_x) - CarRef_y
# get orientation error from fit ( Since we are trying a 3rd order poly, then, f' = a1 + 2*a2*x + 3*a3*x2)
# in this case and since we moved our reference sys to the Car, x = 0 and also yaw = 0
yaw_err = CarRef_yaw - np.arctan(coeffs[1])
speed_err = v_desired - v
######################################################
######################################################
# MODULE 7
# LONGITUDINAL CONTROLLER
# AND
# LATERAL CONTROLLER HERE
######################################################
######################################################
"""
Implement a longitudinal controller here. Remember that you can
access the persistent variables declared above here. For
example, can treat self.vars.v_previous like a "global variable".
"""
"""
Implement a lateral controller here. Remember that you can
access the persistent variables declared above here. For
example, can treat self.vars.v_previous like a "global variable".
"""
#### PID ####
#Longitudinal control PID
self.throttle_brake_pid.update(speed_err,
output_limits=[-1.0, 1.00])
if self.throttle_brake_pid.output < 0.0:
throttle_output = 0
brake_output = -self.throttle_brake_pid.output
else:
throttle_output = self.throttle_brake_pid.output
brake_output = 0
#### PURE PURSUIT ####
#Lateral control PP
steer_output = self.pp.update(coeffs, v)
print("speed Err: ", speed_err)
print("cte : ", cte)
######################################################
# SET CONTROLS OUTPUT
######################################################
self.set_throttle(throttle_output) # in percent (0 to 1)
self.set_steer(steer_output) # in rad (-1.22 to 1.22)
self.set_brake(brake_output) # in percent (0 to 1)
######################################################
######################################################
# MODULE 7: STORE OLD VALUES HERE (ADD MORE IF NECESSARY)
######################################################
######################################################
"""
Use this block to store old values (for example, we can store the
current x, y, and yaw values here using persistent variables for use
in the next iteration)
"""
self.vars.v_previous = v # Store forward speed to be used in next step