From cc6e90c2f52114e00769e87801d2afb1db5eb993 Mon Sep 17 00:00:00 2001 From: KRKeegan Date: Fri, 14 Jan 2022 14:55:53 -0800 Subject: [PATCH 1/5] Decrease Stop_Distance As Lead Pulls Away This rapidly decreases the following distance when the lead pulls away, resulting in an early demand for acceleration. This is helpful because the MPC often stops ~4m behind the lead. Meaning the lead has to move ~2m before the MPC even considers requesting acceleration. The offset is capped at half the total Stop_Distance, is linearly proportional to the speed differential between ego and lead, requires the lead to be traveling faster than Ego, and tapers away to 0 at 10m/s, all for safety and comfort. --- .../lib/longitudinal_mpc_lib/long_mpc.py | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 2c821ef33f337a..81c620d1e58b3e 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -55,14 +55,26 @@ COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_stopped_equivalence_factor(v_lead): - return (v_lead**2) / (2 * COMFORT_BRAKE) +def get_stopped_equivalence_factor(v_lead, v_ego): + # KRKeegan this offset rapidly decreases the following distance when the lead pulls + # away, resulting in an early demand for acceleration. This is helpful because the + # MPC often stops ~4m behind the lead. Meaning the lead has to move ~2m before the + # MPC even considers requesting acceleration. + # The offset is capped at half the total Stop_Distance, is linearly proportional to + # the speed differential between ego and lead, and tapers away to 0 at 10m/s, all + # for safety and comfort. + v_diff_offset = 0 + if np.all(v_lead - v_ego > 0): + v_diff_offset = ((v_lead - v_ego) * 1.5) * ((10 - v_ego)/10) + v_diff_offset = np.minimum(v_diff_offset, STOP_DISTANCE / 2) + distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset + return distance def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW): return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW): - return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) + return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead, v_ego) def gen_long_model(): @@ -344,8 +356,8 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False): # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance # and then treat that as a stopped car/obstacle at this new distance. - lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) - lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) + lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.x_sol[:,1]) + lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.x_sol[:,1]) # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. From 0d3a8c7d0bcae1fddacb2cab9677102b557233bd Mon Sep 17 00:00:00 2001 From: KRKeegan Date: Fri, 21 Jan 2022 11:22:53 -0800 Subject: [PATCH 2/5] Clip vDiff at 0; Apply VDiff tapering after clipping If not clipped, it can go negative at higher speeds causing more distance. Also if tapering is not last, then there can be a clip-like transition. --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 5 +++-- 1 file changed, 3 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 81c620d1e58b3e..12b411bd420bf2 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -65,8 +65,9 @@ def get_stopped_equivalence_factor(v_lead, v_ego): # for safety and comfort. v_diff_offset = 0 if np.all(v_lead - v_ego > 0): - v_diff_offset = ((v_lead - v_ego) * 1.5) * ((10 - v_ego)/10) - v_diff_offset = np.minimum(v_diff_offset, STOP_DISTANCE / 2) + v_diff_offset = ((v_lead - v_ego) * 1.5) + v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2) + v_diff_offset = v_diff_offset * ((10 - v_ego)/10) distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset return distance From 1e4010b506eb0d50074ece842d059975290374db Mon Sep 17 00:00:00 2001 From: KRKeegan Date: Sat, 22 Jan 2022 15:13:51 -0800 Subject: [PATCH 3/5] Prevent vDiff from Going Negative --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 1 file changed, 1 insertion(+), 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 12b411bd420bf2..9a954af131dc6f 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -67,7 +67,7 @@ def get_stopped_equivalence_factor(v_lead, v_ego): if np.all(v_lead - v_ego > 0): v_diff_offset = ((v_lead - v_ego) * 1.5) v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2) - v_diff_offset = v_diff_offset * ((10 - v_ego)/10) + v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0) distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset return distance From 41b7cdedd390043c913ac760b01daeeee8ada123 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 25 Jan 2022 14:47:50 -0800 Subject: [PATCH 4/5] tune costs --- .../controls/lib/dynamic_follow/__init__.py | 2 -- .../lib/longitudinal_mpc_lib/long_mpc.py | 17 ++++++++++------- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/dynamic_follow/__init__.py b/selfdrive/controls/lib/dynamic_follow/__init__.py index 2ee4f5285f19fa..a460827f3f3b03 100644 --- a/selfdrive/controls/lib/dynamic_follow/__init__.py +++ b/selfdrive/controls/lib/dynamic_follow/__init__.py @@ -2,7 +2,6 @@ import numpy as np import cereal.messaging as messaging from common.realtime import sec_since_boot, DT_MDL -from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG from common.op_params import opParams from common.numpy_fast import interp, clip, mean from selfdrive.config import Conversions as CV @@ -103,7 +102,6 @@ def _setup_changing_variables(self): self.lead_data = LeadData() self.df_data = dfData() # dynamic follow data - self.last_cost = 0.0 self.last_predict_time = 0.0 self.auto_df_model_data = [] self._get_live_params() # so they're defined just in case diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9a954af131dc6f..7513743f9be242 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -254,14 +254,17 @@ def set_weights_for_lead_policy(self): # 1.1 TR fails at 3+ m/s/s test # 1.2-1.8 TR succeeds at all tests with no FCW - # TRs = [1.2, 1.8, 2.7] - x_ego_obstacle_cost_multiplier = 1 # interp(self.desired_TR, TRs, [3., 1.0, 0.1]) - j_ego_cost_multiplier = 1 # interp(self.desired_TR, TRs, [0.5, 1.0, 1.0]) - d_zone_cost_multiplier = 1 # interp(self.desired_TR, TRs, [4., 1.0, 1.0]) - - W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, X_EGO_COST, V_EGO_COST, A_EGO_COST, A_CHANGE_COST, J_EGO_COST * j_ego_cost_multiplier])) + TRs = [1.2, 1.6, 2.7] + x_ego_obstacle_cost_multiplier = interp(self.desired_TR, TRs, [3., 1.0, 0.1]) + accel_cost_multiplier = interp(self.desired_TR, TRs, [0.75, 1.0, 1.25]) + d_zone_cost_multiplier = interp(self.desired_TR, TRs, [4., 1.0, 1.0]) + + W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, + X_EGO_COST, V_EGO_COST, A_EGO_COST, + A_CHANGE_COST * accel_cost_multiplier, + J_EGO_COST * accel_cost_multiplier])) for i in range(N): - W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) + W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0]) * accel_cost_multiplier self.solver.cost_set(i, 'W', W) # Setting the slice without the copy make the array not contiguous, # causing issues with the C interface. From ff006c1f449938a7e12a2d0f3379237c9126cd48 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 25 Jan 2022 14:47:57 -0800 Subject: [PATCH 5/5] follow plan sooner --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 262777fee2afed..aea92e617db7d5 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -70,7 +70,7 @@ def update(self, sm): prev_accel_constraint = True if long_control_state == LongCtrlState.off or sm['carState'].gasPressed: self.v_desired_filter.x = v_ego - self.a_desired = a_ego + self.a_desired = 0.0 # Smoothly changing between accel trajectory is only relevant when OP is driving prev_accel_constraint = False