You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
335 lines
13 KiB
335 lines
13 KiB
import math
|
|
import numpy as np
|
|
from common.numpy_fast import clip, interp
|
|
import selfdrive.messaging as messaging
|
|
|
|
# lookup tables VS speed to determine min and max accels in cruise
|
|
_A_CRUISE_MIN_V = [-1.0, -.8, -.67, -.5, -.30]
|
|
_A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
|
|
|
# need fast accel at very low speed for stop and go
|
|
_A_CRUISE_MAX_V = [1., 1., .8, .5, .30]
|
|
_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
|
|
|
|
def calc_cruise_accel_limits(v_ego):
|
|
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
|
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
|
|
return np.vstack([a_cruise_min, a_cruise_max])
|
|
|
|
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
|
|
_A_TOTAL_MAX_BP = [0., 20., 40.]
|
|
|
|
def limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP):
|
|
#*** this function returns a limited long acceleration allowed, depending on the existing lateral acceleration
|
|
# this should avoid accelerating when losing the target in turns
|
|
deg_to_rad = np.pi / 180. # from can reading to rad
|
|
|
|
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
|
a_y = v_ego**2 * angle_steers * deg_to_rad / (VP.steer_ratio * VP.wheelbase)
|
|
a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
|
|
|
|
a_target[1] = min(a_target[1], a_x_allowed)
|
|
a_pcm = min(a_pcm, a_x_allowed)
|
|
return a_target, a_pcm
|
|
|
|
def process_a_lead(a_lead):
|
|
# soft threshold of 0.5m/s^2 applied to a_lead to reject noise, also not considered positive a_lead
|
|
a_lead_threshold = 0.5
|
|
a_lead = min(a_lead + a_lead_threshold, 0)
|
|
return a_lead
|
|
|
|
def calc_desired_distance(v_lead):
|
|
#*** compute desired distance ***
|
|
t_gap = 1.7 # good to be far away
|
|
d_offset = 4 # distance when at zero speed
|
|
return d_offset + v_lead * t_gap
|
|
|
|
|
|
#linear slope
|
|
_L_SLOPE_V = [0.40, 0.10]
|
|
_L_SLOPE_BP = [0., 40]
|
|
|
|
# parabola slope
|
|
_P_SLOPE_V = [1.0, 0.25]
|
|
_P_SLOPE_BP = [0., 40]
|
|
|
|
def calc_desired_speed(d_lead, d_des, v_lead, a_lead):
|
|
#*** compute desired speed ***
|
|
# the desired speed curve is divided in 4 portions:
|
|
# 1-constant
|
|
# 2-linear to regain distance
|
|
# 3-linear to shorten distance
|
|
# 4-parabolic (constant decel)
|
|
|
|
max_runaway_speed = -2. # no slower than 2m/s over the lead
|
|
|
|
# interpolate the lookups to find the slopes for a give lead speed
|
|
l_slope = interp(v_lead, _L_SLOPE_BP, _L_SLOPE_V)
|
|
p_slope = interp(v_lead, _P_SLOPE_BP, _P_SLOPE_V)
|
|
|
|
# this is where parabola and linear curves are tangents
|
|
x_linear_to_parabola = p_slope / l_slope**2
|
|
|
|
# parabola offset to have the parabola being tangent to the linear curve
|
|
x_parabola_offset = p_slope / (2 * l_slope**2)
|
|
|
|
if d_lead < d_des:
|
|
# calculate v_rel_des on the line that connects 0m at max_runaway_speed to d_des
|
|
v_rel_des_1 = (- max_runaway_speed) / d_des * (d_lead - d_des)
|
|
# calculate v_rel_des on one third of the linear slope
|
|
v_rel_des_2 = (d_lead - d_des) * l_slope / 3.
|
|
# take the min of the 2 above
|
|
v_rel_des = min(v_rel_des_1, v_rel_des_2)
|
|
v_rel_des = max(v_rel_des, max_runaway_speed)
|
|
elif d_lead < d_des + x_linear_to_parabola:
|
|
v_rel_des = (d_lead - d_des) * l_slope
|
|
v_rel_des = max(v_rel_des, max_runaway_speed)
|
|
else:
|
|
v_rel_des = math.sqrt(2 * (d_lead - d_des - x_parabola_offset) * p_slope)
|
|
|
|
# compute desired speed
|
|
v_target = v_rel_des + v_lead
|
|
|
|
# compute v_coast: above this speed we want to coast
|
|
t_lookahead = 1. # how far in time we consider a_lead to anticipate the coast region
|
|
v_coast_shift = max(a_lead * t_lookahead, - v_lead) # don't consider projections that would make v_lead<0
|
|
v_coast = (v_lead + v_target)/2 + v_coast_shift # no accel allowed above this line
|
|
v_coast = min(v_coast, v_target)
|
|
|
|
return v_target, v_coast
|
|
|
|
def calc_critical_decel(d_lead, v_rel, d_offset, v_offset):
|
|
# this function computes the required decel to avoid crashing, given safety offsets
|
|
a_critical = - max(0., v_rel + v_offset)**2/max(2*(d_lead - d_offset), 0.5)
|
|
return a_critical
|
|
|
|
|
|
# maximum acceleration adjustment
|
|
_A_CORR_BY_SPEED_V = [0.4, 0.4, 0]
|
|
# speeds
|
|
_A_CORR_BY_SPEED_BP = [0., 5., 20.]
|
|
|
|
def calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ref, v_rel_ref, v_coast, v_target, a_lead_contr, a_max):
|
|
a_coast_min = -1.0 # never coast faster then -1m/s^2
|
|
# coasting behavior above v_coast. Forcing a_max to be negative will force the pid_speed to decrease,
|
|
# regardless v_target
|
|
if v_ref > min(v_coast, v_target):
|
|
# for smooth coast we can be agrressive and target a point where car would actually crash
|
|
v_offset_coast = 0.
|
|
d_offset_coast = d_des/2. - 4.
|
|
|
|
# acceleration value to smoothly coast until we hit v_target
|
|
if d_lead > d_offset_coast + 0.1:
|
|
a_coast = calc_critical_decel(d_lead, v_rel_ref, d_offset_coast, v_offset_coast)
|
|
# if lead is decelerating, then offset the coast decel
|
|
a_coast += a_lead_contr
|
|
a_max = max(a_coast, a_coast_min)
|
|
else:
|
|
a_max = a_coast_min
|
|
else:
|
|
# same as cruise accel, but add a small correction based on lead acceleration at low speeds
|
|
# when lead car accelerates faster, we can do the same, and vice versa
|
|
|
|
a_max = a_max + interp(v_ego, _A_CORR_BY_SPEED_BP, _A_CORR_BY_SPEED_V) \
|
|
* clip(-v_rel / 4., -.5, 1)
|
|
return a_max
|
|
|
|
# arbitrary limits to avoid too high accel being computed
|
|
_A_SAT = [-10., 5.]
|
|
|
|
# do not consider a_lead at 0m/s, fully consider it at 10m/s
|
|
_A_LEAD_LOW_SPEED_V = [0., 1.]
|
|
|
|
# speed break points
|
|
_A_LEAD_LOW_SPEED_BP = [0., 10.]
|
|
|
|
# add a small offset to the desired decel, just for safety margin
|
|
_DECEL_OFFSET_V = [-0.3, -0.5, -0.5, -0.4, -0.3]
|
|
|
|
# speed bp: different offset based on the likelyhood that lead decels abruptly
|
|
_DECEL_OFFSET_BP = [0., 4., 15., 30, 40.]
|
|
|
|
|
|
def calc_acc_accel_limits(d_lead, d_des, v_ego, v_pid, v_lead, v_rel, a_lead,
|
|
v_target, v_coast, a_target, a_pcm):
|
|
#*** compute max accel ***
|
|
# v_rel is now your velocity in lead car frame
|
|
v_rel = -v_rel # this simplifiess things when thinking in d_rel-v_rel diagram
|
|
|
|
v_rel_pid = v_pid - v_lead
|
|
|
|
# this is how much lead accel we consider in assigning the desired decel
|
|
a_lead_contr = a_lead * interp(v_lead, _A_LEAD_LOW_SPEED_BP,
|
|
_A_LEAD_LOW_SPEED_V) * 0.8
|
|
|
|
# first call of calc_positive_accel_limit is used to shape v_pid
|
|
a_target[1] = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_pid,
|
|
v_rel_pid, v_coast, v_target,
|
|
a_lead_contr, a_target[1])
|
|
# second call of calc_positive_accel_limit is used to limit the pcm throttle
|
|
# control (only useful when we don't control throttle directly)
|
|
a_pcm = calc_positive_accel_limit(d_lead, d_des, v_ego, v_rel, v_ego, v_rel,
|
|
v_coast, v_target, a_lead_contr, a_pcm)
|
|
|
|
#*** compute max decel ***
|
|
v_offset = 1. # assume the car is 1m/s slower
|
|
d_offset = 1. # assume the distance is 1m lower
|
|
if v_target - v_ego > 0.5:
|
|
pass # acc target speed is above vehicle speed, so we can use the cruise limits
|
|
elif d_lead > d_offset + 0.01: # add small value to avoid by zero divisions
|
|
# compute needed accel to get to 1m distance with -1m/s rel speed
|
|
decel_offset = interp(v_lead, _DECEL_OFFSET_BP, _DECEL_OFFSET_V)
|
|
|
|
critical_decel = calc_critical_decel(d_lead, v_rel, d_offset, v_offset)
|
|
a_target[0] = min(decel_offset + critical_decel + a_lead_contr,
|
|
a_target[0])
|
|
else:
|
|
a_target[0] = _A_SAT[0]
|
|
# a_min can't be higher than a_max
|
|
a_target[0] = min(a_target[0], a_target[1])
|
|
# final check on limits
|
|
a_target = np.clip(a_target, _A_SAT[0], _A_SAT[1])
|
|
a_target = a_target.tolist()
|
|
return a_target, a_pcm
|
|
|
|
def calc_jerk_factor(d_lead, v_rel):
|
|
# we don't have an explicit jerk limit, so this function calculates a factor
|
|
# that is used by the PID controller to scale the gains. Not the cleanest solution
|
|
# but we need this for the demo.
|
|
# TODO: Calculate Kp and Ki directly in this function.
|
|
|
|
# the higher is the decel required to avoid a crash, the higher is the PI factor scaling
|
|
d_offset = 0.5
|
|
v_offset = 2.
|
|
a_offset = 1.
|
|
jerk_factor_max = 1.0 # can't increase Kp and Ki more than double.
|
|
if d_lead < d_offset + 0.1: # add small value to avoid by zero divisions
|
|
jerk_factor = jerk_factor_max
|
|
else:
|
|
a_critical = - calc_critical_decel(d_lead, -v_rel, d_offset, v_offset)
|
|
# increase Kp and Ki by 20% for every 1m/s2 of decel required above 1m/s2
|
|
jerk_factor = max(a_critical - a_offset, 0.)/5.
|
|
jerk_factor = min(jerk_factor, jerk_factor_max)
|
|
return jerk_factor
|
|
|
|
|
|
def calc_ttc(d_rel, v_rel, a_rel, v_lead):
|
|
# this function returns the time to collision (ttc), assuming that a_rel will stay constant
|
|
# TODO: Review these assumptions.
|
|
# change sign to rel quantities as it's going to be easier for calculations
|
|
v_rel = -v_rel
|
|
a_rel = -a_rel
|
|
|
|
# assuming that closing gap a_rel comes from lead vehicle decel, then limit a_rel so that v_lead will get to zero in no sooner than t_decel
|
|
# this helps overweighting a_rel when v_lead is close to zero.
|
|
t_decel = 2.
|
|
a_rel = min(a_rel, v_lead/t_decel)
|
|
|
|
delta = v_rel**2 + 2 * d_rel * a_rel
|
|
# assign an arbitrary high ttc value if there is no solution to ttc
|
|
if delta < 0.1:
|
|
ttc = 5.
|
|
elif math.sqrt(delta) + v_rel < 0.1:
|
|
ttc = 5.
|
|
else:
|
|
ttc = 2 * d_rel / (math.sqrt(delta) + v_rel)
|
|
return ttc
|
|
|
|
|
|
def limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status):
|
|
decel_bp = [0. , 40.]
|
|
decel_v = [-0.3, -0.2]
|
|
decel = interp(v_ego, decel_bp, decel_v)
|
|
# gives 18 seconds before decel begins (w 6 minute timeout)
|
|
if awareness_status < -0.05:
|
|
a_target[1] = min(a_target[1], decel)
|
|
a_target[0] = min(a_target[1], a_target[0])
|
|
a_pcm = 0.
|
|
return a_target, a_pcm
|
|
|
|
MAX_SPEED_POSSIBLE = 55.
|
|
|
|
def compute_speed_with_leads(v_ego, angle_steers, v_pid, l1, l2, awareness_status, VP):
|
|
# drive limits
|
|
# TODO: Make lims function of speed (more aggressive at low speed).
|
|
a_lim = [-3., 1.5]
|
|
|
|
#*** set target speed pretty high, as lead hasn't been considered yet
|
|
v_target_lead = MAX_SPEED_POSSIBLE
|
|
|
|
#*** set accel limits as cruise accel/decel limits ***
|
|
a_target = calc_cruise_accel_limits(v_ego)
|
|
# Always 1 for now.
|
|
a_pcm = 1
|
|
|
|
#*** limit max accel in sharp turns
|
|
a_target, a_pcm = limit_accel_in_turns(v_ego, angle_steers, a_target, a_pcm, VP)
|
|
jerk_factor = 0.
|
|
|
|
if l1 is not None and l1.status:
|
|
#*** process noisy a_lead signal from radar processing ***
|
|
a_lead_p = process_a_lead(l1.aLeadK)
|
|
|
|
#*** compute desired distance ***
|
|
d_des = calc_desired_distance(l1.vLead)
|
|
|
|
#*** compute desired speed ***
|
|
v_target_lead, v_coast = calc_desired_speed(l1.dRel, d_des, l1.vLead, a_lead_p)
|
|
|
|
if l2 is not None and l2.status:
|
|
#*** process noisy a_lead signal from radar processing ***
|
|
a_lead_p2 = process_a_lead(l2.aLeadK)
|
|
|
|
#*** compute desired distance ***
|
|
d_des2 = calc_desired_distance(l2.vLead)
|
|
|
|
#*** compute desired speed ***
|
|
v_target_lead2, v_coast2 = calc_desired_speed(l2.dRel, d_des2, l2.vLead, a_lead_p2)
|
|
|
|
# listen to lead that makes you go slower
|
|
if v_target_lead2 < v_target_lead:
|
|
l1 = l2
|
|
d_des, a_lead_p, v_target_lead, v_coast = d_des2, a_lead_p2, v_target_lead2, v_coast2
|
|
|
|
# l1 is the main lead now
|
|
|
|
#*** compute accel limits ***
|
|
a_target1, a_pcm1 = calc_acc_accel_limits(l1.dRel, d_des, v_ego, v_pid, l1.vLead,
|
|
l1.vRel, a_lead_p, v_target_lead, v_coast, a_target, a_pcm)
|
|
|
|
# we can now limit a_target to a_lim
|
|
a_target = np.clip(a_target1, a_lim[0], a_lim[1])
|
|
a_pcm = np.clip(a_pcm1, a_lim[0], a_lim[1]).tolist()
|
|
|
|
#*** compute max factor ***
|
|
jerk_factor = calc_jerk_factor(l1.dRel, l1.vRel)
|
|
|
|
# force coasting decel if driver hasn't been controlling car in a while
|
|
a_target, a_pcm = limit_accel_driver_awareness(v_ego, a_target, a_pcm, awareness_status)
|
|
|
|
return v_target_lead, a_target, a_pcm, jerk_factor
|
|
|
|
|
|
class AdaptiveCruise(object):
|
|
def __init__(self, live20):
|
|
self.live20 = live20
|
|
self.last_cal = 0.
|
|
self.l1, self.l2 = None, None
|
|
self.logMonoTime = 0
|
|
self.dead = True
|
|
def update(self, cur_time, v_ego, angle_steers, v_pid, awareness_status, VP):
|
|
l20 = messaging.recv_sock(self.live20)
|
|
if l20 is not None:
|
|
self.l1 = l20.live20.leadOne
|
|
self.l2 = l20.live20.leadTwo
|
|
self.logMonoTime = l20.logMonoTime
|
|
|
|
# TODO: no longer has anything to do with calibration
|
|
self.last_cal = cur_time
|
|
self.dead = False
|
|
elif cur_time - self.last_cal > 0.5:
|
|
self.dead = True
|
|
|
|
self.v_target_lead, self.a_target, self.a_pcm, self.jerk_factor = \
|
|
compute_speed_with_leads(v_ego, angle_steers, v_pid, self.l1, self.l2, awareness_status, VP)
|
|
self.has_lead = self.v_target_lead != MAX_SPEED_POSSIBLE
|
|
|