pull/23296/head
Shane Smiskol 3 years ago
parent b80b08e6e9
commit 481c390f40
  1. 9
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@ -39,7 +39,6 @@ DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .5 CRASH_DISTANCE = .5
LIMIT_COST = 1e6 LIMIT_COST = 1e6
# Less timestamps doesn't hurt performance and leads to # Less timestamps doesn't hurt performance and leads to
# much better convergence of the MPC with low iterations # much better convergence of the MPC with low iterations
N = 12 N = 12
@ -53,12 +52,15 @@ T_FOLLOW = 1.45
COMFORT_BRAKE = 2.5 COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0 STOP_DISTANCE = 6.0
def get_stopped_equivalence_factor(v_lead): def get_stopped_equivalence_factor(v_lead):
return (v_lead ** 2) / (2 * COMFORT_BRAKE) return (v_lead ** 2) / (2 * COMFORT_BRAKE)
def get_safe_obstacle_distance(v_ego): def get_safe_obstacle_distance(v_ego):
return (v_ego ** 2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE return (v_ego ** 2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE
def desired_follow_distance(v_ego, v_lead): def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead) return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
@ -144,7 +146,7 @@ def gen_long_mpc_solver():
# Constraints on speed, acceleration and desired distance to # Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it # the obstacle, which is treated as a slack constraint so it
# behaves like an assymetrical cost. # behaves like an assymetrical cost.
constraints = vertcat((v_ego), constraints = vertcat(v_ego,
(a_ego - a_min), (a_ego - a_min),
(a_max - a_ego), (a_max - a_ego),
((x_obstacle - x_ego) - (3 / 4) * (desired_dist_comfort)) / (v_ego + 10.)) ((x_obstacle - x_ego) - (3 / 4) * (desired_dist_comfort)) / (v_ego + 10.))
@ -190,7 +192,7 @@ def gen_long_mpc_solver():
return ocp return ocp
class LongitudinalMpc(): class LongitudinalMpc:
def __init__(self, e2e=False): def __init__(self, e2e=False):
self.e2e = e2e self.e2e = e2e
self.reset() self.reset()
@ -355,7 +357,6 @@ class LongitudinalMpc():
self.prev_a[:, None]], axis=1) self.prev_a[:, None]], axis=1)
self.run() self.run()
def run(self): def run(self):
for i in range(N + 1): for i in range(N + 1):
self.solver.set(i, 'p', self.params[i]) self.solver.set(i, 'p', self.params[i])

Loading…
Cancel
Save