def calculate_raw_throttle_feedback(
vehicle,
state,
trajectory,
velocity_gain,
velocity_integral_gain,
integral_velocity_error,
velocity_damping_gain,
windup_gain,
traction_gain,
speed_reduction_activation,
throttle_filter_constant,
dt_sec,
):
desired_speed = trajectory[3][0]
# If the vehicle is on the curvy portion of the road, then the desired speed
# will be reduced to 80 percent of the desired speed of the trajectory.
# Value 4 is the number of ahead trajectory points for starting the calculation
# of the radius of curvature. Value 100 is the threshold for reducing the
# desired speed profile. This value can be approximated using the
# formula v^2/R=mu*g. In addition, we used additional threshold 30 for very
# sharp turn in which the desired speed of the vehicle is set to 80
# percent of the original values with the upperbound of 29.8 m/s(30 km/hr).
absolute_ahead_curvature = abs(
TrajectoryTrackingController.curvature_calculation(trajectory, 4)
)
if absolute_ahead_curvature < 30 and speed_reduction_activation:
desired_speed = np.clip(0.8 * desired_speed, 0, 8.3)
elif absolute_ahead_curvature < 100 and speed_reduction_activation:
desired_speed *= 0.8
# Main velocity profile tracking controller, the default gain value is 0.1.
# Default value 3 for the traction controller term involving lateral velocity is
# chosen for better performance on curvy portion of the road. Note that
# it should be at least one order of magnitude above the velocity
# tracking term to ensure stability.
velocity_error = vehicle.speed - desired_speed
velocity_error_damping_term = (velocity_error - state.velocity_error) / dt_sec
raw_throttle = METER_PER_SECOND_TO_KM_PER_HR * (
-1 * velocity_gain * velocity_error
- velocity_integral_gain
* (integral_velocity_error + windup_gain * state.integral_windup_error)
- velocity_damping_gain * velocity_error_damping_term
)
state.velocity_error = velocity_error
# The anti-windup term inspired by:
# https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-30-feedback-control-systems-fall-2010/lecture-notes/MIT16_30F10_lec23.pdf
state.integral_windup_error = np.clip(raw_throttle, -1, 1) - raw_throttle
# The low pass filter for throttle. The traction control term is
# applied after filter is applied to the velocity feedback.
state.throttle_state = low_pass_filter(
raw_throttle,
state.throttle_state,
throttle_filter_constant,
dt_sec,
raw_value=-traction_gain
* abs(vehicle.chassis.longitudinal_lateral_speed[1]),
)
return (state.throttle_state, desired_speed)
def perform_lane_following(
cls,
sim,
agent_id,
vehicle,
controller_state,
sensor_state,
target_speed=12.5,
lane_change=0,
):
assert isinstance(vehicle.chassis, AckermannChassis)
state = controller_state
# This lookahead value is coupled with a few calculations below, changing it
# may affect stability of the controller.
print(f"start wp_paths")
wp_paths = sensor_state.mission_planner.waypoint_paths_at(sim,
vehicle.pose,
lookahead=30)
print(f"in l_f_c, wps {wp_paths}")
current_lane = LaneFollowingController.find_current_lane(
wp_paths, vehicle.position)
wp_path = wp_paths[np.clip(current_lane + lane_change, 0,
len(wp_paths) - 1)]
# we compute a road "curviness" to inform our throttle activation.
# We should move slowly when we are on curvy roads.
ewma_road_curviness = 0.0
for wp_a, wp_b in reversed(list(zip(wp_path, wp_path[1:]))):
ewma_road_curviness = lerp(
ewma_road_curviness,
math.degrees(abs(wp_a.relative_heading(wp_b.heading))),
0.03,
)
# print(f"[wp] point is {wp_a}")
road_curviness_normalization = 2.5
road_curviness = np.clip(
ewma_road_curviness / road_curviness_normalization, 0, 1)
# Number of trajectory point used for curvature calculation.
num_trajectory_points = min([10, len(wp_path)])
trajectory = [
[wp_path[i].pos[0] for i in range(num_trajectory_points)],
[wp_path[i].pos[1] for i in range(num_trajectory_points)],
[wp_path[i].heading for i in range(num_trajectory_points)],
]
# The following calculates the radius of curvature for the 4th
# waypoints in the waypoint list. Value 4 is chosen to ensure
# that the heading error correction is triggered before the vehicle
# reaches to a sharp turn defined be min_curvature.
look_ahead_curvature = abs(
TrajectoryTrackingController.curvature_calculation(trajectory, 4))
# Minimum curvature limit for pushing forward the waypoint
# which is used for heading error calculation.
min_curvature = 2
# If the look_ahead_curvature is less than the min_curvature, then
# update the location of the points which its curvature is less than
# min_curvature.
if look_ahead_curvature <= min_curvature:
state.min_curvature_location = (wp_path[4].pos[0],
wp_path[4].pos[1])
# LOOK AHEAD ERROR SETTING
# look_ahead_wp_num is the ahead waypoint which is used to
# calculate the lateral error TODO: use adaptive setting to
# choose the ahead waypoint
# if the road_curviness is high(i.e. > 0.5), we reduce the
# look_ahead_wp_num to calculate a more accurate lateral error
# normal look ahead distant is set to 8 meters which is reduced
# to 6 meters when the curvature increases.
# Note: waypoints are spaced at roughly 1 meter apart
if road_curviness > 0.5:
look_ahead_wp_num = 3
else:
look_ahead_wp_num = 4
look_ahead_wp_num = min(look_ahead_wp_num, len(wp_path) - 1)
reference_heading = wp_path[0].heading
look_ahead_wp = wp_path[look_ahead_wp_num]
look_ahead_dist = look_ahead_wp.dist_to(vehicle.position)
vehicle_look_ahead_pt = [
vehicle.position[0] - look_ahead_dist * math.sin(vehicle.heading),
vehicle.position[1] + look_ahead_dist * math.cos(vehicle.heading),
]
# 5.56 m/s (20 km/h), 6.94 m/s (25 km/h) are desired speed for different thresholds
# for road curviness
# 0.5 , 0.8 are dimensionless thresholds for road_curviness.
# 1.8 and 0.6 are the longitudinal velocity controller
# proportional gains for different road curvinesss.
if road_curviness < 0.3:
raw_throttle = (-METER_PER_SECOND_TO_KM_PER_HR * 1.8 *
(vehicle.speed - target_speed))
elif road_curviness > 0.3 and road_curviness < 0.8:
raw_throttle = (-0.6 * METER_PER_SECOND_TO_KM_PER_HR *
(vehicle.speed - np.clip(target_speed, 0, 6.94)))
else:
raw_throttle = (-0.6 * METER_PER_SECOND_TO_KM_PER_HR *
(vehicle.speed - np.clip(target_speed, 0, 5.56)))
speed_error = vehicle.speed - target_speed
state.integral_speed_error += speed_error * sim.timestep_sec
velocity_error_damping_term = (speed_error -
state.speed_error) / sim.timestep_sec
# 0.2 is the coefficent of d-controller for speed tracking
# 0.1 is the coefficent of I-controller for speed tracking
# 5.5 is the gain of feedforward term. This term is directly
# related to the steering angle, this is added to further enhance
# the speed tracking performance.
raw_throttle += (
-0.2 * velocity_error_damping_term -
0.1 * state.integral_speed_error +
abs(5.5 *
math.sin(state.steering_state * vehicle.max_steering_wheel)))
state.speed_error = speed_error
# If the distance of the vehicle to the ahead point for which
# the waypoint curvature is less than min_curvature is less than
# 2 meters, then push forward the waypoint which is used to
# calculate the heading error.
if (state.min_curvature_location != (None, None)) and math.sqrt(
(vehicle.position[0] - state.min_curvature_location[0])**2 +
(vehicle.position[1] - state.min_curvature_location[1])**2) < 2:
reference_heading = wp_path[look_ahead_wp_num].heading
# Desired closed loop poles of the lateral dynamics
# The higher the absolute value, the closed loop response will
# be faster for that state, the four states of that are used for
# Linearization of the lateral dynamics are:
# [lateral error, heading error, yaw_rate, side_slip angle]
desired_poles = np.array([
cls.lateral_error,
cls.heading_error,
cls.yaw_rate,
cls.side_slip_angle,
])
LaneFollowingController.calculate_lateral_gains(
sim, state, vehicle, desired_poles, target_speed)
# LOOK AHEAD CONTROLLER
controller_lat_error = wp_path[look_ahead_wp_num].signed_lateral_error(
vehicle_look_ahead_pt)
abs_heading_error = min(
abs((vehicle.heading % (2 * math.pi)) - reference_heading),
abs(2 * math.pi - abs((vehicle.heading %
(2 * math.pi)) - reference_heading)),
)
curvature_radius = TrajectoryTrackingController.curvature_calculation(
trajectory)
brake_norm = 0
if raw_throttle < 0:
brake_norm = np.clip(-raw_throttle, 0, 1)
throttle_norm = 0
else:
# The term involving absolute value of the lateral speed is
# added as a traction control strategy, The traction controller
# gain is set to 4.5, the lower the value, the vehicle becomes
# more agile but may result in instability in harsh curves
# with high speeds.
if vehicle.speed > 70 / 3.6 and abs(curvature_radius) <= 1e3:
traction_gain = 4.5
elif 40 / 3.6 <= vehicle.speed <= 70 / 3.6 and abs(
curvature_radius) <= 3:
traction_gain = 2.5
else:
traction_gain = 0.5
throttle_norm = np.clip(
raw_throttle - traction_gain * METER_PER_SECOND_TO_KM_PER_HR *
abs(vehicle.chassis.longitudinal_lateral_speed[1]),
0,
1,
)
# The feedback term involving yaw rate is added to reduce
# the oscillation in vehicle heading, the proportional gain for
# yaw rate is set to 2.75, the higher value results in less
# oscillation in heading angle. 0.3 is the integral controller
# gain for lateral error. The feedforward term based on the
# curvature is added to enhance the transient performance when
# the road curvature changes locally.
state.lateral_integral_error += sim.timestep_sec * controller_lat_error
# The feed forward term for the steering controller. This
# term is proportionate to Ux^2/R. The coefficient 0.15 is
# chosen to enhance the transient tracking performance.
# This coefficient also depends on the inertia properties
# and the cornering stiffness of the tires. See:
# https://www.tandfonline.com/doi/full/10.1080/00423114.2015.1055279
steering_feed_forward_gain = 0.15
if abs(curvature_radius) < 7:
steering_feed_forward_gain = 0.45
steering_controller_feed_forward = (1 * steering_feed_forward_gain *
(1 / curvature_radius) *
(vehicle.speed)**2)
normalized_speed = np.clip(vehicle.speed * 3.6 / 100, 0, 1)
heading_speed_gain = lerp(0.5, 14, normalized_speed)
yaw_rate_speed_gain = lerp(5.75, 11.75, normalized_speed)
lateral_speed_gain = np.clip(lerp(-1, 14, normalized_speed), 1, 2)
steering_norm = np.clip(
-heading_speed_gain * math.degrees(state.heading_error_gain) *
(abs_heading_error * np.sign(reference_heading -
(vehicle.heading % (2 * math.pi)))) +
lateral_speed_gain * state.lateral_error_gain *
(controller_lat_error) +
yaw_rate_speed_gain * vehicle.chassis.yaw_rate[2] +
0.3 * state.lateral_integral_error -
steering_controller_feed_forward,
-1,
1,
)
# The steering low pass filter, 5.5 is the constant of the
# first order linear low pass filter.
steering_filter_constant = 5.5
state.steering_state = low_pass_filter(
steering_norm,
state.steering_state,
steering_filter_constant,
sim.timestep_sec,
)
# The Throttle low pass filter, 2 is the constant of the
# first order linear low pass filter.
# TODO: Add low pass filter for brake.
throttle_filter_constant = 2
state.throttle_state = low_pass_filter(
throttle_norm,
state.throttle_state,
throttle_filter_constant,
sim.timestep_sec,
lower_bound=0,
)
# Applying control actions to the vehicle
vehicle.control(
throttle=state.throttle_state,
brake=brake_norm,
steering=state.steering_state,
)
LaneFollowingController._update_target_lane_if_reached_end_of_lane(
agent_id, vehicle, controller_state, sensor_state)
def perform_trajectory_tracking_PD(
trajectory, vehicle, state, dt_sec,
):
# Controller parameters for trajectory tracking.
params = vehicle.chassis.controller_parameters
final_heading_gain = params["final_heading_gain"]
final_lateral_gain = params["final_lateral_gain"]
final_steering_filter_constant = params["final_steering_filter_constant"]
throttle_filter_constant = params["throttle_filter_constant"]
velocity_gain = params["velocity_gain"]
velocity_integral_gain = params["velocity_integral_gain"]
traction_gain = params["traction_gain"]
final_lateral_error_derivative_gain = params[
"final_lateral_error_derivative_gain"
]
final_heading_error_derivative_gain = params[
"final_heading_error_derivative_gain"
]
initial_look_ahead_distant = params["initial_look_ahead_distant"]
derivative_activation = params["derivative_activation"]
speed_reduction_activation = params["speed_reduction_activation"]
velocity_damping_gain = params["velocity_damping_gain"]
windup_gain = params["windup_gain"]
curvature_radius = TrajectoryTrackingController.curvature_calculation(
trajectory
)
# The gains are varying according to the desired velocity along
# the trajectory. To achieve this, the desired speed is normalized
# between 20 (km/hr) to 80 (km/hr) and the respective gains are
# calculated using interpolation. 3, 0.03, 1.5, 0.2 are the
# controller gains for lateral error, heading error and their
# derivatives at the desired speed 20 (km/hr).
normalized_speed = np.clip(
(METER_PER_SECOND_TO_KM_PER_HR * trajectory[3][0] - 20) / (80 - 20), 0, 1
)
lateral_gain = lerp(3, final_lateral_gain, normalized_speed)
heading_gain = lerp(0.03, final_heading_gain, normalized_speed)
steering_filter_constant = lerp(
2, final_steering_filter_constant, normalized_speed
)
heading_error_derivative_gain = lerp(
1.5, final_heading_error_derivative_gain, normalized_speed
)
lateral_error_derivative_gain = lerp(
0.2, final_lateral_error_derivative_gain, normalized_speed
)
(
heading_error,
lateral_error,
) = TrajectoryTrackingController.calulate_heading_lateral_error(
vehicle, trajectory, initial_look_ahead_distant, speed_reduction_activation
)
# Derivative terms of the controller (use with caution for large time steps>=0.1).
# Increasing the values will increase the convergence time and reduces the oscillation.
derivative_term = (
+heading_error_derivative_gain * vehicle.chassis.yaw_rate[2]
+ lateral_error_derivative_gain
* (lateral_error - state.lateral_error)
/ dt_sec
)
# Raw steering controller, default values 0.11 and 0.65 are used for heading and
# lateral control gains.
# TODO: The lateral and heading gains of the steering controller should be
# calculated based on the current velocity. The coefficient value for the
# feed forward term is 0.1 and it depends on the cornering stifness and
# vehicle inertia properties.
steering_feed_forward_term = 0.1 * (1 / curvature_radius) * (vehicle.speed) ** 2
steering_raw = np.clip(
derivative_activation * derivative_term
+ math.degrees(heading_gain * (heading_error))
+ 1 * lateral_gain * lateral_error
- steering_feed_forward_term,
-1,
1,
)
# The steering linear low pass filter.
state.steering_state = low_pass_filter(
steering_raw, state.steering_state, steering_filter_constant, dt_sec
)
(
raw_throttle,
desired_speed,
) = TrajectoryTrackingController.calculate_raw_throttle_feedback(
vehicle,
state,
trajectory,
velocity_gain,
velocity_integral_gain,
state.integral_velocity_error,
velocity_damping_gain,
windup_gain,
traction_gain,
speed_reduction_activation,
throttle_filter_constant,
dt_sec,
)
if raw_throttle > 0:
brake_norm, throttle_norm = 0, np.clip(raw_throttle, 0, 1)
else:
brake_norm, throttle_norm = np.clip(-raw_throttle, 0, 1), 0
state.heading_error = heading_error
state.lateral_error = lateral_error
state.integral_velocity_error += (vehicle.speed - desired_speed) * dt_sec
vehicle.control(
throttle=throttle_norm, brake=brake_norm, steering=state.steering_state,
)
def perform_trajectory_tracking_PD(
trajectory,
vehicle,
state,
dt_sec,
):
"""Attempts proportional derivative control for the vehicle given an expected
trajectory.
"""
# Controller parameters for trajectory tracking.
params = vehicle.chassis.controller_parameters
final_heading_gain = params["final_heading_gain"]
final_lateral_gain = params["final_lateral_gain"]
final_steering_filter_constant = params[
"final_steering_filter_constant"]
throttle_filter_constant = params["throttle_filter_constant"]
velocity_gain = params["velocity_gain"]
velocity_integral_gain = params["velocity_integral_gain"]
traction_gain = params["traction_gain"]
final_lateral_error_derivative_gain = params[
"final_lateral_error_derivative_gain"]
final_heading_error_derivative_gain = params[
"final_heading_error_derivative_gain"]
initial_look_ahead_distant = params["initial_look_ahead_distant"]
derivative_activation = params["derivative_activation"]
speed_reduction_activation = params["speed_reduction_activation"]
velocity_damping_gain = params["velocity_damping_gain"]
windup_gain = params["windup_gain"]
# XXX: note that these values may be further adjusted below based on speed and curvature
# XXX: we might want to combine the computation of these into a single fn
lateral_gain = 0.61
heading_gain = 0.01
lateral_error_derivative_gain = 0.15
heading_error_derivative_gain = 0.5
# The gains can vary according to the desired velocity along
# the trajectory. To achieve this, the desired speed is normalized
# between 20 (km/hr) to 80 (km/hr) and the respective gains are
# calculated using interpolation. 3, 0.03, 1.5, 0.2 are the
# controller gains for lateral error, heading error and their
# derivatives at the desired speed 20 (km/hr).
normalized_speed = np.clip(
(METER_PER_SECOND_TO_KM_PER_HR * trajectory[3][0] - 20) /
(80 - 20), 0, 1)
adjust_gains_for_normalized_speed = False # XXX: not using model config here
if adjust_gains_for_normalized_speed:
lateral_gain = lerp(3, final_lateral_gain, normalized_speed)
lateral_error_derivative_gain = lerp(
0.2, final_lateral_error_derivative_gain, normalized_speed)
heading_gain = lerp(0.03, final_heading_gain, normalized_speed)
heading_error_derivative_gain = lerp(
1.5, final_heading_error_derivative_gain, normalized_speed)
steering_filter_constant = lerp(12, final_steering_filter_constant,
normalized_speed)
elif vehicle.speed > 70 / METER_PER_SECOND_TO_KM_PER_HR:
lateral_gain = 1.51
heading_error_derivative_gain = 0.1
# XXX: This should be handled like the other gains above...
steering_filter_constant = lerp(12, final_steering_filter_constant,
normalized_speed)
if abs(
min_angles_difference_signed(trajectory[2][-1],
trajectory[2][0])) > 2:
throttle_filter_constant = 2.5
if (abs(
TrajectoryTrackingController.curvature_calculation(
trajectory, 0, num_points=3)) < 150):
heading_gain = 0.05
lateral_error_derivative_gain = 0.015
heading_error_derivative_gain = 0.05
(
heading_error,
lateral_error,
) = TrajectoryTrackingController.calculate_heading_lateral_error(
vehicle, trajectory, initial_look_ahead_distant,
speed_reduction_activation)
curvature_radius = TrajectoryTrackingController.curvature_calculation(
trajectory)
# Derivative terms of the controller (use with caution for large time steps>=0.1).
# Increasing the values will increase the convergence time and reduces the oscillation.
z_yaw = vehicle.chassis.velocity_vectors[1][2]
derivative_term = (+heading_error_derivative_gain * z_yaw +
lateral_error_derivative_gain *
(lateral_error - state.lateral_error) / dt_sec)
# Raw steering controller, default values 0.11 and 0.65 are used for heading and
# lateral control gains.
# TODO: The lateral and heading gains of the steering controller should be
# calculated based on the current velocity. The coefficient value for the
# feed forward term is 0.1 and it depends on the cornering stiffness and
# vehicle inertia properties.
steering_feed_forward_term = 0.1 * (1 / curvature_radius) * (
vehicle.speed)**2
steering_raw = np.clip(
derivative_activation * derivative_term +
math.degrees(heading_gain * (heading_error)) +
1 * lateral_gain * lateral_error - steering_feed_forward_term,
-1,
1,
)
# The steering linear low pass filter.
state.steering_state = low_pass_filter(steering_raw,
state.steering_state,
steering_filter_constant,
dt_sec)
(
raw_throttle,
desired_speed,
) = TrajectoryTrackingController.calculate_raw_throttle_feedback(
vehicle,
state,
trajectory,
velocity_gain,
velocity_integral_gain,
state.integral_velocity_error,
velocity_damping_gain,
windup_gain,
traction_gain,
speed_reduction_activation,
throttle_filter_constant,
dt_sec,
)
if raw_throttle > 0:
brake_norm, throttle_norm = 0, np.clip(raw_throttle, 0, 1)
else:
brake_norm, throttle_norm = np.clip(-raw_throttle, 0, 1), 0
state.heading_error = heading_error
state.lateral_error = lateral_error
state.integral_velocity_error += (vehicle.speed -
desired_speed) * dt_sec
vehicle.control(
throttle=throttle_norm,
brake=brake_norm,
steering=state.steering_state,
)
