275 lines
12 KiB
Python
275 lines
12 KiB
Python
import numpy as np
|
|
|
|
from iqdbc.car.common.filter_simple import FirstOrderFilter
|
|
from iqdbc.car.common.conversions import Conversions as CV
|
|
from iqdbc.car.common.pid import PIDController
|
|
from iqdbc.car import DT_CTRL
|
|
|
|
|
|
class LongControlJerk():
|
|
JERK_LIMIT_MIN = 0.5
|
|
JERK_LIMIT_MIN_NO_LEAD = 0.7
|
|
JERK_LIMIT_MAX = 5.0
|
|
FILTER_GAIN_DISTANCE = [10, 50]
|
|
FILTER_GAIN_DISTANCE_CHANGE = [0, 20]
|
|
FILTER_GAIN_MAX = 0.95
|
|
FILTER_GAIN_MIN = 0.75
|
|
FILTER_GAIN_NO_LEAD = 0.95
|
|
|
|
def __init__(self, dt=DT_CTRL):
|
|
self.dy_up = 0.
|
|
self.dy_down = 0.
|
|
self.jerk_up = 0.
|
|
self.jerk_down = 0.
|
|
self.dt = dt
|
|
self.accel_last = 0.
|
|
self.distance_last = 0.
|
|
self.jerk_limit_min = self.JERK_LIMIT_MIN_NO_LEAD
|
|
|
|
def update(self, enabled, override, distance, has_lead, accel, critical_state):
|
|
# jerk limits by accel change and distance are used to improve comfort while ensuring a fast enough car reaction
|
|
# override mechanics reminder:
|
|
# (1) sending accel = 0 and directly setting jerk to zero results in round about steady accel until harder accel pedal press -> lack of control
|
|
# (2) sending accel = 0 and allowing a high jerk results in a abrupt accel cut -> lack of comfort
|
|
if not enabled:
|
|
self.jerk_up = 0.
|
|
self.jerk_down = 0.
|
|
self.dy_up = 0.
|
|
self.dy_down = 0.
|
|
elif override:
|
|
self.jerk_up = self.JERK_LIMIT_MIN
|
|
self.jerk_down = self.JERK_LIMIT_MIN
|
|
self.dy_up = 0.
|
|
self.dy_down = 0.
|
|
elif critical_state: # force best car reaction
|
|
self.jerk_up = self.JERK_LIMIT_MAX
|
|
self.jerk_down = self.JERK_LIMIT_MAX
|
|
self.dy_up = 0.
|
|
self.dy_down = 0.
|
|
else:
|
|
jerk_limit_min_target = self.JERK_LIMIT_MIN if has_lead else self.JERK_LIMIT_MIN_NO_LEAD # jerk limit min base line
|
|
jerk_limit_min_delta = abs(self.JERK_LIMIT_MIN_NO_LEAD - self.JERK_LIMIT_MIN) * self.dt
|
|
if self.jerk_limit_min < jerk_limit_min_target:
|
|
self.jerk_limit_min = min(self.jerk_limit_min + jerk_limit_min_delta, jerk_limit_min_target)
|
|
elif self.jerk_limit_min > jerk_limit_min_target:
|
|
self.jerk_limit_min = max(self.jerk_limit_min - jerk_limit_min_delta, jerk_limit_min_target)
|
|
|
|
if has_lead:
|
|
distance_change = (self.distance_last - distance) / self.dt if 0 not in (self.distance_last, distance) else 0
|
|
filter_gain_dist = np.interp(distance, self.FILTER_GAIN_DISTANCE, [self.FILTER_GAIN_MAX, self.jerk_limit_min]) # gain by distance
|
|
filter_gain_dist_change = np.interp(abs(distance_change), self.FILTER_GAIN_DISTANCE_CHANGE, [self.jerk_limit_min, self.FILTER_GAIN_MAX]) # gain by distance change
|
|
filter_gain = max(filter_gain_dist, filter_gain_dist_change) # use highest gain
|
|
else:
|
|
filter_gain = self.FILTER_GAIN_NO_LEAD
|
|
|
|
j = (accel - self.accel_last) / self.dt
|
|
|
|
tgt_up = abs(j) if j > 0 else 0.
|
|
tgt_down = abs(j) if j < 0 else 0.
|
|
|
|
# how fast does the car react to acceleration
|
|
self.dy_up += filter_gain * (tgt_up - self.jerk_up - self.dy_up)
|
|
self.jerk_up += self.dt * self.dy_up
|
|
self.jerk_up = np.clip(self.jerk_up, self.jerk_limit_min, self.JERK_LIMIT_MAX)
|
|
|
|
# how fast does the car react to braking
|
|
self.dy_down += filter_gain * (tgt_down - self.jerk_down - self.dy_down)
|
|
self.jerk_down += self.dt * self.dy_down
|
|
self.jerk_down = np.clip(self.jerk_down, self.jerk_limit_min, self.JERK_LIMIT_MAX)
|
|
|
|
self.accel_last = accel
|
|
self.distance_last = distance
|
|
|
|
def get_jerk_up(self):
|
|
return self.jerk_up
|
|
|
|
def get_jerk_down(self):
|
|
return self.jerk_down
|
|
|
|
|
|
class LongControlLimit():
|
|
LOWER_LIMIT_FACTOR = 0.024
|
|
LOWER_LIMIT_MAX = LOWER_LIMIT_FACTOR * 8
|
|
LOWER_LIMIT_MIN = LOWER_LIMIT_FACTOR * 2
|
|
UPPER_LIMIT_FACTOR = 0.0625
|
|
UPPER_LIMIT_MAX = UPPER_LIMIT_FACTOR * 3
|
|
LIMIT_MIN = 0.
|
|
LIMIT_DISTANCE = [10, 100] # limit range
|
|
LIMIT_DISTANCE_CHANGE_DOWN = [0, 20] # high precision for worst case high speed approaching a stopped lead
|
|
LIMIT_DISTANCE_CHANGE_UP = [0, 5] # precisely follow an accelerating lead especially from stop
|
|
LIMIT_DISTANCE_CHANGE_UP_ACT = [0, 60]
|
|
DISTANCE_FILTER_RC = [0.15, 0.6] # smooth noisy distance signal for distant leads
|
|
DISTANCE_TIMEOUT = 1. # seconds
|
|
|
|
def __init__(self, dt=DT_CTRL):
|
|
self.upper_limit = self.LIMIT_MIN
|
|
self.lower_limit = self.LIMIT_MIN
|
|
self.dt = dt
|
|
self.distance_last = 0.
|
|
self.distance_filter = FirstOrderFilter(0.0, rc=self.DISTANCE_FILTER_RC[0], dt=dt, initialized=False)
|
|
self.distance_valid_timer = 0
|
|
|
|
def update(self, enabled: bool, speed: float, set_speed: float, distance: float, has_lead: bool, critical_state: bool):
|
|
# control limits by distance are used to improve comfort while ensuring precise car reaction if neccessary
|
|
# also used to reduce an effect of decel overshoot when target is breaking
|
|
# limits are controlled mainly by distance of lead car
|
|
if not enabled or critical_state: # force most precise accel command execution
|
|
self.upper_limit = self.LIMIT_MIN
|
|
self.lower_limit = self.LIMIT_MIN
|
|
self.distance_valid_timer = 0
|
|
elif not has_lead:
|
|
if self.distance_valid_timer < self.DISTANCE_TIMEOUT: # fluctuation block: keep alive
|
|
self.distance_valid_timer += self.dt
|
|
else: # force most precise
|
|
self.upper_limit = self.LIMIT_MIN
|
|
self.lower_limit = self.LIMIT_MIN
|
|
else:
|
|
distance_change_raw = (self.distance_last - distance) / self.dt if 0 not in (self.distance_last, distance) else 0
|
|
distance_filter_rc = np.interp(distance, self.LIMIT_DISTANCE, self.DISTANCE_FILTER_RC)
|
|
if (self.distance_last == 0 or self.distance_valid_timer != 0) and distance != 0: # for new lead detection reset filter and correctly force current state upon next iteration
|
|
self.distance_filter = FirstOrderFilter(0.0, rc=distance_filter_rc, dt=self.dt, initialized=False)
|
|
distance_change = distance_change_raw
|
|
else:
|
|
self.distance_filter.update_alpha(distance_filter_rc)
|
|
distance_change = self.distance_filter.update(distance_change_raw)
|
|
self.distance_valid_timer = 0
|
|
|
|
# how far can the true accel vary downwards from requested accel
|
|
upper_limit_dist = np.interp(distance, self.LIMIT_DISTANCE, [self.LIMIT_MIN, self.UPPER_LIMIT_MAX]) # base line based on distance
|
|
upper_limit_dist_change = np.interp(-min(0, distance_change), self.LIMIT_DISTANCE_CHANGE_UP, [self.UPPER_LIMIT_MAX, self.LIMIT_MIN]) # limit by distance change up
|
|
upper_limit_dist_change = np.interp(distance, self.LIMIT_DISTANCE_CHANGE_UP_ACT, [upper_limit_dist_change, upper_limit_dist]) # distance change activation
|
|
self.upper_limit = min(upper_limit_dist, upper_limit_dist_change) # use lowest limit
|
|
|
|
# how far can the true accel vary upwards from requested accel
|
|
set_speed_diff_up = max(0, abs(speed) - abs(set_speed)) # set speed difference down requested by user or speed overshoot (includes hud - real speed difference!)
|
|
set_speed_diff_up_factor = np.interp(set_speed_diff_up, [1, 1.75], [1., 0.]) # faster requested speed decrease and less speed overshoot downhill
|
|
lower_limit_dist = np.interp(distance, self.LIMIT_DISTANCE, [self.LOWER_LIMIT_MIN, self.LOWER_LIMIT_MAX]) # base line based on distance
|
|
lower_limit_dist_speed = lower_limit_dist * set_speed_diff_up_factor
|
|
lower_limit_dist_change = np.interp(max(0, distance_change), self.LIMIT_DISTANCE_CHANGE_DOWN, [self.LOWER_LIMIT_MAX, self.LIMIT_MIN]) # limit by distance change down
|
|
self.lower_limit = min(lower_limit_dist_speed, lower_limit_dist_change) # use lowest limit
|
|
|
|
self.distance_last = distance
|
|
|
|
def get_upper_limit(self):
|
|
return self.upper_limit
|
|
|
|
def get_lower_limit(self):
|
|
return self.lower_limit
|
|
|
|
|
|
def sigmoid_curvature_boost_meb(kappa: float, v_ego: float, kappa_thresh: float = 0.0) -> float:
|
|
# compensate non linear behaviour: boost low curvatures
|
|
# this is either a model issue (nerfing low curvatures) or a specific steering rack behaviour
|
|
v_points = np.array([20.0, 40.0])
|
|
boost_values = np.array([1.5, 2.1]) # increase boost amplitude with speed
|
|
boost = float(np.interp(v_ego, v_points, boost_values))
|
|
steepness_values = np.array([5000.0, 3200.0]) # increase boost area with speed
|
|
steepness = float(np.interp(v_ego, v_points, steepness_values))
|
|
|
|
abs_kappa = abs(kappa)
|
|
boost_factor = 1.0 + (boost - 1.0) / (1 + np.exp(steepness * (abs_kappa - kappa_thresh)))
|
|
|
|
return np.sign(kappa) * abs_kappa * boost_factor
|
|
|
|
|
|
def map_speed_to_acc_tempolimit(v_ms):
|
|
acc_tempolimit_kph = { # DBC Mapping
|
|
1: 5, 2: 7, 3: 10, 4: 15, 5: 20, 6: 25, 7: 30, 8: 35,
|
|
9: 40, 10: 45, 11: 50, 12: 55, 13: 60, 14: 65, 15: 70,
|
|
16: 75, 17: 80, 18: 85, 19: 90, 20: 95, 21: 100, 22: 110,
|
|
23: 120, 24: 130, 25: 140, 26: 150, 27: 160, 28: 200,
|
|
30: 250
|
|
}
|
|
|
|
v_kph = int(round(v_ms * CV.MS_TO_KPH))
|
|
acc_value = 0
|
|
|
|
for val, limit in sorted(acc_tempolimit_kph.items()):
|
|
if v_kph >= limit:
|
|
acc_value = val
|
|
else:
|
|
break
|
|
|
|
return acc_value
|
|
|
|
def get_acc_warning_meb(self, acc_hud):
|
|
# this works as long our radar does not fault while using OP
|
|
if (acc_hud["ACC_Status_ACC"] in (3, 4) # ACC active or in override mode
|
|
and acc_hud["ACC_EGO_Fahrzeug"] == 2 # a warning for the lead car is active
|
|
and acc_hud["ACC_Optischer_Fahrerhinweis"] != 0 # there is an optical warning
|
|
and acc_hud["ACC_Akustischer_Fahrerhinweis"] != 0 # there is a sound warning
|
|
and acc_hud["ACC_Display_Prio"] == 0): # this warning has highest priority
|
|
return True
|
|
return False
|
|
|
|
class MultiplicativeUnwindPID(PIDController):
|
|
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100, min_cmd=1e-10, ki_red_time=1.0):
|
|
super().__init__(k_p, k_i, k_f=k_f, k_d=k_d, pos_limit=pos_limit, neg_limit=neg_limit, rate=rate)
|
|
self.min_cmd = abs(min_cmd)
|
|
self.ki_red_time = float(ki_red_time)
|
|
self.rate = rate
|
|
self.override_prev = False
|
|
self.i_unwind_factor = 1.0
|
|
|
|
def _calc_unwind_factor(self, override):
|
|
if not override or self.override_prev:
|
|
return
|
|
if self.ki_red_time <= 0.0:
|
|
self.i_unwind_factor = 1.0
|
|
return
|
|
if abs(self.i) <= self.min_cmd:
|
|
self.i_unwind_factor = 0.0
|
|
return
|
|
steps = max(int(self.ki_red_time * self.rate), 1)
|
|
factor = (self.min_cmd / abs(self.i)) ** (1.0 / steps)
|
|
self.i_unwind_factor = min(factor, 1.0)
|
|
|
|
def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
|
|
self.speed = speed
|
|
|
|
self.p = float(error) * self.k_p
|
|
self.f = feedforward * self.k_f
|
|
self.d = error_rate * self.k_d
|
|
|
|
if override:
|
|
self._calc_unwind_factor(override)
|
|
self.i *= self.i_unwind_factor
|
|
if abs(self.i) < self.min_cmd:
|
|
self.i = 0.0
|
|
else:
|
|
if not freeze_integrator:
|
|
self.i = self.i + error * self.k_i * self.i_rate
|
|
|
|
# Clip i to prevent exceeding control limits
|
|
control_no_i = self.p + self.d + self.f
|
|
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
|
|
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
|
|
|
|
control = self.p + self.i + self.d + self.f
|
|
|
|
self.control = np.clip(control, self.neg_limit, self.pos_limit)
|
|
self.override_prev = override
|
|
return self.control
|
|
|
|
class LatControlCurvature():
|
|
def __init__(self, pid_params, limit, rate):
|
|
self.pid = MultiplicativeUnwindPID((pid_params.kpBP, pid_params.kpV),
|
|
(pid_params.kiBP, pid_params.kiV),
|
|
k_f=pid_params.kf, pos_limit=limit, neg_limit=-limit,
|
|
rate=rate, min_cmd=6.7e-6, ki_red_time=2.0)
|
|
def reset(self):
|
|
self.pid.reset()
|
|
|
|
def update(self, CS, CC, desired_curvature):
|
|
actual_curvature_vm = CC.currentCurvature # includes roll
|
|
speed_floor = max(CS.vEgo, 0.1)
|
|
yaw_rate_pose = CC.angularVelocity[2] if len(CC.angularVelocity) > 2 else actual_curvature_vm * speed_floor
|
|
actual_curvature_pose = yaw_rate_pose / speed_floor
|
|
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
|
|
desired_curvature_corr = desired_curvature - CC.rollCompensation
|
|
error = desired_curvature - actual_curvature
|
|
freeze_integrator = CC.steerLimited or CS.vEgo < 5
|
|
output_curvature = self.pid.update(error, feedforward=desired_curvature_corr, speed=CS.vEgo,
|
|
freeze_integrator=freeze_integrator, override=CS.steeringPressed)
|
|
return output_curvature
|