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 _process_interp_for_lane_lp( shape_lp: LinkedLanePoint, first_linked_lanepoint: LinkedLanePoint, next_shape_lp: LinkedLanePoint, spacing: float, newly_created_lanepoints: List[LinkedLanePoint], ) -> LinkedLanePoint: lane_id = shape_lp.lp.lane_id curr_lanepoint = first_linked_lanepoint lane_seg_vec = next_shape_lp.lp.pos - shape_lp.lp.pos lane_seg_len = np.linalg.norm(lane_seg_vec) # We set the initial distance into the lane at `spacing` because # we already have a lanepoint along this segment (curr_lanepoint) dist_into_lane_seg = spacing while dist_into_lane_seg < lane_seg_len: p = dist_into_lane_seg / lane_seg_len pos = shape_lp.lp.pos + lane_seg_vec * p # The thresholds for calculating last lanepoint. If the # midpoint between the current lanepoint and the next shape # lanepoint is less than the minimum distance then the last # lanepoint position will be that midpoint. If the midpoint # is closer than last spacing threshold to the next shape # lanepoint, then the last lanepoint will be the current # lanepoint. # XXX: the map scale should be taken into account here. last_spacing_threshold_dist = 0.8 * spacing minimum_dist_next_shape_lp = 1.4 half_distant_current_next_shape_lp = np.linalg.norm( 0.5 * (curr_lanepoint.lp.pos - next_shape_lp.lp.pos)) mid_point_current_next_shape_lp = 0.5 * (next_shape_lp.lp.pos + curr_lanepoint.lp.pos) if half_distant_current_next_shape_lp < minimum_dist_next_shape_lp: pos = mid_point_current_next_shape_lp dist_pos_next_shape_lp = np.linalg.norm(next_shape_lp.lp.pos - pos) if dist_pos_next_shape_lp < last_spacing_threshold_dist: break heading = vec_to_radians(lane_seg_vec) lane_width = lerp(shape_lp.lp.lane_width, next_shape_lp.lp.lane_width, p) speed_limit = lerp(shape_lp.lp.speed_limit, next_shape_lp.lp.speed_limit, p) linked_lanepoint = LinkedLanePoint( lp=LanePoint( pos=pos, heading=Heading(heading), lane_width=lane_width, speed_limit=speed_limit, lane_id=lane_id, lane_index=shape_lp.lp.lane_index, ), nexts=[], is_shape_lp=False, ) curr_lanepoint.nexts.append(linked_lanepoint) curr_lanepoint = linked_lanepoint newly_created_lanepoints.append(linked_lanepoint) dist_into_lane_seg += spacing return curr_lanepoint
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, ): """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, )