From 53d94edd75a5282d55dc8597028c0e4e1bbce797 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 15:09:47 +0100 Subject: [PATCH 1/2] longitudinal mpc: weigh a_change in 1 place only --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f9f9c31bb48207..fddb6bec687417 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -34,7 +34,7 @@ V_EGO_COST = 0. A_EGO_COST = 0. J_EGO_COST = 5.0 -A_CHANGE_COST = .5 +A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .5 LIMIT_COST = 1e6 @@ -136,7 +136,7 @@ def gen_long_mpc_solver(): x_ego, v_ego, a_ego, - 20*(a_ego - prev_a), + a_ego - prev_a, j_ego] ocp.model.cost_y_expr = vertcat(*costs) ocp.model.cost_y_expr_e = vertcat(*costs[:-1]) From 52f556143b4bd8b3c4e6c1e91606306b7f7f0a48 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Feb 2022 16:18:02 +0100 Subject: [PATCH 2/2] longitudinal MPC: modify get_safe_obstacle_distance with smooth transition to avoid creeping --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index fddb6bec687417..3d6286e60349ef 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -57,7 +57,13 @@ 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 + v_min = .5 # below this speed the follow distance does not decrease further + eps = 1e-3 + # smooth_max(v_min, v_ego) + vego_smooth_max = (v_ego + v_min + np.sqrt(eps+ (v_ego-v_min)**2))/2 + # NOTE: one could add "-v_min * T_FOLLOW" + # to achieve desired_follow_distance(0, 0) = STOP_DISTANCE + return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * (vego_smooth_max) + STOP_DISTANCE def desired_follow_distance(v_ego, v_lead): return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)