def d_star(cls, bwd: "Car", fwd: "Car"):
v0 = utils.not_zero(max(bwd.speed, 0))
v1 = utils.not_zero(max(fwd.speed, 0))
dv = v0 - v1
vdv = v0 * dv
vT = v0 * cls.T
v_fwd = vdv / cls.AB_TERM
s = max(0, vT * v_fwd)
return cls.s0 + s
def calc_acceleration2(cls, bwd: "Car", fwd: "Car" = None) -> float:
if bwd is None:
return 0
v0 = utils.not_zero(max(bwd.speed, 0))
v_star = utils.not_zero(getattr(bwd, "target_speed", 0))
acc = cls.a * (1 - np.power(v0 / v_star, cls.DELTA)
) if v0 <= v_star else cls.a * (1 - v0 / v_star)
if fwd:
s_gap = utils.not_zero(bwd.lane_distance_to(fwd))
s_star = cls.d_star(bwd, fwd)
interactive_term = s_star**2 / s_gap**2
acc -= cls.a * interactive_term
return max(-cls.b, acc)
def calc_acceleration(cls, bwd: "Car", fwd: "Car" = None) -> float:
if bwd is None:
return 0
v0 = utils.not_zero(max(bwd.speed, 0))
v_star = utils.not_zero(getattr(bwd, "target_speed", 0))
a = cls.a
if v0 < 1:
a = a * 3
acc = a * (1 - np.power(v0 / v_star, cls.DELTA)
) if v0 <= v_star else a * (1 - v0 / v_star)
noise = 0
if fwd:
s_gap = utils.not_zero(bwd.lane_distance_to(fwd))
same_lane = bwd.lane.index == fwd.lane.index
if not same_lane:
if bwd.lane.is_onramp or fwd.lane.is_onramp and v0 < 1 and s_gap < 8:
noise = 0.3 * (np.random.random() - 0.5)
if same_lane and s_gap < cls.s0:
return -cls.b_max
s_star = cls.d_star(bwd, fwd)
interactive_term = s_star**2 / s_gap**2
acc -= a * interactive_term
acc += noise
return max(-cls.b, acc)
def steering_control(self) -> float:
"""
Steer the vehicle to follow the center of an given lane.
1. Lateral position is controlled by a proportional controller yielding a lateral speed command
2. Lateral speed 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: lane to follow
:return: a steering wheel angle command [rad]
"""
v = utils.not_zero(self.speed)
heading = self.heading
target_lane = self.target_lane
s1, r1 = target_lane.local_coordinates(self.position)
s2 = s1 + (v * PURSUIT_TAU)
v_lat = (-KP_LATERAL * r1) / v
v_lat = np.clip(v_lat, -1, 1)
heading_long = target_lane.heading_at(s2)
heading_lat = np.clip(np.arcsin(v_lat), -PI_QUARTER, PI_QUARTER)
heading_ref = heading_long + heading_lat - heading
# Heading control
heading_rate = KP_HEADING * utils.wrap_to_pi(heading_ref)
# Heading rate to steering angle
angle = np.clip(heading_rate * (self.HALF_LENGTH / v), -1, 1)
steering_angle = np.arcsin(angle)
steering_angle = min(steering_angle, MAX_STEERING_ANGLE)
steering_angle = max(steering_angle, -MAX_STEERING_ANGLE)
return steering_angle
def calc_acceleration_to_end(cls, bwd: "Car") -> float:
if bwd is None:
return 0
v0 = max(bwd.speed, 0)
v_star = utils.not_zero(getattr(bwd, "target_speed", 0))
s = bwd.s_remaining_on_path
v1 = 0
dv = v0 - v1
vdv = v0 * dv
vT = v0 * cls.T
v_fwd = vdv / cls.AB_TERM
s_star = cls.s0 + max(0, vT * v_fwd)
acc = cls.a * (1 - np.power(v0 / v_star, cls.DELTA) -
(s_star**2 / s**2))
return max(-2, acc)