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, )