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