mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-22 16:53:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Planner

Browse Source
This commit is contained in:
firestar5683
2026-03-10 20:13:09 -05:00
parent d7d844ae35
commit be1c67550c
5 changed files with 136 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/controls/lib/frogpilot_following.py
+17 -3
View File
@@ -73,9 +73,23 @@ class FrogPilotFollowing:
self.following_lead = self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
self.disable_throttle = self.frogpilot_planner.tracking_lead and not self.following_lead
self.disable_throttle &= self.frogpilot_planner.lead_one.dRel + 6.0 < (self.t_follow * 2 * 2) * v_ego
self.disable_throttle &= self.frogpilot_planner.lead_one.vLead < v_ego * 0.75
self.disable_throttle = False
if self.frogpilot_planner.tracking_lead and self.frogpilot_planner.lead_one.status:
lead_distance = self.frogpilot_planner.lead_one.dRel
v_lead = self.frogpilot_planner.lead_one.vLead
closing_speed = max(0.0, v_ego - v_lead)
desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow))
ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6
# Keep a mild coasting behavior only for far/low-risk slower leads.
coast_window_open = lead_distance > desired_gap + max(4.0, 0.2 * v_ego)
coast_window_far = lead_distance < desired_gap + max(25.0, 1.2 * v_ego)
gentle_closing = closing_speed < max(2.0, 0.12 * v_ego)
self.disable_throttle = (not self.following_lead and v_ego > 5.0 and coast_window_open and
coast_window_far and gentle_closing)
# Never coast when we are entering a potentially late-braking scenario.
self.disable_throttle &= ttc > 6.0 and lead_distance > desired_gap + 6.0
if sm["controlsState"].enabled and self.frogpilot_planner.tracking_lead:
self.update_follow_values(self.frogpilot_planner.lead_one.dRel, v_ego, self.frogpilot_planner.lead_one.vLead, frogpilot_toggles)
selfdrive/controls/lib/longcontrol.py
-20
View File
@@ -207,26 +207,6 @@ class LongControl:
else:
output_accel = raw_output_accel
if self.long_control_state == LongCtrlState.pid:
# Smooth acceleration and deceleration with urgency-based rate limiting
base_rate = 1.0
if output_accel < self.last_output_accel: # Deceleration requested
decel_needed = self.last_output_accel - output_accel
# Use a safe default for ACCEL_MIN if not available, to prevent division by zero
max_decel = abs(CarControllerParams.ACCEL_MIN) if CarControllerParams.ACCEL_MIN != 0 else 4.0
urgency = min(1.0, decel_needed / max_decel)
# Adjust rate based on urgency (1.0 m/s^3 for low urgency, up to 4.0 m/s^3 for high urgency)
max_rate = 1.0 + 3.0 * urgency
else:
max_rate = base_rate # Acceleration is always smooth
max_accel_change = max_rate * DT_CTRL
output_accel = clip(output_accel,
self.last_output_accel - max_accel_change,
self.last_output_accel + max_accel_change)
self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
return self.last_output_accel
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+4 -3
View File
@@ -460,10 +460,11 @@ class LongitudinalMpc:
def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, tracking_lead, personality=log.LongitudinalPersonality.standard):
v_ego = self.x0[1]
self.status = lead_one.status and tracking_lead or lead_two.status
self.status = lead_one.status or lead_two.status
lead_xv_0 = self.process_lead(lead_one, tracking_lead)
lead_xv_1 = self.process_lead(lead_two, v_ego)
# Always process valid leads for safety; trackingLead can still be used by higher-level logic/UI.
lead_xv_0 = self.process_lead(lead_one, lead_one.status)
lead_xv_1 = self.process_lead(lead_two, lead_two.status)
# 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
selfdrive/controls/lib/longitudinal_planner.py
+90 -17
View File
@@ -116,9 +116,11 @@ class LongitudinalPlanner:
# Lead stability tracking
self.prev_lead_dist = None
self.last_big_brake_t = 0.0
self.last_lead_brake_cmd_t = 0.0
self.stable_lead = False
# Smoothed lead distance
self.lead_dist_f = None
self.last_safety_log_t = 0.0
# Uncertainty slope tracking
@@ -198,6 +200,26 @@ class LongitudinalPlanner:
accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP)
# Safety override: keep profile comfort limits, but increase available braking
# when lead-closing risk rises so chill profiles cannot under-brake.
lead_one = sm['radarState'].leadOne
if lead_one.status:
lead_dist = float(lead_one.dRel)
rel_v = max(0.0, v_ego - float(lead_one.vLead))
ttc = lead_dist / max(rel_v, 0.1) if rel_v > 0.1 else 1e6
desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0
floor_ttc = interp(ttc, [1.6, 2.8, 4.0, 6.0, 10.0],
[ACCEL_MIN, -2.6, -1.8, -1.2, accel_limits_turns[0]])
floor_rel_v = interp(rel_v, [0.0, 1.0, 2.5, 5.0, 8.0],
[accel_limits_turns[0], -1.1, -1.7, -2.5, ACCEL_MIN])
gap_shortfall = max(0.0, desired_gap - lead_dist)
floor_gap = interp(gap_shortfall, [0.0, 2.0, 5.0, 9.0],
[accel_limits_turns[0], -1.2, -2.0, -2.8])
safety_floor = min(accel_limits_turns[0], floor_ttc, floor_rel_v, floor_gap)
accel_limits_turns[0] = max(ACCEL_MIN, safety_floor)
else:
accel_limits = [ACCEL_MIN, ACCEL_MAX]
accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
@@ -206,6 +228,7 @@ class LongitudinalPlanner:
self.v_desired_filter.x = v_ego
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
self.last_lead_brake_cmd_t = 0.0
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -232,8 +255,14 @@ class LongitudinalPlanner:
lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0
# Smooth lead distance (EMA) to avoid chatter in thresholds
alpha = max(0.02, min(0.15, 0.05 + 0.002 * v_ego))
# Keep only light smoothing on lead distance so ACC reacts quickly like stock.
closing_speed = max(0.0, v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0
opening_speed = max(0.0, self.lead_one.vLead - v_ego) if self.lead_one.status else 0.0
alpha = interp(v_ego, [0.0, 8.0, 15.0, 25.0, 35.0], [0.22, 0.28, 0.34, 0.42, 0.48])
if closing_speed > 0.8:
alpha = max(alpha, interp(closing_speed, [0.8, 2.0, 4.0], [0.48, 0.58, 0.66]))
elif opening_speed > 1.0:
alpha = min(alpha, interp(opening_speed, [1.0, 2.5, 4.0], [alpha, 0.22, 0.18]))
if self.lead_dist_f is None:
self.lead_dist_f = float(lead_dist)
else:
@@ -309,9 +338,29 @@ class LongitudinalPlanner:
uncertainty = self.uncert_slow.x
uncertainty_accel = min(self.uncert_slow.x, self.uncert_fast.x)
self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk,
sm['frogpilotPlan'].dangerJerk,
sm['frogpilotPlan'].speedJerk,
accel_jerk_w = sm['frogpilotPlan'].accelerationJerk
danger_jerk_w = sm['frogpilotPlan'].dangerJerk
speed_jerk_w = sm['frogpilotPlan'].speedJerk
# In stable, low-risk car-following, increase smoothing to reduce rubberbanding.
if self.lead_one.status and self.stable_lead:
lead_dist_used = self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel
desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0
gap_err = abs(lead_dist_used - desired_gap)
rel_v_abs = abs(v_ego - self.lead_one.vLead)
closing_v = max(0.0, v_ego - self.lead_one.vLead)
ttc = lead_dist_used / max(closing_v, 0.1) if closing_v > 0.1 else 1e6
gap_ok = gap_err < interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.0, 2.0, 3.5, 5.0])
rel_v_ok = rel_v_abs < interp(v_ego, [0.0, 10.0, 20.0, 35.0], [0.30, 0.60, 0.90, 1.20])
low_risk = (ttc > 3.0) and gap_ok and rel_v_ok
if low_risk:
accel_jerk_w *= interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.00, 1.08, 1.18, 1.26])
speed_jerk_w *= interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.00, 1.04, 1.10, 1.16])
self.mpc.set_weights(accel_jerk_w,
danger_jerk_w,
speed_jerk_w,
prev_accel_constraint,
personality=sm['controlsState'].personality,
v_ego=v_ego,
@@ -339,10 +388,14 @@ class LongitudinalPlanner:
# Safety checks for rubber-banding mitigation
max_jerk = np.max(np.abs(self.mpc.j_solution))
max_accel_change = np.max(np.abs(np.diff(self.mpc.a_solution)))
if max_jerk > 5.0: # m/s^3
cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3")
if max_accel_change > 2.0: # m/s^2
cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2")
now_t = time.monotonic()
if now_t - self.last_safety_log_t > 2.0:
if max_jerk > 5.0: # m/s^3
cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3")
self.last_safety_log_t = now_t
if max_accel_change > 2.0: # m/s^2
cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2")
self.last_safety_log_t = now_t
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
@@ -352,16 +405,36 @@ class LongitudinalPlanner:
# Anticipatory pre-brake to avoid "coming in hot" when closing on a lead
if self.lead_one.status:
rel_v = max(0.0, v_ego - self.lead_one.vLead)
# dynamic time headway adds a small buffer when uncertainty is elevated
base_th = 1.6
th = base_th + 0.6 * max(0.0, uncertainty - 0.42)
desired_gap = th * v_ego
if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5):
k_rel, k_unc = 0.04, 0.20
pre_brake = k_rel * rel_v + k_unc * max(0.0, uncertainty - 0.42)
pre_brake = min(pre_brake, 0.06)
lead_dist_f = self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel
ttc = lead_dist_f / max(rel_v, 0.1) if rel_v > 0.1 else 1e6
desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0
gap_shortfall = max(0.0, desired_gap - lead_dist_f)
pre_brake_dist_trigger = desired_gap + interp(v_ego, [0.0, 10.0, 20.0, 30.0], [5.0, 5.8, 6.8, 8.0])
if rel_v > 0.5 and lead_dist_f < pre_brake_dist_trigger:
pre_brake = 0.0
pre_brake += interp(rel_v, [0.5, 2.0, 5.0, 8.0], [0.0, 0.02, 0.06, 0.11])
pre_brake += interp(ttc, [1.4, 2.2, 3.5, 5.0, 7.5], [0.16, 0.09, 0.04, 0.01, 0.0])
pre_brake += interp(gap_shortfall, [0.0, 2.0, 6.0, 10.0], [0.0, 0.015, 0.04, 0.07])
pre_brake += 0.10 * max(0.0, uncertainty - 0.35)
# Mild low-speed soften to avoid excess early braking while retaining high-speed safety.
pre_brake *= interp(v_ego, [0.0, 8.0, 15.0, 25.0], [0.50, 0.68, 0.88, 1.00])
pre_brake = min(pre_brake, interp(v_ego, [0.0, 5.0, 15.0, 30.0], [0.05, 0.08, 0.13, 0.16]))
self.a_desired = float(self.a_desired - pre_brake)
# Shape accel release after low-speed lead-brake events to reduce stop-and-go brake->surge snapback.
if v_ego < 8.0 and rel_v > 0.2 and lead_dist_f < desired_gap + 2.5 and self.a_desired < -0.35:
self.last_lead_brake_cmd_t = now_t
t_since_brake = now_t - self.last_lead_brake_cmd_t
release_window = interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.6, 0.7, 0.8, 0.9])
low_risk_release = ttc > 2.0 and rel_v < interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.3, 0.45, 0.6, 0.75])
near_lead = lead_dist_f < desired_gap + 2.0
if 0.0 < t_since_brake < release_window and v_ego < 8.0 and near_lead and low_risk_release and self.a_desired > -0.05:
release_cap_t = interp(t_since_brake, [0.0, 0.15, 0.35, 0.60, release_window], [0.05, 0.14, 0.24, 0.34, 0.48])
release_cap_v = interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.15, 0.24, 0.34, 0.42])
self.a_desired = float(min(self.a_desired, min(release_cap_t, release_cap_v)))
# Small deadzone around zero accel to kill micro-dithers
if -0.05 < self.a_desired < 0.05:
self.a_desired = 0.0
selfdrive/test/longitudinal_maneuvers/plant.py
+25 -2
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import time
import types
import numpy as np
from cereal import log
@@ -62,6 +63,7 @@ class Plant:
radar = messaging.new_message('radarState')
control = messaging.new_message('controlsState')
car_state = messaging.new_message('carState')
lp = messaging.new_message('liveParameters')
car_control = messaging.new_message('carControl')
model = messaging.new_message('modelV2')
a_lead = (v_lead - self.v_lead_prev)/self.ts
@@ -113,22 +115,43 @@ class Plant:
model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
control.controlsState.enabled = bool(self.enabled)
control.controlsState.vCruise = float(v_cruise * 3.6)
control.controlsState.experimentalMode = self.e2e
control.controlsState.personality = self.personality
control.controlsState.forceDecel = self.force_decel
car_state.carState.vEgo = float(self.speed)
car_state.carState.standstill = self.speed < 0.01
car_state.carState.vCruise = float(v_cruise * 3.6)
# Backward/forward compatible cruise field for host-side maneuver tests
if hasattr(car_state.carState, "vCruise"):
car_state.carState.vCruise = float(v_cruise * 3.6)
elif hasattr(car_state.carState, "cruiseState") and hasattr(car_state.carState.cruiseState, "speed"):
car_state.carState.cruiseState.speed = float(v_cruise)
car_control.carControl.orientationNED = [0., float(pitch), 0.]
# ******** get controlsState messages for plotting ***
frogpilot_plan = types.SimpleNamespace(
vCruise=float(v_cruise),
minAcceleration=-3.5,
maxAcceleration=2.0,
disableThrottle=False,
accelerationJerk=5.0,
dangerJerk=5.0,
speedJerk=5.0,
tFollow=1.45,
trackingLead=True,
forcingStopLength=2.0,
)
lp.liveParameters.angleOffsetDeg = 0.0
sm = {'radarState': radar.radarState,
'carState': car_state.carState,
'carControl': car_control.carControl,
'controlsState': control.controlsState,
'liveParameters': lp.liveParameters,
'frogpilotPlan': frogpilot_plan,
'modelV2': model.modelV2}
self.planner.update(sm)
self.planner.update(False, sm, types.SimpleNamespace(model_version='v11', taco_tune=False))
self.speed = self.planner.v_desired_filter.x
self.acceleration = self.planner.a_desired
self.speeds = self.planner.v_desired_trajectory.tolist()