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

fix hem

Browse Source
This commit is contained in:
Prabhaav Pillai
2026-08-25 15:47:40 -04:00
parent 5fe9b3ac8d
commit 8b5e95a9d5
1 changed files with 49 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
starpilot/controls/lib/hybrid_experimental_mode.py
+49 -27
View File
@@ -21,14 +21,16 @@ def smooth_min(a: float, b: float, k: float = 6.0) -> float:
def smooth_max(a: float, b: float, k: float = 6.0) -> float:
return lerp(b, a, sigmoid(a - b, k=k))
class HybridExperimentalMode:
"""
Fuses Chill Mode (radar/lead tracking) and Experimental Mode (vision/stop signs/lights):
1. Detects vision braking intent from the E2E model trajectory.
2. Blends Chill and Exp acceleration smoothly based on intent.
3. Holds 0 m/s at standstills to prevent creep.
4. Falls back safely to Chill if lead vehicle distance is compromised.
5. Slew-rate limits acceleration to respect vehicle jerk limits.
1. Detects vision stopping intent from trajectory v_min and distance horizon.
2. Calculates kinematically required stopping deceleration (-v^2 / 2d).
3. Clamps Chill positive throttle during stop events to prevent brake fighting.
4. Holds 0 m/s at standstills to prevent creep.
5. Falls back safely to Chill if lead vehicle distance is compromised.
6. Slew-rate limits acceleration to respect vehicle jerk limits.
"""
# Base physical actuator jerk limits (m/s^3)
@@ -45,8 +47,8 @@ class HybridExperimentalMode:
self.exp_authority = 0.5
# User tuning
self.HYBRID_EXP_BIAS = 0.0 # [-1.0, 1.0]
self.VISION_BRAKE_SENSITIVITY = 1.0 # [0.0, 2.0]
self.HYBRID_EXP_BIAS = 0.2 # [-1.0, 1.0]
self.VISION_BRAKE_SENSITIVITY = 1.2 # [0.0, 2.0]
# Active profile parameters
self.t_follow = self.BASE_T_FOLLOW
@@ -84,6 +86,14 @@ class HybridExperimentalMode:
return np.array([v_ego], dtype=float)
return np.asarray(traj_v, dtype=float)
@staticmethod
def _get_model_trajectory_x(model_v2) -> np.ndarray:
position = getattr(model_v2, "position", None)
traj_x = getattr(position, "x", None) if position is not None else None
if traj_x is None or len(traj_x) == 0:
return np.array([], dtype=float)
return np.asarray(traj_x, dtype=float)
def update(self, v_ego, v_cruise, lead_one, model_v2, a_chill, a_exp,
t_follow=None, jerk_factor=None):
# 0. Sync profile parameters if passed per-frame
@@ -94,53 +104,65 @@ class HybridExperimentalMode:
lead_status = bool(getattr(lead_one, "status", False))
lead_d_rel = float(getattr(lead_one, "dRel", 150.0))
# 1. VISION INTENT DETECTION (How urgently does the vision model want to slow?)
# 1. VISION INTENT & STOP HORIZON DETECTION
traj_v = self._get_model_trajectory_v(model_v2, v_ego)
v_terminal = float(traj_v[-1])
v_min = float(np.min(traj_v))
traj_x = self._get_model_trajectory_x(model_v2)
min_idx = int(np.argmin(traj_v))
v_min = float(traj_v[min_idx])
d_min = float(traj_x[min_idx]) if len(traj_x) > min_idx else 100.0
v_ref = max(v_ego, 2.0)
speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref)
stop_ahead_intent = max(0.0, (v_ego - v_terminal) / v_ref) * sigmoid(v_ego, k=3.0, x0=1.0)
model_decel_strength = max(0.0, -a_exp / 3.0)
# Detect drop anywhere along trajectory (stop sign / red light profile)
speed_drop_ratio = max(0.0, (v_ego - v_min) / v_ref)
is_stopping_profile = sigmoid(1.5 - v_min, k=4.0, x0=0.0) * sigmoid(v_ego, k=3.0, x0=1.0)
model_decel_strength = max(0.0, -a_exp / 2.5)
raw_vision_metric = max(speed_drop_ratio, stop_ahead_intent, model_decel_strength)
raw_vision_metric = max(speed_drop_ratio, is_stopping_profile, model_decel_strength)
w_vision = float(np.clip(raw_vision_metric * self.VISION_BRAKE_SENSITIVITY, 0.0, 1.0))
# Experimental authority weight (bias + vision confidence)
base_auth = 0.5 + (0.35 * self.HYBRID_EXP_BIAS)
alpha_exp = float(np.clip(base_auth + (0.5 * w_vision), 0.0, 1.0))
# Calculate actual kinematic braking required to stop at the detected stop line
if v_min < 1.2 and v_ego > 1.0 and d_min > 0.5:
# Kinematic decel: -v^2 / (2 * d) with 2.0m stop line cushion
d_stop_effective = max(d_min - 2.0, 1.5)
a_kinematic_stop = - (v_ego ** 2) / (2.0 * d_stop_effective)
a_exp_effective = min(a_exp, a_kinematic_stop)
else:
a_exp_effective = a_exp
# Dynamic Exp Authority: scales directly to 100% when vision intent is high
base_auth = float(np.clip(0.5 + (0.35 * self.HYBRID_EXP_BIAS), 0.0, 1.0))
alpha_exp = lerp(base_auth, 1.0, w_vision)
self.exp_authority = alpha_exp
# 2. ACCELERATION FUSION (Throttle vs Braking Regimes)
# Throttle: Be responsive (smooth_max), but prioritize stopping if vision sees a stop
# Throttle Regime: Snappy pickup on open roads
a_throttle_optimal = smooth_max(a_chill, a_exp, k=4.0)
a_throttle_conservative = smooth_min(a_chill, a_exp, k=4.0)
a_throttle_fused = lerp(a_throttle_optimal, a_throttle_conservative, w_vision)
# Braking: Smoothly hand control to Exp based on vision confidence
a_brake_fused = lerp(a_chill, a_exp, alpha_exp)
# Braking Regime: CLAMP Chill to <= 0 so cruise throttle cannot fight the stop!
a_chill_brake = min(a_chill, 0.0)
a_brake_fused = lerp(a_chill_brake, a_exp_effective, alpha_exp)
# Pick between Accel and Brake regimes
# Regime Selection: If vision sees a stop (w_vision -> 1), FORCE braking regime (w_accel -> 0)
phase_metric = smooth_min(a_chill, a_exp, k=4.0)
w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1)
w_accel = sigmoid(phase_metric, k=3.0, x0=-0.1) * (1.0 - w_vision)
a_fused = lerp(a_brake_fused, a_throttle_fused, w_accel)
# 3. STANDSTILL ANCHOR (Prevent creeping at 0 mph)
is_stopped = sigmoid(0.3 - v_ego, k=8.0, x0=0.0)
is_terminal_stopped = sigmoid(0.8 - v_terminal, k=4.0, x0=0.0)
standstill_weight = is_stopped * is_terminal_stopped
is_stopped = sigmoid(0.4 - v_ego, k=8.0, x0=0.0)
is_min_stopped = sigmoid(0.8 - v_min, k=4.0, x0=0.0)
standstill_weight = is_stopped * is_min_stopped
a_anchored = lerp(a_fused, smooth_min(a_fused, 0.0, k=8.0), standstill_weight)
# 4. SAFETY BARRIER (Lead Vehicle Proximity Check)
d_static_effective = self.D_STATIC_SAFE + max(0.0, 1.5 * (1.0 - (v_ego / 4.0)))
d_safe = (v_ego * self.T_FOLLOW_SAFE) + d_static_effective
# Compute safety risk if lead is within minimum buffer
distance_ratio = (lead_d_rel - d_static_effective) / max(d_safe - d_static_effective, 1.0)
lead_safety_risk = sigmoid(1.0 - distance_ratio, k=5.0, x0=0.0) * float(lead_status)
# Fall back to Chill braking if Chill is more conservative
a_emergency_brake = smooth_min(a_anchored, a_chill, k=6.0)
a_safe = lerp(a_anchored, a_emergency_brake, lead_safety_risk)