def curvature_calculation(trajectory, offset=0):
number_ahead_points = 5
relative_heading_sum, relative_distant_sum = 0, 0
if len(trajectory[2]) <= number_ahead_points + offset:
return 1e20
for i in range(number_ahead_points):
relative_heading_temp = min_angles_difference_signed(
trajectory[2][i + 1 + offset], trajectory[2][i + offset]
)
relative_heading_sum += relative_heading_temp
relative_distant_sum += abs(
math.sqrt(
(trajectory[0][i + offset] - trajectory[0][i + offset + 1]) ** 2
+ (trajectory[1][i + offset] - trajectory[1][i + offset + 1]) ** 2
)
)
# If relative_heading_sum is zero, then the local radius
# of curvature is infinite, i.e. the local trajectory is
# similar to a straight line.
if relative_heading_sum == 0:
return 1e20
else:
curvature_radius = relative_distant_sum / relative_heading_sum
if curvature_radius == 0:
curvature_radius == 1e-2
return curvature_radius
def yaw_rate(self) -> Optional[float]:
# in rad/s
if self._last_dt and self._last_dt > 0:
delta = min_angles_difference_signed(self._pose.heading,
self._last_heading)
return delta / self._last_dt
return None
def calulate_heading_lateral_error(
vehicle, trajectory, initial_look_ahead_distant, speed_reduction_activation
):
heading_error = min_angles_difference_signed(
(vehicle.heading % (2 * math.pi)), trajectory[2][0]
)
# Number of look ahead points to calculate the
# look ahead error.
# TODO: Find the number of points using the
# distance between trajectory points.
look_ahead_points = initial_look_ahead_distant
# If we are on the curvy portion of the road
# we need to decrease the values for look ahead calculation
# The value 30 for road curviness is obtained empirically
# for bullet model.
if (
abs(TrajectoryTrackingController.curvature_calculation(trajectory, 4)) < 30
and speed_reduction_activation
):
initial_look_ahead_distant = 1
look_ahead_points = 1
path_vector = radians_to_vec(
trajectory[2][min([look_ahead_points, len(trajectory[2]) - 1])]
)
vehicle_look_ahead_pt = [
vehicle.position[0]
- initial_look_ahead_distant * math.sin(vehicle.heading),
vehicle.position[1]
+ initial_look_ahead_distant * math.cos(vehicle.heading),
]
lateral_error = signed_dist_to_line(
vehicle_look_ahead_pt,
[
trajectory[0][min([look_ahead_points, len(trajectory[0]) - 1])],
trajectory[1][min([look_ahead_points, len(trajectory[1]) - 1])],
],
path_vector,
)
return (heading_error, lateral_error)
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.
wp_paths = sensor_state.mission_planner.waypoint_paths_at(sim,
vehicle.pose,
lookahead=16)
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,
)
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
# 5.5 is the gain of feedforward term for throttle. This term is
# directly related to the steering angle, this is added to further
# enhance the speed tracking performance. TODO: currently, the bullet
# does not provide the lateral acceleration which is needed for
# calculating the front laterl force. we need to replace the coefficent
# with better approximation of the front lateral forces using explicit
# differention.
lateral_force_coefficient = 1.5
if vehicle.speed < 8 or target_speed < 6:
lateral_force_coefficient = 0
# 0.2 is the coefficent of d-controller for speed tracking
# 0.1 is the coefficent of I-controller for speed tracking
raw_throttle += (
-0.2 * velocity_error_damping_term -
0.1 * state.integral_speed_error +
abs(lateral_force_coefficient *
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)
max_steering_nomralized = 1
if abs(curvature_radius) > 1e7 and lane_change != 0:
heading_speed_gain = -4.95
yaw_rate_speed_gain = 1
lateral_speed_gain = 0.22
max_steering_nomralized = 0.12
heading_error = min_angles_difference_signed(
(vehicle.heading % (2 * math.pi)), reference_heading)
steering_norm = np.clip(
-heading_speed_gain * math.degrees(state.heading_error_gain) *
heading_error + 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,
-max_steering_nomralized,
max_steering_nomralized,
)
# 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,
):
"""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,
)
