def steering_control(self, target_lane_index):
"""
Steer the vehicle to follow the center of an given lane.
1. Lateral position is controlled by a proportional controller yielding a lateral velocity command
2. Lateral velocity command is converted to a heading reference
3. Heading is controlled by a proportional controller yielding a heading rate command
4. Heading rate command is converted to a steering angle
:param target_lane_index: index of the lane to follow
:return: a steering wheel angle command [rad]
"""
target_lane = self.road.network.get_lane(target_lane_index)
lane_coords = target_lane.local_coordinates(self.position)
lane_next_coords = lane_coords[0] + self.velocity * self.PURSUIT_TAU
lane_future_heading = target_lane.heading_at(lane_next_coords)
# Lateral position control
lateral_velocity_command = -self.KP_LATERAL * lane_coords[1]
# Lateral velocity to heading
heading_command = np.arcsin(
np.clip(lateral_velocity_command / utils.not_zero(self.velocity),
-1, 1))
heading_ref = lane_future_heading + np.clip(heading_command,
-np.pi / 4, np.pi / 4)
# Heading control
heading_rate_command = self.KP_HEADING * utils.wrap_to_pi(heading_ref -
self.heading)
# Heading rate to steering angle
steering_angle = np.arctan(
self.LENGTH / utils.not_zero(self.velocity) * heading_rate_command)
steering_angle = np.clip(steering_angle, -self.MAX_STEERING_ANGLE,
self.MAX_STEERING_ANGLE)
return steering_angle
def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]:
delta = position - self.center
phi = np.arctan2(delta[1], delta[0])
phi = self.start_phase + utils.wrap_to_pi(phi - self.start_phase)
r = np.linalg.norm(delta)
longitudinal = self.direction * (phi - self.start_phase) * self.radius
lateral = self.direction * (self.radius - r)
return longitudinal, lateral
def distance_with_heading(self,
position: np.ndarray,
heading: Optional[float],
heading_weight: float = 1.0):
"""Compute a weighted distance in position and heading to the lane."""
if heading is None:
return self.distance(position)
s, r = self.local_coordinates(position)
angle = np.abs(wrap_to_pi(heading - self.heading_at(s)))
return abs(r) + max(s - self.length, 0) + max(
0 - s, 0) + heading_weight * angle
def steering_features(self, target_lane_index):
"""
A collection of features used to follow a lane
:param target_lane_index: index of the lane to follow
:return: a array of features
"""
lane = self.road.network.get_lane(target_lane_index)
lane_coords = lane.local_coordinates(self.position)
lane_next_coords = lane_coords[0] + self.velocity * self.PURSUIT_TAU
lane_future_heading = lane.heading_at(lane_next_coords)
features = np.array([utils.wrap_to_pi(lane_future_heading - self.heading) *
self.LENGTH / utils.not_zero(self.velocity),
-lane_coords[1] * self.LENGTH / (utils.not_zero(self.velocity) ** 2)])
return features
