diff --git a/cereal/log.capnp b/cereal/log.capnp index 9db3e1d8c..e491c8b2e 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1262,6 +1262,12 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { solverExecutionTime @35 :Float32; + # lead trajectories the MPC solved against (13 points at T_IDXS) + leadTrajectoryX0 @40 :List(Float32); + leadTrajectoryV0 @41 :List(Float32); + leadTrajectoryX1 @42 :List(Float32); + leadTrajectoryV1 @43 :List(Float32); + enum LongitudinalPlanSource { cruise @0; lead0 @1; diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39f08b886..e5e3dab56 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -15,7 +15,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias # WARNING: imports outside of constants will not trigger a rebuild -from openpilot.selfdrive.modeld.constants import index_function +from openpilot.selfdrive.modeld.constants import index_function, ModelConstants if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -30,6 +30,7 @@ EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code") JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") SOURCES = ['lead0', 'lead1', 'cruise', 'e2e'] +LEAD_T_IDXS_MODEL = np.array(ModelConstants.LEAD_T_IDXS) X_DIM = 3 U_DIM = 1 @@ -349,6 +350,8 @@ class LongitudinalMpc: self.time_linearization = 0.0 self.time_integrator = 0.0 self.x0 = np.zeros(X_DIM) + self.lead_xv_0 = np.zeros((N+1, 2)) + self.lead_xv_1 = np.zeros((N+1, 2)) self.set_weights() def set_cost_weights(self, cost_weights, constraint_cost_weights): @@ -484,13 +487,30 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead, tracking_lead=True): + def process_lead(self, model_lead, radar_lead, tracking_lead=True): v_ego = self.x0[1] - if lead is not None and lead.status and tracking_lead: - x_lead = lead.dRel - v_lead = lead.vLead - a_lead = lead.aLeadK - a_lead_tau = lead.aLeadTau + model_prob = float(getattr(model_lead, "prob", 0.0)) + model_x = getattr(model_lead, "x", ()) + model_v = getattr(model_lead, "v", ()) + + if tracking_lead and radar_lead is not None and radar_lead.status: + if model_prob > 0.5 and len(model_x) and len(model_v): + x_lead_traj = float(radar_lead.dRel) + (np.asarray(model_x, dtype=np.float64) - float(model_x[0])) + v_lead_traj = float(radar_lead.vLead) + (np.asarray(model_v, dtype=np.float64) - float(model_v[0])) + + v_lead_0 = float(v_lead_traj[0]) + min_x_lead = ((v_ego + v_lead_0) / 2) * (v_ego - v_lead_0) / (-ACCEL_MIN * 2) + x_lead_traj[0] = max(x_lead_traj[0], min_x_lead) + v_lead_traj = np.clip(v_lead_traj, 0.0, 1e8) + + x_lead_mpc = np.maximum.accumulate(np.interp(T_IDXS, LEAD_T_IDXS_MODEL, x_lead_traj)) + v_lead_mpc = np.interp(T_IDXS, LEAD_T_IDXS_MODEL, v_lead_traj) + return np.column_stack((x_lead_mpc, v_lead_mpc)) + + x_lead = radar_lead.dRel + v_lead = radar_lead.vLead + a_lead = radar_lead.aLeadK + a_lead_tau = radar_lead.aLeadTau else: # Fake a fast lead car, so mpc can keep running in the same mode x_lead = 50.0 @@ -518,15 +538,18 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, + def update(self, radarstate, modelV2, v_cruise, x, v, a, j, danger_factor, t_follow, personality=log.LongitudinalPersonality.standard, tracking_lead=True): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo self.status = (lead_one.status and tracking_lead) or lead_two.status + model_leads = modelV2.leadsV3 - lead_xv_0 = self.process_lead(lead_one, tracking_lead) - lead_xv_1 = self.process_lead(lead_two, tracking_lead) + lead_xv_0 = self.process_lead(model_leads[0] if len(model_leads) > 0 else None, lead_one, tracking_lead) + lead_xv_1 = self.process_lead(model_leads[1] if len(model_leads) > 1 else None, lead_two, tracking_lead) + self.lead_xv_0 = lead_xv_0 + self.lead_xv_1 = lead_xv_1 # 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 @@ -590,8 +613,9 @@ class LongitudinalMpc: self.params[:,4] = t_follow self.run() + lead_one_prob = float(model_leads[0].prob) if len(model_leads) > 0 else float(lead_one.modelProb) if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and - lead_one.modelProb > 0.9): + lead_one_prob > 0.9): self.crash_cnt += 1 else: self.crash_cnt = 0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 68777dbfe..ff660a561 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -454,7 +454,7 @@ class LongitudinalPlanner: dec_mpc_mode = self.get_mpc_mode() if not self.mlsim: self.mpc.mode = dec_mpc_mode - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, + self.mpc.update(sm['radarState'], sm['modelV2'], v_cruise, x, v, a, j, sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow, personality=personality, tracking_lead=lead_control_active) @@ -569,10 +569,16 @@ class LongitudinalPlanner: longitudinalPlan.accels = self.a_desired_trajectory.tolist() longitudinalPlan.jerks = self.j_desired_trajectory.tolist() - longitudinalPlan.hasLead = sm['radarState'].leadOne.status + model_has_lead = len(sm['modelV2'].leadsV3) > 0 and sm['modelV2'].leadsV3[0].prob > 0.5 + longitudinalPlan.hasLead = bool(sm['radarState'].leadOne.status or model_has_lead) longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw + longitudinalPlan.leadTrajectoryX0 = self.mpc.lead_xv_0[:, 0].tolist() + longitudinalPlan.leadTrajectoryV0 = self.mpc.lead_xv_0[:, 1].tolist() + longitudinalPlan.leadTrajectoryX1 = self.mpc.lead_xv_1[:, 0].tolist() + longitudinalPlan.leadTrajectoryV1 = self.mpc.lead_xv_1[:, 1].tolist() + longitudinalPlan.aTarget = float(self.output_a_target) longitudinalPlan.shouldStop = bool(self.output_should_stop) or sm['starpilotPlan'].forcingStopLength < 1 longitudinalPlan.allowBrake = True diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index baf0fc7cc..6151e92e3 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -19,6 +19,10 @@ from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 0.6 +# Compare model v against dRel motion over a short window to correct sustained over-prediction. +_BIAS_FD_WINDOW_S = 2.0 +_BIAS_EMA_TAU = 0.5 + # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -157,16 +161,19 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ return None -def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): +def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, + lead_bias: float = 0.0): prev_aLeadK = getattr(get_RadarState_from_vision, "prev_aLeadK", 0.0) blended_aLeadK = 0.8 * float(lead_msg.a[0]) + 0.2 * prev_aLeadK get_RadarState_from_vision.prev_aLeadK = blended_aLeadK + v_lead = max(0.0, v_ego + lead_msg.v[0] - model_v_ego - lead_bias) + lead_v_rel_pred = v_lead - v_ego return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_msg.v[0] - model_v_ego), - "vLead": float(v_ego + (lead_msg.v[0] - model_v_ego)), - "vLeadK": float(v_ego + (lead_msg.v[0] - model_v_ego)), + "vRel": float(lead_v_rel_pred), + "vLead": float(v_lead), + "vLeadK": float(v_lead), "aLeadK": blended_aLeadK, "aLeadTau": 0.3, "fcw": False, @@ -178,13 +185,13 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, - starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace, + model_v_ego: float, lead_prob: float, model_data: capnp._DynamicStructReader, standstill: bool, + starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace, lead_bias: float = 0.0, low_speed_override: bool = True) -> dict[str, Any]: lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35)) # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_probability: + if len(tracks) > 0 and ready and lead_prob > lead_detection_probability: track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles) else: track = None @@ -192,8 +199,8 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: lead_dict = track.get_RadarState(lead_msg.prob) - elif (track is None) and ready and (lead_msg.prob > lead_detection_probability): - lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) + elif (track is None) and ready and (lead_prob > lead_detection_probability): + lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_bias) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] @@ -230,6 +237,12 @@ class RadarD: self.tracks: dict[int, Track] = {} self.kalman_params = KalmanParams(radar_ts) + self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)] + + self._bias_fd_k = int(round(_BIAS_FD_WINDOW_S / DT_MDL)) + self.lead_drel_hists = [deque(maxlen=self._bias_fd_k + 1) for _ in range(2)] + self.vego_hist_bias = deque(maxlen=self._bias_fd_k + 1) + self.bias_ema_filters = [FirstOrderFilter(0.0, _BIAS_EMA_TAU, DT_MDL) for _ in range(2)] self.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1) @@ -285,10 +298,28 @@ class RadarD: leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], - sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], - sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False) + lead_probs = [self.lead_prob_filters[i].update(leads_v3[i].prob) for i in range(2)] + + self.vego_hist_bias.append(self.v_ego) + lead_biases = [0.0, 0.0] + for i in range(2): + lead_x = float(leads_v3[i].x[0]) if len(leads_v3[i].x) else 0.0 + self.lead_drel_hists[i].append(lead_x) + if len(self.lead_drel_hists[i]) == self._bias_fd_k + 1 and len(self.vego_hist_bias) == self._bias_fd_k + 1: + dt_win = _BIAS_FD_WINDOW_S + v_ego_avg = sum(self.vego_hist_bias) / len(self.vego_hist_bias) + a_ego_win = (self.vego_hist_bias[-1] - self.vego_hist_bias[0]) / dt_win + fd_vlead = v_ego_avg + (self.lead_drel_hists[i][-1] - self.lead_drel_hists[i][0]) / dt_win + a_ego_win * dt_win / 2 + bias_raw = float(leads_v3[i].v[0]) - fd_vlead + self.bias_ema_filters[i].update(bias_raw) + lead_biases[i] = self.bias_ema_filters[i].x + + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, lead_probs[0], + sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'], + self.starpilot_toggles, lead_bias=lead_biases[0], low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, lead_probs[1], + sm['modelV2'], sm['carState'].standstill, sm['starpilotPlan'], + self.starpilot_toggles, lead_bias=lead_biases[1], low_speed_override=False) if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes): self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 262a568ab..c8c141ea1 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -26,7 +26,21 @@ def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead return lead -def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): +def fill_model_lead(lead_msg, lead, prob: float): + lead_msg.prob = float(prob) + lead_msg.probTime = 0.0 + lead_msg.t = [float(t) for t in ModelConstants.LEAD_T_IDXS] + lead_msg.x = [float(lead.dRel + lead.vLead * t) for t in ModelConstants.LEAD_T_IDXS] + lead_msg.xStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.y = [0.0] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.yStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.v = [float(lead.vLead)] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.vStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.a = [float(lead.aLeadK)] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.aStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + + +def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0, lead_one=None, lead_two=None): model = log.ModelDataV2.new_message() t_idxs = ModelConstants.T_IDXS @@ -45,6 +59,13 @@ def make_model(v_ego: float, desired_accel: float, gas_press_prob: float = 1.0): model.acceleration.z = [0.0] * len(t_idxs) model.acceleration.t = [float(t) for t in t_idxs] + model.init('leadsV3', 3) + fill_model_lead(model.leadsV3[0], lead_one if lead_one is not None else make_lead(status=False), + 1.0 if lead_one is not None and lead_one.status else 0.0) + fill_model_lead(model.leadsV3[1], lead_two if lead_two is not None else make_lead(status=False), + 1.0 if lead_two is not None and lead_two.status else 0.0) + fill_model_lead(model.leadsV3[2], make_lead(status=False), 0.0) + model.meta.disengagePredictions.gasPressProbs = [float(gas_press_prob)] * 6 model.action.desiredAcceleration = desired_accel model.action.shouldStop = False @@ -69,7 +90,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta forceDecel=False, ), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), - "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob), + "modelV2": make_model(v_ego, desired_accel, gas_press_prob=gas_press_prob, lead_one=lead_one, lead_two=lead_two), "radarState": SimpleNamespace( leadOne=lead_one if lead_one is not None else make_lead(status=False), leadTwo=lead_two if lead_two is not None else make_lead(status=False), diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 1b688a1e9..5d0296fcb 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -168,6 +168,25 @@ class Plant: acceleration = log.XYZTData.new_message() acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] model.modelV2.acceleration = acceleration + model.modelV2.init('leadsV3', 3) + + def fill_model_lead(lead_msg, d_rel_now, v_lead_now, a_lead_now, prob): + lead_msg.prob = float(prob) + lead_msg.probTime = 0.0 + lead_msg.t = [float(t) for t in ModelConstants.LEAD_T_IDXS] + lead_msg.x = [float(d_rel_now + v_lead_now * t) for t in ModelConstants.LEAD_T_IDXS] + lead_msg.xStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.y = [0.0] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.yStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.v = [float(v_lead_now)] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.vStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.a = [float(a_lead_now)] * len(ModelConstants.LEAD_T_IDXS) + lead_msg.aStd = [0.5] * len(ModelConstants.LEAD_T_IDXS) + + lead0_prob = 0.0 if self.only_lead2 else prob_lead + fill_model_lead(model.modelV2.leadsV3[0], d_rel, v_lead, a_lead, lead0_prob) + fill_model_lead(model.modelV2.leadsV3[1], d_rel, v_lead, a_lead, prob_lead) + fill_model_lead(model.modelV2.leadsV3[2], 200.0, self.speed, 0.0, 0.0) model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)] control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off